The unified diff between revisions [bfc9e27f..] 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 [bfc9e27f5c40da31ae4269394aaf0545e5856a70] # new_revision [dc88787ecd1d574feba045763baed2a7651ff33d] # # patch "sensors.c" # from [ba01cef6cd239a12b9171dccb25fd02092ca037c] # to [178f9bbaac429772d293d7749bb23c32fbed0c98] # ============================================================ --- sensors.c ba01cef6cd239a12b9171dccb25fd02092ca037c +++ sensors.c 178f9bbaac429772d293d7749bb23c32fbed0c98 @@ -35,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); @@ -103,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) @@ -136,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 */ @@ -163,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++; @@ -192,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); } @@ -211,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(); @@ -226,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("(");