altos/kernel: MPU9250 support

Use MPU9250 for accel, gyro and mag data in logging, telemetry and
flight status computations.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
index 9a3b389c073dd6a291b6776faeb69829533b6ad5..88d0e91675a4c7cf61eec8ab454f4fd161e9fe72 100644 (file)
--- a/src/kernel/ao_data.h
+++ b/src/kernel/ao_data.h
@@ -330,6 +330,47 @@ typedef int16_t angle_t;   /* in degrees */
 #define ao_data_pitch(packet)  ((packet)->mpu6000.gyro_x)
 #define ao_data_yaw(packet)    ((packet)->mpu6000.gyro_z)
 
+static inline float ao_convert_gyro(float sensor)
+{
+       return ao_mpu6000_gyro(sensor);
+}
+
+static inline float ao_convert_accel(int16_t sensor)
+{
+       return ao_mpu6000_accel(sensor);
+}
+
+#endif
+
+#if !HAS_GYRO && HAS_MPU9250
+
+#define HAS_GYRO       1
+
+typedef int16_t        gyro_t;         /* in raw sample units */
+typedef int16_t angle_t;       /* in degrees */
+
+/* Y axis is aligned with the direction of motion (along) */
+/* X axis is aligned in the other board axis (across) */
+/* Z axis is aligned perpendicular to the board (through) */
+
+#define ao_data_along(packet)  ((packet)->mpu9250.accel_y)
+#define ao_data_across(packet) ((packet)->mpu9250.accel_x)
+#define ao_data_through(packet)        ((packet)->mpu9250.accel_z)
+
+#define ao_data_roll(packet)   ((packet)->mpu9250.gyro_y)
+#define ao_data_pitch(packet)  ((packet)->mpu9250.gyro_x)
+#define ao_data_yaw(packet)    ((packet)->mpu9250.gyro_z)
+
+static inline float ao_convert_gyro(float sensor)
+{
+       return ao_mpu9250_gyro(sensor);
+}
+
+static inline float ao_convert_accel(int16_t sensor)
+{
+       return ao_mpu9250_accel(sensor);
+}
+
 #endif
 
 #if !HAS_MAG && HAS_HMC5883
@@ -344,4 +385,21 @@ typedef int16_t ao_mag_t;          /* in raw sample units */
 
 #endif
 
+#if !HAS_MAG && HAS_MPU9250
+
+#define HAS_MAG                1
+
+typedef int16_t ao_mag_t;              /* in raw sample units */
+
+/* Note that this order is different from the accel and gyro. For some
+ * reason, the mag sensor axes aren't the same as the other two
+ * sensors. Also, the Z axis is flipped in sign.
+ */
+
+#define ao_data_mag_along(packet)      ((packet)->mpu9250.mag_x)
+#define ao_data_mag_across(packet)     ((packet)->mpu9250.mag_y)
+#define ao_data_mag_through(packet)    ((packet)->mpu9250.mag_z)
+
+#endif
+
 #endif /* _AO_DATA_H_ */

diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c
index f06125cd1329f361f7e91e92afcaadc2be420b38..cb02c4549ee20564981f8fafa7c2072b1bdd1b65 100644 (file)
--- a/src/kernel/ao_flight.c
+++ b/src/kernel/ao_flight.c
@@ -21,7 +21,7 @@
 #include <ao_log.h>
 #endif
 
-#if HAS_MPU6000
+#if HAS_MPU6000 || HAS_MPU9250
 #include <ao_quaternion.h>
 #endif
 

diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h
index 1c186364a4e1fe3a854d99e4b56c18356902b1d1..5f04ef9ab676839e7b136308803eee200cbf470d 100644 (file)
--- a/src/kernel/ao_log.h
+++ b/src/kernel/ao_log.h
@@ -54,6 +54,7 @@ extern __pdata enum ao_flight_state ao_log_state;
 #define AO_LOG_FORMAT_TELEMINI3                12      /* 16-byte MS5607 baro only, 3.3V supply, stm32f042 SoC */
 #define AO_LOG_FORMAT_TELEFIRETWO      13      /* 32-byte test stand data */
 #define AO_LOG_FORMAT_EASYMINI2                14      /* 16-byte MS5607 baro only, 3.3V supply, stm32f042 SoC */
+#define AO_LOG_FORMAT_TELEMEGA_3       15      /* 32 byte typed telemega records with 32 bit gyro cal and mpu9250 */
 #define AO_LOG_FORMAT_NONE             127     /* No log at all */
 
 /* Return the flight number from the given log slot, 0 if none, -slot on failure */
@@ -473,7 +474,7 @@ struct ao_log_gps {
        } u;
 };
 
-#if AO_LOG_FORMAT == AO_LOG_FOMAT_TELEMEGA_OLD || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA
+#if AO_LOG_FORMAT == AO_LOG_FOMAT_TELEMEGA_OLD || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA || AO_LOG_FORMAT == AO_LOG_FORMAT_TELEMEGA_3
 typedef struct ao_log_mega ao_log_type;
 #endif
 

diff --git a/src/kernel/ao_log_mega.c b/src/kernel/ao_log_mega.c
index d1cf4f13f16ba89a1e496c2f37e5a8e9002d8144..c6bdf1e23c53cba1d4329bccdfdc21a591c8c851 100644 (file)
--- a/src/kernel/ao_log_mega.c
+++ b/src/kernel/ao_log_mega.c
@@ -93,6 +93,17 @@ ao_log(void)
                                log.u.sensor.mag_x = ao_data_ring[ao_log_data_pos].hmc5883.x;
                                log.u.sensor.mag_z = ao_data_ring[ao_log_data_pos].hmc5883.z;
                                log.u.sensor.mag_y = ao_data_ring[ao_log_data_pos].hmc5883.y;
+#endif
+#if HAS_MPU9250
+                               log.u.sensor.accel_x = ao_data_ring[ao_log_data_pos].mpu9250.accel_x;
+                               log.u.sensor.accel_y = ao_data_ring[ao_log_data_pos].mpu9250.accel_y;
+                               log.u.sensor.accel_z = ao_data_ring[ao_log_data_pos].mpu9250.accel_z;
+                               log.u.sensor.gyro_x = ao_data_ring[ao_log_data_pos].mpu9250.gyro_x;
+                               log.u.sensor.gyro_y = ao_data_ring[ao_log_data_pos].mpu9250.gyro_y;
+                               log.u.sensor.gyro_z = ao_data_ring[ao_log_data_pos].mpu9250.gyro_z;
+                               log.u.sensor.mag_x = ao_data_ring[ao_log_data_pos].mpu9250.mag_x;
+                               log.u.sensor.mag_z = ao_data_ring[ao_log_data_pos].mpu9250.mag_z;
+                               log.u.sensor.mag_y = ao_data_ring[ao_log_data_pos].mpu9250.mag_y;
 #endif
                                log.u.sensor.accel = ao_data_accel(&ao_data_ring[ao_log_data_pos]);
                                ao_log_write(&log);

diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c
index f0ab0169805fb6a1db38ddde5ca7b803b2f530c9..61519478a2a2c6ab1df8f36c7db14869e37fe20c 100644 (file)
--- a/src/kernel/ao_sample.c
+++ b/src/kernel/ao_sample.c
@@ -184,9 +184,9 @@ ao_sample_rotate(void)
 #else
        static const float dt = 1/TIME_DIV;
 #endif
-       float   x = ao_mpu6000_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt;
-       float   y = ao_mpu6000_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt;
-       float   z = ao_mpu6000_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt;
+       float   x = ao_convert_gyro((float) ((ao_sample_pitch << 9) - ao_ground_pitch) / 512.0f) * dt;
+       float   y = ao_convert_gyro((float) ((ao_sample_yaw << 9) - ao_ground_yaw) / 512.0f) * dt;
+       float   z = ao_convert_gyro((float) ((ao_sample_roll << 9) - ao_ground_roll) / 512.0f) * dt;
        struct ao_quaternion    rot;
 
        ao_quaternion_init_half_euler(&rot, x, y, z);

diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c
index 2ae1e41bb42ffb31701176424c461831c52d98b6..9ed612ceeb263ce8dc10e91e6028522072aaa492 100644 (file)
--- a/src/kernel/ao_telemetry.c
+++ b/src/kernel/ao_telemetry.c
@@ -141,7 +141,7 @@ ao_send_mega_sensor(void)
        telemetry.generic.tick = packet->tick;
        telemetry.generic.type = AO_TELEMETRY_MEGA_SENSOR;
 
-#if HAS_MPU6000
+#if HAS_MPU6000 || HAS_MPU9250
        telemetry.mega_sensor.orient = ao_sample_orient;
 #endif
        telemetry.mega_sensor.accel = ao_data_accel(packet);
@@ -164,6 +164,20 @@ ao_send_mega_sensor(void)
        telemetry.mega_sensor.mag_y = packet->hmc5883.y;
 #endif
 
+#if HAS_MPU9250
+       telemetry.mega_sensor.accel_x = packet->mpu9250.accel_x;
+       telemetry.mega_sensor.accel_y = packet->mpu9250.accel_y;
+       telemetry.mega_sensor.accel_z = packet->mpu9250.accel_z;
+
+       telemetry.mega_sensor.gyro_x = packet->mpu9250.gyro_x;
+       telemetry.mega_sensor.gyro_y = packet->mpu9250.gyro_y;
+       telemetry.mega_sensor.gyro_z = packet->mpu9250.gyro_z;
+
+       telemetry.mega_sensor.mag_x = packet->mpu9250.mag_x;
+       telemetry.mega_sensor.mag_z = packet->mpu9250.mag_z;
+       telemetry.mega_sensor.mag_y = packet->mpu9250.mag_y;
+#endif
+
        ao_telemetry_send();
 }