From ea5887027e7a39da2b7d84a142d74950b7a24703 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 5 Jul 2014 00:09:25 -0700 Subject: altos: Call ao_telemetry_reset_interval when telemetry rate changes This lets the radio code adjust the telemetry packet sending pattern when the data rate changes. Signed-off-by: Keith Packard --- src/cc1111/ao_pins.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) (limited to 'src/cc1111') diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 2b19f1f6..b40acbbd 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -56,6 +56,7 @@ #define HAS_ACCEL 1 #define HAS_IGNITE 1 #define HAS_MONITOR 0 + #define HAS_TELEMETRY 1 #endif #if defined(TELEMETRUM_V_1_1) @@ -96,6 +97,7 @@ #define HAS_ACCEL 1 #define HAS_IGNITE 1 #define HAS_MONITOR 0 + #define HAS_TELEMETRY 1 #endif #if defined(TELEMETRUM_V_1_2) @@ -136,6 +138,7 @@ #define HAS_ACCEL 1 #define HAS_IGNITE 1 #define HAS_MONITOR 0 + #define HAS_TELEMETRY 1 #endif #if defined(TELEDONGLE_V_0_2) @@ -164,6 +167,7 @@ #define LEGACY_MONITOR 1 #define HAS_RSSI 1 #define HAS_AES 0 + #define HAS_TELEMETRY 0 #endif #if defined(TELEMINI_V_1_0) @@ -193,6 +197,8 @@ #define HAS_ACCEL 0 #define HAS_IGNITE 1 #define HAS_MONITOR 0 + #define HAS_TELEMETRY 1 + #define HAS_RADIO_RATE 0 /* not enough space for this */ #endif #if defined(TELENANO_V_0_1) @@ -220,6 +226,8 @@ #define HAS_ACCEL 0 #define HAS_IGNITE 0 #define HAS_MONITOR 0 + #define HAS_TELEMETRY 1 + #define HAS_RADIO_RATE 0 /* not enough space for this */ #endif #if defined(TELEMETRUM_V_0_1) @@ -252,6 +260,8 @@ #define HAS_ACCEL 1 #define HAS_IGNITE 1 #define HAS_MONITOR 0 + #define HAS_TELEMETRY 1 + #define HAS_RADIO_RATE 0 /* not enough space for this */ #define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX ((uint32_t) 127 * (uint32_t) 1024) #endif @@ -283,6 +293,7 @@ #define LEGACY_MONITOR 1 #define HAS_RSSI 1 #define HAS_AES 0 + #define HAS_TELEMETRY 0 #endif #if defined(TIDONGLE) @@ -312,6 +323,7 @@ #define LEGACY_MONITOR 1 #define HAS_RSSI 1 #define HAS_AES 0 + #define HAS_TELEMETRY 0 #endif #if defined(TELEBT_V_0_0) @@ -350,6 +362,7 @@ #define LEGACY_MONITOR 1 #define HAS_RSSI 0 #define HAS_AES 0 + #define HAS_TELEMETRY 0 #endif #if defined(TELEBT_V_0_1) @@ -396,6 +409,7 @@ #define LEGACY_MONITOR 1 #define HAS_RSSI 0 #define HAS_AES 0 + #define HAS_TELEMETRY 0 #endif #if defined(TELELAUNCH_V_0_1) @@ -428,6 +442,7 @@ #define HAS_IGNITE 1 #define HAS_MONITOR 0 #define HAS_AES 1 + #define HAS_TELEMETRY 0 #endif #if DBG_ON_P1 -- cgit v1.2.3 From c20ddde2f9eb0ad8dbb982e9d0cbe91639160a34 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 2 Jul 2014 22:30:23 -0700 Subject: altos/cc1111: Adjust receive parameters to improve sensitivity This removes the packet quality test and carrier sense tests when deciding whether to start decoding a packet. This lets more bad packets through, but the CRC check catches those and now we're regularly receiving packets down to -110 or even -112 dBm. Before this change, we'd rarely see packets as low as -105dBm Signed-off-by: Keith Packard --- src/cc1111/ao_pins.h | 3 +++ src/cc1111/ao_radio.c | 51 ++++++++++++++++++++++++++++++--------- src/cc1111/cc1111.h | 61 +++++++++++++++++++++++++++++++++++++++++++++++ src/telebt-v1.0/ao_pins.h | 1 + 4 files changed, 105 insertions(+), 11 deletions(-) (limited to 'src/cc1111') diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index b40acbbd..2d524188 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -168,6 +168,7 @@ #define HAS_RSSI 1 #define HAS_AES 0 #define HAS_TELEMETRY 0 + #define AO_RADIO_REG_TEST 1 #endif #if defined(TELEMINI_V_1_0) @@ -363,6 +364,7 @@ #define HAS_RSSI 0 #define HAS_AES 0 #define HAS_TELEMETRY 0 + #define AO_RADIO_REG_TEST 1 #endif #if defined(TELEBT_V_0_1) @@ -410,6 +412,7 @@ #define HAS_RSSI 0 #define HAS_AES 0 #define HAS_TELEMETRY 0 + #define AO_RADIO_REG_TEST 1 #endif #if defined(TELELAUNCH_V_0_1) diff --git a/src/cc1111/ao_radio.c b/src/cc1111/ao_radio.c index 190647ce..fbdf7762 100644 --- a/src/cc1111/ao_radio.c +++ b/src/cc1111/ao_radio.c @@ -126,9 +126,9 @@ static __code uint8_t radio_setup[] = { (CHANBW_M << RF_MDMCFG4_CHANBW_M_SHIFT) | (DRATE_E << RF_MDMCFG4_DRATE_E_SHIFT)), RF_MDMCFG3_OFF, (DRATE_M << RF_MDMCFG3_DRATE_M_SHIFT), - RF_MDMCFG2_OFF, (RF_MDMCFG2_DEM_DCFILT_OFF | + RF_MDMCFG2_OFF, (RF_MDMCFG2_DEM_DCFILT_ON | RF_MDMCFG2_MOD_FORMAT_GFSK | - RF_MDMCFG2_SYNC_MODE_15_16_THRES), + RF_MDMCFG2_SYNC_MODE_15_16), RF_MDMCFG1_OFF, (RF_MDMCFG1_FEC_EN | RF_MDMCFG1_NUM_PREAMBLE_4 | (2 << RF_MDMCFG1_CHANSPC_E_SHIFT)), @@ -155,8 +155,8 @@ static __code uint8_t radio_setup[] = { RF_FSCAL1_OFF, 0x00, RF_FSCAL0_OFF, 0x1F, - RF_TEST2_OFF, 0x88, - RF_TEST1_OFF, 0x31, + RF_TEST2_OFF, RF_TEST2_RX_LOW_DATA_RATE_MAGIC, + RF_TEST1_OFF, RF_TEST1_RX_LOW_DATA_RATE_MAGIC, RF_TEST0_OFF, 0x09, /* default sync values */ @@ -187,10 +187,16 @@ static __code uint8_t radio_setup[] = { RF_BSCFG_BS_POST_KI_PRE_KI| RF_BSCFG_BS_POST_KP_PRE_KP| RF_BSCFG_BS_LIMIT_0), - RF_AGCCTRL2_OFF, 0x03, - RF_AGCCTRL1_OFF, 0x40, - RF_AGCCTRL0_OFF, 0x91, - + RF_AGCCTRL2_OFF, (RF_AGCCTRL2_MAX_DVGA_GAIN_ALL| + RF_AGCCTRL2_MAX_LNA_GAIN_0| + RF_AGCCTRL2_MAGN_TARGET_33dB), + RF_AGCCTRL1_OFF, (RF_AGCCTRL1_AGC_LNA_PRIORITY_0 | + RF_AGCCTRL1_CARRIER_SENSE_REL_THR_DISABLE | + RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_0DB), + RF_AGCCTRL0_OFF, (RF_AGCCTRL0_HYST_LEVEL_NONE | + RF_AGCCTRL0_WAIT_TIME_8 | + RF_AGCCTRL0_AGC_FREEZE_NORMAL | + RF_AGCCTRL0_FILTER_LENGTH_8), RF_IOCFG2_OFF, 0x00, RF_IOCFG1_OFF, 0x00, RF_IOCFG0_OFF, 0x00, @@ -212,7 +218,7 @@ static __code uint8_t rdf_setup[] = { (RDF_DEVIATION_M << RF_DEVIATN_DEVIATION_M_SHIFT)), /* packet length is set in-line */ - RF_PKTCTRL1_OFF, ((1 << PKTCTRL1_PQT_SHIFT)| + RF_PKTCTRL1_OFF, ((0 << PKTCTRL1_PQT_SHIFT)| PKTCTRL1_ADR_CHK_NONE), RF_PKTCTRL0_OFF, (RF_PKTCTRL0_PKT_FORMAT_NORMAL| RF_PKTCTRL0_LENGTH_CONFIG_FIXED), @@ -223,9 +229,9 @@ static __code uint8_t fixed_pkt_setup[] = { (CHANBW_M << RF_MDMCFG4_CHANBW_M_SHIFT) | (DRATE_E << RF_MDMCFG4_DRATE_E_SHIFT)), RF_MDMCFG3_OFF, (DRATE_M << RF_MDMCFG3_DRATE_M_SHIFT), - RF_MDMCFG2_OFF, (RF_MDMCFG2_DEM_DCFILT_OFF | + RF_MDMCFG2_OFF, (RF_MDMCFG2_DEM_DCFILT_ON | RF_MDMCFG2_MOD_FORMAT_GFSK | - RF_MDMCFG2_SYNC_MODE_15_16_THRES), + RF_MDMCFG2_SYNC_MODE_15_16), RF_MDMCFG1_OFF, (RF_MDMCFG1_FEC_EN | RF_MDMCFG1_NUM_PREAMBLE_4 | (2 << RF_MDMCFG1_CHANSPC_E_SHIFT)), @@ -551,8 +557,31 @@ ao_radio_test_cmd(void) ao_radio_test(0); } +#if AO_RADIO_REG_TEST +static void +ao_radio_set_reg(void) +{ + uint8_t offset; + ao_cmd_hex(); + offset = ao_cmd_lex_i; + if (ao_cmd_status != ao_cmd_success) + return; + ao_cmd_hex(); + printf("RF[%x] %x", offset, RF[offset]); + if (ao_cmd_status == ao_cmd_success) { + RF[offset] = ao_cmd_lex_i; + printf (" -> %x", RF[offset]); + } + ao_cmd_status = ao_cmd_success; + printf("\n"); +} +#endif + __code struct ao_cmds ao_radio_cmds[] = { { ao_radio_test_cmd, "C <1 start, 0 stop, none both>\0Radio carrier test" }, +#if AO_RADIO_REG_TEST + { ao_radio_set_reg, "V \0Set radio register" }, +#endif { 0, NULL }, }; diff --git a/src/cc1111/cc1111.h b/src/cc1111/cc1111.h index 80d3fb70..ac1e71c9 100644 --- a/src/cc1111/cc1111.h +++ b/src/cc1111/cc1111.h @@ -1165,12 +1165,73 @@ __xdata __at (0xdf16) uint8_t RF_BSCFG; __xdata __at (0xdf17) uint8_t RF_AGCCTRL2; #define RF_AGCCTRL2_OFF 0x17 +#define RF_AGCCTRL2_MAX_DVGA_GAIN_ALL (0 << 6) +#define RF_AGCCTRL2_MAX_DVGA_GAIN_BUT_1 (1 << 6) +#define RF_AGCCTRL2_MAX_DVGA_GAIN_BUT_2 (2 << 6) +#define RF_AGCCTRL2_MAX_DVGA_GAIN_BUT_3 (3 << 6) +#define RF_AGCCTRL2_MAX_LNA_GAIN_0 (0 << 3) +#define RF_AGCCTRL2_MAX_LNA_GAIN_2_6 (1 << 3) +#define RF_AGCCTRL2_MAX_LNA_GAIN_6_1 (2 << 3) +#define RF_AGCCTRL2_MAX_LNA_GAIN_7_4 (3 << 3) +#define RF_AGCCTRL2_MAX_LNA_GAIN_9_2 (4 << 3) +#define RF_AGCCTRL2_MAX_LNA_GAIN_11_5 (5 << 3) +#define RF_AGCCTRL2_MAX_LNA_GAIN_14_6 (6 << 3) +#define RF_AGCCTRL2_MAX_LNA_GAIN_17_1 (7 << 3) +#define RF_AGCCTRL2_MAGN_TARGET_24dB (0 << 0) +#define RF_AGCCTRL2_MAGN_TARGET_27dB (1 << 0) +#define RF_AGCCTRL2_MAGN_TARGET_30dB (2 << 0) +#define RF_AGCCTRL2_MAGN_TARGET_33dB (3 << 0) +#define RF_AGCCTRL2_MAGN_TARGET_36dB (4 << 0) +#define RF_AGCCTRL2_MAGN_TARGET_38dB (5 << 0) +#define RF_AGCCTRL2_MAGN_TARGET_40dB (6 << 0) +#define RF_AGCCTRL2_MAGN_TARGET_42dB (7 << 0) + __xdata __at (0xdf18) uint8_t RF_AGCCTRL1; #define RF_AGCCTRL1_OFF 0x18 +#define RF_AGCCTRL1_AGC_LNA_PRIORITY_0 (0 << 6) +#define RF_AGCCTRL1_AGC_LNA_PRIORITY_1 (1 << 6) +#define RF_AGCCTRL1_CARRIER_SENSE_REL_THR_DISABLE (0 << 4) +#define RF_AGCCTRL1_CARRIER_SENSE_REL_THR_6DB (1 << 4) +#define RF_AGCCTRL1_CARRIER_SENSE_REL_THR_10DB (2 << 4) +#define RF_AGCCTRL1_CARRIER_SENSE_REL_THR_14DB (3 << 4) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_DISABLE (0x8 << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_7DB_BELOW (0x9 << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_6DB_BELOW (0xa << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_5DB_BELOW (0xb << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_4DB_BELOW (0xc << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_3DB_BELOW (0xd << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_2DB_BELOW (0xe << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_1DB_BELOW (0xf << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_0DB (0x0 << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_1DB_ABOVE (0x1 << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_2DB_ABOVE (0x2 << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_3DB_ABOVE (0x3 << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_4DB_ABOVE (0x4 << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_5DB_ABOVE (0x5 << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_6DB_ABOVE (0x6 << 0) +#define RF_AGCCTRL1_CARRIER_SENSE_ABS_THR_7DB_ABOVE (0x7 << 0) + __xdata __at (0xdf19) uint8_t RF_AGCCTRL0; #define RF_AGCCTRL0_OFF 0x19 +#define RF_AGCCTRL0_HYST_LEVEL_NONE (0 << 6) +#define RF_AGCCTRL0_HYST_LEVEL_LOW (1 << 6) +#define RF_AGCCTRL0_HYST_LEVEL_MEDIUM (2 << 6) +#define RF_AGCCTRL0_HYST_LEVEL_HIGH (3 << 6) +#define RF_AGCCTRL0_WAIT_TIME_8 (0 << 4) +#define RF_AGCCTRL0_WAIT_TIME_16 (1 << 4) +#define RF_AGCCTRL0_WAIT_TIME_24 (2 << 4) +#define RF_AGCCTRL0_WAIT_TIME_32 (3 << 4) +#define RF_AGCCTRL0_AGC_FREEZE_NORMAL (0 << 2) +#define RF_AGCCTRL0_AGC_FREEZE_SYNC (1 << 2) +#define RF_AGCCTRL0_AGC_FREEZE_MANUAL_ANALOG (2 << 2) +#define RF_AGCCTRL0_AGC_FREEZE_MANUAL_BOTH (3 << 2) +#define RF_AGCCTRL0_FILTER_LENGTH_8 (0 << 0) +#define RF_AGCCTRL0_FILTER_LENGTH_16 (1 << 0) +#define RF_AGCCTRL0_FILTER_LENGTH_32 (2 << 0) +#define RF_AGCCTRL0_FILTER_LENGTH_64 (3 << 0) + __xdata __at (0xdf1a) uint8_t RF_FREND1; #define RF_FREND1_OFF 0x1a diff --git a/src/telebt-v1.0/ao_pins.h b/src/telebt-v1.0/ao_pins.h index b5562573..4181ef35 100644 --- a/src/telebt-v1.0/ao_pins.h +++ b/src/telebt-v1.0/ao_pins.h @@ -49,6 +49,7 @@ #define HAS_MONITOR 1 #define LEGACY_MONITOR 0 #define HAS_TELEMETRY 0 +#define AO_RADIO_REG_TEST 1 #define HAS_ADC 1 #define AO_PAD_ADC_BATT 0 -- cgit v1.2.3 From aba1703a1cff3da001d64bf2d15a591816e3350e Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Wed, 2 Jul 2014 22:31:59 -0700 Subject: altos/cc1111: Add 2400 and 9600 baud telemetry rate support Signed-off-by: Keith Packard --- src/cc1111/ao_pins.h | 3 ++ src/cc1111/ao_radio.c | 86 +++++++++++++++++++++++++++++++++++++++++++++------ 2 files changed, 80 insertions(+), 9 deletions(-) (limited to 'src/cc1111') diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 2d524188..83a3c774 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -57,6 +57,7 @@ #define HAS_IGNITE 1 #define HAS_MONITOR 0 #define HAS_TELEMETRY 1 + #define HAS_RADIO_RATE 0 /* not enough space for this */ #endif #if defined(TELEMETRUM_V_1_1) @@ -98,6 +99,7 @@ #define HAS_IGNITE 1 #define HAS_MONITOR 0 #define HAS_TELEMETRY 1 + #define HAS_RADIO_RATE 0 /* not enough space for this */ #endif #if defined(TELEMETRUM_V_1_2) @@ -139,6 +141,7 @@ #define HAS_IGNITE 1 #define HAS_MONITOR 0 #define HAS_TELEMETRY 1 + #define HAS_RADIO_RATE 0 /* not enough space for this */ #endif #if defined(TELEDONGLE_V_0_2) diff --git a/src/cc1111/ao_radio.c b/src/cc1111/ao_radio.c index fbdf7762..8f519958 100644 --- a/src/cc1111/ao_radio.c +++ b/src/cc1111/ao_radio.c @@ -40,6 +40,42 @@ #define IF_FREQ_CONTROL 6 /* + * http://www.ntia.doc.gov/files/ntia/publications/84-168.pdf + * + * Necessary bandwidth for a FSK modulated signal: + * + * bw = 2.6d + 0.55b 1.5 < m < 5.5 + * bw = 2.1d + 1.9b 5.5 < m < 20 + * + * b is the modulation rate in bps + * d is the peak deviation (from the center) + * + * m = 2d / b + * + * 20.5 kHz deviation 38.4kbps signal: + * + * m = 41 / 38.4, which is < 5.5: + * + * bw = 2.6 * 20.5 + 0.55 * 38.4 = 74.42kHz + * + * M = 1, E = 3, bw = 75kHz + * + * 20.5 kHz deviation, 9.6kbps signal + * + * m = 41 / 9.6, which is < 5.5: + * + * bw = 2.6 * 20.5 + 0.55 * 9.6 = 58.58kHz + * + * M = 2, E = 3, bw = 62.5kHz + * + * 20.5kHz deviation, 2.4kbps signal + * + * m = 41 / 2.4, which is > 5.5: + * + * bw = 2.1 * 20.5 + 1.9 * 2.4 = 47.61kHz + * + * M = 3, E = 3, bw = 53.6kHz + * * For channel bandwidth of 93.75 kHz, the CHANBW_E and CHANBW_M values are * * BW = 24e6 / (8 * (4 + M) * 2 ** E) @@ -47,7 +83,9 @@ * So, M = 0 and E = 3 */ -#define CHANBW_M 0 +#define CHANBW_M_384 1 +#define CHANBW_M_96 2 +#define CHANBW_M_24 3 #define CHANBW_E 3 /* @@ -55,12 +93,23 @@ * * R = (256 + M) * 2** E * 24e6 / 2**28 * - * So M is 163 and E is 10 + * So for 38360kBaud, M is 163 and E is 10 */ -#define DRATE_E 10 #define DRATE_M 163 +#define DRATE_E_384 10 + +/* For 9600 baud, M is 163 and E is 8 + */ + +#define DRATE_E_96 8 + +/* For 2400 baud, M is 163 and E is 6 + */ + +#define DRATE_E_24 6 + /* * For a channel deviation of 20.5kHz, the DEVIATION_E and DEVIATION_M values are: * @@ -122,9 +171,6 @@ static __code uint8_t radio_setup[] = { RF_FSCTRL1_OFF, (IF_FREQ_CONTROL << RF_FSCTRL1_FREQ_IF_SHIFT), RF_FSCTRL0_OFF, (0 << RF_FSCTRL0_FREQOFF_SHIFT), - RF_MDMCFG4_OFF, ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | - (CHANBW_M << RF_MDMCFG4_CHANBW_M_SHIFT) | - (DRATE_E << RF_MDMCFG4_DRATE_E_SHIFT)), RF_MDMCFG3_OFF, (DRATE_M << RF_MDMCFG3_DRATE_M_SHIFT), RF_MDMCFG2_OFF, (RF_MDMCFG2_DEM_DCFILT_ON | RF_MDMCFG2_MOD_FORMAT_GFSK | @@ -204,7 +250,7 @@ static __code uint8_t radio_setup[] = { static __code uint8_t rdf_setup[] = { RF_MDMCFG4_OFF, ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | - (CHANBW_M << RF_MDMCFG4_CHANBW_M_SHIFT) | + (CHANBW_M_384 << RF_MDMCFG4_CHANBW_M_SHIFT) | (RDF_DRATE_E << RF_MDMCFG4_DRATE_E_SHIFT)), RF_MDMCFG3_OFF, (RDF_DRATE_M << RF_MDMCFG3_DRATE_M_SHIFT), RF_MDMCFG2_OFF, (RF_MDMCFG2_DEM_DCFILT_OFF | @@ -225,9 +271,11 @@ static __code uint8_t rdf_setup[] = { }; static __code uint8_t fixed_pkt_setup[] = { +#if !HAS_RADIO_RATE RF_MDMCFG4_OFF, ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | - (CHANBW_M << RF_MDMCFG4_CHANBW_M_SHIFT) | - (DRATE_E << RF_MDMCFG4_DRATE_E_SHIFT)), + (CHANBW_M_384 << RF_MDMCFG4_CHANBW_M_SHIFT) | + (DRATE_E_384 << RF_MDMCFG4_DRATE_E_SHIFT)), +#endif RF_MDMCFG3_OFF, (DRATE_M << RF_MDMCFG3_DRATE_M_SHIFT), RF_MDMCFG2_OFF, (RF_MDMCFG2_DEM_DCFILT_ON | RF_MDMCFG2_MOD_FORMAT_GFSK | @@ -249,6 +297,23 @@ static __code uint8_t fixed_pkt_setup[] = { RF_PKTCTRL0_LENGTH_CONFIG_FIXED), }; +#if HAS_RADIO_RATE +static __code uint8_t packet_rate_setup[] = { + /* 38400 */ + ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | + (CHANBW_M_384 << RF_MDMCFG4_CHANBW_M_SHIFT) | + (DRATE_E_384 << RF_MDMCFG4_DRATE_E_SHIFT)), + /* 9600 */ + ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | + (CHANBW_M_96 << RF_MDMCFG4_CHANBW_M_SHIFT) | + (DRATE_E_96 << RF_MDMCFG4_DRATE_E_SHIFT)), + /* 2400 */ + ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | + (CHANBW_M_24 << RF_MDMCFG4_CHANBW_M_SHIFT) | + (DRATE_E_24 << RF_MDMCFG4_DRATE_E_SHIFT)), +}; +#endif + __xdata uint8_t ao_radio_dma; __xdata uint8_t ao_radio_dma_done; __xdata uint8_t ao_radio_done; @@ -314,6 +379,9 @@ ao_radio_get(uint8_t len) RF_FREQ1 = (uint8_t) (ao_config.radio_setting >> 8); RF_FREQ0 = (uint8_t) (ao_config.radio_setting); RF_PKTLEN = len; +#if HAS_RADIO_RATE + RF_MDMCFG4 = packet_rate_setup[ao_config.radio_rate]; +#endif } -- cgit v1.2.3 From 292cb8380b478542555b5f370e8252eafa2f74ac Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 5 Jul 2014 00:04:06 -0700 Subject: altos: Rework packet receive for cc1120 Instead of blocking on PQT, just set up the receiver to start going and when the first bit interrupt comes in, grab the SPI bus if possible and configure it for reception. This improves sensitivity in the radio by a significant amount while making the code conceptually a bit nicer. Signed-off-by: Keith Packard --- src/cc1111/ao_pins.h | 3 + src/drivers/ao_cc1120.c | 160 +++++++++++++++++++++++++++++++----------------- src/kernel/ao.h | 3 + src/kernel/ao_mutex.c | 23 +++++++ src/lpc/ao_arch_funcs.h | 17 ++--- src/stm/ao_arch_funcs.h | 25 ++++++-- src/stm/ao_spi_stm.c | 51 +++++++++++++-- 7 files changed, 208 insertions(+), 74 deletions(-) (limited to 'src/cc1111') diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 83a3c774..1bc3d716 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -58,6 +58,7 @@ #define HAS_MONITOR 0 #define HAS_TELEMETRY 1 #define HAS_RADIO_RATE 0 /* not enough space for this */ + #define HAS_MUTEX_TRY 0 #endif #if defined(TELEMETRUM_V_1_1) @@ -100,6 +101,7 @@ #define HAS_MONITOR 0 #define HAS_TELEMETRY 1 #define HAS_RADIO_RATE 0 /* not enough space for this */ + #define HAS_MUTEX_TRY 0 #endif #if defined(TELEMETRUM_V_1_2) @@ -142,6 +144,7 @@ #define HAS_MONITOR 0 #define HAS_TELEMETRY 1 #define HAS_RADIO_RATE 0 /* not enough space for this */ + #define HAS_MUTEX_TRY 0 #endif #if defined(TELEDONGLE_V_0_2) diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c index 5d51fbcd..1b907940 100644 --- a/src/drivers/ao_cc1120.c +++ b/src/drivers/ao_cc1120.c @@ -41,8 +41,10 @@ extern const uint32_t ao_radio_cal; #define FOSC 32000000 +#define ao_radio_try_select(task_id) ao_spi_try_get_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS,AO_SPI_SPEED_4MHz, task_id) #define ao_radio_select() ao_spi_get_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS,AO_SPI_SPEED_4MHz) #define ao_radio_deselect() ao_spi_put_mask(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN),AO_CC1120_SPI_BUS) +#define ao_radio_spi_send_sync(d,l) ao_spi_send_sync((d), (l), AO_CC1120_SPI_BUS) #define ao_radio_spi_send(d,l) ao_spi_send((d), (l), AO_CC1120_SPI_BUS) #define ao_radio_spi_send_fixed(d,l) ao_spi_send_fixed((d), (l), AO_CC1120_SPI_BUS) #define ao_radio_spi_recv(d,l) ao_spi_recv((d), (l), AO_CC1120_SPI_BUS) @@ -107,7 +109,7 @@ ao_radio_reg_write(uint16_t addr, uint8_t value) } static void -ao_radio_burst_read_start (uint16_t addr) +_ao_radio_burst_read_start (uint16_t addr) { uint8_t data[2]; uint8_t d; @@ -124,8 +126,8 @@ ao_radio_burst_read_start (uint16_t addr) addr); d = 1; } - ao_radio_select(); - ao_radio_spi_send(data, d); + + ao_radio_spi_send_sync(data, d); } static void @@ -209,7 +211,7 @@ ao_radio_tx_fifo_space(void) return CC1120_FIFO_SIZE - ao_radio_reg_read(CC1120_NUM_TXBYTES); } -#if 0 +#if CC1120_DEBUG static uint8_t ao_radio_status(void) { @@ -275,11 +277,13 @@ static void ao_radio_idle(void) { for (;;) { - uint8_t state = ao_radio_strobe(CC1120_SIDLE); - if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_IDLE) + uint8_t state = (ao_radio_strobe(CC1120_SIDLE) >> CC1120_STATUS_STATE) & CC1120_STATUS_STATE_MASK; + if (state == CC1120_STATUS_STATE_IDLE) break; - if ((state >> CC1120_STATUS_STATE) == CC1120_STATUS_STATE_TX_FIFO_ERROR) + if (state == CC1120_STATUS_STATE_TX_FIFO_ERROR) ao_radio_strobe(CC1120_SFTX); + if (state == CC1120_STATUS_STATE_RX_FIFO_ERROR) + ao_radio_strobe(CC1120_SFRX); } /* Flush any pending TX bytes */ ao_radio_strobe(CC1120_SFTX); @@ -948,6 +952,11 @@ static uint16_t rx_data_consumed; static uint16_t rx_data_cur; static uint8_t rx_ignore; static uint8_t rx_waiting; +static uint8_t rx_starting; +static uint8_t rx_task_id; +static uint32_t rx_fast_start; +static uint32_t rx_slow_start; +static uint32_t rx_missed; #if AO_PROFILE static uint32_t rx_start_tick, rx_packet_tick, rx_done_tick, rx_last_done_tick; @@ -962,13 +971,34 @@ ao_radio_rx_isr(void) { uint8_t d; + if (rx_task_id) { + if (ao_radio_try_select(rx_task_id)) { + ++rx_fast_start; + rx_task_id = 0; + _ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT); + } else { + if (rx_ignore) + --rx_ignore; + else { + ao_radio_abort = 1; + rx_missed++; + } + return; + } + } + if (rx_starting) { + rx_starting = 0; + ao_wakeup(&ao_radio_wake); + } d = AO_CC1120_SPI.dr; AO_CC1120_SPI.dr = 0; if (rx_ignore == 0) { - if (rx_data_cur >= rx_data_count) - ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); - else + if (rx_data_cur < rx_data_count) rx_data[rx_data_cur++] = d; + if (rx_data_cur >= rx_data_count) { + ao_spi_clr_cs(AO_CC1120_SPI_CS_PORT,(1 << AO_CC1120_SPI_CS_PIN)); + ao_exti_disable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); + } if (rx_waiting && rx_data_cur - rx_data_consumed >= AO_FEC_DECODE_BLOCK) { #if AO_PROFILE if (!rx_packet_tick) @@ -987,24 +1017,20 @@ ao_radio_rx_isr(void) static uint16_t ao_radio_rx_wait(void) { - do { - if (ao_radio_mcu_wake) - ao_radio_check_marc_status(); - ao_alarm(AO_MS_TO_TICKS(100)); - ao_arch_block_interrupts(); - rx_waiting = 1; - while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK && - !ao_radio_abort && - !ao_radio_mcu_wake) - { - if (ao_sleep(&ao_radio_wake)) - ao_radio_abort = 1; - } - rx_waiting = 0; - ao_arch_release_interrupts(); - ao_clear_alarm(); - } while (ao_radio_mcu_wake); - if (ao_radio_abort) + ao_alarm(AO_MS_TO_TICKS(100)); + ao_arch_block_interrupts(); + rx_waiting = 1; + while (rx_data_cur - rx_data_consumed < AO_FEC_DECODE_BLOCK && + !ao_radio_abort && + !ao_radio_mcu_wake) + { + if (ao_sleep(&ao_radio_wake)) + ao_radio_abort = 1; + } + rx_waiting = 0; + ao_arch_release_interrupts(); + ao_clear_alarm(); + if (ao_radio_abort || ao_radio_mcu_wake) return 0; rx_data_consumed += AO_FEC_DECODE_BLOCK; #if AO_PROFILE @@ -1044,48 +1070,67 @@ ao_radio_recv(__xdata void *d, uint8_t size, uint8_t timeout) */ ao_radio_abort = 0; - /* configure interrupt pin */ ao_radio_get(len); - ao_radio_set_mode(AO_RADIO_MODE_PACKET_RX); ao_radio_wake = 0; ao_radio_mcu_wake = 0; - AO_CC1120_SPI.cr2 = 0; - - /* clear any RXNE */ - (void) AO_CC1120_SPI.dr; + ao_radio_set_mode(AO_RADIO_MODE_PACKET_RX); - /* Have the radio signal when the preamble quality goes high */ - ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_PQT_REACHED); + /* configure interrupt pin */ + ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT); ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, - AO_EXTI_MODE_RISING|AO_EXTI_PRIORITY_HIGH); - ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_isr); + AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH); + + ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr); ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); - ao_exti_enable(AO_CC1120_MCU_WAKEUP_PORT, AO_CC1120_MCU_WAKEUP_PIN); + + rx_starting = 1; + rx_task_id = ao_cur_task->task_id; ao_radio_strobe(CC1120_SRX); - /* Wait for the preamble to appear */ - ao_radio_wait_isr(timeout); + if (timeout) + ao_alarm(timeout); + ao_arch_block_interrupts(); + while (rx_starting && !ao_radio_abort) { + if (ao_sleep(&ao_radio_wake)) + ao_radio_abort = 1; + } + uint8_t rx_task_id_save = rx_task_id; + rx_task_id = 0; + rx_starting = 0; + ao_arch_release_interrupts(); + if (timeout) + ao_clear_alarm(); + if (ao_radio_abort) { ret = 0; + rx_task_id = 0; goto abort; } - ao_radio_reg_write(AO_CC1120_INT_GPIO_IOCFG, CC1120_IOCFG_GPIO_CFG_CLKEN_SOFT); - ao_exti_set_mode(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, - AO_EXTI_MODE_FALLING|AO_EXTI_PRIORITY_HIGH); - - ao_exti_set_callback(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN, ao_radio_rx_isr); - ao_exti_enable(AO_CC1120_INT_PORT, AO_CC1120_INT_PIN); - - ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT); + if (rx_task_id_save) { + ++rx_slow_start; + ao_radio_select(); + _ao_radio_burst_read_start(CC1120_SOFT_RX_DATA_OUT); + if (rx_ignore) { + uint8_t ignore = AO_CC1120_SPI.dr; + (void) ignore; + AO_CC1120_SPI.dr = 0; + --rx_ignore; + } + } ret = ao_fec_decode(rx_data, rx_data_count, d, size + 2, ao_radio_rx_wait); ao_radio_burst_read_stop(); + if (ao_radio_mcu_wake) + ao_radio_check_marc_status(); + if (ao_radio_abort) + ret = 0; + abort: /* Convert from 'real' rssi to cc1111-style values */ @@ -1100,7 +1145,7 @@ abort: radio_rssi = AO_RADIO_FROM_RSSI (rssi); } - ao_radio_strobe(CC1120_SIDLE); + ao_radio_idle(); ao_radio_put(); @@ -1139,7 +1184,7 @@ struct ao_cc1120_reg { char *name; }; -const static struct ao_cc1120_reg ao_cc1120_reg[] = { +static const struct ao_cc1120_reg ao_cc1120_reg[] = { { .addr = CC1120_IOCFG3, .name = "IOCFG3" }, { .addr = CC1120_IOCFG2, .name = "IOCFG2" }, { .addr = CC1120_IOCFG1, .name = "IOCFG1" }, @@ -1320,7 +1365,7 @@ const static struct ao_cc1120_reg ao_cc1120_reg[] = { static void ao_radio_show(void) { uint8_t status = ao_radio_status(); - int i; + unsigned int i; ao_radio_get(0xff); status = ao_radio_status(); @@ -1331,6 +1376,10 @@ static void ao_radio_show(void) { for (i = 0; i < AO_NUM_CC1120_REG; i++) printf ("\t%02x %-20.20s\n", ao_radio_reg_read(ao_cc1120_reg[i].addr), ao_cc1120_reg[i].name); + + printf("RX fast start: %u\n", rx_fast_start); + printf("RX slow start: %u\n", rx_slow_start); + printf("RX missed: %u\n", rx_missed); ao_radio_put(); } @@ -1354,12 +1403,12 @@ static void ao_radio_packet(void) { } void -ao_radio_test_recv() +ao_radio_test_recv(void) { uint8_t bytes[34]; uint8_t b; - if (ao_radio_recv(bytes, 34)) { + if (ao_radio_recv(bytes, 34, 0)) { if (bytes[33] & 0x80) printf ("CRC OK"); else @@ -1375,13 +1424,12 @@ ao_radio_test_recv() #include static void -ao_radio_aprs() +ao_radio_aprs(void) { ao_packet_slave_stop(); ao_aprs_send(); } #endif - #endif static const struct ao_cmds ao_radio_cmds[] = { diff --git a/src/kernel/ao.h b/src/kernel/ao.h index c11aa028..a225bc4a 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -132,6 +132,9 @@ ao_clock_init(void); */ #ifndef ao_mutex_get +uint8_t +ao_mutex_try(__xdata uint8_t *ao_mutex, uint8_t task_id) __reentrant; + void ao_mutex_get(__xdata uint8_t *ao_mutex) __reentrant; diff --git a/src/kernel/ao_mutex.c b/src/kernel/ao_mutex.c index 952ff462..a36fe939 100644 --- a/src/kernel/ao_mutex.c +++ b/src/kernel/ao_mutex.c @@ -17,6 +17,29 @@ #include "ao.h" +#ifndef HAS_MUTEX_TRY +#define HAS_MUTEX_TRY 1 +#endif + +#if HAS_MUTEX_TRY + +uint8_t +ao_mutex_try(__xdata uint8_t *mutex, uint8_t task_id) __reentrant +{ + uint8_t ret; + if (*mutex == task_id) + ao_panic(AO_PANIC_MUTEX); + ao_arch_critical( + if (*mutex) + ret = 0; + else { + *mutex = task_id; + ret = 1; + }); + return ret; +} +#endif + void ao_mutex_get(__xdata uint8_t *mutex) __reentrant { diff --git a/src/lpc/ao_arch_funcs.h b/src/lpc/ao_arch_funcs.h index 0891903e..21a7a8e5 100644 --- a/src/lpc/ao_arch_funcs.h +++ b/src/lpc/ao_arch_funcs.h @@ -161,16 +161,17 @@ static inline void ao_arch_restore_stack(void) { #endif /* HAS_TASK */ -#define ao_arch_wait_interrupt() do { \ - asm(".global ao_idle_loc\n\twfi\nao_idle_loc:"); \ - ao_arch_release_interrupts(); \ - ao_arch_block_interrupts(); \ +#define ao_arch_wait_interrupt() do { \ + asm("\twfi\n"); \ + ao_arch_release_interrupts(); \ + asm(".global ao_idle_loc\n\nao_idle_loc:"); \ + ao_arch_block_interrupts(); \ } while (0) -#define ao_arch_critical(b) do { \ - ao_arch_block_interrupts(); \ - do { b } while (0); \ - ao_arch_release_interrupts(); \ +#define ao_arch_critical(b) do { \ + uint32_t __mask = ao_arch_irqsave(); \ + do { b } while (0); \ + ao_arch_irqrestore(__mask); \ } while (0) /* diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h index b461cd3f..7ad3b4b8 100644 --- a/src/stm/ao_arch_funcs.h +++ b/src/stm/ao_arch_funcs.h @@ -64,6 +64,9 @@ #define AO_SPI_INDEX(id) ((id) & AO_SPI_INDEX_MASK) #define AO_SPI_CONFIG(id) ((id) & AO_SPI_CONFIG_MASK) +uint8_t +ao_spi_try_get(uint8_t spi_index, uint32_t speed, uint8_t task_id); + void ao_spi_get(uint8_t spi_index, uint32_t speed); @@ -76,6 +79,9 @@ ao_spi_send(void *block, uint16_t len, uint8_t spi_index); void ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index); +void +ao_spi_send_sync(void *block, uint16_t len, uint8_t spi_index); + void ao_spi_recv(void *block, uint16_t len, uint8_t spi_index); @@ -95,6 +101,15 @@ ao_spi_init(void); ao_spi_set_cs(reg,mask); \ } while (0) +static inline uint8_t +ao_spi_try_get_mask(struct stm_gpio *reg, uint16_t mask, uint8_t bus, uint32_t speed, uint8_t task_id) +{ + if (!ao_spi_try_get(bus, speed, task_id)) + return 0; + ao_spi_set_cs(reg, mask); + return 1; +} + #define ao_spi_put_mask(reg,mask,bus) do { \ ao_spi_clr_cs(reg,mask); \ ao_spi_put(bus); \ @@ -252,6 +267,8 @@ extern struct ao_stm_usart ao_stm_usart3; #define ARM_PUSH32(stack, val) (*(--(stack)) = (val)) +typedef uint32_t ao_arch_irq_t; + static inline uint32_t ao_arch_irqsave(void) { uint32_t primask; @@ -369,10 +386,10 @@ static inline void ao_arch_start_scheduler(void) { ao_arch_block_interrupts(); \ } while (0) -#define ao_arch_critical(b) do { \ - ao_arch_block_interrupts(); \ - do { b } while (0); \ - ao_arch_release_interrupts(); \ +#define ao_arch_critical(b) do { \ + uint32_t __mask = ao_arch_irqsave(); \ + do { b } while (0); \ + ao_arch_irqrestore(__mask); \ } while (0) #endif /* _AO_ARCH_FUNCS_H_ */ diff --git a/src/stm/ao_spi_stm.c b/src/stm/ao_spi_stm.c index 56329c24..885af544 100644 --- a/src/stm/ao_spi_stm.c +++ b/src/stm/ao_spi_stm.c @@ -153,6 +153,28 @@ ao_spi_send_fixed(uint8_t value, uint16_t len, uint8_t spi_index) ao_dma_done_transfer(miso_dma_index); } +void +ao_spi_send_sync(void *block, uint16_t len, uint8_t spi_index) +{ + uint8_t *b = block; + struct stm_spi *stm_spi = ao_spi_stm_info[AO_SPI_INDEX(spi_index)].stm_spi; + + stm_spi->cr2 = ((0 << STM_SPI_CR2_TXEIE) | + (0 << STM_SPI_CR2_RXNEIE) | + (0 << STM_SPI_CR2_ERRIE) | + (0 << STM_SPI_CR2_SSOE) | + (0 << STM_SPI_CR2_TXDMAEN) | + (0 << STM_SPI_CR2_RXDMAEN)); + + /* Clear RXNE */ + (void) stm_spi->dr; + + while (len--) { + while (!(stm_spi->sr & (1 << STM_SPI_SR_TXE))); + stm_spi->dr = *b++; + } +} + void ao_spi_recv(void *block, uint16_t len, uint8_t spi_index) { @@ -356,13 +378,11 @@ ao_spi_enable_index(uint8_t spi_index) } } -void -ao_spi_get(uint8_t spi_index, uint32_t speed) +static void +ao_spi_config(uint8_t spi_index, uint32_t speed) { uint8_t id = AO_SPI_INDEX(spi_index); struct stm_spi *stm_spi = ao_spi_stm_info[id].stm_spi; - - ao_mutex_get(&ao_spi_mutex[id]); stm_spi->cr1 = ((0 << STM_SPI_CR1_BIDIMODE) | /* Three wire mode */ (0 << STM_SPI_CR1_BIDIOE) | (0 << STM_SPI_CR1_CRCEN) | /* CRC disabled */ @@ -378,7 +398,7 @@ ao_spi_get(uint8_t spi_index, uint32_t speed) (0 << STM_SPI_CR1_CPOL) | /* Format 0 */ (0 << STM_SPI_CR1_CPHA)); if (spi_index != ao_spi_index[id]) { - + /* Disable old config */ ao_spi_disable_index(ao_spi_index[id]); @@ -386,13 +406,32 @@ ao_spi_get(uint8_t spi_index, uint32_t speed) /* Enable new config */ ao_spi_enable_index(spi_index); - + /* Remember current config */ ao_spi_index[id] = spi_index; } } +uint8_t +ao_spi_try_get(uint8_t spi_index, uint32_t speed, uint8_t task_id) +{ + uint8_t id = AO_SPI_INDEX(spi_index); + + if (!ao_mutex_try(&ao_spi_mutex[id], task_id)) + return 0; + ao_spi_config(spi_index, speed); + return 1; +} + +void +ao_spi_get(uint8_t spi_index, uint32_t speed) +{ + uint8_t id = AO_SPI_INDEX(spi_index); + ao_mutex_get(&ao_spi_mutex[id]); + ao_spi_config(spi_index, speed); +} + void ao_spi_put(uint8_t spi_index) { -- cgit v1.2.3 From 34d5be68ca23e8beb05db9a480faef63ecc911d0 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 10 Jul 2014 17:07:48 -0700 Subject: altos: Extend GPS altitudes to at least 24 bits everywhere Telemetry gets a special 'mode' flag indicating that 24-bit data is present; log files get new data and log readers are expected to detect that via the firmware version number. Signed-off-by: Keith Packard --- src/cc1111/ao_pins.h | 1 + src/drivers/ao_aprs.c | 2 +- src/drivers/ao_gps_skytraq.c | 3 ++- src/drivers/ao_gps_ublox.c | 2 +- src/kernel/ao_gps_report.c | 8 ++++++-- src/kernel/ao_gps_report_mega.c | 7 ++++--- src/kernel/ao_gps_report_metrum.c | 3 ++- src/kernel/ao_gps_show.c | 8 +++++++- src/kernel/ao_log.h | 23 +++++++++++++++-------- src/kernel/ao_log_gps.c | 3 ++- src/kernel/ao_log_gps.h | 3 --- src/kernel/ao_telemetry.c | 2 +- src/kernel/ao_telemetry.h | 34 ++++++++++++++++++++++++++-------- src/kernel/ao_tracker.c | 13 +++++++------ src/test/ao_aprs_test.c | 7 +++---- src/test/ao_flight_test.c | 7 +++++-- src/test/ao_gps_test.c | 5 ++++- src/test/ao_gps_test_skytraq.c | 4 ++++ src/test/ao_gps_test_ublox.c | 9 ++++++++- 19 files changed, 99 insertions(+), 45 deletions(-) (limited to 'src/cc1111') diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 1bc3d716..4db49215 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -20,6 +20,7 @@ #define HAS_RADIO 1 #define DISABLE_LOG_SPACE 1 +#define HAS_WIDE_GPS 0 #if defined(TELEMETRUM_V_1_0) /* Discontinued and was never built with CC1111 chips needing this */ diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index a9047149..19beb78f 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -713,7 +713,7 @@ static int tncPositionPacket(void) if (ao_gps_data.flags & AO_GPS_VALID) { latitude = ao_gps_data.latitude; longitude = ao_gps_data.longitude; - altitude = ao_gps_data.altitude; + altitude = AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data); if (altitude < 0) altitude = 0; } diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index 944a37f9..81178051 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -287,7 +287,8 @@ ao_nmea_gga(void) ao_gps_next.hdop = i; ao_gps_skip_field(); - ao_gps_next.altitude = ao_gps_decimal(0xff); + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_next, ao_gps_decimal(0xff)); + ao_gps_skip_field(); /* skip any fractional portion */ ao_nmea_finish(); diff --git a/src/drivers/ao_gps_ublox.c b/src/drivers/ao_gps_ublox.c index 077698a9..48765998 100644 --- a/src/drivers/ao_gps_ublox.c +++ b/src/drivers/ao_gps_ublox.c @@ -728,7 +728,7 @@ ao_gps(void) __reentrant if (nav_timeutc.valid & (1 << NAV_TIMEUTC_VALID_UTC)) ao_gps_data.flags |= AO_GPS_DATE_VALID; - ao_gps_data.altitude = nav_posllh.alt_msl / 1000; + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, nav_posllh.alt_msl / 1000); ao_gps_data.latitude = nav_posllh.lat; ao_gps_data.longitude = nav_posllh.lon; diff --git a/src/kernel/ao_gps_report.c b/src/kernel/ao_gps_report.c index 07201ac2..7ef98a97 100644 --- a/src/kernel/ao_gps_report.c +++ b/src/kernel/ao_gps_report.c @@ -52,8 +52,12 @@ ao_gps_report(void) gps_log.u.gps_longitude = gps_data.longitude; ao_log_data(&gps_log); gps_log.type = AO_LOG_GPS_ALT; - gps_log.u.gps_altitude.altitude = gps_data.altitude; - gps_log.u.gps_altitude.unused = 0xffff; + gps_log.u.gps_altitude.altitude_low = gps_data.altitude_low; +#if HAS_WIDE_GPS + gps_log.u.gps_altitude.altitude_high = gps_data.altitude_high; +#else + gps_log.u.gps_altitude.altitude_high = 0xffff; +#endif ao_log_data(&gps_log); if (!date_reported && (gps_data.flags & AO_GPS_DATE_VALID)) { gps_log.type = AO_LOG_GPS_DATE; diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c index cb0c0fd9..f3711fb1 100644 --- a/src/kernel/ao_gps_report_mega.c +++ b/src/kernel/ao_gps_report_mega.c @@ -78,7 +78,8 @@ ao_gps_report_mega(void) #if GPS_SPARSE_LOG /* Don't log data if GPS has a fix and hasn't moved for a while */ if ((gps_data.flags & AO_GPS_VALID) && - !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude, gps_data.altitude)) + !ao_gps_sparse_should_log(gps_data.latitude, gps_data.longitude, + AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data)) continue; #endif if ((new & AO_GPS_NEW_DATA) && (gps_data.flags & AO_GPS_VALID)) { @@ -87,8 +88,8 @@ ao_gps_report_mega(void) gps_log.type = AO_LOG_GPS_TIME; gps_log.u.gps.latitude = gps_data.latitude; gps_log.u.gps.longitude = gps_data.longitude; - gps_log.u.gps.altitude = gps_data.altitude; - + gps_log.u.gps.altitude_low = gps_data.altitude_low; + gps_log.u.gps.altitude_high = gps_data.altitude_high; gps_log.u.gps.hour = gps_data.hour; gps_log.u.gps.minute = gps_data.minute; gps_log.u.gps.second = gps_data.second; diff --git a/src/kernel/ao_gps_report_metrum.c b/src/kernel/ao_gps_report_metrum.c index 696a833b..31939385 100644 --- a/src/kernel/ao_gps_report_metrum.c +++ b/src/kernel/ao_gps_report_metrum.c @@ -44,7 +44,8 @@ ao_gps_report_metrum(void) gps_log.type = AO_LOG_GPS_POS; gps_log.u.gps.latitude = gps_data.latitude; gps_log.u.gps.longitude = gps_data.longitude; - gps_log.u.gps.altitude = gps_data.altitude; + gps_log.u.gps.altitude_low = gps_data.altitude_low; + gps_log.u.gps.altitude_high = gps_data.altitude_high; ao_log_metrum(&gps_log); gps_log.type = AO_LOG_GPS_TIME; diff --git a/src/kernel/ao_gps_show.c b/src/kernel/ao_gps_show.c index 3a05e35a..e45cd795 100644 --- a/src/kernel/ao_gps_show.c +++ b/src/kernel/ao_gps_show.c @@ -19,6 +19,8 @@ #include #endif +#include + void ao_gps_show(void) __reentrant { @@ -27,7 +29,11 @@ ao_gps_show(void) __reentrant printf ("Date: %02d/%02d/%02d\n", ao_gps_data.year, ao_gps_data.month, ao_gps_data.day); printf ("Time: %02d:%02d:%02d\n", ao_gps_data.hour, ao_gps_data.minute, ao_gps_data.second); printf ("Lat/Lon: %ld %ld\n", (long) ao_gps_data.latitude, (long) ao_gps_data.longitude); - printf ("Alt: %d\n", ao_gps_data.altitude); +#if HAS_WIDE_GPS + printf ("Alt: %ld\n", (long) AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data)); +#else + printf ("Alt: %d\n", AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_data)); +#endif printf ("Flags: 0x%x\n", ao_gps_data.flags); printf ("Sats: %d", ao_gps_tracking_data.channels); for (i = 0; i < ao_gps_tracking_data.channels; i++) diff --git a/src/kernel/ao_log.h b/src/kernel/ao_log.h index 33cda3eb..080cfb02 100644 --- a/src/kernel/ao_log.h +++ b/src/kernel/ao_log.h @@ -176,8 +176,8 @@ struct ao_log_record { int32_t gps_latitude; int32_t gps_longitude; struct { - int16_t altitude; - uint16_t unused; + uint16_t altitude_low; + int16_t altitude_high; } gps_altitude; struct { uint16_t svid; @@ -246,7 +246,7 @@ struct ao_log_mega { struct { int32_t latitude; /* 4 */ int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ + uint16_t altitude_low; /* 12 */ uint8_t hour; /* 14 */ uint8_t minute; /* 15 */ uint8_t second; /* 16 */ @@ -261,7 +261,8 @@ struct ao_log_mega { uint8_t hdop; /* 27 */ uint8_t vdop; /* 28 */ uint8_t mode; /* 29 */ - } gps; /* 30 */ + int16_t altitude_high; /* 30 */ + } gps; /* 32 */ /* AO_LOG_GPS_SAT */ struct { uint16_t channels; /* 4 */ @@ -273,6 +274,11 @@ struct ao_log_mega { } u; }; +#define AO_LOG_MEGA_GPS_ALTITUDE(l) ((int32_t) ((l)->u.gps.altitude_high << 16) | ((l)->u.gps.altitude_low)) +#define AO_LOG_MEGA_SET_GPS_ALTITUDE(l,a) (((l)->u.gps.mode |= AO_GPS_MODE_ALTITUDE_24), \ + ((l)->u.gps.altitude_high = (a) >> 16), \ + (l)->u.gps.altitude_low = (a)) + struct ao_log_metrum { char type; /* 0 */ uint8_t csum; /* 1 */ @@ -306,8 +312,9 @@ struct ao_log_metrum { struct { int32_t latitude; /* 4 */ int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ - } gps; /* 14 */ + uint16_t altitude_low; /* 12 */ + int16_t altitude_high; /* 14 */ + } gps; /* 16 */ /* AO_LOG_GPS_TIME */ struct { uint8_t hour; /* 4 */ @@ -381,7 +388,7 @@ struct ao_log_gps { struct { int32_t latitude; /* 4 */ int32_t longitude; /* 8 */ - int16_t altitude; /* 12 */ + uint16_t altitude_low; /* 12 */ uint8_t hour; /* 14 */ uint8_t minute; /* 15 */ uint8_t second; /* 16 */ @@ -396,7 +403,7 @@ struct ao_log_gps { uint8_t hdop; /* 27 */ uint8_t vdop; /* 28 */ uint8_t mode; /* 29 */ - uint8_t state; /* 30 */ + int16_t altitude_high; /* 30 */ } gps; /* 31 */ /* AO_LOG_GPS_SAT */ struct { diff --git a/src/kernel/ao_log_gps.c b/src/kernel/ao_log_gps.c index 3b728c25..a5a6358b 100644 --- a/src/kernel/ao_log_gps.c +++ b/src/kernel/ao_log_gps.c @@ -75,7 +75,8 @@ ao_log_gps_data(uint16_t tick, struct ao_telemetry_location *gps_data) log.type = AO_LOG_GPS_TIME; log.u.gps.latitude = gps_data->latitude; log.u.gps.longitude = gps_data->longitude; - log.u.gps.altitude = gps_data->altitude; + log.u.gps.altitude_low = gps_data->altitude_low; + log.u.gps.altitude_high = gps_data->altitude_high; log.u.gps.hour = gps_data->hour; log.u.gps.minute = gps_data->minute; diff --git a/src/kernel/ao_log_gps.h b/src/kernel/ao_log_gps.h index 5851f4d1..a9e8c831 100644 --- a/src/kernel/ao_log_gps.h +++ b/src/kernel/ao_log_gps.h @@ -21,9 +21,6 @@ #ifndef _AO_LOG_GPS_H_ #define _AO_LOG_GPS_H_ -uint8_t -ao_log_gps_should_log(int32_t lat, int32_t lon, int16_t alt); - void ao_log_gps_flight(void); diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index d321c8ff..56bd715e 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -344,7 +344,7 @@ ao_send_location(void) ao_mutex_get(&ao_gps_mutex); ao_xmemcpy(&telemetry.location.flags, &ao_gps_data.flags, - 26); + 27); telemetry.location.tick = ao_gps_tick; ao_mutex_put(&ao_gps_mutex); ao_radio_send(&telemetry, sizeof (telemetry)); diff --git a/src/kernel/ao_telemetry.h b/src/kernel/ao_telemetry.h index be7d0340..83d432cf 100644 --- a/src/kernel/ao_telemetry.h +++ b/src/kernel/ao_telemetry.h @@ -86,12 +86,9 @@ struct ao_telemetry_configuration { #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' +/* Mode bits */ + +#define AO_GPS_MODE_ALTITUDE_24 (1 << 0) /* reports 24-bits of altitude */ struct ao_telemetry_location { uint16_t serial; /* 0 */ @@ -99,7 +96,7 @@ struct ao_telemetry_location { uint8_t type; /* 4 */ uint8_t flags; /* 5 Number of sats and other flags */ - int16_t altitude; /* 6 GPS reported altitude (m) */ + uint16_t altitude_low; /* 6 GPS reported altitude (m) */ int32_t latitude; /* 8 latitude (degrees * 10⁷) */ int32_t longitude; /* 12 longitude (degrees * 10⁷) */ uint8_t year; /* 16 (- 2000) */ @@ -115,10 +112,31 @@ struct ao_telemetry_location { uint16_t ground_speed; /* 26 cm/s */ int16_t climb_rate; /* 28 cm/s */ uint8_t course; /* 30 degrees / 2 */ - uint8_t unused; /* 31 unused */ + int8_t altitude_high; /* 31 high byte of altitude */ /* 32 */ }; +#if HAS_GPS + +#ifndef HAS_WIDE_GPS +#define HAS_WIDE_GPS 1 +#endif + +#if HAS_WIDE_GPS +typedef int32_t gps_alt_t; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) (((gps_alt_t) (l)->altitude_high << 16) | ((l)->altitude_low)) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->mode |= AO_GPS_MODE_ALTITUDE_24), \ + ((l)->altitude_high = (a) >> 16), \ + ((l)->altitude_low = (a))) +#else +typedef int16_t gps_alt_t; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) ((gps_alt_t) (l)->altitude_low) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->mode = 0, \ + (l)->altitude_low = (a))) +#endif /* HAS_WIDE_GPS */ + +#endif /* HAS_GPS */ + #define AO_TELEMETRY_SATELLITE 0x06 struct ao_telemetry_satellite_info { diff --git a/src/kernel/ao_tracker.c b/src/kernel/ao_tracker.c index 9febc7f0..d9434048 100644 --- a/src/kernel/ao_tracker.c +++ b/src/kernel/ao_tracker.c @@ -36,9 +36,9 @@ ao_usb_connected(void) #endif struct gps_position { - int32_t latitude; - int32_t longitude; - int16_t altitude; + int32_t latitude; + int32_t longitude; + gps_alt_t altitude; }; #define GPS_RING 16 @@ -122,12 +122,13 @@ ao_tracker(void) { uint8_t ring; uint8_t moving = 0; + gps_alt_t altitude = AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data); for (ring = ao_gps_ring_next(gps_head); ring != gps_head; ring = ao_gps_ring_next(ring)) { ground_distance = ao_distance(gps_data.latitude, gps_data.longitude, gps_position[ring].latitude, gps_position[ring].longitude); - height = gps_position[ring].altitude - gps_data.altitude; + height = gps_position[ring].altitude - altitude; if (height < 0) height = -height; @@ -153,7 +154,7 @@ ao_tracker(void) ao_log_gps_data(gps_tick, &gps_data); gps_position[gps_head].latitude = gps_data.latitude; gps_position[gps_head].longitude = gps_data.longitude; - gps_position[gps_head].altitude = gps_data.altitude; + gps_position[gps_head].altitude = altitude; gps_head = ao_gps_ring_next(gps_head); ao_mutex_put(&tracker_mutex); } @@ -203,7 +204,7 @@ ao_tracker_set_telem(void) printf ("log_started: %d\n", log_started); printf ("latitude: %ld\n", (long) gps_data.latitude); printf ("longitude: %ld\n", (long) gps_data.longitude); - printf ("altitude: %d\n", gps_data.altitude); + printf ("altitude: %ld\n", AO_TELEMETRY_LOCATION_ALTITUDE(&gps_data)); printf ("log_running: %d\n", ao_log_running); printf ("log_start_pos: %ld\n", (long) ao_log_start_pos); printf ("log_cur_pos: %ld\n", (long) ao_log_current_pos); diff --git a/src/test/ao_aprs_test.c b/src/test/ao_aprs_test.c index 573b5cb2..ae505dea 100644 --- a/src/test/ao_aprs_test.c +++ b/src/test/ao_aprs_test.c @@ -21,6 +21,8 @@ #include #include +#define HAS_GPS 1 + #include #define AO_GPS_NUM_SAT_MASK (0xf << 0) @@ -100,14 +102,11 @@ audio_gap(int secs) // This is where we go after reset. int main(int argc, char **argv) { - int e, x; - int a; - audio_gap(1); ao_gps_data.latitude = (45.0 + 28.25 / 60.0) * 10000000; ao_gps_data.longitude = (-(122 + 44.2649 / 60.0)) * 10000000; - ao_gps_data.altitude = 84; + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_data, 84); ao_gps_data.flags = (AO_GPS_VALID|AO_GPS_RUNNING); /* Transmit one packet */ diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 1ab22e5b..314998c1 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -175,7 +175,7 @@ ao_gps_angle(void) ao_gps_static.latitude / 1e7, ao_gps_static.longitude / 1e7, &dist, &bearing); - height = ao_gps_static.altitude - ao_gps_prev.altitude; + height = AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_static) - AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_prev); angle = atan2(dist, height); return angle * 180/M_PI; @@ -756,7 +756,10 @@ ao_sleep(void *wchan) ao_gps_static.tick = tick; ao_gps_static.latitude = int32(bytes, 0); ao_gps_static.longitude = int32(bytes, 4); - ao_gps_static.altitude = int32(bytes, 8); + { + int32_t altitude = int32(bytes, 8); + AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_static, altitude); + } ao_gps_static.flags = bytes[13]; if (!ao_gps_count) ao_gps_first = ao_gps_static; diff --git a/src/test/ao_gps_test.c b/src/test/ao_gps_test.c index e799ab0f..543bbcc3 100644 --- a/src/test/ao_gps_test.c +++ b/src/test/ao_gps_test.c @@ -53,6 +53,9 @@ struct ao_gps_orig { uint16_t v_error; /* m */ }; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) ((l)->altitude) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) ((l)->altitude = (a)) + #define SIRF_SAT_STATE_ACQUIRED (1 << 0) #define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) #define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2) @@ -433,7 +436,7 @@ ao_dump_state(void *wchan) if (wchan != &ao_gps_new) return; - + if (ao_gps_new & AO_GPS_NEW_DATA) { ao_gps_print(&ao_gps_data); putchar('\n'); diff --git a/src/test/ao_gps_test_skytraq.c b/src/test/ao_gps_test_skytraq.c index 1b590d5e..5eb7118d 100644 --- a/src/test/ao_gps_test_skytraq.c +++ b/src/test/ao_gps_test_skytraq.c @@ -16,6 +16,7 @@ */ #define AO_GPS_TEST +#define HAS_GPS 1 #include "ao_host.h" #include #include @@ -53,6 +54,9 @@ struct ao_gps_orig { uint16_t v_error; /* m */ }; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) ((l)->altitude) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) ((l)->altitude = (a)) + #define SIRF_SAT_STATE_ACQUIRED (1 << 0) #define SIRF_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) #define SIRF_SAT_BIT_SYNC_COMPLETE (1 << 2) diff --git a/src/test/ao_gps_test_ublox.c b/src/test/ao_gps_test_ublox.c index 4eb4b837..5ea205d6 100644 --- a/src/test/ao_gps_test_ublox.c +++ b/src/test/ao_gps_test_ublox.c @@ -16,6 +16,7 @@ */ #define AO_GPS_TEST +#define HAS_GPS 1 #include "ao_host.h" #include #include @@ -44,7 +45,7 @@ struct ao_telemetry_location { uint8_t flags; int32_t latitude; /* degrees * 10⁷ */ int32_t longitude; /* degrees * 10⁷ */ - int16_t altitude; /* m */ + int16_t altitude_low; /* m */ uint16_t ground_speed; /* cm/s */ uint8_t course; /* degrees / 2 */ uint8_t pdop; /* * 5 */ @@ -53,8 +54,14 @@ struct ao_telemetry_location { int16_t climb_rate; /* cm/s */ uint16_t h_error; /* m */ uint16_t v_error; /* m */ + int16_t altitude_high; /* m */ }; +typedef int32_t gps_alt_t; +#define AO_TELEMETRY_LOCATION_ALTITUDE(l) (((gps_alt_t) (l)->altitude_high << 16) | ((l)->altitude_low)) +#define AO_TELEMETRY_LOCATION_SET_ALTITUDE(l,a) (((l)->altitude_high = (a) >> 16), \ + ((l)->altitude_low = (a))) + #define UBLOX_SAT_STATE_ACQUIRED (1 << 0) #define UBLOX_SAT_STATE_CARRIER_PHASE_VALID (1 << 1) #define UBLOX_SAT_BIT_SYNC_COMPLETE (1 << 2) -- cgit v1.2.3 From 013e9ccfbe76dc46e8c69ea314950bed83d9a39f Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Thu, 10 Jul 2014 17:18:38 -0700 Subject: altos: Use 32-bits for flight state data (alt/speed/accel) Stores 32-bits for all of the flight parameters. Uses 64-bit intermediates for kalman computation. Signed-off-by: Keith Packard --- src/Makefile | 2 +- src/cc1111/ao_pins.h | 1 + src/kalman/kalman.5c | 2 +- src/kalman/kalman_micro.5c | 2 +- src/kernel/ao.h | 14 ++-- src/kernel/ao_cmd.c | 3 + src/kernel/ao_convert.c | 10 +-- src/kernel/ao_convert_pa.c | 16 ++--- src/kernel/ao_convert_pa_test.c | 1 + src/kernel/ao_data.h | 6 -- src/kernel/ao_flight.c | 12 ++-- src/kernel/ao_kalman.c | 147 ++++++++++++++++++++-------------------- src/kernel/ao_microkalman.c | 12 ++-- src/kernel/ao_sample.c | 2 +- src/kernel/ao_sample.h | 56 +++++++++++---- src/product/Makefile.telemetrum | 1 + src/telefire-v0.1/Makefile | 1 - src/telefire-v0.2/Makefile | 1 - src/telemega-v1.0/Makefile | 1 + src/telemini-v2.0/ao_pins.h | 1 + src/teleterra-v0.2/ao_pins.h | 2 + src/test/ao_flight_test.c | 5 +- src/test/ao_micropeak_test.c | 1 + 23 files changed, 170 insertions(+), 129 deletions(-) (limited to 'src/cc1111') diff --git a/src/Makefile b/src/Makefile index a7a26b26..3494ba62 100644 --- a/src/Makefile +++ b/src/Makefile @@ -109,7 +109,7 @@ altitude-pa.h: make-altitude-pa altitude-pa-small.h: make-altitude-pa nickle $< --sample 3 > $@ -ao_kalman.h: make-kalman kalman.5c kalman_filter.5c load_csv.5c matrix.5c +ao_kalman.h: make-kalman kalman.5c kalman_micro.5c kalman_filter.5c load_csv.5c matrix.5c bash $< kalman > $@ ao_whiten.h: make-whiten diff --git a/src/cc1111/ao_pins.h b/src/cc1111/ao_pins.h index 4db49215..e12f813f 100644 --- a/src/cc1111/ao_pins.h +++ b/src/cc1111/ao_pins.h @@ -20,6 +20,7 @@ #define HAS_RADIO 1 #define DISABLE_LOG_SPACE 1 +#define AO_VALUE_32 0 #define HAS_WIDE_GPS 0 #if defined(TELEMETRUM_V_1_0) diff --git a/src/kalman/kalman.5c b/src/kalman/kalman.5c index 46fa8e1f..55fde04c 100755 --- a/src/kalman/kalman.5c +++ b/src/kalman/kalman.5c @@ -457,7 +457,7 @@ void main() { name = sprintf("%s_K%d_%d", prefix, i, time_inc); else name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc); - printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]); + printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]); } printf ("\n"); exit(0); diff --git a/src/kalman/kalman_micro.5c b/src/kalman/kalman_micro.5c index 266a1182..1b080384 100644 --- a/src/kalman/kalman_micro.5c +++ b/src/kalman/kalman_micro.5c @@ -307,7 +307,7 @@ void main() { name = sprintf("%s_K%d_%d", prefix, i, time_inc); else name = sprintf("%s_K%d%d_%d", prefix, i, j, time_inc); - printf ("#define %s to_fix32(%12.10f)\n", name, k[i,j]); + printf ("#define %s to_fix_k(%12.10f)\n", name, k[i,j]); } printf ("\n"); exit(0); diff --git a/src/kernel/ao.h b/src/kernel/ao.h index a225bc4a..ad5bbf8e 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -278,15 +278,17 @@ ao_report_init(void); * Given raw data, convert to SI units */ +#if HAS_BARO /* pressure from the sensor to altitude in meters */ -int16_t -ao_pres_to_altitude(int16_t pres) __reentrant; +alt_t +ao_pres_to_altitude(pres_t pres) __reentrant; -int16_t -ao_altitude_to_pres(int16_t alt) __reentrant; +pres_t +ao_altitude_to_pres(alt_t alt) __reentrant; int16_t ao_temp_to_dC(int16_t temp) __reentrant; +#endif /* * ao_convert_pa.c @@ -296,11 +298,13 @@ ao_temp_to_dC(int16_t temp) __reentrant; #include +#if HAS_BARO alt_t -ao_pa_to_altitude(int32_t pa); +ao_pa_to_altitude(pres_t pa); int32_t ao_altitude_to_pa(alt_t alt); +#endif #if HAS_DBG #include diff --git a/src/kernel/ao_cmd.c b/src/kernel/ao_cmd.c index 0052bdce..d2f583ef 100644 --- a/src/kernel/ao_cmd.c +++ b/src/kernel/ao_cmd.c @@ -289,6 +289,9 @@ version(void) #endif #if defined(AO_BOOT_APPLICATION_BASE) && defined(AO_BOOT_APPLICATION_BOUND) "program-space %u\n" +#endif +#if AO_VALUE_32 + "altitude-32 1\n" #endif , ao_manufacturer , ao_product diff --git a/src/kernel/ao_convert.c b/src/kernel/ao_convert.c index aa9b5f48..db1f2301 100644 --- a/src/kernel/ao_convert.c +++ b/src/kernel/ao_convert.c @@ -19,14 +19,16 @@ #include "ao.h" #endif -static const int16_t altitude_table[] = { +#include + +static const ao_v_t altitude_table[] = { #include "altitude.h" }; #define ALT_FRAC_SCALE (1 << ALT_FRAC_BITS) #define ALT_FRAC_MASK (ALT_FRAC_SCALE - 1) -int16_t +ao_v_t ao_pres_to_altitude(int16_t pres) __reentrant { uint8_t o; @@ -43,9 +45,9 @@ ao_pres_to_altitude(int16_t pres) __reentrant #if AO_NEED_ALTITUDE_TO_PRES int16_t -ao_altitude_to_pres(int16_t alt) __reentrant +ao_altitude_to_pres(ao_v_t alt) __reentrant { - int16_t span, sub_span; + ao_v_t span, sub_span; uint8_t l, h, m; int32_t pres; diff --git a/src/kernel/ao_convert_pa.c b/src/kernel/ao_convert_pa.c index 20162c1f..410815b6 100644 --- a/src/kernel/ao_convert_pa.c +++ b/src/kernel/ao_convert_pa.c @@ -39,11 +39,11 @@ static const alt_t altitude_table[] AO_CONST_ATTRIB = { #define ALT_MASK (ALT_SCALE - 1) alt_t -ao_pa_to_altitude(int32_t pa) +ao_pa_to_altitude(pres_t pa) { int16_t o; int16_t part; - int32_t low, high; + alt_t low, high; if (pa < 0) pa = 0; @@ -52,16 +52,16 @@ ao_pa_to_altitude(int32_t pa) o = pa >> ALT_SHIFT; part = pa & ALT_MASK; - low = (int32_t) FETCH_ALT(o) * (ALT_SCALE - part); - high = (int32_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1); + low = (alt_t) FETCH_ALT(o) * (ALT_SCALE - part); + high = (alt_t) FETCH_ALT(o+1) * part + (ALT_SCALE >> 1); return (low + high) >> ALT_SHIFT; } #ifdef AO_CONVERT_TEST -int32_t -ao_altitude_to_pa(int32_t alt) +pres_t +ao_altitude_to_pa(alt_t alt) { - int32_t span, sub_span; + alt_t span, sub_span; uint16_t l, h, m; int32_t pa; @@ -76,7 +76,7 @@ ao_altitude_to_pa(int32_t alt) } span = altitude_table[l] - altitude_table[h]; sub_span = altitude_table[l] - alt; - pa = ((((int32_t) l * (span - sub_span) + (int32_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span; + pa = ((((alt_t) l * (span - sub_span) + (alt_t) h * sub_span) << ALT_SHIFT) + (span >> 1)) / span; if (pa > 120000) pa = 120000; if (pa < 0) diff --git a/src/kernel/ao_convert_pa_test.c b/src/kernel/ao_convert_pa_test.c index 7d5b1922..95422862 100644 --- a/src/kernel/ao_convert_pa_test.c +++ b/src/kernel/ao_convert_pa_test.c @@ -18,6 +18,7 @@ #include #define AO_CONVERT_TEST typedef int32_t alt_t; +typedef int32_t pres_t; #include "ao_host.h" #include "ao_convert_pa.c" diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h index c4b062fd..8f75ad87 100644 --- a/src/kernel/ao_data.h +++ b/src/kernel/ao_data.h @@ -117,9 +117,7 @@ extern volatile __data uint8_t ao_data_count; typedef int32_t pres_t; -#ifndef AO_ALT_TYPE #define AO_ALT_TYPE int32_t -#endif typedef AO_ALT_TYPE alt_t; @@ -146,10 +144,6 @@ typedef int16_t alt_t; #endif -#if !HAS_BARO -typedef int16_t alt_t; -#endif - /* * Need a few macros to pull data from the sensors: * diff --git a/src/kernel/ao_flight.c b/src/kernel/ao_flight.c index 2b433ee9..251dbc02 100644 --- a/src/kernel/ao_flight.c +++ b/src/kernel/ao_flight.c @@ -60,10 +60,10 @@ __xdata uint8_t ao_sensor_errors; * resting */ static __data uint16_t ao_interval_end; -static __data int16_t ao_interval_min_height; -static __data int16_t ao_interval_max_height; +static __data ao_v_t ao_interval_min_height; +static __data ao_v_t ao_interval_max_height; #if HAS_ACCEL -static __data int16_t ao_coast_avg_accel; +static __data ao_v_t ao_coast_avg_accel; #endif __pdata uint8_t ao_flight_force_idle; @@ -398,14 +398,14 @@ ao_flight(void) } #if HAS_FLIGHT_DEBUG -static inline int int_part(int16_t i) { return i >> 4; } -static inline int frac_part(int16_t i) { return ((i & 0xf) * 100 + 8) / 16; } +static inline int int_part(ao_v_t i) { return i >> 4; } +static inline int frac_part(ao_v_t i) { return ((i & 0xf) * 100 + 8) / 16; } static void ao_flight_dump(void) { #if HAS_ACCEL - int16_t accel; + ao_v_t accel; accel = ((ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale) >> 16; #endif diff --git a/src/kernel/ao_kalman.c b/src/kernel/ao_kalman.c index 9aea1f14..7b0f8145 100644 --- a/src/kernel/ao_kalman.c +++ b/src/kernel/ao_kalman.c @@ -23,32 +23,31 @@ #include "ao_sample.h" #include "ao_kalman.h" +static __pdata ao_k_t ao_k_height; +static __pdata ao_k_t ao_k_speed; +static __pdata ao_k_t ao_k_accel; -static __pdata int32_t ao_k_height; -static __pdata int32_t ao_k_speed; -static __pdata int32_t ao_k_accel; +#define AO_K_STEP_100 to_fix_v(0.01) +#define AO_K_STEP_2_2_100 to_fix_v(0.00005) -#define AO_K_STEP_100 to_fix16(0.01) -#define AO_K_STEP_2_2_100 to_fix16(0.00005) +#define AO_K_STEP_10 to_fix_v(0.1) +#define AO_K_STEP_2_2_10 to_fix_v(0.005) -#define AO_K_STEP_10 to_fix16(0.1) -#define AO_K_STEP_2_2_10 to_fix16(0.005) +#define AO_K_STEP_1 to_fix_v(1) +#define AO_K_STEP_2_2_1 to_fix_v(0.5) -#define AO_K_STEP_1 to_fix16(1) -#define AO_K_STEP_2_2_1 to_fix16(0.5) +__pdata ao_v_t ao_height; +__pdata ao_v_t ao_speed; +__pdata ao_v_t ao_accel; +__xdata ao_v_t ao_max_height; +static __pdata ao_k_t ao_avg_height_scaled; +__xdata ao_v_t ao_avg_height; -__pdata int16_t ao_height; -__pdata int16_t ao_speed; -__pdata int16_t ao_accel; -__xdata int16_t ao_max_height; -static __pdata int32_t ao_avg_height_scaled; -__xdata int16_t ao_avg_height; - -__pdata int16_t ao_error_h; -__pdata int16_t ao_error_h_sq_avg; +__pdata ao_v_t ao_error_h; +__pdata ao_v_t ao_error_h_sq_avg; #if HAS_ACCEL -__pdata int16_t ao_error_a; +__pdata ao_v_t ao_error_a; #endif static void @@ -56,40 +55,40 @@ ao_kalman_predict(void) { #ifdef AO_FLIGHT_TEST if (ao_sample_tick - ao_sample_prev_tick > 50) { - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_1 + - (int32_t) ao_accel * AO_K_STEP_2_2_1) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_1; + ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_1 + + (ao_k_t) ao_accel * AO_K_STEP_2_2_1) >> 4; + ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_1; return; } if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_10 + - (int32_t) ao_accel * AO_K_STEP_2_2_10) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_10; + ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_10 + + (ao_k_t) ao_accel * AO_K_STEP_2_2_10) >> 4; + ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_10; return; } if (ao_flight_debug) { printf ("predict speed %g + (%g * %g) = %g\n", ao_k_speed / (65536.0 * 16.0), ao_accel / 16.0, AO_K_STEP_100 / 65536.0, - (ao_k_speed + (int32_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0)); + (ao_k_speed + (ao_k_t) ao_accel * AO_K_STEP_100) / (65536.0 * 16.0)); } #endif - ao_k_height += ((int32_t) ao_speed * AO_K_STEP_100 + - (int32_t) ao_accel * AO_K_STEP_2_2_100) >> 4; - ao_k_speed += (int32_t) ao_accel * AO_K_STEP_100; + ao_k_height += ((ao_k_t) ao_speed * AO_K_STEP_100 + + (ao_k_t) ao_accel * AO_K_STEP_2_2_100) >> 4; + ao_k_speed += (ao_k_t) ao_accel * AO_K_STEP_100; } static void ao_kalman_err_height(void) { - int16_t e; - int16_t height_distrust; + ao_v_t e; + ao_v_t height_distrust; #if HAS_ACCEL - int16_t speed_distrust; + ao_v_t speed_distrust; #endif - ao_error_h = ao_sample_height - (int16_t) (ao_k_height >> 16); + ao_error_h = ao_sample_height - (ao_v_t) (ao_k_height >> 16); e = ao_error_h; if (e < 0) @@ -123,7 +122,7 @@ ao_kalman_err_height(void) #endif if (height_distrust > 0x100) height_distrust = 0x100; - ao_error_h = (int16_t) (((int32_t) ao_error_h * (0x100 - height_distrust)) >> 8); + ao_error_h = (ao_v_t) (((ao_k_t) ao_error_h * (0x100 - height_distrust)) >> 8); #ifdef AO_FLIGHT_TEST if (ao_flight_debug) { printf("over height %g over speed %g distrust: %g height: error %d -> %d\n", @@ -142,21 +141,21 @@ ao_kalman_correct_baro(void) ao_kalman_err_height(); #ifdef AO_FLIGHT_TEST if (ao_sample_tick - ao_sample_prev_tick > 50) { - ao_k_height += (int32_t) AO_BARO_K0_1 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_1 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_1 * ao_error_h; + ao_k_height += (ao_k_t) AO_BARO_K0_1 * ao_error_h; + ao_k_speed += (ao_k_t) AO_BARO_K1_1 * ao_error_h; + ao_k_accel += (ao_k_t) AO_BARO_K2_1 * ao_error_h; return; } if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height += (int32_t) AO_BARO_K0_10 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_10 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_10 * ao_error_h; + ao_k_height += (ao_k_t) AO_BARO_K0_10 * ao_error_h; + ao_k_speed += (ao_k_t) AO_BARO_K1_10 * ao_error_h; + ao_k_accel += (ao_k_t) AO_BARO_K2_10 * ao_error_h; return; } #endif - ao_k_height += (int32_t) AO_BARO_K0_100 * ao_error_h; - ao_k_speed += (int32_t) AO_BARO_K1_100 * ao_error_h; - ao_k_accel += (int32_t) AO_BARO_K2_100 * ao_error_h; + ao_k_height += (ao_k_t) AO_BARO_K0_100 * ao_error_h; + ao_k_speed += (ao_k_t) AO_BARO_K1_100 * ao_error_h; + ao_k_accel += (ao_k_t) AO_BARO_K2_100 * ao_error_h; } #if HAS_ACCEL @@ -164,7 +163,7 @@ ao_kalman_correct_baro(void) static void ao_kalman_err_accel(void) { - int32_t accel; + ao_k_t accel; accel = (ao_config.accel_plus_g - ao_sample_accel) * ao_accel_scale; @@ -187,18 +186,18 @@ ao_kalman_correct_both(void) (double) ao_error_h, AO_BOTH_K10_1 / 65536.0, (double) ao_error_a, AO_BOTH_K11_1 / 65536.0, (ao_k_speed + - (int32_t) AO_BOTH_K10_1 * ao_error_h + - (int32_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0)); + (ao_k_t) AO_BOTH_K10_1 * ao_error_h + + (ao_k_t) AO_BOTH_K11_1 * ao_error_a) / (65536.0 * 16.0)); } ao_k_height += - (int32_t) AO_BOTH_K00_1 * ao_error_h + - (int32_t) AO_BOTH_K01_1 * ao_error_a; + (ao_k_t) AO_BOTH_K00_1 * ao_error_h + + (ao_k_t) AO_BOTH_K01_1 * ao_error_a; ao_k_speed += - (int32_t) AO_BOTH_K10_1 * ao_error_h + - (int32_t) AO_BOTH_K11_1 * ao_error_a; + (ao_k_t) AO_BOTH_K10_1 * ao_error_h + + (ao_k_t) AO_BOTH_K11_1 * ao_error_a; ao_k_accel += - (int32_t) AO_BOTH_K20_1 * ao_error_h + - (int32_t) AO_BOTH_K21_1 * ao_error_a; + (ao_k_t) AO_BOTH_K20_1 * ao_error_h + + (ao_k_t) AO_BOTH_K21_1 * ao_error_a; return; } if (ao_sample_tick - ao_sample_prev_tick > 5) { @@ -208,18 +207,18 @@ ao_kalman_correct_both(void) (double) ao_error_h, AO_BOTH_K10_10 / 65536.0, (double) ao_error_a, AO_BOTH_K11_10 / 65536.0, (ao_k_speed + - (int32_t) AO_BOTH_K10_10 * ao_error_h + - (int32_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0)); + (ao_k_t) AO_BOTH_K10_10 * ao_error_h + + (ao_k_t) AO_BOTH_K11_10 * ao_error_a) / (65536.0 * 16.0)); } ao_k_height += - (int32_t) AO_BOTH_K00_10 * ao_error_h + - (int32_t) AO_BOTH_K01_10 * ao_error_a; + (ao_k_t) AO_BOTH_K00_10 * ao_error_h + + (ao_k_t) AO_BOTH_K01_10 * ao_error_a; ao_k_speed += - (int32_t) AO_BOTH_K10_10 * ao_error_h + - (int32_t) AO_BOTH_K11_10 * ao_error_a; + (ao_k_t) AO_BOTH_K10_10 * ao_error_h + + (ao_k_t) AO_BOTH_K11_10 * ao_error_a; ao_k_accel += - (int32_t) AO_BOTH_K20_10 * ao_error_h + - (int32_t) AO_BOTH_K21_10 * ao_error_a; + (ao_k_t) AO_BOTH_K20_10 * ao_error_h + + (ao_k_t) AO_BOTH_K21_10 * ao_error_a; return; } if (ao_flight_debug) { @@ -228,19 +227,19 @@ ao_kalman_correct_both(void) (double) ao_error_h, AO_BOTH_K10_100 / 65536.0, (double) ao_error_a, AO_BOTH_K11_100 / 65536.0, (ao_k_speed + - (int32_t) AO_BOTH_K10_100 * ao_error_h + - (int32_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0)); + (ao_k_t) AO_BOTH_K10_100 * ao_error_h + + (ao_k_t) AO_BOTH_K11_100 * ao_error_a) / (65536.0 * 16.0)); } #endif ao_k_height += - (int32_t) AO_BOTH_K00_100 * ao_error_h + - (int32_t) AO_BOTH_K01_100 * ao_error_a; + (ao_k_t) AO_BOTH_K00_100 * ao_error_h + + (ao_k_t) AO_BOTH_K01_100 * ao_error_a; ao_k_speed += - (int32_t) AO_BOTH_K10_100 * ao_error_h + - (int32_t) AO_BOTH_K11_100 * ao_error_a; + (ao_k_t) AO_BOTH_K10_100 * ao_error_h + + (ao_k_t) AO_BOTH_K11_100 * ao_error_a; ao_k_accel += - (int32_t) AO_BOTH_K20_100 * ao_error_h + - (int32_t) AO_BOTH_K21_100 * ao_error_a; + (ao_k_t) AO_BOTH_K20_100 * ao_error_h + + (ao_k_t) AO_BOTH_K21_100 * ao_error_a; } #else @@ -251,14 +250,14 @@ ao_kalman_correct_accel(void) ao_kalman_err_accel(); if (ao_sample_tick - ao_sample_prev_tick > 5) { - ao_k_height +=(int32_t) AO_ACCEL_K0_10 * ao_error_a; - ao_k_speed += (int32_t) AO_ACCEL_K1_10 * ao_error_a; - ao_k_accel += (int32_t) AO_ACCEL_K2_10 * ao_error_a; + ao_k_height +=(ao_k_t) AO_ACCEL_K0_10 * ao_error_a; + ao_k_speed += (ao_k_t) AO_ACCEL_K1_10 * ao_error_a; + ao_k_accel += (ao_k_t) AO_ACCEL_K2_10 * ao_error_a; return; } - ao_k_height += (int32_t) AO_ACCEL_K0_100 * ao_error_a; - ao_k_speed += (int32_t) AO_ACCEL_K1_100 * ao_error_a; - ao_k_accel += (int32_t) AO_ACCEL_K2_100 * ao_error_a; + ao_k_height += (ao_k_t) AO_ACCEL_K0_100 * ao_error_a; + ao_k_speed += (ao_k_t) AO_ACCEL_K1_100 * ao_error_a; + ao_k_accel += (ao_k_t) AO_ACCEL_K2_100 * ao_error_a; } #endif /* else FORCE_ACCEL */ diff --git a/src/kernel/ao_microkalman.c b/src/kernel/ao_microkalman.c index 0684ea2b..75a29cc4 100644 --- a/src/kernel/ao_microkalman.c +++ b/src/kernel/ao_microkalman.c @@ -22,19 +22,19 @@ #define FIX_BITS 16 -#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) -#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define to_fix_v(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix_k(x) ((int32_t) ((x) * 65536.0 + 0.5)) #define from_fix8(x) ((x) >> 8) #define from_fix(x) ((x) >> 16) -#define fix8_to_fix16(x) ((x) << 8) +#define fix8_to_fix_v(x) ((x) << 8) #define fix16_to_fix8(x) ((x) >> 8) #include /* Basic time step (96ms) */ -#define AO_MK_STEP to_fix16(0.096) +#define AO_MK_STEP to_fix_v(0.096) /* step ** 2 / 2 */ -#define AO_MK_STEP_2_2 to_fix16(0.004608) +#define AO_MK_STEP_2_2 to_fix_v(0.004608) uint32_t ao_k_pa; /* 24.8 fixed point */ int32_t ao_k_pa_speed; /* 16.16 fixed point */ @@ -49,7 +49,7 @@ ao_microkalman_init(void) { ao_pa = pa; ao_k_pa = pa << 8; -} +} void ao_microkalman_predict(void) diff --git a/src/kernel/ao_sample.c b/src/kernel/ao_sample.c index 34658951..29bf2bf6 100644 --- a/src/kernel/ao_sample.c +++ b/src/kernel/ao_sample.c @@ -245,7 +245,7 @@ ao_sample_preflight(void) } else { #if HAS_ACCEL ao_accel_2g = ao_config.accel_minus_g - ao_config.accel_plus_g; - ao_accel_scale = to_fix32(GRAVITY * 2 * 16) / ao_accel_2g; + ao_accel_scale = to_fix_32(GRAVITY * 2 * 16) / ao_accel_2g; #endif ao_sample_preflight_set(); ao_preflight = FALSE; diff --git a/src/kernel/ao_sample.h b/src/kernel/ao_sample.h index 16d4c507..2ec998bd 100644 --- a/src/kernel/ao_sample.h +++ b/src/kernel/ao_sample.h @@ -24,6 +24,24 @@ * ao_sample.c */ +#ifndef AO_VALUE_32 +#define AO_VALUE_32 1 +#endif + +#if AO_VALUE_32 +/* + * For 32-bit computed values, use 64-bit intermediates. + */ +typedef int64_t ao_k_t; +typedef int32_t ao_v_t; +#else +/* + * For 16-bit computed values, use 32-bit intermediates. + */ +typedef int32_t ao_k_t; +typedef int16_t ao_v_t; +#endif + /* * Barometer calibration * @@ -87,9 +105,9 @@ * 2047m/s² (over 200g) */ -#define AO_M_TO_HEIGHT(m) ((int16_t) (m)) -#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) -#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) +#define AO_M_TO_HEIGHT(m) ((ao_v_t) (m)) +#define AO_MS_TO_SPEED(ms) ((ao_v_t) ((ms) * 16)) +#define AO_MSS_TO_ACCEL(mss) ((ao_v_t) ((mss) * 16)) extern __pdata uint16_t ao_sample_tick; /* time of last data */ extern __data uint8_t ao_sample_adc; /* Ring position of last processed sample */ @@ -134,21 +152,33 @@ uint8_t ao_sample(void); * ao_kalman.c */ -#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5)) -#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define to_fix_16(x) ((int16_t) ((x) * 65536.0 + 0.5)) +#define to_fix_32(x) ((int32_t) ((x) * 65536.0 + 0.5)) +#define to_fix_64(x) ((int64_t) ((x) * 65536.0 + 0.5)) + +#ifdef AO_VALUE_32 +#if AO_VALUE_32 +#define to_fix_v(x) to_fix_32(x) +#define to_fix_k(x) to_fix_64(x) +#else +#define to_fix_v(x) to_fix_16(x) +#define to_fix_k(x) to_fix_32(x) +#endif + #define from_fix(x) ((x) >> 16) -extern __pdata int16_t ao_height; /* meters */ -extern __pdata int16_t ao_speed; /* m/s * 16 */ -extern __pdata int16_t ao_accel; /* m/s² * 16 */ -extern __xdata int16_t ao_max_height; /* max of ao_height */ -extern __xdata int16_t ao_avg_height; /* running average of height */ +extern __pdata ao_v_t ao_height; /* meters */ +extern __pdata ao_v_t ao_speed; /* m/s * 16 */ +extern __pdata ao_v_t ao_accel; /* m/s² * 16 */ +extern __xdata ao_v_t ao_max_height; /* max of ao_height */ +extern __xdata ao_v_t ao_avg_height; /* running average of height */ -extern __pdata int16_t ao_error_h; -extern __pdata int16_t ao_error_h_sq_avg; +extern __pdata ao_v_t ao_error_h; +extern __pdata ao_v_t ao_error_h_sq_avg; #if HAS_ACCEL -extern __pdata int16_t ao_error_a; +extern __pdata ao_v_t ao_error_a; +#endif #endif void ao_kalman(void); diff --git a/src/product/Makefile.telemetrum b/src/product/Makefile.telemetrum index dbbf57d8..e9b144c0 100644 --- a/src/product/Makefile.telemetrum +++ b/src/product/Makefile.telemetrum @@ -24,6 +24,7 @@ INC = \ altitude.h \ ao_kalman.h \ ao_product.h \ + ao_telemetry.h \ $(TM_INC) CORE_SRC = \ diff --git a/src/telefire-v0.1/Makefile b/src/telefire-v0.1/Makefile index 25267268..99d29826 100644 --- a/src/telefire-v0.1/Makefile +++ b/src/telefire-v0.1/Makefile @@ -25,7 +25,6 @@ INC = \ CORE_SRC = \ ao_cmd.c \ ao_config.c \ - ao_convert.c \ ao_mutex.c \ ao_panic.c \ ao_stdio.c \ diff --git a/src/telefire-v0.2/Makefile b/src/telefire-v0.2/Makefile index ad5065c1..944543c5 100644 --- a/src/telefire-v0.2/Makefile +++ b/src/telefire-v0.2/Makefile @@ -25,7 +25,6 @@ INC = \ CORE_SRC = \ ao_cmd.c \ ao_config.c \ - ao_convert.c \ ao_mutex.c \ ao_panic.c \ ao_stdio.c \ diff --git a/src/telemega-v1.0/Makefile b/src/telemega-v1.0/Makefile index 46c768e4..4a1b3908 100644 --- a/src/telemega-v1.0/Makefile +++ b/src/telemega-v1.0/Makefile @@ -31,6 +31,7 @@ INC = \ ao_mpu.h \ stm32l.h \ math.h \ + ao_ms5607_convert.c \ Makefile # diff --git a/src/telemini-v2.0/ao_pins.h b/src/telemini-v2.0/ao_pins.h index 948310e5..ed911798 100644 --- a/src/telemini-v2.0/ao_pins.h +++ b/src/telemini-v2.0/ao_pins.h @@ -22,6 +22,7 @@ #define HAS_FLIGHT 1 #define HAS_USB 1 +#define AO_VALUE_32 0 #define HAS_USB_PULLUP 1 #define AO_USB_PULLUP_PORT P1 diff --git a/src/teleterra-v0.2/ao_pins.h b/src/teleterra-v0.2/ao_pins.h index 60d627ad..472af534 100644 --- a/src/teleterra-v0.2/ao_pins.h +++ b/src/teleterra-v0.2/ao_pins.h @@ -72,6 +72,8 @@ #define BATTERY_PIN 5 #define HAS_TELEMETRY 0 + + #define AO_VALUE_32 0 #endif #if DBG_ON_P1 diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 314998c1..bb5c3a7d 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -34,9 +34,11 @@ #define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) #define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1)) +#if 0 #define AO_M_TO_HEIGHT(m) ((int16_t) (m)) #define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16)) #define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16)) +#endif #define AO_GPS_NEW_DATA 1 #define AO_GPS_NEW_TRACKING 2 @@ -93,6 +95,7 @@ struct ao_adc { #include #include #include +#include #if TELEMEGA int ao_gps_count; @@ -234,7 +237,7 @@ double main_time; int tick_offset; -static int32_t ao_k_height; +static ao_k_t ao_k_height; int16_t ao_time(void) diff --git a/src/test/ao_micropeak_test.c b/src/test/ao_micropeak_test.c index 5961bd93..f4af707e 100644 --- a/src/test/ao_micropeak_test.c +++ b/src/test/ao_micropeak_test.c @@ -33,6 +33,7 @@ uint8_t ao_flight_debug; #define AO_FLIGHT_TEST typedef int32_t alt_t; +typedef int32_t pres_t; #define AO_MS_TO_TICKS(ms) ((ms) / 10) -- cgit v1.2.3 From 1530c24cc75cdf9ba87c7e153ff28bf7beb4384c Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sun, 17 Aug 2014 20:57:16 -0700 Subject: cc1111: Wait for internal flash write to complete This ensures that we don't try to start another write too soon. Signed-off-by: Keith Packard --- src/cc1111/ao_intflash.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/cc1111') diff --git a/src/cc1111/ao_intflash.c b/src/cc1111/ao_intflash.c index 632e2a85..eb3535c6 100644 --- a/src/cc1111/ao_intflash.c +++ b/src/cc1111/ao_intflash.c @@ -129,6 +129,8 @@ ao_intflash_write_aligned(uint16_t pos, __xdata void *d, uint16_t len) __reentra nop _endasm; } + __critical while (!ao_intflash_dma_done) + ao_sleep(&ao_intflash_dma_done); } static void -- cgit v1.2.3 From f395bcaa620490954d4a42de9b4870bc12bedc91 Mon Sep 17 00:00:00 2001 From: Keith Packard Date: Sat, 6 Sep 2014 20:39:37 -0700 Subject: altos: Reduce deviation at 2400/9600 baud There's no reason to use 20.5kHz deviation at 2400 and 9600 baud, and if we get a better receiver, we'll want to use narrower deviation to have less bandwidth sucking noise into the radio. The new values are (nominally) 5.125kHz deviation for 9600 baud and 1.5kHz deviation for 2400 baud. Signed-off-by: Keith Packard --- src/cc1111/ao_radio.c | 77 ++++++++++++++++++++++++++++++++----------------- src/drivers/ao_cc1120.c | 69 +++++++++++++++++++++++++++++++++++--------- src/drivers/ao_cc1120.h | 6 ++++ src/drivers/ao_cc115l.c | 64 ++++++++++++++++++++++++++++++---------- 4 files changed, 160 insertions(+), 56 deletions(-) (limited to 'src/cc1111') diff --git a/src/cc1111/ao_radio.c b/src/cc1111/ao_radio.c index 8f519958..b9821a42 100644 --- a/src/cc1111/ao_radio.c +++ b/src/cc1111/ao_radio.c @@ -60,19 +60,19 @@ * * M = 1, E = 3, bw = 75kHz * - * 20.5 kHz deviation, 9.6kbps signal + * 5.125 kHz deviation, 9.6kbps signal * - * m = 41 / 9.6, which is < 5.5: + * m = 10.25 / 9.6, which is < 5.5: * - * bw = 2.6 * 20.5 + 0.55 * 9.6 = 58.58kHz + * bw = 2.6 * 5.125 + 0.55 * 9.6 = 18.6kHz * - * M = 2, E = 3, bw = 62.5kHz + * M = 2, E = 3, bw = 53.6kHz * - * 20.5kHz deviation, 2.4kbps signal + * 1.28125kHz deviation, 2.4kbps signal * - * m = 41 / 2.4, which is > 5.5: + * m = 2.565 / 2.4, which is < 5.5: * - * bw = 2.1 * 20.5 + 1.9 * 2.4 = 47.61kHz + * bw = 2.6 * 20.5 + 1.9 * 2.4 = 47.61kHz * * M = 3, E = 3, bw = 53.6kHz * @@ -84,7 +84,7 @@ */ #define CHANBW_M_384 1 -#define CHANBW_M_96 2 +#define CHANBW_M_96 3 #define CHANBW_M_24 3 #define CHANBW_E 3 @@ -115,11 +115,19 @@ * * F = 24e6/2**17 * (8 + DEVIATION_M) * 2**DEVIATION_E * - * So M is 6 and E is 3 + * For 20.5kHz deviation, M is 6 and E is 3 + * For 5.125kHz deviation, M is 6 and E is 1 + * For 1.28125kHz deviation, M is 0 and E is 0 */ -#define DEVIATION_M 6 -#define DEVIATION_E 3 +#define DEVIATION_M_384 6 +#define DEVIATION_E_384 3 + +#define DEVIATION_M_96 6 +#define DEVIATION_E_96 1 + +#define DEVIATION_M_24 0 +#define DEVIATION_E_24 0 /* * For our RDF beacon, set the symbol rate to 2kBaud (for a 1kHz tone), @@ -182,9 +190,6 @@ static __code uint8_t radio_setup[] = { RF_CHANNR_OFF, 0, - RF_DEVIATN_OFF, ((DEVIATION_E << RF_DEVIATN_DEVIATION_E_SHIFT) | - (DEVIATION_M << RF_DEVIATN_DEVIATION_M_SHIFT)), - /* SmartRF says set LODIV_BUF_CURRENT_TX to 0 * And, we're not using power ramping, so use PA_POWER 0 */ @@ -284,8 +289,10 @@ static __code uint8_t fixed_pkt_setup[] = { RF_MDMCFG1_NUM_PREAMBLE_4 | (2 << RF_MDMCFG1_CHANSPC_E_SHIFT)), - RF_DEVIATN_OFF, ((DEVIATION_E << RF_DEVIATN_DEVIATION_E_SHIFT) | - (DEVIATION_M << RF_DEVIATN_DEVIATION_M_SHIFT)), +#if !HAS_RADIO_RATE + RF_DEVIATN_OFF, ((DEVIATION_E_384 << RF_DEVIATN_DEVIATION_E_SHIFT) | + (DEVIATION_M_384 << RF_DEVIATN_DEVIATION_M_SHIFT)), +#endif /* max packet length -- now set inline */ RF_PKTCTRL1_OFF, ((1 << PKTCTRL1_PQT_SHIFT)| @@ -298,19 +305,34 @@ static __code uint8_t fixed_pkt_setup[] = { }; #if HAS_RADIO_RATE -static __code uint8_t packet_rate_setup[] = { +static __code struct { + uint8_t mdmcfg4; + uint8_t deviatn; +} packet_rate_setup[] = { /* 38400 */ - ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | - (CHANBW_M_384 << RF_MDMCFG4_CHANBW_M_SHIFT) | - (DRATE_E_384 << RF_MDMCFG4_DRATE_E_SHIFT)), + { + ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | + (CHANBW_M_384 << RF_MDMCFG4_CHANBW_M_SHIFT) | + (DRATE_E_384 << RF_MDMCFG4_DRATE_E_SHIFT)), + ((DEVIATION_E_384 << RF_DEVIATN_DEVIATION_E_SHIFT) | + (DEVIATION_M_384 << RF_DEVIATN_DEVIATION_M_SHIFT)), + }, /* 9600 */ - ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | - (CHANBW_M_96 << RF_MDMCFG4_CHANBW_M_SHIFT) | - (DRATE_E_96 << RF_MDMCFG4_DRATE_E_SHIFT)), + { + ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | + (CHANBW_M_96 << RF_MDMCFG4_CHANBW_M_SHIFT) | + (DRATE_E_96 << RF_MDMCFG4_DRATE_E_SHIFT)), + ((DEVIATION_E_96 << RF_DEVIATN_DEVIATION_E_SHIFT) | + (DEVIATION_M_96 << RF_DEVIATN_DEVIATION_M_SHIFT)), + }, /* 2400 */ - ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | - (CHANBW_M_24 << RF_MDMCFG4_CHANBW_M_SHIFT) | - (DRATE_E_24 << RF_MDMCFG4_DRATE_E_SHIFT)), + { + ((CHANBW_E << RF_MDMCFG4_CHANBW_E_SHIFT) | + (CHANBW_M_24 << RF_MDMCFG4_CHANBW_M_SHIFT) | + (DRATE_E_24 << RF_MDMCFG4_DRATE_E_SHIFT)), + ((DEVIATION_E_24 << RF_DEVIATN_DEVIATION_E_SHIFT) | + (DEVIATION_M_24 << RF_DEVIATN_DEVIATION_M_SHIFT)), + }, }; #endif @@ -380,7 +402,8 @@ ao_radio_get(uint8_t len) RF_FREQ0 = (uint8_t) (ao_config.radio_setting); RF_PKTLEN = len; #if HAS_RADIO_RATE - RF_MDMCFG4 = packet_rate_setup[ao_config.radio_rate]; + RF_MDMCFG4 = packet_rate_setup[ao_config.radio_rate].mdmcfg4; + RF_DEVIATN = packet_rate_setup[ao_config.radio_rate].deviatn; #endif } diff --git a/src/drivers/ao_cc1120.c b/src/drivers/ao_cc1120.c index 1b907940..3ea8b704 100644 --- a/src/drivers/ao_cc1120.c +++ b/src/drivers/ao_cc1120.c @@ -290,15 +290,32 @@ ao_radio_idle(void) } /* - * Packet deviation is 20.5kHz + * Packet deviation * * fdev = fosc >> 24 * (256 + dev_m) << dev_e * + * Deviation for 38400 baud should be 20.5kHz: + * * 32e6Hz / (2 ** 24) * (256 + 80) * (2 ** 5) = 20508Hz + * + * Deviation for 9600 baud should be 5.125kHz: + * + * 32e6Hz / (2 ** 24) * (256 + 80) * (2 ** 3) = 5127Hz + * + * Deviation for 2400 baud should be 1.28125kHz, but cc1111 and + * cc115l can't do that, so we'll use 1.5kHz instead: + * + * 32e6Hz / (2 ** 24) * (256 + 137) * (2 ** 1) = 1499Hz */ -#define PACKET_DEV_E 5 -#define PACKET_DEV_M 80 +#define PACKET_DEV_M_384 80 +#define PACKET_DEV_E_384 5 + +#define PACKET_DEV_M_96 80 +#define PACKET_DEV_E_96 3 + +#define PACKET_DEV_M_24 137 +#define PACKET_DEV_E_24 1 /* * For our packet data @@ -307,41 +324,55 @@ ao_radio_idle(void) * Rdata = -------------------------------------- * fosc * 2 ** 39 * + * Given the bit period of the baseband, T, the bandwidth of the + * baseband signal is B = 1/(2T). The overall bandwidth of the + * modulated signal is then Channel bandwidth = 2Δf + 2B. + * + * 38400 -- 2 * 20500 + 38400 = 79.4 kHz + * 9600 -- 2 * 5.125 + 9600 = 19.9 kHz + * 2400 -- 2 * 1.5 + 2400 = 5.4 khz + * * Symbol rate 38400 Baud: * * DATARATE_M = 239914 * DATARATE_E = 9 - * CHANBW = 74.42 (round to 100) + * CHANBW = 79.4 (round to 100) * * Symbol rate 9600 Baud: * * DATARATE_M = 239914 * DATARATE_E = 7 - * CHANBW = 58.58 (round to 62.5) + * CHANBW = 19.9 (round to 20) * * Symbol rate 2400 Baud: * * DATARATE_M = 239914 * DATARATE_E = 5 - * CHANBW = 47.61 (round to 50) + * CHANBW = 5.0 (round to 8.0) */ #define PACKET_DRATE_M 239914 #define PACKET_DRATE_E_384 9 -#define PACKET_CHAN_BW_384 0x02 /* 200 / 2 = 100 */ + +/* 200 / 2 = 100 */ +#define PACKET_CHAN_BW_384 ((0 << CC1120_CHAN_BW_CHFILT_BYPASS) | \ + (CC1120_CHAN_BW_ADC_CIC_DECFACT_20 << CC1120_CHAN_BW_ADC_CIC_DECFACT) | \ + (2 << CC1120_CHAN_BW_BB_CIC_DECFACT)) #define PACKET_DRATE_E_96 7 -#define PACKET_CHAN_BW_96 0x42 /* 125 / 2 = 62.5 */ +/* 200 / 10 = 20 */ +#define PACKET_CHAN_BW_96 ((0 << CC1120_CHAN_BW_CHFILT_BYPASS) | \ + (CC1120_CHAN_BW_ADC_CIC_DECFACT_20 << CC1120_CHAN_BW_ADC_CIC_DECFACT) | \ + (10 << CC1120_CHAN_BW_BB_CIC_DECFACT)) #define PACKET_DRATE_E_24 5 -#define PACKET_CHAN_BW_24 0x04 /* 200 / 4 = 50 */ +/* 200 / 25 = 8 */ +#define PACKET_CHAN_BW_24 ((0 << CC1120_CHAN_BW_CHFILT_BYPASS) | \ + (CC1120_CHAN_BW_ADC_CIC_DECFACT_20 << CC1120_CHAN_BW_ADC_CIC_DECFACT) | \ + (25 << CC1120_CHAN_BW_BB_CIC_DECFACT)) static const uint16_t packet_setup[] = { - CC1120_DEVIATION_M, PACKET_DEV_M, - CC1120_MODCFG_DEV_E, ((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) | - (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) | - (PACKET_DEV_E << CC1120_MODCFG_DEV_E_DEV_E)), CC1120_DRATE1, ((PACKET_DRATE_M >> 8) & 0xff), CC1120_DRATE0, ((PACKET_DRATE_M >> 0) & 0xff), CC1120_PKT_CFG2, ((CC1120_PKT_CFG2_CCA_MODE_ALWAYS_CLEAR << CC1120_PKT_CFG2_CCA_MODE) | @@ -361,6 +392,10 @@ static const uint16_t packet_setup[] = { }; static const uint16_t packet_setup_384[] = { + CC1120_DEVIATION_M, PACKET_DEV_M_384, + CC1120_MODCFG_DEV_E, ((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) | + (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) | + (PACKET_DEV_E_384 << CC1120_MODCFG_DEV_E_DEV_E)), CC1120_DRATE2, ((PACKET_DRATE_E_384 << CC1120_DRATE2_DATARATE_E) | (((PACKET_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)), CC1120_CHAN_BW, PACKET_CHAN_BW_384, @@ -368,6 +403,10 @@ static const uint16_t packet_setup_384[] = { }; static const uint16_t packet_setup_96[] = { + CC1120_DEVIATION_M, PACKET_DEV_M_96, + CC1120_MODCFG_DEV_E, ((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) | + (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) | + (PACKET_DEV_E_96 << CC1120_MODCFG_DEV_E_DEV_E)), CC1120_DRATE2, ((PACKET_DRATE_E_96 << CC1120_DRATE2_DATARATE_E) | (((PACKET_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)), CC1120_CHAN_BW, PACKET_CHAN_BW_96, @@ -375,6 +414,10 @@ static const uint16_t packet_setup_96[] = { }; static const uint16_t packet_setup_24[] = { + CC1120_DEVIATION_M, PACKET_DEV_M_24, + CC1120_MODCFG_DEV_E, ((CC1120_MODCFG_DEV_E_MODEM_MODE_NORMAL << CC1120_MODCFG_DEV_E_MODEM_MODE) | + (CC1120_MODCFG_DEV_E_MOD_FORMAT_2_GFSK << CC1120_MODCFG_DEV_E_MOD_FORMAT) | + (PACKET_DEV_E_24 << CC1120_MODCFG_DEV_E_DEV_E)), CC1120_DRATE2, ((PACKET_DRATE_E_24 << CC1120_DRATE2_DATARATE_E) | (((PACKET_DRATE_M >> 16) & CC1120_DRATE2_DATARATE_M_19_16_MASK) << CC1120_DRATE2_DATARATE_M_19_16)), CC1120_CHAN_BW, PACKET_CHAN_BW_24, diff --git a/src/drivers/ao_cc1120.h b/src/drivers/ao_cc1120.h index 5d24c49a..943f3449 100644 --- a/src/drivers/ao_cc1120.h +++ b/src/drivers/ao_cc1120.h @@ -189,6 +189,12 @@ #define CC1120_FREQ_IF_CFG 0x0f #define CC1120_IQIC 0x10 #define CC1120_CHAN_BW 0x11 +#define CC1120_CHAN_BW_CHFILT_BYPASS 7 +#define CC1120_CHAN_BW_ADC_CIC_DECFACT 6 +#define CC1120_CHAN_BW_ADC_CIC_DECFACT_20 0 +#define CC1120_CHAN_BW_ADC_CIC_DECFACT_32 1 +#define CC1120_CHAN_BW_BB_CIC_DECFACT 0 + #define CC1120_MDMCFG1 0x12 #define CC1120_MDMCFG1_CARRIER_SENSE_GATE 7 #define CC1120_MDMCFG1_FIFO_EN 6 diff --git a/src/drivers/ao_cc115l.c b/src/drivers/ao_cc115l.c index b5103f9b..cf61acfe 100644 --- a/src/drivers/ao_cc115l.c +++ b/src/drivers/ao_cc115l.c @@ -249,23 +249,41 @@ ao_radio_idle(void) } /* - * Packet deviation is 20.5kHz + * Packet deviation * * fdev = fosc >> 17 * (8 + dev_m) << dev_e * + * For 38400 baud, use 20.5kHz: + * * 26e6 / (2 ** 17) * (8 + 5) * (2 ** 3) = 20630Hz + * + * For 9600 baud, use 5.125kHz: + * + * 26e6 / (2 ** 17) * (8 + 5) * (2 ** 1) = 5157Hz + * + * For 2400 baud, use 1.5kHz: + * + * 26e6 / (2 ** 17) * (8 + 0) * (2 ** 0) = 1587Hz */ -#define PACKET_DEV_E 3 -#define PACKET_DEV_M 5 +#define PACKET_DEV_E_384 3 +#define PACKET_DEV_M_384 5 + +#define PACKET_DEV_E_96 1 +#define PACKET_DEV_M_96 5 + +#define PACKET_DEV_E_24 0 +#define PACKET_DEV_M_24 0 /* - * For our packet data, set the symbol rate to 38400 Baud + * For our packet data: * * (256 + DATARATE_M) * 2 ** DATARATE_E * Rdata = -------------------------------------- * fosc * 2 ** 28 * + * For 38400 baud: + * * (256 + 131) * (2 ** 10) / (2**28) * 26e6 = 38383 * * DATARATE_M = 131 @@ -279,20 +297,33 @@ ao_radio_idle(void) #define PACKET_DRATE_E_96 8 #define PACKET_DRATE_E_24 6 -static const uint16_t packet_rate_setup[] = { - [AO_RADIO_RATE_38400] = ((0xf << 4) | - (PACKET_DRATE_E_384 << CC115L_MDMCFG4_DRATE_E)), - - [AO_RADIO_RATE_9600] = ((0xf << 4) | - (PACKET_DRATE_E_96 << CC115L_MDMCFG4_DRATE_E)), - - [AO_RADIO_RATE_2400] = ((0xf << 4) | - (PACKET_DRATE_E_24 << CC115L_MDMCFG4_DRATE_E)), +static const struct { + uint8_t mdmcfg4; + uint8_t deviatn; +} packet_rate_setup[] = { + [AO_RADIO_RATE_38400] = { + .mdmcfg4 = ((0xf << 4) | + (PACKET_DRATE_E_384 << CC115L_MDMCFG4_DRATE_E)), + .deviatn = ((PACKET_DEV_E_384 << CC115L_DEVIATN_DEVIATION_E) | + (PACKET_DEV_M_384 << CC115L_DEVIATN_DEVIATION_M)), + }, + + [AO_RADIO_RATE_9600] = { + .mdmcfg4 = ((0xf << 4) | + (PACKET_DRATE_E_96 << CC115L_MDMCFG4_DRATE_E)), + .deviatn = ((PACKET_DEV_E_96 << CC115L_DEVIATN_DEVIATION_E) | + (PACKET_DEV_M_96 << CC115L_DEVIATN_DEVIATION_M)), + }, + + [AO_RADIO_RATE_2400] = { + .mdmcfg4 = ((0xf << 4) | + (PACKET_DRATE_E_24 << CC115L_MDMCFG4_DRATE_E)), + .deviatn = ((PACKET_DEV_E_24 << CC115L_DEVIATN_DEVIATION_E) | + (PACKET_DEV_M_24 << CC115L_DEVIATN_DEVIATION_M)), + }, }; static const uint16_t packet_setup[] = { - CC115L_DEVIATN, ((PACKET_DEV_E << CC115L_DEVIATN_DEVIATION_E) | - (PACKET_DEV_M << CC115L_DEVIATN_DEVIATION_M)), CC115L_MDMCFG3, (PACKET_DRATE_M), CC115L_MDMCFG2, (0x00 | (CC115L_MDMCFG2_MOD_FORMAT_GFSK << CC115L_MDMCFG2_MOD_FORMAT) | @@ -418,7 +449,8 @@ ao_radio_set_mode(uint16_t new_mode) changes = new_mode & (~ao_radio_mode); if (changes & AO_RADIO_MODE_BITS_PACKET_TX) { - ao_radio_reg_write(CC115L_MDMCFG4, packet_rate_setup[ao_config.radio_rate]); + ao_radio_reg_write(CC115L_MDMCFG4, packet_rate_setup[ao_config.radio_rate].mdmcfg4); + ao_radio_reg_write(CC115L_DEVIATN, packet_rate_setup[ao_config.radio_rate].deviatn); for (i = 0; i < sizeof (packet_setup) / sizeof (packet_setup[0]); i += 2) ao_radio_reg_write(packet_setup[i], packet_setup[i+1]); -- cgit v1.2.3