altos: Write xdata versions of memory functions

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

diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c
index c7338a581f64cef7e1dd73ac2c4dd94e274d9209..95e53917152c176c6dabc9067ab8ef659a7aa1e3 100644 (file)
--- a/src/core/ao_telemetry.c
+++ b/src/core/ao_telemetry.c
@@ -94,6 +94,30 @@ ao_send_sensor(void)
        ao_radio_send(&telemetry, sizeof (telemetry));
 }
 
+static uint8_t         ao_baro_sample;
+
+#ifdef AO_SEND_ALL_BARO
+static void
+ao_send_baro(void)
+{
+       uint8_t         sample = ao_sample_adc;
+       uint8_t         samples = (sample - ao_baro_sample) & (AO_ADC_RING - 1);
+
+       if (samples > 12) {
+               ao_baro_sample = (ao_baro_sample + (samples - 12)) & (AO_ADC_RING - 1);
+               samples = 12;
+       }
+       telemetry.generic.tick = ao_adc_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);
+       }
+       ao_radio_send(&telemetry, sizeof (telemetry));
+}
+#endif
+
 static void
 ao_send_configuration(void)
 {
@@ -107,10 +131,10 @@ 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_xmemcpy (telemetry.configuration.callsign,
                        ao_config.callsign,
                        AO_MAX_CALLSIGN);
-               memcpy (telemetry.configuration.version,
+               ao_xmemcpy (telemetry.configuration.version,
                        ao_version,
                        AO_MAX_VERSION);
                ao_radio_send(&telemetry, sizeof (telemetry));
@@ -126,7 +150,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);
@@ -143,7 +167,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);
@@ -163,7 +187,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);
@@ -193,6 +217,9 @@ ao_telemetry(void)
                while (ao_telemetry_interval) {
 
 
+#ifdef AO_SEND_ALL_BARO
+                       ao_send_baro();
+#endif
                        ao_send_sensor();
 #if HAS_COMPANION
                        if (ao_companion_running)
@@ -203,12 +230,14 @@ ao_telemetry(void)
                        ao_send_location();
                        ao_send_satellite();
 #endif
+#ifndef AO_SEND_ALL_BARO
                        if (ao_rdf &&
                            (int16_t) (ao_time() - ao_rdf_time) >= 0)
                        {
                                ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS;
-                               ao_radio_rdf(AO_RDF_LENGTH_MS);
+                               ao_radio_rdf(AO_MS_TO_RDF_LEN(AO_RDF_LENGTH_MS));
                        }
+#endif
                        time += ao_telemetry_interval;
                        delay = time - ao_time();
                        if (delay > 0)
@@ -258,7 +287,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;