|
|
|
/* 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;
|
|
|
|
|
|
|
|
float sensors_gyro_roll;
|
|
|
|
float sensors_gyro_pitch;
|
|
|
|
float sensors_gyro_yaw;
|
|
|
|
|
|
|
|
float sensors_temp;
|
|
|
|
|
|
|
|
float sensors_accel_x;
|
|
|
|
float sensors_accel_y;
|
|
|
|
float sensors_accel_z;
|
|
|
|
|
|
|
|
float gyro_yaw_zero;
|
|
|
|
float gyro_pitch_zero;
|
|
|
|
float gyro_roll_zero;
|
|
|
|
|
|
|
|
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(float roll, float pitch, float yaw)
|
|
|
|
{
|
|
|
|
#if 0
|
|
|
|
sensors_gyro_roll = roll - gyro_roll_zero;
|
|
|
|
sensors_gyro_pitch = pitch - gyro_pitch_zero;
|
|
|
|
sensors_gyro_yaw = yaw - gyro_yaw_zero;
|
|
|
|
#else
|
|
|
|
sensors_gyro_roll = roll;
|
|
|
|
sensors_gyro_pitch = pitch;
|
|
|
|
sensors_gyro_yaw = yaw;
|
|
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
|
|
void sensors_write_accel_data(float x, float y, float z)
|
|
|
|
{
|
|
|
|
sensors_accel_x = x;
|
|
|
|
sensors_accel_y = y;
|
|
|
|
sensors_accel_z = z;
|
|
|
|
}
|
|
|
|
|
|
|
|
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_roll);
|
|
|
|
log_put_float(sensors_gyro_pitch);
|
|
|
|
log_put_float(sensors_gyro_yaw);
|
|
|
|
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
|
|
|
|
dcm_update(-sensors_gyro_pitch, -sensors_gyro_roll,
|
|
|
|
-sensors_gyro_yaw);
|
|
|
|
#else
|
|
|
|
dcm_update(0.0, 0.0, 0.0);
|
|
|
|
#endif
|
|
|
|
if (!status_armed()) {
|
|
|
|
if ( (abs(sensors_gyro_roll) < GYRO_RATE_THRESHOLD) &&
|
|
|
|
(abs(sensors_gyro_pitch) < GYRO_RATE_THRESHOLD) &&
|
|
|
|
(abs(sensors_gyro_yaw) < 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_yaw_zero += sensors_gyro_yaw;
|
|
|
|
gyro_pitch_zero += sensors_gyro_pitch;
|
|
|
|
gyro_roll_zero += sensors_gyro_roll;
|
|
|
|
sensors_generation++;
|
|
|
|
if (sensors_generation >= GYRO_ZERO_COUNT) {
|
|
|
|
sensors_zero = FALSE;
|
|
|
|
sensors_update = TRUE;
|
|
|
|
sensors_generation = 0;
|
|
|
|
gyro_yaw_zero /= GYRO_ZERO_COUNT;
|
|
|
|
gyro_pitch_zero /= GYRO_ZERO_COUNT;
|
|
|
|
gyro_roll_zero /= GYRO_ZERO_COUNT;
|
|
|
|
putstr("Zero finished\r\n");
|
|
|
|
status_set_ready(STATUS_MODULE_GYRO_ZERO, TRUE);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
watchdog_kick(WATCHDOG_GYRO);
|
|
|
|
|
|
|
|
#if 1
|
|
|
|
dcm_drift_correction(-sensors_accel_y, -sensors_accel_x,
|
|
|
|
-sensors_accel_z);
|
|
|
|
#endif
|
|
|
|
#if 0
|
|
|
|
dcm_drift_correction(sensors_accel_x, sensors_accel_y,
|
|
|
|
sensors_accel_z);
|
|
|
|
#endif
|
|
|
|
|
|
|
|
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_roll * 1000.0));
|
|
|
|
putstr(",");
|
|
|
|
putint_s((int)(sensors_gyro_pitch * 1000.0));
|
|
|
|
putstr(",");
|
|
|
|
putint_s((int)(sensors_gyro_yaw * 1000.0));
|
|
|
|
putstr(")");
|
|
|
|
|
|
|
|
putstr("(");
|
|
|
|
putint_s((int)(sensors_temp * 1000.0));
|
|
|
|
putstr(")\r\n");
|
|
|
|
}
|