From fa30825d015d92bdbc95d2684386beef9fe9dc01 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Mon, 8 Jun 2020 21:26:23 -0700 Subject: [PATCH] altos: Leave accel data in ring raw; swap for orientation on fetch Instead of re-writing the acceleration data in the ring to adjust for pad orientation, just leave it alone and flip it each time it is fetched. Much simpler this way. Signed-off-by: Keith Packard --- src/kernel/ao_data.c | 15 +++++++++++---- src/kernel/ao_data.h | 40 ++++++++++++++++------------------------ src/kernel/ao_sample.c | 5 +---- 3 files changed, 28 insertions(+), 32 deletions(-) diff --git a/src/kernel/ao_data.c b/src/kernel/ao_data.c index 536ffa2b..77cd98e4 100644 --- a/src/kernel/ao_data.c +++ b/src/kernel/ao_data.c @@ -27,11 +27,18 @@ volatile uint8_t ao_data_present; void ao_data_get(struct ao_data *packet) { -#if HAS_FLIGHT - uint8_t i = ao_data_ring_prev(ao_sample_data); -#else uint8_t i = ao_data_ring_prev(ao_data_head); -#endif memcpy(packet, (void *) &ao_data_ring[i], sizeof (struct ao_data)); } #endif + +#if HAS_ACCEL +accel_t +ao_data_accel(volatile struct ao_data *packet) { + accel_t raw; + raw = ao_data_accel_raw(packet); + if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) + raw = ao_data_accel_invert(raw); + return raw; +} +#endif diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h index a22eee87..b43a1cd4 100644 --- a/src/kernel/ao_data.h +++ b/src/kernel/ao_data.h @@ -197,9 +197,7 @@ typedef AO_ALT_TYPE alt_t; /* * Need a few macros to pull data from the sensors: * - * ao_data_accel_sample - pull raw sensor and convert to normalized values - * ao_data_accel - pull normalized value (lives in the same memory) - * ao_data_set_accel - store normalized value back in the sensor location + * ao_data_accel_raw - pull raw sensor * ao_data_accel_invert - flip rocket ends for positive acceleration */ @@ -211,8 +209,7 @@ typedef AO_ALT_TYPE alt_t; */ typedef int16_t accel_t; -#define ao_data_accel(packet) ((packet)->adc.accel) -#define ao_data_set_accel(packet, a) ((packet)->adc.accel = (a)) +#define ao_data_accel_raw(packet) ((packet)->adc.accel) #define ao_data_accel_invert(a) (0x7fff -(a)) /* @@ -298,12 +295,12 @@ typedef int16_t accel_t; #if HAS_ACCEL_REF -#define ao_data_accel_cook(packet) \ +#define ao_data_accel_raw(packet) \ ((uint16_t) ((((uint32_t) (packet)->adc.accel << 16) / ((packet)->adc.accel_ref << 1))) >> 1) #else -#define ao_data_accel_cook(packet) ((packet)->adc.accel) +#define ao_data_accel_raw(packet) ((packet)->adc.accel) #endif /* HAS_ACCEL_REF */ @@ -323,13 +320,11 @@ typedef int16_t accel_t; #error AO_MMA655X_INVERT not defined #endif -#define ao_data_accel(packet) ((packet)->mma655x) #if AO_MMA655X_INVERT -#define ao_data_accel_cook(packet) (AO_ACCEL_INVERT - (packet)->mma655x) +#define ao_data_accel_raw(packet) (AO_ACCEL_INVERT - (packet)->mma655x) #else -#define ao_data_accel_cook(packet) ((packet)->mma655x) +#define ao_data_accel_raw(packet) ((packet)->mma655x) #endif -#define ao_data_set_accel(packet, accel) ((packet)->mma655x = (accel)) #define ao_data_accel_invert(accel) (AO_ACCEL_INVERT - (accel)) #endif @@ -344,13 +339,11 @@ typedef int16_t accel_t; #error AO_ADXL375_INVERT not defined #endif -#define ao_data_accel(packet) ((packet)->adxl375.AO_ADXL375_AXIS) #if AO_ADXL375_INVERT -#define ao_data_accel_cook(packet) (-ao_data_accel(packet)) +#define ao_data_accel_raw(packet) (-(packet)->adxl375.AO_ADXL375_AXIS) #else -#define ao_data_accel_cook(packet) ao_data_accel(packet) +#define ao_data_accel_raw(packet) ((packet)->adxl375.AO_ADXL375_AXIS) #endif -#define ao_data_set_accel(packet, accel) (ao_data_accel(packet) = (accel)) #define ao_data_accel_invert(accel) (-(accel)) #endif /* HAS_ADXL375 */ @@ -362,9 +355,7 @@ typedef int16_t accel_t; typedef int16_t accel_t; /* MPU6000 is hooked up so that positive y is positive acceleration */ -#define ao_data_accel(packet) ((packet)->z_accel) -#define ao_data_accel_cook(packet) (-(packet)->mpu6000.accel_y) -#define ao_data_set_accel(packet, accel) ((packet)->z_accel = (accel)) +#define ao_data_accel_raw(packet) (-(packet)->mpu6000.accel_y) #define ao_data_accel_invert(a) (-(a)) #endif @@ -407,9 +398,7 @@ static inline float ao_convert_accel(int16_t sensor) typedef int16_t accel_t; /* MPU9250 is hooked up so that positive y is positive acceleration */ -#define ao_data_accel(packet) ((packet)->z_accel) -#define ao_data_accel_cook(packet) (-(packet)->mpu9250.accel_y) -#define ao_data_set_accel(packet, accel) ((packet)->z_accel = (accel)) +#define ao_data_accel_raw(packet) (-(packet)->mpu9250.accel_y) #define ao_data_accel_invert(a) (-(a)) #endif @@ -453,10 +442,8 @@ static inline float ao_convert_accel(int16_t sensor) typedef int16_t accel_t; -#define ao_data_accel(packet) ((packet)->z_accel) -#define ao_data_set_accel(packet, accel) ((packet)->z_accel = (accel)) +#define ao_data_accel_raw(packet) -ao_data_along(packet) #define ao_data_accel_invert(a) (-(a)) - #define ao_data_accel_to_sample(accel) ao_bmx_accel_to_sample(accel) #endif @@ -555,4 +542,9 @@ ao_data_fill(int head) { #endif +#if HAS_ACCEL +accel_t +ao_data_accel(volatile struct ao_data *packet); +#endif + #endif /* _AO_DATA_H_ */ diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index b3e12b19..391188c5 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -348,10 +348,7 @@ ao_sample(void) #endif #if HAS_ACCEL - ao_sample_accel = ao_data_accel_cook(ao_data); - if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) - ao_sample_accel = ao_data_accel_invert(ao_sample_accel); - ao_data_set_accel(ao_data, ao_sample_accel); + ao_sample_accel = ao_data_accel(ao_data); #endif #if HAS_GYRO ao_sample_accel_along = ao_data_along(ao_data); -- 2.30.2