The unified diff between revisions [4f22e7ef..] and [9142f333..] 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 [9142f3330490a5aa00c1686475633b620c2ef5e7] # # add_file "sensors.c" # content [129fa873f84fea1677a1d27563afd5402f8d6677] # ============================================================ --- /dev/null +++ sensors.c 129fa873f84fea1677a1d27563afd5402f8d6677 @@ -0,0 +1,233 @@ +/* sensors.c */ + +#include "mpu6050.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_start_fns[])(void) = { + mpu6050_start_sample, +}; + +#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) +{ + next_sensor = 0; + + if (!mpu6050_init()) + 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"); +}