X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fkernel%2Fao_telemetry.c;h=e70791792d5644c30ab5745ee2d8a7966648beda;hp=dcda0dd7fdb451e3f797fa2f2213f4465c931111;hb=1085ec5d57e0ed5d132f2bbdac1a0b6a32c0ab4a;hpb=ef85b3bc5300904ebfb878b1c7313a82b5b7aebf diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index dcda0dd7..e7079179 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -1,9 +1,10 @@ /* - * Copyright © 2011 Keith Packard + * Copyright © 2011 Keth Packard * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; version 2 of the License. + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of @@ -19,15 +20,32 @@ #include "ao_log.h" #include "ao_product.h" -#ifndef HAS_RDF -#define HAS_RDF 1 +static __pdata uint16_t ao_telemetry_interval; + +#if HAS_RADIO_RATE +static __xdata uint16_t ao_telemetry_desired_interval; #endif -static __pdata uint16_t ao_telemetry_interval; +/* TeleMetrum v1.0 just doesn't have enough space to + * manage the more complicated telemetry scheduling, so + * it loses the ability to disable telem/rdf separately + */ + +#if defined(TELEMETRUM_V_1_0) +#define SIMPLIFY +#endif + +#ifdef SIMPLIFY +#define ao_telemetry_time time +#define RDF_SPACE __pdata +#else +#define RDF_SPACE __xdata +static __pdata uint16_t ao_telemetry_time; +#endif #if HAS_RDF -static __pdata uint8_t ao_rdf = 0; -static __pdata uint16_t ao_rdf_time; +static RDF_SPACE uint8_t ao_rdf = 0; +static RDF_SPACE uint16_t ao_rdf_time; #endif #if HAS_APRS @@ -58,13 +76,20 @@ static __pdata uint16_t ao_aprs_time; static __xdata union ao_telemetry_all telemetry; +static void +ao_telemetry_send(void) +{ + ao_radio_send(&telemetry, sizeof (telemetry)); + ao_delay(1); +} + #if defined AO_TELEMETRY_SENSOR /* Send sensor packet */ static void ao_send_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_SENSOR; @@ -100,22 +125,25 @@ ao_send_sensor(void) telemetry.sensor.accel_minus_g = 0; #endif - ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_send(); } #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; +#if HAS_MPU6000 telemetry.mega_sensor.orient = ao_sample_orient; +#endif telemetry.mega_sensor.accel = ao_data_accel(packet); telemetry.mega_sensor.pres = ao_data_pres(packet); telemetry.mega_sensor.temp = ao_data_temp(packet); @@ -136,7 +164,7 @@ ao_send_mega_sensor(void) telemetry.mega_sensor.mag_z = packet->hmc5883.z; #endif - ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_send(); } static __pdata int8_t ao_telemetry_mega_data_max; @@ -170,8 +198,8 @@ ao_send_mega_data(void) 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; + ao_telemetry_send(); } } #endif /* AO_SEND_MEGA */ @@ -201,7 +229,7 @@ ao_send_metrum_sensor(void) telemetry.metrum_sensor.sense_a = packet->adc.sense_a; telemetry.metrum_sensor.sense_m = packet->adc.sense_m; - ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_send(); } static __pdata int8_t ao_telemetry_metrum_data_max; @@ -228,8 +256,8 @@ ao_send_metrum_data(void) telemetry.metrum_data.accel_minus_g = 2; #endif - ao_radio_send(&telemetry, sizeof (telemetry)); ao_telemetry_metrum_data_cur = ao_telemetry_metrum_data_max; + ao_telemetry_send(); } } #endif /* AO_SEND_METRUM */ @@ -240,7 +268,7 @@ static void ao_send_mini(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_MINI; @@ -259,35 +287,11 @@ ao_send_mini(void) telemetry.mini.ground_pres = ao_ground_pres; - ao_radio_send(&telemetry, sizeof (telemetry)); + ao_telemetry_send(); } #endif /* AO_SEND_MINI */ -#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; @@ -320,13 +324,14 @@ ao_send_configuration(void) 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; + ao_telemetry_send(); } } #if HAS_GPS +static __pdata int8_t ao_telemetry_gps_max; static __pdata int8_t ao_telemetry_loc_cur; static __pdata int8_t ao_telemetry_sat_cur; @@ -339,14 +344,11 @@ ao_send_location(void) ao_mutex_get(&ao_gps_mutex); ao_xmemcpy(&telemetry.location.flags, &ao_gps_data.flags, - 26); -#if HAS_TRACKER - telemetry.location.state = ao_flight_state | AO_GPS_STATE_VALID; -#endif + 27); telemetry.location.tick = ao_gps_tick; ao_mutex_put(&ao_gps_mutex); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_loc_cur = ao_telemetry_config_max; + ao_telemetry_loc_cur = ao_telemetry_gps_max; + ao_telemetry_send(); } } @@ -362,8 +364,8 @@ ao_send_satellite(void) &ao_gps_tracking_data.sats, AO_MAX_GPS_TRACKING * sizeof (struct ao_telemetry_satellite_info)); ao_mutex_put(&ao_gps_mutex); - ao_radio_send(&telemetry, sizeof (telemetry)); - ao_telemetry_sat_cur = ao_telemetry_config_max; + ao_telemetry_sat_cur = ao_telemetry_gps_max; + ao_telemetry_send(); } } #endif @@ -386,8 +388,8 @@ ao_send_companion(void) ao_companion_data, ao_companion_setup.channels * 2); ao_mutex_put(&ao_companion_mutex); - ao_radio_send(&telemetry, sizeof (telemetry)); ao_telemetry_companion_cur = ao_telemetry_companion_max; + ao_telemetry_send(); } } #endif @@ -409,6 +411,7 @@ ao_telemetry(void) while (ao_telemetry_interval == 0) ao_sleep(&telemetry); time = ao_time(); + ao_telemetry_time = time; #if HAS_RDF ao_rdf_time = time; #endif @@ -416,89 +419,114 @@ ao_telemetry(void) ao_aprs_time = time; #endif while (ao_telemetry_interval) { -#if HAS_APRS + time = ao_time() + AO_SEC_TO_TICKS(100); +#ifndef SIMPLIFY if (!(ao_config.radio_enable & AO_RADIO_DISABLE_TELEMETRY)) #endif { -#ifdef AO_SEND_ALL_BARO - ao_send_baro(); +#ifndef SIMPLIFY + if ( (int16_t) (ao_time() - ao_telemetry_time) >= 0) #endif - -#if HAS_FLIGHT + { + ao_telemetry_time = ao_time() + ao_telemetry_interval; # ifdef AO_SEND_MEGA - ao_send_mega_sensor(); - ao_send_mega_data(); + ao_send_mega_sensor(); + ao_send_mega_data(); # endif # ifdef AO_SEND_METRUM - ao_send_metrum_sensor(); - ao_send_metrum_data(); + ao_send_metrum_sensor(); + ao_send_metrum_data(); # endif # ifdef AO_SEND_MINI - ao_send_mini(); + ao_send_mini(); # endif # ifdef AO_TELEMETRY_SENSOR - ao_send_sensor(); + ao_send_sensor(); # endif -#endif /* HAS_FLIGHT */ - #if HAS_COMPANION - if (ao_companion_running) - ao_send_companion(); + if (ao_companion_running) + ao_send_companion(); #endif - ao_send_configuration(); #if HAS_GPS - ao_send_location(); - ao_send_satellite(); + ao_send_location(); + ao_send_satellite(); +#endif + ao_send_configuration(); + } +#ifndef SIMPLIFY + time = ao_telemetry_time; #endif } -#ifndef AO_SEND_ALL_BARO #if HAS_RDF - if (ao_rdf && -#if HAS_APRS - !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) && -#endif /* HAS_APRS */ - (int16_t) (ao_time() - ao_rdf_time) >= 0) + if (ao_rdf +#ifndef SIMPLIFY + && !(ao_config.radio_enable & AO_RADIO_DISABLE_RDF) +#endif + ) { + if ((int16_t) (ao_time() - ao_rdf_time) >= 0) { #if HAS_IGNITE_REPORT - uint8_t c; -#endif /* HAS_IGNITE_REPORT */ - ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; + uint8_t c; +#endif + ao_rdf_time = ao_time() + AO_RDF_INTERVAL_TICKS; #if HAS_IGNITE_REPORT - if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter())) - ao_radio_continuity(c); - else -#endif /* HAS_IGNITE_REPORT*/ - ao_radio_rdf(); + if (ao_flight_state == ao_flight_pad && (c = ao_report_igniter())) + ao_radio_continuity(c); + else +#endif + ao_radio_rdf(); + } +#ifndef SIMPLIFY + if ((int16_t) (time - ao_rdf_time) > 0) + time = ao_rdf_time; +#endif } #endif /* HAS_RDF */ #if HAS_APRS - if (ao_config.aprs_interval != 0 && - (int16_t) (ao_time() - ao_aprs_time) >= 0) - { - ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); - ao_aprs_send(); + if (ao_config.aprs_interval != 0) { + if ((int16_t) (ao_time() - ao_aprs_time) >= 0) { + ao_aprs_time = ao_time() + AO_SEC_TO_TICKS(ao_config.aprs_interval); + ao_aprs_send(); + } + if ((int16_t) (time - ao_aprs_time) > 0) + time = ao_aprs_time; } #endif /* HAS_APRS */ -#endif /* !AO_SEND_ALL_BARO */ - time += ao_telemetry_interval; delay = time - ao_time(); if (delay > 0) { - ao_alarm(delay); - ao_sleep(&telemetry); - ao_clear_alarm(); + ao_sleep_for(&telemetry, delay); } - else - time = ao_time(); } } } +#if HAS_RADIO_RATE +void +ao_telemetry_reset_interval(void) +{ + ao_telemetry_set_interval(ao_telemetry_desired_interval); +} +#endif + void ao_telemetry_set_interval(uint16_t interval) { int8_t cur = 0; + +#if HAS_RADIO_RATE + /* Limit max telemetry rate based on available radio bandwidth. + */ + static __xdata const uint16_t min_interval[] = { + /* [AO_RADIO_RATE_38400] = */ AO_MS_TO_TICKS(100), + /* [AO_RADIO_RATE_9600] = */ AO_MS_TO_TICKS(500), + /* [AO_RADIO_RATE_2400] = */ AO_MS_TO_TICKS(1000) + }; + + ao_telemetry_desired_interval = interval; + if (interval && interval < min_interval[ao_config.radio_rate]) + interval = min_interval[ao_config.radio_rate]; +#endif ao_telemetry_interval = interval; - #if AO_SEND_MEGA if (interval > 1) ao_telemetry_mega_data_max = 1; @@ -524,21 +552,31 @@ ao_telemetry_set_interval(uint16_t interval) ao_telemetry_companion_cur = cur; #endif - ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval; -#if HAS_COMPANION - if (ao_telemetry_config_max > cur) - cur++; - ao_telemetry_config_cur = cur; -#endif - #if HAS_GPS - if (ao_telemetry_config_max > cur) + ao_telemetry_gps_max = AO_SEC_TO_TICKS(1) / interval; + if (ao_telemetry_gps_max > cur) cur++; ao_telemetry_loc_cur = cur; - if (ao_telemetry_config_max > cur) + if (ao_telemetry_gps_max > cur) cur++; ao_telemetry_sat_cur = cur; #endif + + ao_telemetry_config_max = AO_SEC_TO_TICKS(5) / interval; + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_config_cur = cur; + +#ifndef SIMPLIFY + ao_telemetry_time = +#if HAS_RDF + ao_rdf_time = +#endif +#if HAS_APRS + ao_aprs_time = +#endif + ao_time(); +#endif ao_wakeup(&telemetry); }