From 440292f53125d7da29b3b4e8caa2c94c557b8325 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 12 Feb 2020 13:01:08 -0800 Subject: [PATCH] altos: Add new telemetry packet format for TeleMega v4 Bmx160 instead of MPU9250 changes orientation of IMU data Signed-off-by: Keith Packard --- src/kernel/ao_telemetry.c | 26 ++++++++++++++++++++++++-- src/kernel/ao_telemetry.h | 3 ++- 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index 2aceaf28..2092c84f 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -139,9 +139,17 @@ ao_send_mega_sensor(void) struct ao_data *packet = (struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)]; telemetry.generic.tick = packet->tick; - telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR; - +#if HAS_BMX160 + telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR_BMX160; +#else #if HAS_MPU6000 || HAS_MPU9250 + telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR_MPU; +#else +#error unknown IMU +#endif +#endif + +#if HAS_GYRO telemetry.mega_sensor.orient = ao_sample_orient; #endif telemetry.mega_sensor.accel = ao_data_accel(packet); @@ -178,6 +186,20 @@ ao_send_mega_sensor(void) telemetry.mega_sensor.mag_y = packet->mpu9250.mag_y; #endif +#if HAS_BMX160 + telemetry.mega_sensor.accel_x = packet->bmx160.acc_x; + telemetry.mega_sensor.accel_y = packet->bmx160.acc_y; + telemetry.mega_sensor.accel_z = packet->bmx160.acc_z; + + telemetry.mega_sensor.gyro_x = packet->bmx160.gyr_x; + telemetry.mega_sensor.gyro_y = packet->bmx160.gyr_y; + telemetry.mega_sensor.gyro_z = packet->bmx160.gyr_z; + + telemetry.mega_sensor.mag_x = packet->bmx160.mag_x; + telemetry.mega_sensor.mag_z = packet->bmx160.mag_z; + telemetry.mega_sensor.mag_y = packet->bmx160.mag_y; +#endif + ao_telemetry_send(); } diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h index 23e3ed7d..c7aebe59 100644 --- a/src/kernel/ao_telemetry.h +++ b/src/kernel/ao_telemetry.h @@ -176,7 +176,8 @@ struct ao_telemetry_companion { /* 32 */ }; -#define AO_TELEMETRY_MEGA_SENSOR 0x08 +#define AO_TELEMETRY_MEGA_SENSOR_MPU 0x08 /* Invensense IMU */ +#define AO_TELEMETRY_MEGA_SENSOR_BMX160 0x12 /* BMX160 IMU */ struct ao_telemetry_mega_sensor { uint16_t serial; /* 0 */ -- 2.30.2