From f10009b07b651f69014ac5608f3ca29bce874c24 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Fri, 16 Oct 2020 12:59:48 -0700 Subject: [PATCH] altos: Add motor pressure calibration data to easy motor log Compute an average of ground motor pressure values and store those in the log data in the AO_FLIGHT record. Signed-off-by: Keith Packard --- src/easymotor-v2/ao_pins.h | 12 +++++++++--- src/kernel/ao_log.h | 3 ++- src/kernel/ao_log_motor.c | 5 ++++- src/kernel/ao_sample.c | 19 +++++++++++++++++++ src/kernel/ao_sample.h | 4 ++++ 5 files changed, 38 insertions(+), 5 deletions(-) diff --git a/src/easymotor-v2/ao_pins.h b/src/easymotor-v2/ao_pins.h index 6ea08e1a..1d970e19 100644 --- a/src/easymotor-v2/ao_pins.h +++ b/src/easymotor-v2/ao_pins.h @@ -104,14 +104,14 @@ #define AO_DATA_RING 64 struct ao_adc { - int16_t pressure; + int16_t motor_pressure; int16_t v_batt; }; #define AO_ADC_DUMP(p) \ - printf("tick: %5u pressure: %5d batt: %5d\n", \ + printf("tick: %5u motor_pressure: %5d batt: %5d\n", \ (p)->tick, \ - (p)->adc.pressure, \ + (p)->adc.motor_pressure, \ (p)->adc.v_batt); @@ -162,4 +162,10 @@ struct ao_adc { #define HAS_IMU 1 #define USE_ADXL375_IMU 1 +/* Motor pressure */ +#define HAS_MOTOR_PRESSURE 1 +#define ao_data_motor_pressure(packet) ((packet)->adc.motor_pressure) + +typedef int16_t motor_pressure_t; + #endif /* _AO_PINS_H_ */ diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h index 12eefe74..5c150abc 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -514,7 +514,8 @@ struct ao_log_motor { int16_t ground_accel_along; /* 8 */ int16_t ground_accel_across; /* 10 */ int16_t ground_accel_through; /* 12 */ - } flight; + int16_t ground_motor_pressure; /* 14 */ + } flight; /* 16 */ /* AO_LOG_STATE */ struct { uint16_t state; /* 4 */ diff --git a/src/kernel/ao_log_motor.c b/src/kernel/ao_log_motor.c index 18d6b79e..3fca80fb 100644 --- a/src/kernel/ao_log_motor.c +++ b/src/kernel/ao_log_motor.c @@ -48,6 +48,9 @@ ao_log(void) ao_log_data.type = AO_LOG_FLIGHT; ao_log_data.tick = ao_sample_tick; ao_log_data.u.flight.ground_accel = ao_ground_accel; + ao_log_data.u.flight.ground_accel_along = ao_ground_accel_along; + ao_log_data.u.flight.ground_accel_through = ao_ground_accel_through; + ao_log_data.u.flight.ground_motor_pressure = ao_ground_motor_pressure; ao_log_data.u.flight.flight = ao_flight_number; ao_log_write(&ao_log_data); @@ -63,7 +66,7 @@ ao_log(void) ao_log_data.tick = ao_data_ring[ao_log_data_pos].tick; if ((int16_t) (ao_log_data.tick - next_sensor) >= 0) { ao_log_data.type = AO_LOG_SENSOR; - ao_log_data.u.sensor.pressure = ao_data_ring[ao_log_data_pos].adc.pressure; + ao_log_data.u.sensor.pressure = ao_data_motor_pressure(&ao_data_ring[ao_log_data_pos]); ao_log_data.u.sensor.v_batt = ao_data_ring[ao_log_data_pos].adc.v_batt; ao_log_data.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]); ao_log_data.u.sensor.accel_across = ao_data_across(&ao_data_ring[ao_log_data_pos]); diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 46771496..874c9110 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -57,6 +57,9 @@ angle_t ao_sample_orient; angle_t ao_sample_orients[AO_NUM_ORIENT]; uint8_t ao_sample_orient_pos; #endif +#ifdef HAS_MOTOR_PRESSURE +motor_pressure_t ao_sample_motor_pressure; +#endif uint8_t ao_sample_data; @@ -87,6 +90,10 @@ int32_t ao_ground_yaw; int32_t ao_ground_roll; #endif +#if HAS_MOTOR_PRESSURE +motor_pressure_t ao_ground_motor_pressure; +#endif + static uint8_t ao_preflight; /* in preflight mode */ static uint16_t nsamples; @@ -107,6 +114,9 @@ int32_t ao_sample_yaw_sum; int32_t ao_sample_roll_sum; static struct ao_quaternion ao_rotation; #endif +#if HAS_MOTOR_PRESSURE +int32_t ao_sample_motor_pressure_sum; +#endif #if HAS_FLIGHT_DEBUG extern uint8_t ao_orient_test; @@ -130,6 +140,9 @@ ao_sample_preflight_add(void) ao_sample_pitch_sum += ao_sample_pitch; ao_sample_yaw_sum += ao_sample_yaw; ao_sample_roll_sum += ao_sample_roll; +#endif +#if HAS_MOTOR_PRESSURE + ao_sample_motor_pressure_sum += ao_sample_motor_pressure; #endif ++nsamples; } @@ -199,6 +212,9 @@ ao_sample_preflight_set(void) ao_ground_accel_through = ao_sample_accel_through_sum >> 9; #endif +#if HAS_MOTOR_PRESSURE + ao_ground_motor_pressure = ao_sample_motor_pressure_sum >> 9; +#endif #if HAS_GYRO ao_ground_pitch = ao_sample_pitch_sum; ao_ground_yaw = ao_sample_yaw_sum; @@ -379,6 +395,9 @@ ao_sample(void) ao_sample_yaw = ao_data_yaw(ao_data); ao_sample_roll = ao_data_roll(ao_data); #endif +#if HAS_MOTOR_PRESSURE + ao_sample_motor_pressure = ao_data_motor_pressure(ao_data); +#endif if (ao_preflight) ao_sample_preflight(); diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index 475b3f63..8e64095b 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -156,6 +156,10 @@ extern angle_t ao_sample_orient; extern angle_t ao_sample_orients[AO_NUM_ORIENT]; extern uint8_t ao_sample_orient_pos; #endif +#if HAS_MOTOR_PRESSURE +extern motor_pressure_t ao_ground_motor_pressure; +extern motor_pressure_t ao_sample_motor_pressure; +#endif void ao_sample_init(void); -- 2.30.2