diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 20b487925..eb2f2bb8b 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -116,8 +116,6 @@ #define USB_LED_OFF #endif -#define CONNECTED_LIVE 8 - /* Local type definitions */ struct pios_rfm22b_transition { @@ -360,7 +358,7 @@ static const uint8_t reg_72[] = { 0x30, 0x48, 0x48, 0x48, 0x48, 0x60, 0x90, 0xCD static const uint8_t packet_time[] = { 80, 40, 25, 15, 13, 10, 8, 6, 5 }; static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 13, 10, 8, 6, 5 }; -static const uint8_t num_channels[] = { 32, 32, 32, 32, 32, 32, 32, 32, 32 }; +static const uint8_t num_channels[] = { 4, 4, 4, 6, 8, 8, 10, 12, 16 }; static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; @@ -608,8 +606,6 @@ void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datar } } - rfm22b_dev->num_channels = num_found; - // Calculate the maximum packet length from the datarate. float bytes_per_period = (float)data_rate[datarate] * (float)(rfm22b_dev->packet_time - 2) / 9000; @@ -1359,7 +1355,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->packet_start_ticks = 0; rfm22b_dev->tx_complete_ticks = 0; rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING; - rfm22b_dev->connected_timeout = 0; + rfm22b_dev->on_sync_channel = false; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); @@ -1964,16 +1960,12 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield); } - /* - * If the packet is valid and destined for us we synchronize the clock. - */ - if (!rfm22_isCoordinator(radio_dev) && - radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) { + // We only synchronize the clock on packets from our coordinator on the sync channel. + if (!rfm22_isCoordinator(radio_dev) && (radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) && (radio_dev->channel_index == 0)) { rfm22_synchronizeClock(radio_dev); radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + radio_dev->on_sync_channel = false; } - - radio_dev->connected_timeout = CONNECTED_LIVE; } else { ret_event = RADIO_EVENT_RX_COMPLETE; } @@ -2176,8 +2168,7 @@ static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev) // Calculate the adjustment for the preamble uint8_t offset = (uint8_t)ceil(35000.0F / data_rate[rfm22b_dev->datarate]); - rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset + - rfm22b_dev->packet_time * rfm22b_dev->channel_index; + rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset; } /** @@ -2237,7 +2228,9 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind // Are we switching to a new channel? if (idx != rfm22b_dev->channel_index) { - if (!rfm22_isCoordinator(rfm22b_dev) && rfm22b_dev->connected_timeout == 0) { + // If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state. + if (!rfm22_isCoordinator(rfm22b_dev) && (rfm22b_dev->channel_index == 0) && rfm22b_dev->on_sync_channel) { + rfm22b_dev->on_sync_channel = false; // Set the link state to disconnected. if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { @@ -2248,14 +2241,14 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind } } - // Stay on first channel. + // Stay on the sync channel. idx = 0; + } else if (idx == 0) { + // If we're switching to the sync channel, set a flag that can be used to detect if a packet was received. + rfm22b_dev->on_sync_channel = true; } rfm22b_dev->channel_index = idx; - if (rfm22b_dev->connected_timeout > 0) - rfm22b_dev->connected_timeout--; - } return rfm22b_dev->channels[idx]; diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 7efdd449e..bacc2dcda 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -780,7 +780,7 @@ struct pios_rfm22b_dev { portTickType packet_start_ticks; portTickType tx_complete_ticks; portTickType time_delta; - uint8_t connected_timeout; + bool on_sync_channel; };