The unified diff between revisions [056a532c..] 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 [056a532c92301bcb224e1f786c5f6720e8acf3eb] # new_revision [dc88787ecd1d574feba045763baed2a7651ff33d] # # patch "sensors.c" # from [129fa873f84fea1677a1d27563afd5402f8d6677] # to [178f9bbaac429772d293d7749bb23c32fbed0c98] # ============================================================ --- sensors.c 129fa873f84fea1677a1d27563afd5402f8d6677 +++ sensors.c 178f9bbaac429772d293d7749bb23c32fbed0c98 @@ -1,6 +1,8 @@ /* sensors.c */ #include "mpu6050.h" +#include "hmc5883l.h" +#include "mpl3115a2.h" #include "dcm.h" #include "fisqrt.h" #include "watchdog.h" @@ -11,10 +13,19 @@ #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; @@ -24,20 +35,17 @@ static unsigned int sensors_generation; static unsigned int sensors_discard; static unsigned int sensors_generation; -float sensors_gyro_roll; -float sensors_gyro_pitch; -float sensors_gyro_yaw; +/* x = roll + y = pitch + z = yaw + */ +vec3f sensors_gyro; +vec3f gyro_zero; float sensors_temp; -float sensors_accel_x; -float sensors_accel_y; -float sensors_accel_z; +vec3f sensors_accel; -float gyro_yaw_zero; -float gyro_pitch_zero; -float gyro_roll_zero; - void sensors_write_log(void); void sensors_process(void); @@ -53,10 +61,13 @@ bool sensors_init(void) bool sensors_init(void) { + unsigned int i; + next_sensor = 0; - if (!mpu6050_init()) - return FALSE; + for (i = 0; i < SENSOR_INIT_FNS; i++) + if (!(sensor_init_fns[i])()) + return FALSE; return TRUE; } @@ -65,7 +76,7 @@ bool sensors_next_sample(void) { bool result; - result = (sensor_start_fns[next_sensor])(); + result = (sensor_start_fns[next_sensor++])(); if (next_sensor >= SENSOR_START_FNS) next_sensor = 0; @@ -89,24 +100,20 @@ bool sensors_start_sample(void) return sensors_next_sample(); } -void sensors_write_gyro_data(float roll, float pitch, float yaw) +void sensors_write_gyro_data(vec3f gyro) { #if 0 - sensors_gyro_roll = roll - gyro_roll_zero; - sensors_gyro_pitch = pitch - gyro_pitch_zero; - sensors_gyro_yaw = yaw - gyro_yaw_zero; + 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_roll = roll; - sensors_gyro_pitch = pitch; - sensors_gyro_yaw = yaw; + sensors_gyro = gyro; #endif } -void sensors_write_accel_data(float x, float y, float z) +void sensors_write_accel_data(vec3f accel) { - sensors_accel_x = x; - sensors_accel_y = y; - sensors_accel_z = z; + sensors_accel = accel; } void sensors_write_temp_data(float temp) @@ -122,12 +129,12 @@ 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_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 */ @@ -149,20 +156,19 @@ void sensors_process(void) { if (sensors_update) { #if 1 - dcm_update(-sensors_gyro_pitch, -sensors_gyro_roll, - -sensors_gyro_yaw); + vec3f dcm_gyro = {-sensors_gyro.y, -sensors_gyro.x, -sensors_gyro.z}; #else - dcm_update(0.0, 0.0, 0.0); + vec3f dcm_gyro = {0.0, 0.0, 0.0}; #endif + dcm_update(dcm_gyro); 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)) { + 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++; @@ -178,17 +184,17 @@ void sensors_process(void) if (sensors_discard) { sensors_discard--; } else { - gyro_yaw_zero += sensors_gyro_yaw; - gyro_pitch_zero += sensors_gyro_pitch; - gyro_roll_zero += sensors_gyro_roll; + 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_yaw_zero /= GYRO_ZERO_COUNT; - gyro_pitch_zero /= GYRO_ZERO_COUNT; - gyro_roll_zero /= GYRO_ZERO_COUNT; + 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); } @@ -197,13 +203,12 @@ void sensors_process(void) watchdog_kick(WATCHDOG_GYRO); #if 1 - dcm_drift_correction(-sensors_accel_y, -sensors_accel_x, - -sensors_accel_z); + vec3f dcm_accel = {-sensors_accel.y, -sensors_accel.x, -sensors_accel.z}; #endif #if 0 - dcm_drift_correction(sensors_accel_x, sensors_accel_y, - sensors_accel_z); + vec3f dcm_accel = {sensors_accel.x, sensors_accel.y, sensors_accel.z}; #endif + dcm_drift_correction(dcm_accel); watchdog_kick(WATCHDOG_ACCEL); stick_input(); @@ -212,19 +217,19 @@ void sensors_dump(void) void sensors_dump(void) { putstr("("); - putint_s((int)(sensors_accel_x * 1000.0)); + putint_s((int)(sensors_accel.x * 1000.0)); putstr(","); - putint_s((int)(sensors_accel_y * 1000.0)); + putint_s((int)(sensors_accel.y * 1000.0)); putstr(","); - putint_s((int)(sensors_accel_z * 1000.0)); + putint_s((int)(sensors_accel.z * 1000.0)); putstr(")"); putstr("("); - putint_s((int)(sensors_gyro_roll * 1000.0)); + putint_s((int)(sensors_gyro.x * 1000.0)); putstr(","); - putint_s((int)(sensors_gyro_pitch * 1000.0)); + putint_s((int)(sensors_gyro.y * 1000.0)); putstr(","); - putint_s((int)(sensors_gyro_yaw * 1000.0)); + putint_s((int)(sensors_gyro.z * 1000.0)); putstr(")"); putstr("(");