#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
#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_ */
#include <ao_log.h>
#endif
-#if HAS_MPU6000
+#if HAS_MPU6000 || HAS_MPU9250
#include <ao_quaternion.h>
#endif
#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 */
} 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
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);
#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);
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);
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();
}