The unified diff between revisions [4f22e7ef..] and [dc88787e..] is displayed below. It can also be downloaded as a raw diff.
This diff has been restricted to the following files: 'sensors.c'
# # old_revision [4f22e7ef7d3064e3b51a5b868a4722f3f13c747b] # new_revision [dc88787ecd1d574feba045763baed2a7651ff33d] # # add_file "sensors.c" # content [178f9bbaac429772d293d7749bb23c32fbed0c98] # ============================================================ --- /dev/null +++ sensors.c 178f9bbaac429772d293d7749bb23c32fbed0c98 @@ -0,0 +1,238 @@ +/* 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"); +}