summaryrefslogtreecommitdiff
path: root/src/kernel
diff options
context:
space:
mode:
Diffstat (limited to 'src/kernel')
-rw-r--r--src/kernel/ao_data.h58
-rw-r--r--src/kernel/ao_flight.c2
-rw-r--r--src/kernel/ao_log.h3
-rw-r--r--src/kernel/ao_log_mega.c11
-rw-r--r--src/kernel/ao_sample.c6
-rw-r--r--src/kernel/ao_telemetry.c16
6 files changed, 90 insertions, 6 deletions
diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h
index 9a3b389c..88d0e916 100644
--- 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 f06125cd..cb02c454 100644
--- 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 1c186364..5f04ef9a 100644
--- 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 d1cf4f13..c6bdf1e2 100644
--- a/src/kernel/ao_log_mega.c
+++ b/src/kernel/ao_log_mega.c
@@ -94,6 +94,17 @@ ao_log(void)
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);
if (ao_log_state <= ao_flight_coast)
diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c
index f0ab0169..61519478 100644
--- 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 2ae1e41b..9ed612ce 100644
--- 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();
}
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-31 14:13:22 +0000