diff --git a/flight/Modules/Radio/radio.c b/flight/Modules/Radio/radio.c index b5beb1359..8820e4081 100644 --- a/flight/Modules/Radio/radio.c +++ b/flight/Modules/Radio/radio.c @@ -185,69 +185,66 @@ static int32_t RadioInitialize(void) const struct pios_board_info * bdinfo = &pios_board_info_blob; pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - // Not appropriate for a constant struct. Is it necessary to make a local copy of the cfg? - // We should probably be consistent with other drivers and allocate a device structure with - // dynamical configuration in it -#if 0 - pios_rfm22b_cfg->frequencyHz = pipxSettings.Frequency; - pios_rfm22b_cfg->RFXtalCap = pipxSettings.FrequencyCalibration; - switch (pipxSettings.RFSpeed) - { - case PIPXSETTINGS_RFSPEED_2400: - pios_rfm22b_cfg->maxRFBandwidth = 2000; - break; - case PIPXSETTINGS_RFSPEED_4800: - pios_rfm22b_cfg->maxRFBandwidth = 4000; - break; - case PIPXSETTINGS_RFSPEED_9600: - pios_rfm22b_cfg->maxRFBandwidth = 9600; - break; - case PIPXSETTINGS_RFSPEED_19200: - pios_rfm22b_cfg->maxRFBandwidth = 19200; - break; - case PIPXSETTINGS_RFSPEED_38400: - pios_rfm22b_cfg->maxRFBandwidth = 32000; - break; - case PIPXSETTINGS_RFSPEED_57600: - pios_rfm22b_cfg->maxRFBandwidth = 64000; - break; - case PIPXSETTINGS_RFSPEED_115200: - pios_rfm22b_cfg->maxRFBandwidth = 128000; - break; - } - switch (pipxSettings.MaxRFPower) - { - case PIPXSETTINGS_MAXRFPOWER_125: - pios_rfm22b_cfg->maxTxPower = RFM22_tx_pwr_txpow_0; - break; - case PIPXSETTINGS_MAXRFPOWER_16: - pios_rfm22b_cfg->maxTxPower = RFM22_tx_pwr_txpow_1; - break; - case PIPXSETTINGS_MAXRFPOWER_316: - pios_rfm22b_cfg->maxTxPower = RFM22_tx_pwr_txpow_2; - break; - case PIPXSETTINGS_MAXRFPOWER_63: - pios_rfm22b_cfg->maxTxPower = RFM22_tx_pwr_txpow_3; - break; - case PIPXSETTINGS_MAXRFPOWER_126: - pios_rfm22b_cfg->maxTxPower = RFM22_tx_pwr_txpow_4; - break; - case PIPXSETTINGS_MAXRFPOWER_25: - pios_rfm22b_cfg->maxTxPower = RFM22_tx_pwr_txpow_5; - break; - case PIPXSETTINGS_MAXRFPOWER_50: - pios_rfm22b_cfg->maxTxPower = RFM22_tx_pwr_txpow_6; - break; - case PIPXSETTINGS_MAXRFPOWER_100: - pios_rfm22b_cfg->maxTxPower = RFM22_tx_pwr_txpow_7; - break; - } -#endif - /* Initalize the RFM22B radio COM device. */ if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) return -1; + // Set the maximum radio RF power. + switch (pipxSettings.MaxRFPower) + { + case PIPXSETTINGS_MAXRFPOWER_125: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); + break; + case PIPXSETTINGS_MAXRFPOWER_16: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); + break; + case PIPXSETTINGS_MAXRFPOWER_316: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); + break; + case PIPXSETTINGS_MAXRFPOWER_63: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); + break; + case PIPXSETTINGS_MAXRFPOWER_126: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); + break; + case PIPXSETTINGS_MAXRFPOWER_25: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); + break; + case PIPXSETTINGS_MAXRFPOWER_50: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); + break; + case PIPXSETTINGS_MAXRFPOWER_100: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); + break; + } + + switch (pipxSettings.RFSpeed) { + case PIPXSETTINGS_RFSPEED_2400: + RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_2000, true); + break; + case PIPXSETTINGS_RFSPEED_4800: + RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_4000, true); + break; + case PIPXSETTINGS_RFSPEED_9600: + RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_9600, true); + break; + case PIPXSETTINGS_RFSPEED_19200: + RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_19200, true); + break; + case PIPXSETTINGS_RFSPEED_38400: + RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_32000, true); + break; + case PIPXSETTINGS_RFSPEED_57600: + RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_64000, true); + break; + case PIPXSETTINGS_RFSPEED_115200: + RFM22_SetDatarate(pios_rfm22b_id, RFM22_datarate_128000, true); + break; + } + + // Set the radio destination ID. + PIOS_RFM22B_SetDestinationId(pios_rfm22b_id, pipxSettings.PairID); + // Initialize the packet handler PacketHandlerConfig pios_ph_cfg = { .default_destination_id = 0xffffffff, // Broadcast diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 96d735d61..bd3925f03 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -62,6 +62,7 @@ #define ISR_TIMEOUT 5 // ms #define EVENT_QUEUE_SIZE 5 #define PACKET_QUEUE_SIZE 3 +#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_64000 // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms @@ -164,8 +165,12 @@ struct pios_rfm22b_dev { uint32_t spi_id; uint32_t slave_num; + // The device ID uint32_t deviceID; + // The destination ID + uint32_t destination_id; + // The task handle xTaskHandle taskHandle; @@ -184,6 +189,9 @@ struct pios_rfm22b_dev { // the transmit power to use for data transmissions uint8_t tx_power; + // The RF datarate lookup index. + uint8_t datarate; + // The state machine state and the current event enum pios_rfm22b_state state; // The event queue handle @@ -558,6 +566,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu // Bind the configuration to the device instance rfm22b_dev->cfg = *cfg; + rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; // Initialize the packets. rfm22b_dev->rx_packet = NULL; @@ -570,7 +579,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu g_rfm22b_dev = rfm22b_dev; // Calculate the (approximate) maximum amount of time that it should take to transmit / receive a packet. - rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(rfm22b_dev->cfg.maxRFBandwidth) + 0.5); rfm22b_dev->packet_start_ticks = 0; // Create a semaphore to know if an ISR needs responding to @@ -672,24 +680,20 @@ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) return 0; } -void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, uint8_t tx_pwr) +void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if(!PIOS_RFM22B_validate(rfm22b_dev)) return; + rfm22b_dev->tx_power = tx_pwr; +} - switch (tx_pwr) - { - case 0: rfm22b_dev->tx_power = RFM22_tx_pwr_txpow_0; break; // +1dBm ... 1.25mW - case 1: rfm22b_dev->tx_power = RFM22_tx_pwr_txpow_1; break; // +2dBm ... 1.6mW - case 2: rfm22b_dev->tx_power = RFM22_tx_pwr_txpow_2; break; // +5dBm ... 3.16mW - case 3: rfm22b_dev->tx_power = RFM22_tx_pwr_txpow_3; break; // +8dBm ... 6.3mW - case 4: rfm22b_dev->tx_power = RFM22_tx_pwr_txpow_4; break; // +11dBm .. 12.6mW - case 5: rfm22b_dev->tx_power = RFM22_tx_pwr_txpow_5; break; // +14dBm .. 25mW - case 6: rfm22b_dev->tx_power = RFM22_tx_pwr_txpow_6; break; // +17dBm .. 50mW - case 7: rfm22b_dev->tx_power = RFM22_tx_pwr_txpow_7; break; // +20dBm .. 100mW - default: break; - } +void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_validate(rfm22b_dev)) + return; + rfm22b_dev->destination_id = (dest_id == 0) ? 0xffffffff : dest_id; } int16_t PIOS_RFM22B_Resets(uint32_t rfm22b_id) @@ -1120,42 +1124,41 @@ uint32_t rfm22_freqHopSize(void) // // This gives 2(45 + 9.6) = 109.2kHz. -void rfm22_setDatarate(uint32_t datarate_bps, bool data_whitening) +void RFM22_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, bool data_whitening) { + struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_validate(rfm22b_dev)) + return; - // Find the closest data rate that is >= the value passed in - int lookup_index = 0; - while (lookup_index < (LOOKUP_SIZE - 1) && data_rate[lookup_index] < datarate_bps) - lookup_index++; - lookup_index = 10; - - datarate_bps = data_rate[lookup_index]; + uint32_t datarate_bps = data_rate[datarate]; + rfm22b_dev->datarate = datarate; + rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5); // rfm22_if_filter_bandwidth - rfm22_write(0x1C, reg_1C[lookup_index]); + rfm22_write(0x1C, reg_1C[datarate]); // rfm22_afc_loop_gearshift_override - rfm22_write(0x1D, reg_1D[lookup_index]); + rfm22_write(0x1D, reg_1D[datarate]); // RFM22_afc_timing_control - rfm22_write(0x1E, reg_1E[lookup_index]); + rfm22_write(0x1E, reg_1E[datarate]); // RFM22_clk_recovery_gearshift_override - rfm22_write(0x1F, reg_1F[lookup_index]); + rfm22_write(0x1F, reg_1F[datarate]); // rfm22_clk_recovery_oversampling_ratio - rfm22_write(0x20, reg_20[lookup_index]); + rfm22_write(0x20, reg_20[datarate]); // rfm22_clk_recovery_offset2 - rfm22_write(0x21, reg_21[lookup_index]); + rfm22_write(0x21, reg_21[datarate]); // rfm22_clk_recovery_offset1 - rfm22_write(0x22, reg_22[lookup_index]); + rfm22_write(0x22, reg_22[datarate]); // rfm22_clk_recovery_offset0 - rfm22_write(0x23, reg_23[lookup_index]); + rfm22_write(0x23, reg_23[datarate]); // rfm22_clk_recovery_timing_loop_gain1 - rfm22_write(0x24, reg_24[lookup_index]); + rfm22_write(0x24, reg_24[datarate]); // rfm22_clk_recovery_timing_loop_gain0 - rfm22_write(0x25, reg_25[lookup_index]); + rfm22_write(0x25, reg_25[datarate]); // rfm22_afc_limiter - rfm22_write(0x2A, reg_2A[lookup_index]); + rfm22_write(0x2A, reg_2A[datarate]); /* This breaks all bit rates < 100000! if (datarate_bps < 100000) @@ -1167,9 +1170,9 @@ void rfm22_setDatarate(uint32_t datarate_bps, bool data_whitening) */ // rfm22_tx_data_rate1 - rfm22_write(0x6E, reg_6E[lookup_index]); + rfm22_write(0x6E, reg_6E[datarate]); // rfm22_tx_data_rate0 - rfm22_write(0x6F, reg_6F[lookup_index]); + rfm22_write(0x6F, reg_6F[datarate]); // Enable data whitening // uint8_t txdtrtscale_bit = rfm22_read(RFM22_modulation_mode_control1) & RFM22_mmc1_txdtrtscale; @@ -1177,16 +1180,16 @@ void rfm22_setDatarate(uint32_t datarate_bps, bool data_whitening) if (!data_whitening) // rfm22_modulation_mode_control1 - rfm22_write(0x70, reg_70[lookup_index] & ~RFM22_mmc1_enwhite); + rfm22_write(0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); else // rfm22_modulation_mode_control1 - rfm22_write(0x70, reg_70[lookup_index] | RFM22_mmc1_enwhite); + rfm22_write(0x70, reg_70[datarate] | RFM22_mmc1_enwhite); // rfm22_modulation_mode_control2 - rfm22_write(0x71, reg_71[lookup_index]); + rfm22_write(0x71, reg_71[datarate]); // rfm22_frequency_deviation - rfm22_write(0x72, reg_72[lookup_index]); + rfm22_write(0x72, reg_72[datarate]); rfm22_write(RFM22_ook_counter_value1, 0x00); rfm22_write(RFM22_ook_counter_value2, 0x00); @@ -1850,7 +1853,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_setFreqCalibration(rfm22b_dev->cfg.RFXtalCap); rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->cfg.frequencyHz); - rfm22_setDatarate(rfm22b_dev->cfg.maxRFBandwidth, true); + RFM22_SetDatarate((uint32_t)rfm22b_dev, rfm22b_dev->datarate, true); return RFM22B_EVENT_INITIALIZED; } diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 451480960..65c823283 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -49,9 +49,39 @@ struct pios_rfm22b_cfg { enum gpio_direction gpio_direction; }; +enum rfm22b_tx_power { + RFM22_tx_pwr_txpow_0 = 0x00, // +1dBm .. 1.25mW + RFM22_tx_pwr_txpow_1 = 0x01, // +2dBm .. 1.6mW + RFM22_tx_pwr_txpow_2 = 0x02, // +5dBm .. 3.16mW + RFM22_tx_pwr_txpow_3 = 0x03, // +8dBm .. 6.3mW + RFM22_tx_pwr_txpow_4 = 0x04, // +11dBm .. 12.6mW + RFM22_tx_pwr_txpow_5 = 0x05, // +14dBm .. 25mW + RFM22_tx_pwr_txpow_6 = 0x06, // +17dBm .. 50mW + RFM22_tx_pwr_txpow_7 = 0x07 // +20dBm .. 100mW +}; + +enum rfm22b_datarate { + RFM22_datarate_500 = 0, + RFM22_datarate_1000 = 1, + RFM22_datarate_2000 = 2, + RFM22_datarate_4000 = 3, + RFM22_datarate_8000 = 4, + RFM22_datarate_9600 = 5, + RFM22_datarate_16000 = 6, + RFM22_datarate_19200 = 7, + RFM22_datarate_24000 = 8, + RFM22_datarate_32000 = 9, + RFM22_datarate_64000 = 10, + RFM22_datarate_128000 = 11, + RFM22_datarate_192000 = 12, + RFM22_datarate_256000 = 13, +}; + /* Public Functions */ extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg); -extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, uint8_t tx_pwr); +extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr); +extern void RFM22_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, bool data_whitening); +extern void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern int16_t PIOS_RFM22B_Resets(uint32_t rfm22b_id); extern bool PIOS_RFM22B_Send_Packet(uint32_t rfm22b_id, PHPacketHandle p, uint32_t max_delay); diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 4719e722d..aa80f1b62 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -490,8 +490,8 @@ extern const struct pios_com_driver pios_rfm22b_com_driver; #define RFM22_deltasigma_adc_tuning1 0x67 // R/W #define RFM22_deltasigma_adc_tuning2 0x68 // R/W */ -#define RFM22_agc_override1 0x69 // R/W -#define RFM22_agc_ovr1_sgi 0x40 // AGC Loop, Set Gain Increase. If set to 0 then gain increasing will not be allowed. If set to 1 then gain increasing is allowed, default is 0. +#define RFM22_agc_override1 0x69 // R/W +#define RFM22_agc_ovr1_sgi 0x40 // AGC Loop, Set Gain Increase. If set to 0 then gain increasing will not be allowed. If set to 1 then gain increasing is allowed, default is 0. #define RFM22_agc_ovr1_agcen 0x20 // Automatic Gain Control Enable. When this bit is set then the result of the control can be read out from bits [4:0], otherwise the gain can be controlled manually by writing into bits [4:0]. #define RFM22_agc_ovr1_lnagain 0x10 // LNA Gain Select. 0 = min gain = 5dB, 1 = max gain = 25 dB. #define RFM22_agc_ovr1_pga_mask 0x0F // PGA Gain Override Value. @@ -501,30 +501,22 @@ extern const struct pios_com_driver pios_rfm22b_com_driver; //#define RFM22_gfsk_fir_coeff_addr 0x6B // R/W //#define RFM22_gfsk_fir_coeff_value 0x6C // R/W -#define RFM22_tx_power 0x6D // R/W -#define RFM22_tx_pwr_txpow_0 0x00 // +1dBm .. 1.25mW -#define RFM22_tx_pwr_txpow_1 0x01 // +2dBm .. 1.6mW -#define RFM22_tx_pwr_txpow_2 0x02 // +5dBm .. 3.16mW -#define RFM22_tx_pwr_txpow_3 0x03 // +8dBm .. 6.3mW -#define RFM22_tx_pwr_txpow_4 0x04 // +11dBm .. 12.6mW -#define RFM22_tx_pwr_txpow_5 0x05 // +14dBm .. 25mW -#define RFM22_tx_pwr_txpow_6 0x06 // +17dBm .. 50mW -#define RFM22_tx_pwr_txpow_7 0x07 // +20dBm .. 100mW -#define RFM22_tx_pwr_lna_sw 0x08 // LNA Switch Controller. If set, lna_sw control from the digital will go high during TX modes, and low during other times. If reset, the digital control signal is low at all times. +#define RFM22_tx_power 0x6D // R/W +#define RFM22_tx_pwr_lna_sw 0x08 // LNA Switch Controller. If set, lna_sw control from the digital will go high during TX modes, and low during other times. If reset, the digital control signal is low at all times. #define RFM22_tx_pwr_papeaklvl_0 0x10 // " " #define RFM22_tx_pwr_papeaklvl_1 0x20 // PA Peak Detect Level (direct from register). 00 = 6.5, 01 = 7, 10 = 7.5, 11 = 8, 00 = default #define RFM22_tx_pwr_papeaken 0x40 // PA Peak Detector Enable. #define RFM22_tx_pwr_papeakval 0x80 // PA Peak Detector Value Read Register. Reading a 1 in this register when the papeaken=1 then the PA drain voltage is too high and the match network needs adjusting for optimal efficiency. -#define RFM22_tx_data_rate1 0x6E // R/W -#define RFM22_tx_data_rate0 0x6F // R/W +#define RFM22_tx_data_rate1 0x6E // R/W +#define RFM22_tx_data_rate0 0x6F // R/W #define RFM22_modulation_mode_control1 0x70 // R/W -#define RFM22_mmc1_enwhite 0x01 // Data Whitening is Enabled if this bit is set. -#define RFM22_mmc1_enmanch 0x02 // Manchester Coding is Enabled if this bit is set. -#define RFM22_mmc1_enmaninv 0x04 // Manchester Data Inversion is Enabled if this bit is set. -#define RFM22_mmc1_manppol 0x08 // Manchester Preamble Polarity (will transmit a series of 1 if set, or series of 0 if reset). -#define RFM22_mmc1_enphpwdn 0x10 // If set, the Packet Handler will be powered down when chip is in low power mode. +#define RFM22_mmc1_enwhite 0x01 // Data Whitening is Enabled if this bit is set. +#define RFM22_mmc1_enmanch 0x02 // Manchester Coding is Enabled if this bit is set. +#define RFM22_mmc1_enmaninv 0x04 // Manchester Data Inversion is Enabled if this bit is set. +#define RFM22_mmc1_manppol 0x08 // Manchester Preamble Polarity (will transmit a series of 1 if set, or series of 0 if reset). +#define RFM22_mmc1_enphpwdn 0x10 // If set, the Packet Handler will be powered down when chip is in low power mode. #define RFM22_mmc1_txdtrtscale 0x20 // This bit should be set for Data Rates below 30 kbps. #define RFM22_modulation_mode_control2 0x71 // R/W @@ -533,8 +525,8 @@ extern const struct pios_com_driver pios_rfm22b_com_driver; #define RFM22_mmc2_modtyp_ook 0x01 // #define RFM22_mmc2_modtyp_fsk 0x02 // #define RFM22_mmc2_modtyp_gfsk 0x03 // -#define RFM22_mmc2_fd 0x04 // MSB of Frequency Deviation Setting, see "Register 72h. Frequency Deviation". -#define RFM22_mmc2_eninv 0x08 // Invert TX and RX Data. +#define RFM22_mmc2_fd 0x04 // MSB of Frequency Deviation Setting, see "Register 72h. Frequency Deviation". +#define RFM22_mmc2_eninv 0x08 // Invert TX and RX Data. #define RFM22_mmc2_dtmod_mask 0x30 // Modulation source. #define RFM22_mmc2_dtmod_dm_gpio 0x00 // #define RFM22_mmc2_dtmod_dm_sdi 0x10 //