|
|
|
/* sensors.c */
|
|
|
|
|
|
|
|
#include "mpu6050.h"
|
|
|
|
#include "hmc5883l.h"
|
|
|
|
#include "mpl3115a2.h"
|
|
|
|
#include "dcm.h"
|
|
|
|
#include "fisqrt.h"
|
|
|
|
#include "watchdog.h"
|
|
|
|
#include "status.h"
|
|
|
|
#include "abs.h"
|
|
|
|
#include "panic.h"
|
|
|
|
#include "uart.h"
|
|
|
|
#include "log.h"
|
|
|
|
#include "stick.h"
|
|
|
|
|
|
|
|
bool (*sensor_init_fns[])(void) = {
|
|
|
|
mpu6050_init,
|
|
|
|
hmc5883l_init,
|
|
|
|
mpl3115a2_init,
|
|
|
|
};
|
|
|
|
|
|
|
|
bool (*sensor_start_fns[])(void) = {
|
|
|
|
mpu6050_start_sample,
|
|
|
|
hmc5883l_start_sample,
|
|
|
|
mpl3115a2_start_sample,
|
|
|
|
};
|
|
|
|
|
|
|
|
#define SENSOR_INIT_FNS (sizeof(sensor_init_fns)/sizeof(sensor_init_fns[0]))
|
|
|
|
#define SENSOR_START_FNS (sizeof(sensor_start_fns)/sizeof(sensor_start_fns[0]))
|
|
|
|
|
|
|
|
static unsigned int next_sensor;
|
|
|
|
|
|
|
|
static bool sensors_zero;
|
|
|
|
static bool sensors_update;
|
|
|
|
static unsigned int sensors_discard;
|
|
|
|
static unsigned int sensors_generation;
|
|
|
|
|
|
|
|
/* x = roll
|
|
|
|
y = pitch
|
|
|
|
z = yaw
|
|
|
|
*/
|
|
|
|
vec3f sensors_gyro;
|
|
|
|
vec3f gyro_zero;
|
|
|
|
|
|
|
|
float sensors_temp;
|
|
|
|
|
|
|
|
vec3f sensors_accel;
|
|
|
|
|
|
|
|
void sensors_write_log(void);
|
|
|
|
void sensors_process(void);
|
|
|
|
|
|
|
|
#define TWO_PI 6.28318531f
|
|
|
|
#define DEG_TO_RAD (TWO_PI/360.0f)
|
|
|
|
|
|
|
|
/* The gyro has to stay within this limit in each axis in order to arm */
|
|
|
|
#define GYRO_RATE_THRESHOLD (0.01 / DEG_TO_RAD)
|
|
|
|
|
|
|
|
#define GYRO_ZERO_COUNT 1000
|
|
|
|
|
|
|
|
void sensors_dump(void);
|
|
|
|
|
|
|
|
bool sensors_init(void)
|
|
|
|
{
|
|
|
|
unsigned int i;
|
|
|
|
|
|
|
|
next_sensor = 0;
|
|
|
|
|
|
|
|
for (i = 0; i < SENSOR_INIT_FNS; i++)
|
|
|
|
if (!(sensor_init_fns[i])())
|
|
|
|
return FALSE;
|
|
|
|
|
|
|
|
return TRUE;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool sensors_next_sample(void)
|
|
|
|
{
|
|
|
|
bool result;
|
|
|
|
|
|
|
|
result = (sensor_start_fns[next_sensor++])();
|
|
|
|
if (next_sensor >= SENSOR_START_FNS)
|
|
|
|
next_sensor = 0;
|
|
|
|
|
|
|
|
return result;
|
|
|
|
}
|
|
|
|
|
|
|
|
void sensors_sample_done(void)
|
|
|
|
{
|
|
|
|
if (next_sensor == 0) {
|
|
|
|
sensors_write_log();
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!sensors_next_sample())
|
|
|
|
panic(PANIC_SENSOR_FAIL);
|
|
|
|
}
|
|
|
|
|
|
|
|
bool sensors_start_sample(void)
|
|
|
|
{
|
|
|
|
next_sensor = 0;
|
|
|
|
return sensors_next_sample();
|
|
|
|
}
|
|
|
|
|
|
|
|
void sensors_write_gyro_data(vec3f gyro)
|
|
|
|
{
|
|
|
|
#if 0
|
|
|
|
sensors_gyro.x = gyro.x - gyro_zero.x;
|
|
|
|
sensors_gyro.y = gyro.y - gyro_zero.y;
|
|
|
|
sensors_gyro.z = gyro.z - gyro_zero.z;
|
|
|
|
#else
|
|
|
|
sensors_gyro = gyro;
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
void sensors_write_accel_data(vec3f accel)
|
|
|
|
{
|
|
|
|
sensors_accel = accel;
|
|
|
|
}
|
|
|
|
|
|
|
|
void sensors_write_temp_data(float temp)
|
|
|
|
{
|
|
|
|
sensors_temp = temp;
|
|
|
|
/* XXX HACK find a better place for this call */
|
|
|
|
sensors_process();
|
|
|
|
}
|
|
|
|
|
|
|
|
#define LOG_SIGNATURE_SENSORS 0xDA7ADA7A
|
|
|
|
#define LOG_SIGNATURE_SENSORS2 0xDA7AF173
|
|
|
|
void sensors_write_log(void)
|
|
|
|
{
|
|
|
|
#if 0
|
|
|
|
log_put_uint(LOG_SIGNATURE_SENSORS);
|
|
|
|
log_put_float(sensors_accel.x);
|
|
|
|
log_put_float(sensors_accel.y);
|
|
|
|
log_put_float(sensors_accel.z);
|
|
|
|
log_put_float(sensors_gyro.x);
|
|
|
|
log_put_float(sensors_gyro.y);
|
|
|
|
log_put_float(sensors_gyro.z);
|
|
|
|
log_put_float(sensors_temp);
|
|
|
|
#else
|
|
|
|
/* XXX this just about comes out in the right place, but by luck */
|
|
|
|
log_put_uint(LOG_SIGNATURE_SENSORS2);
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
void sensors_start_zero(void)
|
|
|
|
{
|
|
|
|
sensors_zero = TRUE;
|
|
|
|
sensors_update = FALSE;
|
|
|
|
sensors_discard = 100;
|
|
|
|
sensors_generation = 0;
|
|
|
|
putstr("Starting zero\r\n");
|
|
|
|
mpu6050_start_zero();
|
|
|
|
}
|
|
|
|
|
|
|
|
void sensors_process(void)
|
|
|
|
{
|
|
|
|
if (sensors_update) {
|
|
|
|
#if 1
|
|
|
|
vec3f dcm_gyro = {-sensors_gyro.y, -sensors_gyro.x, -sensors_gyro.z};
|
|
|
|
#else
|
|
|
|
vec3f dcm_gyro = {0.0, 0.0, 0.0};
|
|
|
|
#endif
|
|
|
|
dcm_update(dcm_gyro);
|
|
|
|
if (!status_armed()) {
|
|
|
|
if ( (abs(sensors_gyro.x) < GYRO_RATE_THRESHOLD) &&
|
|
|
|
(abs(sensors_gyro.y) < GYRO_RATE_THRESHOLD) &&
|
|
|
|
(abs(sensors_gyro.z) < GYRO_RATE_THRESHOLD)) {
|
|
|
|
status_set_ready(STATUS_MODULE_GYRO_RATE, TRUE);
|
|
|
|
} else {
|
|
|
|
status_set_ready(STATUS_MODULE_GYRO_RATE, FALSE);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
sensors_generation++;
|
|
|
|
|
|
|
|
#if SEND_DCM
|
|
|
|
if ((sensors_generation % 40) == 0) {
|
|
|
|
dcm_send_packet();
|
|
|
|
sensors_dump();
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
} else if (sensors_zero) {
|
|
|
|
if (sensors_discard) {
|
|
|
|
sensors_discard--;
|
|
|
|
} else {
|
|
|
|
gyro_zero.z += sensors_gyro.z;
|
|
|
|
gyro_zero.y += sensors_gyro.y;
|
|
|
|
gyro_zero.x += sensors_gyro.x;
|
|
|
|
sensors_generation++;
|
|
|
|
if (sensors_generation >= GYRO_ZERO_COUNT) {
|
|
|
|
sensors_zero = FALSE;
|
|
|
|
sensors_update = TRUE;
|
|
|
|
sensors_generation = 0;
|
|
|
|
gyro_zero.z /= GYRO_ZERO_COUNT;
|
|
|
|
gyro_zero.y /= GYRO_ZERO_COUNT;
|
|
|
|
gyro_zero.x /= GYRO_ZERO_COUNT;
|
|
|
|
putstr("Zero finished\r\n");
|
|
|
|
status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
watchdog_kick(WATCHDOG_GYRO);
|
|
|
|
|
|
|
|
#if 1
|
|
|
|
vec3f dcm_accel = {-sensors_accel.y, -sensors_accel.x, -sensors_accel.z};
|
|
|
|
#endif
|
|
|
|
#if 0
|
|
|
|
vec3f dcm_accel = {sensors_accel.x, sensors_accel.y, sensors_accel.z};
|
|
|
|
#endif
|
|
|
|
dcm_drift_correction(dcm_accel);
|
|
|
|
|
|
|
|
watchdog_kick(WATCHDOG_ACCEL);
|
|
|
|
stick_input();
|
|
|
|
}
|
|
|
|
|
|
|
|
void sensors_dump(void)
|
|
|
|
{
|
|
|
|
putstr("(");
|
|
|
|
putint_s((int)(sensors_accel.x * 1000.0));
|
|
|
|
putstr(",");
|
|
|
|
putint_s((int)(sensors_accel.y * 1000.0));
|
|
|
|
putstr(",");
|
|
|
|
putint_s((int)(sensors_accel.z * 1000.0));
|
|
|
|
putstr(")");
|
|
|
|
|
|
|
|
putstr("(");
|
|
|
|
putint_s((int)(sensors_gyro.x * 1000.0));
|
|
|
|
putstr(",");
|
|
|
|
putint_s((int)(sensors_gyro.y * 1000.0));
|
|
|
|
putstr(",");
|
|
|
|
putint_s((int)(sensors_gyro.z * 1000.0));
|
|
|
|
putstr(")");
|
|
|
|
|
|
|
|
putstr("(");
|
|
|
|
putint_s((int)(sensors_temp * 1000.0));
|
|
|
|
putstr(")\r\n");
|
|
|
|
}
|