altos: Add telemetry to megametrum

[fw/altos] / src / core / ao_telemetry.c

diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c
index c2707e7d57effeb820743c23d4d362f4dff6b31d..5857c44d6c3499c8170091ba7277bb7c33d446dc 100644 (file)
--- a/src/core/ao_telemetry.c
+++ b/src/core/ao_telemetry.c
@@ -35,7 +35,11 @@ static __pdata uint16_t ao_rdf_time;
 #define AO_RDF_INTERVAL_TICKS  AO_SEC_TO_TICKS(5)
 #define AO_RDF_LENGTH_MS       500
 
-#if defined(TELEMETRUM_V_0_1) || defined(TELEMETRUM_V_0_2) || defined(TELEMETRUM_V_1_0) || defined(TELEMETRUM_V_1_1)
+#if defined(MEGAMETRUM)
+#define AO_TELEMETRY_SENSOR    AO_TELEMETRY_SENSOR_MEGAMETRUM
+#endif
+
+#if defined(TELEMETRUM_V_0_1) || defined(TELEMETRUM_V_0_2) || defined(TELEMETRUM_V_1_0) || defined(TELEMETRUM_V_1_1) || defined(TELEBALLOON_V_1_1) || defined(TELEMETRUM_V_1_2)
 #define AO_TELEMETRY_SENSOR    AO_TELEMETRY_SENSOR_TELEMETRUM
 #endif
 
@@ -53,24 +57,23 @@ static __xdata union ao_telemetry_all       telemetry;
 static void
 ao_send_sensor(void)
 {
-       uint8_t         sample;
-       sample = ao_sample_adc;
+       __xdata struct ao_data *packet = (__xdata struct ao_data *) &ao_data_ring[ao_data_ring_prev(ao_sample_data)];
                        
-       telemetry.generic.tick = ao_adc_ring[sample].tick;
+       telemetry.generic.tick = packet->tick;
        telemetry.generic.type = AO_TELEMETRY_SENSOR;
 
        telemetry.sensor.state = ao_flight_state;
 #if HAS_ACCEL
-       telemetry.sensor.accel = ao_adc_ring[sample].accel;
+       telemetry.sensor.accel = packet->adc.accel;
 #else
        telemetry.sensor.accel = 0;
 #endif
-       telemetry.sensor.pres = ao_adc_ring[sample].pres;
-       telemetry.sensor.temp = ao_adc_ring[sample].temp;
-       telemetry.sensor.v_batt = ao_adc_ring[sample].v_batt;
+       telemetry.sensor.pres = ao_data_pres(packet);
+       telemetry.sensor.temp = packet->adc.temp;
+       telemetry.sensor.v_batt = packet->adc.v_batt;
 #if HAS_IGNITE
-       telemetry.sensor.sense_d = ao_adc_ring[sample].sense_d;
-       telemetry.sensor.sense_m = ao_adc_ring[sample].sense_m;
+       telemetry.sensor.sense_d = packet->adc.sense_d;
+       telemetry.sensor.sense_m = packet->adc.sense_m;
 #else
        telemetry.sensor.sense_d = 0;
        telemetry.sensor.sense_m = 0;
@@ -100,19 +103,19 @@ static uint8_t            ao_baro_sample;
 static void
 ao_send_baro(void)
 {
-       uint8_t         sample = ao_sample_adc;
-       uint8_t         samples = (sample - ao_baro_sample) & (AO_ADC_RING - 1);
+       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_ADC_RING - 1);
+               ao_baro_sample = (ao_baro_sample + (samples - 12)) & (AO_DATA_RING - 1);
                samples = 12;
        }
-       telemetry.generic.tick = ao_adc_ring[sample].tick;
+       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_adc_ring[ao_baro_sample].pres;
-               ao_baro_sample = ao_adc_ring_next(ao_baro_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));
 }
@@ -131,12 +134,12 @@ ao_send_configuration(void)
                telemetry.configuration.apogee_delay = ao_config.apogee_delay;
                telemetry.configuration.main_deploy = ao_config.main_deploy;
                telemetry.configuration.flight_log_max = ao_config.flight_log_max >> 10;
-               memcpy (telemetry.configuration.callsign,
-                       ao_config.callsign,
-                       AO_MAX_CALLSIGN);
-               memcpy (telemetry.configuration.version,
-                       ao_version,
-                       AO_MAX_VERSION);
+               ao_xmemcpy (telemetry.configuration.callsign,
+                           ao_config.callsign,
+                           AO_MAX_CALLSIGN);
+               ao_xmemcpy (telemetry.configuration.version,
+                           CODE_TO_XDATA(ao_version),
+                           AO_MAX_VERSION);
                ao_radio_send(&telemetry, sizeof (telemetry));
                ao_telemetry_config_cur = ao_telemetry_config_max;
        }
@@ -150,7 +153,7 @@ ao_send_location(void)
        {
                telemetry.generic.type = AO_TELEMETRY_LOCATION;
                ao_mutex_get(&ao_gps_mutex);
-               memcpy(&telemetry.location.flags,
+               ao_xmemcpy(&telemetry.location.flags,
                       &ao_gps_data.flags,
                       26);
                ao_mutex_put(&ao_gps_mutex);
@@ -167,7 +170,7 @@ ao_send_satellite(void)
                telemetry.generic.type = AO_TELEMETRY_SATELLITE;
                ao_mutex_get(&ao_gps_mutex);
                telemetry.satellite.channels = ao_gps_tracking_data.channels;
-               memcpy(&telemetry.satellite.sats,
+               ao_xmemcpy(&telemetry.satellite.sats,
                       &ao_gps_tracking_data.sats,
                       AO_MAX_GPS_TRACKING * sizeof (struct ao_telemetry_satellite_info));
                ao_mutex_put(&ao_gps_mutex);
@@ -187,7 +190,7 @@ ao_send_companion(void)
                telemetry.companion.update_period = ao_companion_setup.update_period;
                telemetry.companion.channels = ao_companion_setup.channels;
                ao_mutex_get(&ao_companion_mutex);
-               memcpy(&telemetry.companion.companion_data,
+               ao_xmemcpy(&telemetry.companion.companion_data,
                       ao_companion_data,
                       ao_companion_setup.channels * 2);
                ao_mutex_put(&ao_companion_mutex);
@@ -240,8 +243,11 @@ ao_telemetry(void)
 #endif
                        time += ao_telemetry_interval;
                        delay = time - ao_time();
-                       if (delay > 0)
-                               ao_delay(delay);
+                       if (delay > 0) {
+                               ao_alarm(delay);
+                               ao_sleep(&telemetry);
+                               ao_clear_alarm();
+                       }
                        else
                                time = ao_time();
                }
@@ -287,7 +293,7 @@ ao_rdf_set(uint8_t rdf)
        if (rdf == 0)
                ao_radio_rdf_abort();
        else
-               ao_rdf_time = ao_time();
+               ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS;
 }
 
 __xdata struct ao_task ao_telemetry_task;