+#endif
+
+
+#ifdef AO_SEND_MEGA
+/* Send mega sensor packet */
+static void
+ao_send_mega_sensor(void)
+{
+ __xdata struct ao_data *packet = (__xdata 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;
+
+ telemetry.mega_sensor.accel = ao_data_accel(packet);
+ telemetry.mega_sensor.pres = ao_data_pres(packet);
+ telemetry.mega_sensor.temp = ao_data_temp(packet);
+
+#if HAS_MPU6000
+ telemetry.mega_sensor.accel_x = packet->mpu6000.accel_x;
+ telemetry.mega_sensor.accel_y = packet->mpu6000.accel_y;
+ telemetry.mega_sensor.accel_z = packet->mpu6000.accel_z;
+
+ telemetry.mega_sensor.gyro_x = packet->mpu6000.gyro_x;
+ telemetry.mega_sensor.gyro_y = packet->mpu6000.gyro_y;
+ telemetry.mega_sensor.gyro_z = packet->mpu6000.gyro_z;
+#endif
+
+#if HAS_HMC5883
+ telemetry.mega_sensor.mag_x = packet->hmc5883.x;
+ telemetry.mega_sensor.mag_y = packet->hmc5883.y;
+ telemetry.mega_sensor.mag_z = packet->hmc5883.z;
+#endif
+
+ ao_radio_send(&telemetry, sizeof (telemetry));
+}
+
+static __pdata int8_t ao_telemetry_mega_data_max;
+static __pdata int8_t ao_telemetry_mega_data_cur;
+
+/* Send mega data packet */
+static void
+ao_send_mega_data(void)
+{
+ if (--ao_telemetry_mega_data_cur <= 0) {
+ __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
+ uint8_t i;
+
+ telemetry.generic.tick = packet->tick;
+ telemetry.generic.type = AO_TELEMETRY_MEGA_DATA;
+
+ telemetry.mega_data.state = ao_flight_state;
+ telemetry.mega_data.v_batt = packet->adc.v_batt;
+ telemetry.mega_data.v_pyro = packet->adc.v_pbatt;
+
+ /* ADC range is 0-4095, so shift by four to save the high 8 bits */
+ for (i = 0; i < AO_ADC_NUM_SENSE; i++)
+ telemetry.mega_data.sense[i] = packet->adc.sense[i] >> 4;
+
+ telemetry.mega_data.ground_pres = ao_ground_pres;
+ telemetry.mega_data.ground_accel = ao_ground_accel;
+ telemetry.mega_data.accel_plus_g = ao_config.accel_plus_g;
+ telemetry.mega_data.accel_minus_g = ao_config.accel_minus_g;
+
+ telemetry.mega_data.acceleration = ao_accel;
+ telemetry.mega_data.speed = ao_speed;
+ telemetry.mega_data.height = ao_height;
+
+ ao_radio_send(&telemetry, sizeof (telemetry));
+ ao_telemetry_mega_data_cur = ao_telemetry_mega_data_max;
+ }
+}
+#endif /* AO_SEND_MEGA */
+
+#ifdef AO_SEND_ALL_BARO
+static uint8_t ao_baro_sample;
+
+static void
+ao_send_baro(void)
+{
+ uint8_t sample = ao_sample_data;
+ uint8_t samples = (sample - ao_baro_sample) & (AO_DATA_RING - 1);
+
+ if (samples > 12) {
+ ao_baro_sample = (ao_baro_sample + (samples - 12)) & (AO_DATA_RING - 1);
+ samples = 12;
+ }
+ telemetry.generic.tick = ao_data_ring[sample].tick;
+ telemetry.generic.type = AO_TELEMETRY_BARO;
+ telemetry.baro.samples = samples;
+ for (sample = 0; sample < samples; sample++) {
+ telemetry.baro.baro[sample] = ao_data_ring[ao_baro_sample].adc.pres;
+ ao_baro_sample = ao_data_ring_next(ao_baro_sample);
+ }
+ ao_radio_send(&telemetry, sizeof (telemetry));
+}
+#endif
+
+static __pdata int8_t ao_telemetry_config_max;
+static __pdata int8_t ao_telemetry_config_cur;