From d2bd95edb6f77daeb1e8f043c4a239c248728e0c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 21 Jun 2012 09:45:42 -0700 Subject: [PATCH] altos: Add full MM telemetry Create two new telemetry packets to hold all of the MM data. This patch also splits the telemetry structures out of ao.h Signed-off-by: Keith Packard --- src/core/ao.h | 177 +--------------------------------------- src/core/ao_data.h | 33 ++++---- src/core/ao_log_mega.c | 4 +- src/core/ao_sample_mm.c | 5 +- src/core/ao_telemetry.c | 135 ++++++++++++++++++++++++------ src/stm/ao_adc_stm.c | 2 +- 6 files changed, 138 insertions(+), 218 deletions(-) diff --git a/src/core/ao.h b/src/core/ao.h index 3dae8b40..204c8530 100644 --- a/src/core/ao.h +++ b/src/core/ao.h @@ -325,182 +325,7 @@ ao_spi_slave_init(void); void ao_spi_slave(void); -/* - * ao_telemetry.c - */ -#define AO_MAX_CALLSIGN 8 -#define AO_MAX_VERSION 8 -#if LEGACY_MONITOR -#define AO_MAX_TELEMETRY 128 -#else -#define AO_MAX_TELEMETRY 32 -#endif - -struct ao_telemetry_generic { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t payload[27]; /* 5 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01 -#define AO_TELEMETRY_SENSOR_TELEMINI 0x02 -#define AO_TELEMETRY_SENSOR_TELENANO 0x03 -#define AO_TELEMETRY_SENSOR_MEGAMETRUM 0x08 - -struct ao_telemetry_sensor { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t state; /* 5 flight state */ - int16_t accel; /* 6 accelerometer (TM only) */ - int16_t pres; /* 8 pressure sensor */ - int16_t temp; /* 10 temperature sensor */ - int16_t v_batt; /* 12 battery voltage */ - int16_t sense_d; /* 14 drogue continuity sense (TM/Tm) */ - int16_t sense_m; /* 16 main continuity sense (TM/Tm) */ - - int16_t acceleration; /* 18 m/s² * 16 */ - int16_t speed; /* 20 m/s * 16 */ - int16_t height; /* 22 m */ - - int16_t ground_pres; /* 24 average pres on pad */ - int16_t ground_accel; /* 26 average accel on pad */ - int16_t accel_plus_g; /* 28 accel calibration at +1g */ - int16_t accel_minus_g; /* 30 accel calibration at -1g */ - /* 32 */ -}; - -#define AO_TELEMETRY_CONFIGURATION 0x04 - -struct ao_telemetry_configuration { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t device; /* 5 device type */ - uint16_t flight; /* 6 flight number */ - uint8_t config_major; /* 8 Config major version */ - uint8_t config_minor; /* 9 Config minor version */ - uint16_t apogee_delay; /* 10 Apogee deploy delay in seconds */ - uint16_t main_deploy; /* 12 Main deploy alt in meters */ - uint16_t flight_log_max; /* 14 Maximum flight log size in kB */ - char callsign[AO_MAX_CALLSIGN]; /* 16 Radio operator identity */ - char version[AO_MAX_VERSION]; /* 24 Software version */ - /* 32 */ -}; - -#define AO_TELEMETRY_LOCATION 0x05 - -#define AO_GPS_MODE_NOT_VALID 'N' -#define AO_GPS_MODE_AUTONOMOUS 'A' -#define AO_GPS_MODE_DIFFERENTIAL 'D' -#define AO_GPS_MODE_ESTIMATED 'E' -#define AO_GPS_MODE_MANUAL 'M' -#define AO_GPS_MODE_SIMULATED 'S' - -struct ao_telemetry_location { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - - uint8_t flags; /* 5 Number of sats and other flags */ - int16_t altitude; /* 6 GPS reported altitude (m) */ - int32_t latitude; /* 8 latitude (degrees * 10⁷) */ - int32_t longitude; /* 12 longitude (degrees * 10⁷) */ - uint8_t year; /* 16 (- 2000) */ - uint8_t month; /* 17 (1-12) */ - uint8_t day; /* 18 (1-31) */ - uint8_t hour; /* 19 (0-23) */ - uint8_t minute; /* 20 (0-59) */ - uint8_t second; /* 21 (0-59) */ - uint8_t pdop; /* 22 (m * 5) */ - uint8_t hdop; /* 23 (m * 5) */ - uint8_t vdop; /* 24 (m * 5) */ - uint8_t mode; /* 25 */ - uint16_t ground_speed; /* 26 cm/s */ - int16_t climb_rate; /* 28 cm/s */ - uint8_t course; /* 30 degrees / 2 */ - uint8_t unused[1]; /* 31 */ - /* 32 */ -}; - -#define AO_TELEMETRY_SATELLITE 0x06 - -struct ao_telemetry_satellite_info { - uint8_t svid; - uint8_t c_n_1; -}; - -struct ao_telemetry_satellite { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t channels; /* 5 number of reported sats */ - - struct ao_telemetry_satellite_info sats[12]; /* 6 */ - uint8_t unused[2]; /* 30 */ - /* 32 */ -}; - -#define AO_TELEMETRY_COMPANION 0x07 - -#define AO_COMPANION_MAX_CHANNELS 12 - -struct ao_telemetry_companion { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t board_id; /* 5 */ - - uint8_t update_period; /* 6 */ - uint8_t channels; /* 7 */ - uint16_t companion_data[AO_COMPANION_MAX_CHANNELS]; /* 8 */ - /* 32 */ -}; - -/* #define AO_SEND_ALL_BARO */ - -#define AO_TELEMETRY_BARO 0x80 - -/* - * This packet allows the full sampling rate baro - * data to be captured over the RF link so that the - * flight software can be tested using 'real' data. - * - * Along with this telemetry packet, the flight - * code is modified to send full-rate telemetry all the time - * and never send an RDF tone; this ensure that the full radio - * link is available. - */ -struct ao_telemetry_baro { - uint16_t serial; /* 0 */ - uint16_t tick; /* 2 */ - uint8_t type; /* 4 */ - uint8_t samples; /* 5 number samples */ - - int16_t baro[12]; /* 6 samples */ - /* 32 */ -}; - -union ao_telemetry_all { - struct ao_telemetry_generic generic; - struct ao_telemetry_sensor sensor; - struct ao_telemetry_configuration configuration; - struct ao_telemetry_location location; - struct ao_telemetry_satellite satellite; - struct ao_telemetry_companion companion; - struct ao_telemetry_baro baro; -}; - -struct ao_telemetry_all_recv { - union ao_telemetry_all telemetry; - int8_t rssi; - uint8_t status; -}; - +#include /* * ao_gps.c */ diff --git a/src/core/ao_data.h b/src/core/ao_data.h index 502df6c9..fdc49ca2 100644 --- a/src/core/ao_data.h +++ b/src/core/ao_data.h @@ -36,7 +36,8 @@ struct ao_data { struct ao_adc adc; #endif #if HAS_MS5607 - struct ao_ms5607_sample ms5607; + struct ao_ms5607_sample ms5607_raw; + struct ao_ms5607_value ms5607_cooked; #endif #if HAS_MPU6000 struct ao_mpu6000_sample mpu6000; @@ -57,25 +58,24 @@ extern volatile __data uint8_t ao_data_head; typedef int32_t pres_t; typedef int32_t alt_t; -static inline pres_t ao_data_pres(struct ao_data *packet) -{ - struct ao_ms5607_value value; +#define ao_data_pres_cook(packet) ao_ms5607_convert(&packet->ms5607_raw, &packet->ms5607_cooked) - ao_ms5607_convert(&packet->ms5607, &value); - return value.pres; -} +#define ao_data_pres(packet) ((packet)->ms5607_cooked.pres) +#define ao_data_temp(packet) ((packet)->ms5607_cooked.temp) #define pres_to_altitude(p) ao_pa_to_altitude(p) -#else +#else /* HAS_MS5607 */ typedef int16_t pres_t; typedef int16_t alt_t; #define ao_data_pres(packet) ((packet)->adc.pres) +#define ao_data_temp(packet) ((packet)->adc.temp) #define pres_to_altitude(p) ao_pres_to_altitude(p) +#define ao_data_pres_cook(p) -#endif +#endif /* else HAS_MS5607 */ /* * Need a few macros to pull data from the sensors: @@ -92,11 +92,11 @@ typedef int16_t accel_t; /* MPU6000 is hooked up so that positive y is positive acceleration */ #define ao_data_accel(packet) ((packet)->mpu6000.accel_y) -#define ao_data_accel_sample(packet) (-ao_data_accel(packet)) +#define ao_data_accel_cook(packet) (-(packet)->mpu6000.accel_y) #define ao_data_set_accel(packet, accel) ((packet)->mpu6000.accel_y = (accel)) #define ao_data_accel_invert(a) (-(a)) -#else +#else /* HAS_MPU6000 && !HAS_HIGHG_ACCEL */ typedef int16_t accel_t; #define ao_data_accel(packet) ((packet)->adc.accel) @@ -183,13 +183,18 @@ typedef int16_t accel_t; * provides 11 bits of data, we haven't actually lost any precision, * just dropped a bit of noise off the low end. */ + #if HAS_ACCEL_REF -#define ao_data_accel_sample(packet) \ + +#define ao_data_accel_cook(packet) \ ((uint16_t) ((((uint32_t) (packet)->adc.accel << 16) / ((packet)->adc.accel_ref << 1))) >> 1) + #else -#define ao_data_accel_sample(packet) ((packet)->adc.accel) + +#define ao_data_accel_cook(packet) ((packet)->adc.accel) + #endif /* HAS_ACCEL_REF */ -#endif /* else some other pressure sensor */ +#endif /* else some other accel sensor */ #endif /* _AO_DATA_H_ */ diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c index 68e2af49..e7c2b0d9 100644 --- a/src/core/ao_log_mega.c +++ b/src/core/ao_log_mega.c @@ -112,8 +112,8 @@ ao_log(void) log.tick = ao_data_ring[ao_log_data_pos].tick; if ((int16_t) (log.tick - next_sensor) >= 0) { log.type = AO_LOG_SENSOR; - log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607.pres; - log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607.temp; + log.u.sensor.pres = ao_data_ring[ao_log_data_pos].ms5607_raw.pres; + log.u.sensor.temp = ao_data_ring[ao_log_data_pos].ms5607_raw.temp; log.u.sensor.accel_x = ao_data_ring[ao_log_data_pos].mpu6000.accel_x; log.u.sensor.accel_y = ao_data_ring[ao_log_data_pos].mpu6000.accel_y; log.u.sensor.accel_z = ao_data_ring[ao_log_data_pos].mpu6000.accel_z; diff --git a/src/core/ao_sample_mm.c b/src/core/ao_sample_mm.c index 8cfadc40..ac11eef0 100644 --- a/src/core/ao_sample_mm.c +++ b/src/core/ao_sample_mm.c @@ -101,11 +101,14 @@ ao_sample(void) /* Capture a sample */ ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data]; ao_sample_tick = ao_data->tick; + + ao_data_pres_cook(ao_data); ao_sample_pres = ao_data_pres(ao_data); ao_sample_alt = pres_to_altitude(ao_sample_pres); ao_sample_height = ao_sample_alt - ao_ground_height; + #if HAS_ACCEL - ao_sample_accel = ao_data_accel_sample(ao_data); + ao_sample_accel = ao_data_accel_cook(ao_data); if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP) ao_sample_accel = ao_data_accel_invert(ao_sample_accel); ao_data_set_accel(ao_data, ao_sample_accel); diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c index 5857c44d..9000a149 100644 --- a/src/core/ao_telemetry.c +++ b/src/core/ao_telemetry.c @@ -19,16 +19,6 @@ #include "ao_product.h" static __pdata uint16_t ao_telemetry_interval; -static __pdata int8_t ao_telemetry_config_max; -static __pdata int8_t ao_telemetry_config_cur; -#if HAS_GPS -static __pdata int8_t ao_telemetry_loc_cur; -static __pdata int8_t ao_telemetry_sat_cur; -#endif -#if HAS_COMPANION -static __pdata int8_t ao_telemetry_companion_max; -static __pdata int8_t ao_telemetry_companion_cur; -#endif static __pdata uint8_t ao_rdf = 0; static __pdata uint16_t ao_rdf_time; @@ -36,7 +26,7 @@ static __pdata uint16_t ao_rdf_time; #define AO_RDF_LENGTH_MS 500 #if defined(MEGAMETRUM) -#define AO_TELEMETRY_SENSOR AO_TELEMETRY_SENSOR_MEGAMETRUM +#define AO_SEND_MEGA 1 #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) @@ -53,6 +43,7 @@ static __pdata uint16_t ao_rdf_time; static __xdata union ao_telemetry_all telemetry; +#if defined AO_TELEMETRY_SENSOR /* Send sensor packet */ static void ao_send_sensor(void) @@ -96,10 +87,78 @@ ao_send_sensor(void) ao_radio_send(&telemetry, sizeof (telemetry)); } +#endif -static uint8_t ao_baro_sample; + +#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); + + 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; + + telemetry.mega_sensor.mag_x = packet->hmc5883.x; + telemetry.mega_sensor.mag_y = packet->hmc5883.y; + telemetry.mega_sensor.mag_z = packet->hmc5883.z; + + 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; + + /* XXX figure out right shift value; 4 might suffice */ + for (i = 0; i < AO_ADC_NUM_SENSE; i++) + telemetry.mega_data.sense[i] = packet->adc.sense[i] >> 8; + + 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) { @@ -121,6 +180,9 @@ ao_send_baro(void) } #endif +static __pdata int8_t ao_telemetry_config_max; +static __pdata int8_t ao_telemetry_config_cur; + static void ao_send_configuration(void) { @@ -146,6 +208,10 @@ ao_send_configuration(void) } #if HAS_GPS + +static __pdata int8_t ao_telemetry_loc_cur; +static __pdata int8_t ao_telemetry_sat_cur; + static void ao_send_location(void) { @@ -181,6 +247,10 @@ ao_send_satellite(void) #endif #if HAS_COMPANION + +static __pdata int8_t ao_telemetry_companion_max; +static __pdata int8_t ao_telemetry_companion_cur; + static void ao_send_companion(void) { @@ -223,7 +293,13 @@ ao_telemetry(void) #ifdef AO_SEND_ALL_BARO ao_send_baro(); #endif +#ifdef AO_SEND_MEGA + ao_send_mega_sensor(); + ao_send_mega_data(); +#else ao_send_sensor(); +#endif + #if HAS_COMPANION if (ao_companion_running) ao_send_companion(); @@ -257,31 +333,42 @@ ao_telemetry(void) void ao_telemetry_set_interval(uint16_t interval) { + uint8_t cur = 0; ao_telemetry_interval = interval; + +#if AO_SEND_MEGA + if (interval > 1) + ao_telemetry_mega_data_max = 1; + else + ao_telemetry_mega_data_max = 2; + if (ao_telemetry_mega_data_max > cur) + cur++; + ao_telemetry_mega_data_cur = cur; +#endif #if HAS_COMPANION if (!ao_companion_setup.update_period) ao_companion_setup.update_period = AO_SEC_TO_TICKS(1); ao_telemetry_companion_max = ao_companion_setup.update_period / interval; - ao_telemetry_companion_cur = 1; + if (ao_telemetry_companion_max > cur) + cur++; + ao_telemetry_companion_cur = cur; #endif ao_telemetry_config_max = AO_SEC_TO_TICKS(1) / interval; #if HAS_COMPANION - ao_telemetry_config_cur = ao_telemetry_companion_cur; - if (ao_telemetry_config_max > ao_telemetry_config_cur) - ao_telemetry_config_cur++; -#else - ao_telemetry_config_cur = 1; + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_config_cur = cur; #endif #if HAS_GPS - ao_telemetry_loc_cur = ao_telemetry_config_cur; - if (ao_telemetry_config_max > ao_telemetry_loc_cur) - ao_telemetry_loc_cur++; - ao_telemetry_sat_cur = ao_telemetry_loc_cur; - if (ao_telemetry_config_max > ao_telemetry_sat_cur) - ao_telemetry_sat_cur++; + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_loc_cur = cur; + if (ao_telemetry_config_max > cur) + cur++; + ao_telemetry_sat_cur = cur; #endif ao_wakeup(&telemetry); } diff --git a/src/stm/ao_adc_stm.c b/src/stm/ao_adc_stm.c index 71299de9..ed1d20c9 100644 --- a/src/stm/ao_adc_stm.c +++ b/src/stm/ao_adc_stm.c @@ -60,7 +60,7 @@ static void ao_adc_done(int index) #if HAS_MS5607 if (!ao_ms5607_valid) step = 0; - ao_data_ring[ao_data_head].ms5607 = ao_ms5607_current; + ao_data_ring[ao_data_head].ms5607_raw = ao_ms5607_current; #endif #if HAS_HMC5883 if (!ao_hmc5883_valid) -- 2.30.2