From 4b90f81f6feff2b21580adf2feec8742ea8130d7 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 13 Oct 2012 12:06:17 -0700 Subject: [PATCH 01/31] RFM22B: Modified the method of tracking RX errors, and now reporting Good, Corrected, Error, and Missed packets to the GCS. Also removed some less useful fields from the PipXStatus. --- flight/Libraries/inc/packet_handler.h | 7 +- flight/Modules/Radio/radio.c | 48 +- flight/PiOS/Common/pios_rfm22b.c | 226 ++++----- flight/PiOS/inc/pios_rfm22b.h | 20 +- .../plugins/config/configpipxtremewidget.cpp | 44 +- .../src/plugins/config/pipxtreme.ui | 430 +++++++++--------- shared/uavobjectdefinition/pipxstatus.xml | 18 +- 7 files changed, 400 insertions(+), 393 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index c2cf0639a..736d78710 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -80,11 +80,8 @@ typedef struct { #define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - uint16_t retries; - uint16_t errors; - uint16_t uavtalk_errors; - uint16_t dropped; - uint16_t resets; + uint8_t link_quality; + int8_t received_rssi; uint8_t ecc[RS_ECC_NPARITY]; } PHStatusPacket, *PHStatusPacketHandle; diff --git a/flight/Modules/Radio/radio.c b/flight/Modules/Radio/radio.c index 7578f2ff8..baaaf3814 100644 --- a/flight/Modules/Radio/radio.c +++ b/flight/Modules/Radio/radio.c @@ -62,11 +62,6 @@ typedef struct { uint32_t pairID; - uint16_t retries; - uint16_t errors; - uint16_t uavtalk_errors; - uint16_t resets; - uint16_t dropped; int8_t rssi; uint8_t lastContact; } PairStats; @@ -271,11 +266,6 @@ static int32_t RadioInitialize(void) { data->pairStats[i].pairID = 0; data->pairStats[i].rssi = -127; - data->pairStats[i].retries = 0; - data->pairStats[i].errors = 0; - data->pairStats[i].uavtalk_errors = 0; - data->pairStats[i].resets = 0; - data->pairStats[i].dropped = 0; data->pairStats[i].lastContact = 0; } // The first slot is reserved for our current pairID @@ -355,11 +345,6 @@ static void StatusHandler(PHStatusPacketHandle status, int8_t rssi, int8_t afc) if(found) { data->pairStats[id_idx].rssi = rssi; - data->pairStats[id_idx].retries = status->retries; - data->pairStats[id_idx].errors = status->errors; - data->pairStats[id_idx].uavtalk_errors = status->uavtalk_errors; - data->pairStats[id_idx].resets = status->resets; - data->pairStats[id_idx].dropped = status->dropped; data->pairStats[id_idx].lastContact = 0; } @@ -384,11 +369,6 @@ static void StatusHandler(PHStatusPacketHandle status, int8_t rssi, int8_t afc) } data->pairStats[min_idx].pairID = id; data->pairStats[min_idx].rssi = rssi; - data->pairStats[min_idx].retries = status->retries; - data->pairStats[min_idx].errors = status->errors; - data->pairStats[min_idx].uavtalk_errors = status->uavtalk_errors; - data->pairStats[min_idx].resets = status->resets; - data->pairStats[min_idx].dropped = status->dropped; data->pairStats[min_idx].lastContact = 0; } } @@ -406,19 +386,27 @@ static void radioStatusTask(void *parameters) PipXStatusGet(&pipxStatus); PipXSettingsPairIDGet(&pairID); + // Get the stats from the radio device + struct rfm22b_stats radio_stats; + PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); + // Update the status pipxStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); - pipxStatus.Retries = data->comTxRetries; - pipxStatus.LinkQuality = PIOS_RFM22B_LinkQuality(pios_rfm22b_id); pipxStatus.UAVTalkErrors = data->UAVTalkErrors; - pipxStatus.Dropped = data->droppedPackets; - pipxStatus.Resets = PIOS_RFM22B_Resets(pios_rfm22b_id); + pipxStatus.RxGood = radio_stats.rx_good; + pipxStatus.RxCorrected = radio_stats.rx_corrected; + pipxStatus.RxErrors = radio_stats.rx_error; + pipxStatus.RxMissed = radio_stats.rx_missed; + pipxStatus.TxDropped = radio_stats.tx_dropped; // + data->droppedPackets; + pipxStatus.Resets = radio_stats.resets; + pipxStatus.Timeouts = radio_stats.timeouts; + pipxStatus.RSSI = radio_stats.rssi; + pipxStatus.LinkQuality = radio_stats.link_quality; pipxStatus.TXRate = (uint16_t)((float)(data->txBytes * 1000) / STATS_UPDATE_PERIOD_MS); data->txBytes = 0; pipxStatus.RXRate = (uint16_t)((float)(data->rxBytes * 1000) / STATS_UPDATE_PERIOD_MS); data->rxBytes = 0; pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_DISCONNECTED; - pipxStatus.RSSI = PIOS_RFM22B_LinkQuality(pios_rfm22b_id); LINK_LED_OFF; // Update the potential pairing contacts @@ -432,21 +420,11 @@ static void radioStatusTask(void *parameters) { data->pairStats[i].pairID = 0; data->pairStats[i].rssi = -127; - data->pairStats[i].retries = 0; - data->pairStats[i].errors = 0; - data->pairStats[i].uavtalk_errors = 0; - data->pairStats[i].resets = 0; - data->pairStats[i].dropped = 0; data->pairStats[i].lastContact = 0; } // Add the paired devices statistics to ours. if(pairID && (data->pairStats[i].pairID == pairID) && (data->pairStats[i].rssi > -127)) { - pipxStatus.Retries += data->pairStats[i].retries; - pipxStatus.UAVTalkErrors += data->pairStats[i].uavtalk_errors; - pipxStatus.Dropped += data->pairStats[i].dropped; - pipxStatus.Resets += data->pairStats[i].resets; - pipxStatus.Dropped += data->pairStats[i].dropped; pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_CONNECTED; LINK_LED_ON; } diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 476a56071..d7fca315a 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -158,6 +158,14 @@ enum pios_rfm22b_event { RFM22B_EVENT_NUM_EVENTS // Must be last }; +#define RFM22B_RX_PACKET_STATS_LEN 4 +enum pios_rfm22b_rx_packet_status { + RFM22B_GOOD_RX_PACKET = 0x00, + RFM22B_CORRECTED_RX_PACKET = 0x01, + RFM22B_ERROR_RX_PACKET = 0x2, + RFM22B_MISSED_RX_PACKET = 0x3 +}; + struct pios_rfm22b_dev { enum pios_rfm22b_dev_magic magic; struct pios_rfm22b_cfg cfg; @@ -210,24 +218,17 @@ struct pios_rfm22b_dev { uint32_t tx_packet_count; uint32_t rx_packet_count; - // The dropped packet counters - uint8_t slow_block; - uint8_t fast_block; - uint8_t slow_good_packets; - uint8_t fast_good_packets; - uint8_t slow_corrected_packets; - uint8_t fast_corrected_packets; - uint8_t slow_error_packets; - uint8_t fast_error_packets; - uint8_t slow_link_quality; - uint8_t fast_link_quality; + // The error statistics counters + uint16_t prev_rx_seq_num; + uint32_t rx_packet_stats[RFM22B_RX_PACKET_STATS_LEN]; + + // The packet statistics + struct rfm22b_stats stats; // Stats uint16_t resets; uint16_t timeouts; uint16_t errors; - // the current RSSI (register value) - uint8_t rssi; // RSSI in dBm int8_t rssi_dBm; @@ -316,6 +317,7 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status); // SPI read/write functions static void rfm22_assertCs(); @@ -574,25 +576,26 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->ezmac_status = 0; // Initlize the link stats. - rfm22b_dev->slow_block = 0; - rfm22b_dev->fast_block = 0; - rfm22b_dev->slow_good_packets = 0; - rfm22b_dev->fast_good_packets = 0; - rfm22b_dev->slow_corrected_packets = 0; - rfm22b_dev->fast_corrected_packets = 0; - rfm22b_dev->slow_error_packets = 0; - rfm22b_dev->fast_error_packets = 0; - rfm22b_dev->slow_link_quality = 255; - rfm22b_dev->fast_link_quality = 255; + rfm22b_dev->prev_rx_seq_num = 0; + for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) + rfm22b_dev->rx_packet_stats[i] = 0; + + rfm22b_dev->stats.packets_per_sec = 0; + rfm22b_dev->stats.tx_count = 0; + rfm22b_dev->stats.rx_count = 0; + rfm22b_dev->stats.rx_good = 0; + rfm22b_dev->stats.rx_corrected = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.rx_missed = 0; + rfm22b_dev->stats.tx_dropped = 0; + rfm22b_dev->stats.resets = 0; + rfm22b_dev->stats.timeouts = 0; + rfm22b_dev->stats.link_quality = 0; + rfm22b_dev->stats.rssi = 0; // Initialize the stats. - rfm22b_dev->resets = 0; - rfm22b_dev->timeouts = 0; - rfm22b_dev->errors = 0; rfm22b_dev->tx_packet_count = 0; rfm22b_dev->rx_packet_count = 0; - rfm22b_dev->rssi = 0; - rfm22b_dev->rssi_dBm = -127; // Bind the configuration to the device instance rfm22b_dev->cfg = *cfg; @@ -697,7 +700,7 @@ static void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pio } /** - * Returns the unique device ID for th RFM22B device. + * Returns the unique device ID for the RFM22B device. * \param[in] rfm22b_id The RFM22B device index. * \return The unique device ID */ @@ -726,36 +729,52 @@ void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id) rfm22b_dev->destination_id = (dest_id == 0) ? 0xffffffff : dest_id; } -uint16_t PIOS_RFM22B_Resets(uint32_t rfm22b_id) -{ +/** + * Returns the device statistics RFM22B device. + * \param[in] rfm22b_id The RFM22B device index. + * \param[out] stats The stats are returned in this structure + */ +void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if(!PIOS_RFM22B_validate(rfm22b_dev)) - return 0; - return rfm22b_dev->resets; -} + return; + *stats = rfm22b_dev->stats; -uint16_t PIOS_RFM22B_Timeouts(uint32_t rfm22b_id) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_validate(rfm22b_dev)) - return 0; - return rfm22b_dev->timeouts; -} + // Add the RX packet statistics + stats->rx_count = rfm22b_dev->prev_rx_seq_num; + stats->tx_count = rfm22b_dev->tx_packet_count; + stats->rx_good = 0; + stats->rx_corrected = 0; + stats->rx_error = 0; + stats->rx_missed = 0; + for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) + { + uint32_t val = rfm22b_dev->rx_packet_stats[i]; + for (uint8_t j = 0; j < 16; ++j) + { + switch ((val >> (j * 2)) & 0x3) + { + case RFM22B_GOOD_RX_PACKET: + stats->rx_good++; + break; + case RFM22B_CORRECTED_RX_PACKET: + stats->rx_corrected++; + break; + case RFM22B_ERROR_RX_PACKET: + stats->rx_error++; + break; + case RFM22B_MISSED_RX_PACKET: + stats->rx_missed++; + break; + } + } + } -uint8_t PIOS_RFM22B_LinkQuality(uint32_t rfm22b_id) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_validate(rfm22b_dev)) - return 0; - return rfm22b_dev->slow_link_quality; -} - -int8_t PIOS_RFM22B_RSSI(uint32_t rfm22b_id) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_validate(rfm22b_dev)) - return 0; - return rfm22b_dev->rssi_dBm; + // Calculate the link quality metric, which is related to the number of good packets in relation to the number of bad packets. + // Note: This assumes that the number of packets sampled for the stats is 64. + // Using this equation, error and missed packets are counted as -2, and corrected packets are counted as -1. + // The rage is 0 (all error or missed packets) to 128 (all good packets). + stats->link_quality = 64 + stats->rx_good - stats->rx_error - stats->rx_missed; } static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail) @@ -767,6 +786,24 @@ static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail) } +static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) +{ + struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + bool valid = PIOS_RFM22B_validate(rfm22b_dev); + PIOS_Assert(valid); + +#ifdef NEVER + // Get some data to send + bool need_yield = false; + if(tx_pre_buffer_size == 0) + tx_pre_buffer_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, tx_pre_buffer, + TX_BUFFER_SIZE, NULL, &need_yield); + + // Inject a send packet event + PIOS_RFM22B_InjectEvent(g_rfm22b_dev, RFM22B_EVENT_TX_START, false); +#endif +} + /** * Insert a packet on the packet queue for sending. * Note: If this finction succedds, the packet will be released by the driver, so no release is necessary. @@ -820,6 +857,14 @@ uint32_t PIOS_RFM22B_Receive_Packet(uint32_t rfm22b_id, PHPacketHandle *pret, ui { PHPacketHandle p = rfm22b_dev->rx_packet_prev; uint16_t len = PHPacketSizeECC(p); + uint16_t seq_num = p->header.seq_num; + uint16_t prev_seq_num = rfm22b_dev->prev_rx_seq_num; + uint16_t missed_packets = ((seq_num < prev_seq_num) ? (0xffff - prev_seq_num + seq_num) : (seq_num - prev_seq_num)) - 1; + rfm22b_dev->prev_rx_seq_num = seq_num; + + // Add any missed packets into the stats. + for ( ; missed_packets > 0; --missed_packets) + rfm22b_add_rx_status(rfm22b_dev, RFM22B_MISSED_RX_PACKET); // Attempt to correct any errors in the packet. decode_data((unsigned char*)p, len); @@ -833,43 +878,24 @@ uint32_t PIOS_RFM22B_Receive_Packet(uint32_t rfm22b_id, PHPacketHandle *pret, ui if (correct_errors_erasures((unsigned char*)p, len, 0, 0) == 0) { // We couldn't correct the error, so drop the packet. - rfm22b_dev->fast_error_packets++; + rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET); PHReleaseRXPacket(pios_packet_handler, p); } else { // We corrected the error. - rfm22b_dev->fast_corrected_packets++; + rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET); rx_error = false; } - } + } else + rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET); // Return the packet if there were not uncorrectable errors. if (!rx_error) { - rfm22b_dev->fast_good_packets++; *pret = p; rx_len = rfm22b_dev->rx_packet_len; - - // Update the link statistics if necessary. - uint8_t fast_block = (p->header.seq_num >> 2) & 0xff; - uint8_t slow_block = (p->header.seq_num >> 4) & 0xff; - if (rfm22b_dev->fast_block != fast_block) - { - rfm22b_dev->fast_link_quality = (uint8_t)(((4 + (uint16_t)rfm22b_dev->fast_good_packets - rfm22b_dev->fast_error_packets) << 5) - 1); - rfm22b_dev->slow_good_packets += rfm22b_dev->fast_good_packets; - rfm22b_dev->slow_corrected_packets += rfm22b_dev->fast_corrected_packets; - rfm22b_dev->slow_error_packets += rfm22b_dev->fast_error_packets; - rfm22b_dev->fast_good_packets = rfm22b_dev->fast_corrected_packets = rfm22b_dev->fast_error_packets = 0; - rfm22b_dev->fast_block = fast_block; - } - if (rfm22b_dev->slow_block != slow_block) - { - rfm22b_dev->slow_link_quality = (uint8_t)(((16 + (uint16_t)rfm22b_dev->slow_good_packets - rfm22b_dev->slow_error_packets) << 3) - 1); - rfm22b_dev->slow_good_packets = rfm22b_dev->slow_corrected_packets = rfm22b_dev->slow_error_packets = 0; - rfm22b_dev->slow_block = slow_block; - } } rfm22b_dev->rx_packet_prev = NULL; } @@ -962,24 +988,6 @@ static void PIOS_RFM22B_Task(void *parameters) } } -static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) -{ - struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - bool valid = PIOS_RFM22B_validate(rfm22b_dev); - PIOS_Assert(valid); - -#ifdef NEVER - // Get some data to send - bool need_yield = false; - if(tx_pre_buffer_size == 0) - tx_pre_buffer_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, tx_pre_buffer, - TX_BUFFER_SIZE, NULL, &need_yield); - - // Inject a send packet event - PIOS_RFM22B_InjectEvent(g_rfm22b_dev, RFM22B_EVENT_TX_START, false); -#endif -} - /** * Changes the baud rate of the RFM22B peripheral without re-initialising. * \param[in] rfm22b_id RFM22B name (GPS, TELEM, AUX) @@ -1431,11 +1439,6 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); - rfm22b_dev->status_packet.errors = rfm22b_dev->errors; - rfm22b_dev->status_packet.resets = rfm22b_dev->resets; - rfm22b_dev->status_packet.retries = 0; - rfm22b_dev->status_packet.uavtalk_errors = 0; - rfm22b_dev->status_packet.dropped = 0; if (xQueueSend(rfm22b_dev->packetQueue, &sph, 0) != pdTRUE) return false; @@ -1447,8 +1450,6 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) return true; } -// ************************************ - /** * Read the RFM22B interrupt and device status registers * \param[in] rfm22b_dev The device structure @@ -1482,6 +1483,21 @@ static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev) return true; } +/** + * Add a status value to the RX packet status array. + * \param[in] rfm22b_dev The device structure + * \param[in] status The packet status value + */ +static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status) +{ + // Shift the status registers + for (uint8_t i = RFM22B_RX_PACKET_STATS_LEN - 1; i > 0; --i) + { + rfm22b_dev->rx_packet_stats[i] = (rfm22b_dev->rx_packet_stats[i] << 2) | (rfm22b_dev->rx_packet_stats[i - 1] >> 30); + } + rfm22b_dev->rx_packet_stats[0] = (rfm22b_dev->rx_packet_stats[0] << 2) | status; +} + static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev) { @@ -1524,9 +1540,9 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de rfm22b_dev->afc_correction_Hz = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f); // read rx signal strength .. 45 = -100dBm, 205 = -20dBm - rfm22b_dev->rssi = rfm22_read(RFM22_rssi); + uint8_t rssi = rfm22_read(RFM22_rssi); // convert to dBm - rfm22b_dev->rssi_dBm = (int8_t)(rfm22b_dev->rssi >> 1) - 122; + rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122; // remember the afc value for this packet rfm22b_dev->rx_packet_start_afc_Hz = rfm22b_dev->afc_correction_Hz; diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 901d86788..7d719f9ed 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -77,16 +77,28 @@ enum rfm22b_datarate { RFM22_datarate_256000 = 13, }; +struct rfm22b_stats { + uint16_t packets_per_sec; + uint16_t tx_count; + uint16_t rx_count; + uint8_t rx_good; + uint8_t rx_corrected; + uint8_t rx_error; + uint8_t rx_missed; + uint8_t tx_dropped; + uint8_t resets; + uint8_t timeouts; + uint8_t link_quality; + int8_t rssi; +}; + /* 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, 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 uint16_t PIOS_RFM22B_Resets(uint32_t rfm22b_id); -extern uint16_t PIOS_RFM22B_Timeouts(uint32_t rfm22b_id); -extern uint8_t PIOS_RFM22B_LinkQuality(uint32_t rfm22b_id); -extern int8_t PIOS_RFM22B_RSSI(uint32_t rfm22b_id); +extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); extern bool PIOS_RFM22B_Send_Packet(uint32_t rfm22b_id, PHPacketHandle p, uint32_t max_delay); extern uint32_t PIOS_RFM22B_Receive_Packet(uint32_t rfm22b_id, PHPacketHandle *p, uint32_t max_delay); diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index d6bd1b3fe..ded7984e0 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -68,17 +68,16 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("PipXSettings", "FrequencyCalibration", m_pipx->FrequencyCalibration); addUAVObjectToWidgetRelation("PipXSettings", "Frequency", m_pipx->Frequency); - addUAVObjectToWidgetRelation("PipXStatus", "MinFrequency", m_pipx->MinFrequency); - addUAVObjectToWidgetRelation("PipXStatus", "MaxFrequency", m_pipx->MaxFrequency); - addUAVObjectToWidgetRelation("PipXStatus", "FrequencyStepSize", m_pipx->FrequencyStepSize); - addUAVObjectToWidgetRelation("PipXStatus", "FrequencyBand", m_pipx->FreqBand); - addUAVObjectToWidgetRelation("PipXStatus", "RSSI", m_pipx->RSSI); - addUAVObjectToWidgetRelation("PipXStatus", "AFC", m_pipx->RxAFC); - addUAVObjectToWidgetRelation("PipXStatus", "Retries", m_pipx->Retries); - addUAVObjectToWidgetRelation("PipXStatus", "LinkQuality", m_pipx->LinkQuality); + addUAVObjectToWidgetRelation("PipXStatus", "RxGood", m_pipx->Good); + addUAVObjectToWidgetRelation("PipXStatus", "RxCorrected", m_pipx->Corrected); + addUAVObjectToWidgetRelation("PipXStatus", "RxErrors", m_pipx->Errors); + addUAVObjectToWidgetRelation("PipXStatus", "RxMissed", m_pipx->Missed); addUAVObjectToWidgetRelation("PipXStatus", "UAVTalkErrors", m_pipx->UAVTalkErrors); + addUAVObjectToWidgetRelation("PipXStatus", "TxDropped", m_pipx->Dropped); addUAVObjectToWidgetRelation("PipXStatus", "Resets", m_pipx->Resets); - addUAVObjectToWidgetRelation("PipXStatus", "Dropped", m_pipx->Dropped); + addUAVObjectToWidgetRelation("PipXStatus", "Timeouts", m_pipx->Timeouts); + addUAVObjectToWidgetRelation("PipXStatus", "RSSI", m_pipx->RSSI); + addUAVObjectToWidgetRelation("PipXStatus", "LinkQuality", m_pipx->LinkQuality); addUAVObjectToWidgetRelation("PipXStatus", "RXRate", m_pipx->RXRate); addUAVObjectToWidgetRelation("PipXStatus", "TXRate", m_pipx->TXRate); @@ -97,7 +96,6 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget // Request and update of the setting object. settingsUpdated = false; - //pipxSettingsObj->requestUpdate(); disableMouseWheelEvents(); } @@ -182,15 +180,15 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) } UAVObjectField* pairRssiField = object->getField("PairSignalStrengths"); if (pairRssiField) { - m_pipx->PairSignalStrengthBar1->setValue(pairRssiField->getValue(0).toInt()); - m_pipx->PairSignalStrengthBar2->setValue(pairRssiField->getValue(1).toInt()); - m_pipx->PairSignalStrengthBar3->setValue(pairRssiField->getValue(2).toInt()); - m_pipx->PairSignalStrengthBar4->setValue(pairRssiField->getValue(3).toInt()); - m_pipx->PairSignalStrengthLabel1->setText(QString("%1dB").arg(pairRssiField->getValue(0).toInt())); - m_pipx->PairSignalStrengthLabel2->setText(QString("%1dB").arg(pairRssiField->getValue(1).toInt())); - m_pipx->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); - m_pipx->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); - } else { + m_pipx->PairSignalStrengthBar1->setValue(pairRssiField->getValue(0).toInt()); + m_pipx->PairSignalStrengthBar2->setValue(pairRssiField->getValue(1).toInt()); + m_pipx->PairSignalStrengthBar3->setValue(pairRssiField->getValue(2).toInt()); + m_pipx->PairSignalStrengthBar4->setValue(pairRssiField->getValue(3).toInt()); + m_pipx->PairSignalStrengthLabel1->setText(QString("%1dB").arg(pairRssiField->getValue(0).toInt())); + m_pipx->PairSignalStrengthLabel2->setText(QString("%1dB").arg(pairRssiField->getValue(1).toInt())); + m_pipx->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); + m_pipx->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); + } else { qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; } @@ -269,10 +267,10 @@ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) Q_UNUSED(object); if (!settingsUpdated) - { - settingsUpdated = true; - enableControls(true); - } + { + settingsUpdated = true; + enableControls(true); + } } void ConfigPipXtremeWidget::disconnected() diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 6abe1a830..25461372b 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -434,7 +434,7 @@ - + Serial Number @@ -444,7 +444,7 @@ - + @@ -498,7 +498,7 @@ - + Device ID @@ -508,7 +508,7 @@ - + @@ -552,7 +552,7 @@ - + Pair ID @@ -562,7 +562,7 @@ - + @@ -601,9 +601,9 @@ - + - Min Frequency + Link State Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -611,9 +611,9 @@ - + - + 0 0 @@ -637,13 +637,13 @@ - The modems minimum allowed frequency + The modems current state QLineEdit { border: none; border-radius: 1px; - padding: 0 4px; + padding: 0 3px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ @@ -655,63 +655,15 @@ true - - - - - - Max Frequency - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - The modems maximum allowed frequency - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true + + Disconnected - + - Freq. Step Size + Link Quality Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -719,19 +671,7 @@ - - - - 0 - 0 - - - - - 0 - 0 - - + 101 @@ -744,9 +684,6 @@ true - - The modems minimum frequency step size - QLineEdit { border: none; @@ -757,59 +694,8 @@ /* selection-background-color: darkgray;*/ } - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - Freq. Band - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - The current frequency band - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + false true @@ -861,7 +747,7 @@ - + Rx AFC @@ -871,7 +757,7 @@ - + @@ -900,7 +786,7 @@ - + TX Rate (B/s) @@ -910,7 +796,7 @@ - + @@ -948,7 +834,7 @@ - + RX Rate (B/s) @@ -958,7 +844,7 @@ - + @@ -990,20 +876,20 @@ - - + + - Link State + RX Good Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - + + - + 0 0 @@ -1027,52 +913,7 @@ - The modems current state - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 3px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - Disconnected - - - - - - - Link Quality - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 101 - 16777215 - - - - - 75 - true - + The percentage of packets that were corrected with error correction QLineEdit { @@ -1084,26 +925,32 @@ /* selection-background-color: darkgray;*/ } - - false + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - - + + - Retries + RX Corrected Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - + + + + + 0 + 0 + + 0 @@ -1122,6 +969,9 @@ true + + The percentage of packets that were corrected with error correction + QLineEdit { border: none; @@ -1132,15 +982,174 @@ /* selection-background-color: darkgray;*/ } - - false + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter true - + + + + RX Errors + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + The percentage of packets that could not be corrected with error correction + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + RX Missed + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + The percentage of packets that were not received at all + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + TX Dropped + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + The number of packets that were unable to be transmitted + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + UAVTalk Errors @@ -1150,7 +1159,7 @@ - + @@ -1182,7 +1191,7 @@ - + Resets @@ -1192,7 +1201,7 @@ - + @@ -1230,18 +1239,18 @@ - - + + - Dropped + Timeouts Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - + + 101 @@ -1747,9 +1756,6 @@ FirmwareVersion SerialNumber DeviceID - MinFrequency - MaxFrequency - FrequencyStepSize LinkState RxAFC Retries diff --git a/shared/uavobjectdefinition/pipxstatus.xml b/shared/uavobjectdefinition/pipxstatus.xml index bbdc656c8..88ea3d3b3 100755 --- a/shared/uavobjectdefinition/pipxstatus.xml +++ b/shared/uavobjectdefinition/pipxstatus.xml @@ -5,19 +5,19 @@ - - - - - + + + + - - - - + + + + + From 0113b6a74894f812d383952cfc8424239b8f4e45 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 20 Oct 2012 14:24:29 -0400 Subject: [PATCH 02/31] Added UAVTalkRelayInputStream to the UAVTalk library that parses a UAVTalk packet and sends it when it is complete. This allows the interlieving of packets on an output UAVTalk stream, and is used by the OPLink device to relay packets from an input com port to a different output com port without sending one packet in the middle of another packet. --- flight/UAVTalk/inc/uavtalk.h | 1 + flight/UAVTalk/uavtalk.c | 67 ++++++++++++++++++++++++++++++++++++ 2 files changed, 68 insertions(+) diff --git a/flight/UAVTalk/inc/uavtalk.h b/flight/UAVTalk/inc/uavtalk.h index f7fc58cd1..a11979a95 100644 --- a/flight/UAVTalk/inc/uavtalk.h +++ b/flight/UAVTalk/inc/uavtalk.h @@ -59,6 +59,7 @@ int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId); int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len); UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte); UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte); +UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats); void UAVTalkResetStats(UAVTalkConnection connection); void UAVTalkGetLastTimestamp(UAVTalkConnection connection, uint16_t *timestamp); diff --git a/flight/UAVTalk/uavtalk.c b/flight/UAVTalk/uavtalk.c index b2f8515c8..1f25cb767 100644 --- a/flight/UAVTalk/uavtalk.c +++ b/flight/UAVTalk/uavtalk.c @@ -558,6 +558,73 @@ UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uin return state; } +/** + * Process an byte from the telemetry stream, sending the packet out the output stream when it's complete + * This allows the interlieving of packets on an output UAVTalk stream, and is used by the OPLink device to + * relay packets from an input com port to a different output com port without sending one packet in the middle + * of another packet. Because this uses both the receive buffer and transmit buffer, it should only be used + * for relaying a packet, not for the standard sending and receiving of packets. + * \param[in] connection UAVTalkConnection to be used + * \param[in] rxbyte Received byte + * \return UAVTalkRxState + */ +UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) +{ + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); + + if (state == UAVTALK_STATE_COMPLETE) + { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle,connection,return -1); + UAVTalkInputProcessor *iproc = &connection->iproc; + + if (!connection->outStream) return -1; + + // Setup type and object id fields + connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + connection->txBuffer[1] = iproc->type; + // data length inserted here below + connection->txBuffer[4] = (uint8_t)(iproc->objId & 0xFF); + connection->txBuffer[5] = (uint8_t)((iproc->objId >> 8) & 0xFF); + connection->txBuffer[6] = (uint8_t)((iproc->objId >> 16) & 0xFF); + connection->txBuffer[7] = (uint8_t)((iproc->objId >> 24) & 0xFF); + + // Setup instance ID if one is required + int32_t dataOffset = 8; + if (iproc->instanceLength > 0) + { + connection->txBuffer[8] = (uint8_t)(iproc->instId & 0xFF); + connection->txBuffer[9] = (uint8_t)((iproc->instId >> 8) & 0xFF); + dataOffset = 10; + } + + // Add timestamp when the transaction type is appropriate + if (iproc->type & UAVTALK_TIMESTAMPED) + { + portTickType time = xTaskGetTickCount(); + connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); + connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); + dataOffset += 2; + } + + // Copy data (if any) + memcpy(&connection->txBuffer[dataOffset], connection->rxBuffer, iproc->length); + + // Store the packet length + connection->txBuffer[2] = (uint8_t)((dataOffset + iproc->length) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((dataOffset + iproc->length) >> 8) & 0xFF); + + // Copy the checksum + connection->txBuffer[dataOffset + iproc->length] = iproc->cs; + + // Send the buffer. + if (UAVTalkSendBuf(connectionHandle, connection->txBuffer, iproc->rxPacketLength) < 0) + return UAVTALK_STATE_ERROR; + } + + return state; +} + /** * Send a ACK through the telemetry link. * \param[in] connectionHandle UAVTalkConnection to be used From dae382564a200b7ce6ca8166b4c769c665c7461b Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 20 Oct 2012 14:48:22 -0400 Subject: [PATCH 03/31] RFM22B/RM: Added rfm22b com device, moved remaining functionality in the radio module to the rfm22 driver, and simplified configuration of the PipX/OPLink. The radio device now presents itself completely as a com device, both in RadioComBridge (OPLink devices) and Telemetry (RM). Also change the PipXStatus and PipXSettings UAVOs to OPLinkStatus/OPLinkSettings. --- flight/Libraries/inc/packet_handler.h | 25 +- flight/Libraries/packet_handler.c | 50 - flight/Modules/PipXtreme/pipxtrememod.c | 71 +- flight/Modules/Radio/radio.c | 454 --------- .../Modules/RadioComBridge/RadioComBridge.c | 930 +++++------------- flight/Modules/Telemetry/telemetry.c | 41 +- .../PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 22 +- flight/PiOS/Boards/STM32F4xx_RevoMini.h | 5 +- flight/PiOS/Common/pios_rfm22b.c | 584 +++++------ flight/PiOS/Common/pios_rfm22b_com.c | 141 +++ flight/PiOS/inc/pios_rfm22b.h | 9 +- .../radio.h => PiOS/inc/pios_rfm22b_com.h} | 44 +- flight/PiOS/inc/pios_rfm22b_priv.h | 184 +++- flight/PipXtreme/Makefile | 7 +- flight/PipXtreme/System/inc/pios_config.h | 2 +- flight/PipXtreme/System/pios_board.c | 270 +++-- flight/RevoMini/Makefile | 2 +- flight/RevoMini/System/pios_board.c | 90 +- flight/RevoMini/UAVObjects.inc | 5 +- .../src/plugins/config/configgadgetwidget.cpp | 40 +- .../src/plugins/config/configgadgetwidget.h | 18 +- .../plugins/config/configpipxtremewidget.cpp | 208 ++-- .../plugins/config/configpipxtremewidget.h | 10 +- .../src/plugins/config/pipxtreme.ui | 217 ++-- .../src/plugins/uavobjects/uavobjects.pro | 8 +- .../src/plugins/uavtalk/telemetry.cpp | 4 +- shared/uavobjectdefinition/hwsettings.xml | 4 +- shared/uavobjectdefinition/oplinksettings.xml | 23 + .../{pipxstatus.xml => oplinkstatus.xml} | 5 +- shared/uavobjectdefinition/pipxsettings.xml | 24 - 30 files changed, 1385 insertions(+), 2112 deletions(-) delete mode 100644 flight/Modules/Radio/radio.c create mode 100644 flight/PiOS/Common/pios_rfm22b_com.c rename flight/{Modules/Radio/inc/radio.h => PiOS/inc/pios_rfm22b_com.h} (67%) create mode 100644 shared/uavobjectdefinition/oplinksettings.xml rename shared/uavobjectdefinition/{pipxstatus.xml => oplinkstatus.xml} (90%) delete mode 100644 shared/uavobjectdefinition/pipxsettings.xml diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 736d78710..789807a04 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -40,19 +40,14 @@ // Public types typedef enum { PACKET_TYPE_NONE = 0, - PACKET_TYPE_CONNECT, // for requesting a connection - PACKET_TYPE_DISCONNECT, // to tell the other modem they cannot connect to us - PACKET_TYPE_READY, // tells the other modem we are ready to accept more data - PACKET_TYPE_NOTREADY, // tells the other modem we're not ready to accept more data - we can also send user data in this packet type PACKET_TYPE_STATUS, // broadcasts status of this modem - PACKET_TYPE_DATARATE, // for changing the RF data rate - PACKET_TYPE_PING, // used to check link is still up - PACKET_TYPE_ADJUST_TX_PWR, // used to ask the other modem to adjust it's tx power + PACKET_TYPE_CON_REQUEST, // request a connection to another modem + PACKET_TYPE_CON_ACCEPT, // accecpt a connection to another modem + PACKET_TYPE_CON_DECLINE, // decline a connection to another modem PACKET_TYPE_DATA, // data packet (packet contains user data) - PACKET_TYPE_ACKED_DATA, // data packet that requies an ACK PACKET_TYPE_PPM, // PPM relay values PACKET_TYPE_ACK, - PACKET_TYPE_NACK + PACKET_TYPE_NACK, } PHPacketType; typedef struct { @@ -85,6 +80,17 @@ typedef struct { uint8_t ecc[RS_ECC_NPARITY]; } PHStatusPacket, *PHStatusPacketHandle; +#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) +typedef struct { + PHPacketHeader header; + uint32_t datarate; + uint32_t frequencyHz; + uint32_t minFrequencyHz; + uint32_t maxFrequencyHz; + uint8_t maxTxPower; + uint8_t ecc[RS_ECC_NPARITY]; +} PHConnectionPacket, *PHConnectionPacketHandle; + typedef struct { uint32_t default_destination_id; uint32_t source_id; @@ -112,7 +118,6 @@ PHPacketHandle PHGetTXPacket(PHInstHandle h); void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p); uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p); uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len); -uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p); #endif // __PACKET_HANDLER_H__ diff --git a/flight/Libraries/packet_handler.c b/flight/Libraries/packet_handler.c index c2b54c54f..b417100de 100644 --- a/flight/Libraries/packet_handler.c +++ b/flight/Libraries/packet_handler.c @@ -314,56 +314,6 @@ uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len) return PHLTransmitPacket(data, p); } -/** - * Process a packet that has been received. - * \param[in] h The packet handler instance data pointer. - * \param[in] p A pointer to the packet buffer. - * \param[in] received_len The length of data received. - * \return 0 Failure - * \return 1 Success - */ -uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - uint16_t len = PHPacketSizeECC(p); - - // Extract the RSSI and AFC. - int8_t rssi = *(((int8_t*)p) + len); - int8_t afc = *(((int8_t*)p) + len + 1); - - switch (p->header.type) { - - case PACKET_TYPE_STATUS: - - // Pass on the channels to the status handler. - if(data->status_handler) - data->status_handler((PHStatusPacketHandle)p, rssi, afc); - break; - - case PACKET_TYPE_PPM: - - // Pass on the channels to the PPM handler. - if(data->ppm_handler) - data->ppm_handler(((PHPpmPacketHandle)p)->channels); - break; - - case PACKET_TYPE_DATA: - - // Pass on the data to the data handler. - if(data->data_handler) - data->data_handler(p->data, p->header.data_size, rssi, afc); - break; - - default: - break; - } - - // Release the packet. - PHReleaseRXPacket(h, p); - - return 1; -} - /** * Transmit a packet from the transmit packet buffer window. * \param[in] data The packet handler instance data pointer. diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c index 5165e1781..8402574d7 100644 --- a/flight/Modules/PipXtreme/pipxtrememod.c +++ b/flight/Modules/PipXtreme/pipxtrememod.c @@ -39,8 +39,10 @@ */ #include -#include +#include +#include #include +#include #include "systemmod.h" // Private constants @@ -94,20 +96,20 @@ int32_t PipXtremeModInitialize(void) // Must registers objects here for system thread because ObjectManager started in OpenPilotInit // Initialize out status object. - PipXStatusInitialize(); - PipXStatusData pipxStatus; - PipXStatusGet(&pipxStatus); + OPLinkStatusInitialize(); + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); // Get our hardware information. const struct pios_board_info * bdinfo = &pios_board_info_blob; - pipxStatus.BoardType= bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(pipxStatus.Description, PIPXSTATUS_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(pipxStatus.CPUSerial); - pipxStatus.BoardRevision= bdinfo->board_rev; + oplinkStatus.BoardType= bdinfo->board_type; + PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); + oplinkStatus.BoardRevision= bdinfo->board_rev; // Update the object - PipXStatusSet(&pipxStatus); + OPLinkStatusSet(&oplinkStatus); // Call the module start function. PipXtremeModStart(); @@ -123,6 +125,9 @@ MODULE_INITCALL(PipXtremeModInitialize, 0) static void systemTask(void *parameters) { portTickType lastSysTime; + uint16_t prev_tx_count = 0; + uint16_t prev_rx_count = 0; + bool first_time = true; /* create all modules thread */ MODULE_TASKCREATE_ALL; @@ -148,6 +153,54 @@ static void systemTask(void *parameters) PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); #endif /* PIOS_LED_HEARTBEAT */ + // Update the PipXstatus UAVO + OPLinkStatusData oplinkStatus; + uint32_t pairID; + OPLinkStatusGet(&oplinkStatus); + OPLinkSettingsPairIDGet(&pairID); + + // Get the other device stats. + PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); + + // Get the stats from the radio device + struct rfm22b_stats radio_stats; + PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); + + // Update the status + oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); + //oplinkStatus.UAVTalkErrors = data->UAVTalkErrors; + oplinkStatus.RxGood = radio_stats.rx_good; + oplinkStatus.RxCorrected = radio_stats.rx_corrected; + oplinkStatus.RxErrors = radio_stats.rx_error; + oplinkStatus.RxMissed = radio_stats.rx_missed; + oplinkStatus.TxDropped = radio_stats.tx_dropped; // + data->droppedPackets; + oplinkStatus.Resets = radio_stats.resets; + oplinkStatus.Timeouts = radio_stats.timeouts; + oplinkStatus.RSSI = radio_stats.rssi; + oplinkStatus.AFCCorrection = radio_stats.afc_correction; + oplinkStatus.LinkQuality = radio_stats.link_quality; + if (first_time) + first_time = false; + else + { + uint16_t tx_count = radio_stats.tx_byte_count; + uint16_t rx_count = radio_stats.rx_byte_count; + uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); + uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); + oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + prev_tx_count = tx_count; + prev_rx_count = rx_count; + } + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED; + if (radio_stats.connected) + LINK_LED_ON; + else + LINK_LED_OFF; + + // Update the object + OPLinkStatusSet(&oplinkStatus); + // Wait until next period vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS); } diff --git a/flight/Modules/Radio/radio.c b/flight/Modules/Radio/radio.c deleted file mode 100644 index baaaf3814..000000000 --- a/flight/Modules/Radio/radio.c +++ /dev/null @@ -1,454 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Radio Input / Output Module - * @brief Read and Write packets from/to a radio device. - * @{ - * - * @file radio.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Bridges selected Com Port to the COM VCP emulated serial port - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// **************** -// Private constants - -#define STACK_SIZE_BYTES 200 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 2) -#define PACKET_QUEUE_SIZE PIOS_PH_WIN_SIZE -#define MAX_PORT_DELAY 200 -#define STATS_UPDATE_PERIOD_MS 500 -#define RADIOSTATS_UPDATE_PERIOD_MS 250 -#define MAX_LOST_CONTACT_TIME 4 -#define PACKET_MAX_DELAY 50 - -#ifndef LINK_LED_ON -#define LINK_LED_ON -#define LINK_LED_OFF -#endif - -// **************** -// Private types - -typedef struct { - uint32_t pairID; - int8_t rssi; - uint8_t lastContact; -} PairStats; - -typedef struct { - - // The task handles. - xTaskHandle radioReceiveTaskHandle; - xTaskHandle radioStatusTaskHandle; - - // Queue handles. - xQueueHandle radioPacketQueue; - - // Error statistics. - uint32_t radioTxErrors; - uint32_t radioRxErrors; - uint16_t txBytes; - uint16_t rxBytes; - - // External error statistics - uint32_t droppedPackets; - uint32_t comTxRetries; - uint32_t UAVTalkErrors; - - // The destination ID - uint32_t destination_id; - - // Track other radios that are in range. - PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM]; - - // The RSSI of the last packet received. - int8_t RSSI; - -} RadioData; - -// **************** -// Private functions - -static void radioReceiveTask(void *parameters); -static void radioStatusTask(void *parameters); -static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc); -static int32_t transmitPacket(PHPacketHandle packet); -static void PPMHandler(uint16_t *channels); - -// **************** -// Private variables - -static RadioData *data = 0; - -// **************** -// Global variables -uint32_t pios_rfm22b_id = 0; -uint32_t pios_com_rfm22b_id = 0; -uint32_t pios_packet_handler = 0; -const struct pios_rfm22b_cfg *pios_rfm22b_cfg; - -// *************** -// External functions -extern const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision); - -/** - * Start the module - * \return -1 if initialisation failed - * \return 0 on success - */ -static int32_t RadioStart(void) -{ - if (!data) - return -1; - - // Start the tasks. - xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioReceiveTaskHandle)); - xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES * 2, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle)); - - // Install the monitors - TaskMonitorAdd(TASKINFO_RUNNING_MODEMRX, data->radioReceiveTaskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_MODEMSTAT, data->radioStatusTaskHandle); - - // Register the watchdog timers. -#ifdef PIOS_WDG_RADIORECEIVE - PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE); -#endif /* PIOS_WDG_RADIORECEIVE */ - - return 0; -} - - -/** - * Initialise the module - * \return -1 if initialisation failed - * \return 0 on success - */ -static int32_t RadioInitialize(void) -{ - - // See if this module is enabled. -#ifndef RADIO_BUILTIN - HwSettingsInitialize(); - uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; - HwSettingsOptionalModulesGet(optionalModules); - if (optionalModules[HWSETTINGS_OPTIONALMODULES_RADIO] != HWSETTINGS_OPTIONALMODULES_ENABLED) { - pios_packet_handler = 0; - return -1; - } -#endif - - // Initalize out UAVOs - PipXSettingsInitialize(); - PipXStatusInitialize(); - - PipXSettingsData pipxSettings; - PipXSettingsGet(&pipxSettings); - - /* Retrieve hardware settings. */ - const struct pios_board_info * bdinfo = &pios_board_info_blob; - pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); - - /* 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 - .source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id), - .win_size = PIOS_PH_WIN_SIZE, - .max_connections = PIOS_PH_MAX_CONNECTIONS, - }; - pios_packet_handler = PHInitialize(&pios_ph_cfg); - - // allocate and initialize the static data storage only if module is enabled - data = (RadioData *)pvPortMalloc(sizeof(RadioData)); - if (!data) - return -1; - - // Initialize the statistics. - data->radioTxErrors = 0; - data->radioRxErrors = 0; - data->droppedPackets = 0; - data->comTxRetries = 0; - data->UAVTalkErrors = 0; - data->RSSI = -127; - - // Initialize the detected device statistics. - for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i) - { - data->pairStats[i].pairID = 0; - data->pairStats[i].rssi = -127; - data->pairStats[i].lastContact = 0; - } - // The first slot is reserved for our current pairID - PipXSettingsPairIDGet(&(data->pairStats[0].pairID)); - data->destination_id = data->pairStats[0].pairID ? data->pairStats[0].pairID : 0xffffffff; - - // Register the callbacks with the packet handler - PHRegisterStatusHandler(pios_packet_handler, StatusHandler); - PHRegisterOutputStream(pios_packet_handler, transmitPacket); - PHRegisterPPMHandler(pios_packet_handler, PPMHandler); - - return 0; -} -MODULE_INITCALL(RadioInitialize, RadioStart) - -/** - * The task that receives packets from the radio. - */ -static void radioReceiveTask(void *parameters) -{ - PHPacketHandle p = NULL; - - /* Handle radio -> usart/usb direction */ - while (1) { - uint32_t rx_bytes; - -#ifdef PIOS_WDG_RADIORECEIVE - // Update the watchdog timer. - PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE); -#endif /* PIOS_INCLUDE_WDG */ - - // Receive data from the radio port - p = NULL; - rx_bytes = PIOS_RFM22B_Receive_Packet(pios_rfm22b_id, &p, MAX_PORT_DELAY); - if(rx_bytes == 0) - continue; - data->rxBytes += rx_bytes; - PHReceivePacket(pios_packet_handler, p); - p = NULL; - } -} - -/** - * Transmit a packet to the radio port. - * \param[in] buf Data buffer to send - * \param[in] length Length of buffer - * \return -1 on failure - * \return number of bytes transmitted on success - */ -static int32_t transmitPacket(PHPacketHandle p) -{ - uint16_t len = PH_PACKET_SIZE(p); - data->txBytes += len; - if (!PIOS_RFM22B_Send_Packet(pios_rfm22b_id, p, PACKET_MAX_DELAY)) - return -1; - return len; -} - -/** - * Receive a status packet - * \param[in] status The status structure - */ -static void StatusHandler(PHStatusPacketHandle status, int8_t rssi, int8_t afc) -{ - uint32_t id = status->header.source_id; - bool found = false; - // Have we seen this device recently? - uint8_t id_idx = 0; - for ( ; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx) - if(data->pairStats[id_idx].pairID == id) - { - found = true; - break; - } - - // If we have seen it, update the RSSI and reset the last contact couter - if(found) - { - data->pairStats[id_idx].rssi = rssi; - data->pairStats[id_idx].lastContact = 0; - } - - // If we haven't seen it, find a slot to put it in. - if (!found) - { - uint32_t pairID; - PipXSettingsPairIDGet(&pairID); - - uint8_t min_idx = 0; - if(id != pairID) - { - int8_t min_rssi = data->pairStats[0].rssi; - for (id_idx = 1; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx) - { - if(data->pairStats[id_idx].rssi < min_rssi) - { - min_rssi = data->pairStats[id_idx].rssi; - min_idx = id_idx; - } - } - } - data->pairStats[min_idx].pairID = id; - data->pairStats[min_idx].rssi = rssi; - data->pairStats[min_idx].lastContact = 0; - } -} - -/** - * The stats update task. - */ -static void radioStatusTask(void *parameters) -{ - while (1) { - PipXStatusData pipxStatus; - uint32_t pairID; - - // Get object data - PipXStatusGet(&pipxStatus); - PipXSettingsPairIDGet(&pairID); - - // Get the stats from the radio device - struct rfm22b_stats radio_stats; - PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); - - // Update the status - pipxStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); - pipxStatus.UAVTalkErrors = data->UAVTalkErrors; - pipxStatus.RxGood = radio_stats.rx_good; - pipxStatus.RxCorrected = radio_stats.rx_corrected; - pipxStatus.RxErrors = radio_stats.rx_error; - pipxStatus.RxMissed = radio_stats.rx_missed; - pipxStatus.TxDropped = radio_stats.tx_dropped; // + data->droppedPackets; - pipxStatus.Resets = radio_stats.resets; - pipxStatus.Timeouts = radio_stats.timeouts; - pipxStatus.RSSI = radio_stats.rssi; - pipxStatus.LinkQuality = radio_stats.link_quality; - pipxStatus.TXRate = (uint16_t)((float)(data->txBytes * 1000) / STATS_UPDATE_PERIOD_MS); - data->txBytes = 0; - pipxStatus.RXRate = (uint16_t)((float)(data->rxBytes * 1000) / STATS_UPDATE_PERIOD_MS); - data->rxBytes = 0; - pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_DISCONNECTED; - LINK_LED_OFF; - - // Update the potential pairing contacts - for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i) - { - pipxStatus.PairIDs[i] = data->pairStats[i].pairID; - pipxStatus.PairSignalStrengths[i] = data->pairStats[i].rssi; - data->pairStats[i].lastContact++; - // Remove this device if it's stale. - if(data->pairStats[i].lastContact > MAX_LOST_CONTACT_TIME) - { - data->pairStats[i].pairID = 0; - data->pairStats[i].rssi = -127; - data->pairStats[i].lastContact = 0; - } - // Add the paired devices statistics to ours. - if(pairID && (data->pairStats[i].pairID == pairID) && (data->pairStats[i].rssi > -127)) - { - pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_CONNECTED; - LINK_LED_ON; - } - } - - // Update the object - PipXStatusSet(&pipxStatus); - - vTaskDelay(STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS); - } -} - -/** - * Receive a ppm packet - * \param[in] channels The ppm channels - */ -static void PPMHandler(uint16_t *channels) -{ - GCSReceiverData rcvr; - - // Copy the receiver channels into the GCSReceiver object. - for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i) - rcvr.Channel[i] = channels[i]; - - // Set the GCSReceiverData object. - GCSReceiverSet(&rcvr); -} diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index 5a69f5d25..ba246d44e 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -34,9 +34,9 @@ #include #include #include -#include +#include #include -#include +#include #include #include #include @@ -49,46 +49,28 @@ // **************** // Private constants -#define TEMP_BUFFER_SIZE 25 #define STACK_SIZE_BYTES 150 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define BRIDGE_BUF_LEN 512 #define MAX_RETRIES 2 #define RETRY_TIMEOUT_MS 20 -#define STATS_UPDATE_PERIOD_MS 1000 -#define RADIOSTATS_UPDATE_PERIOD_MS 500 -#define MAX_LOST_CONTACT_TIME 4 -#define PACKET_QUEUE_SIZE 10 +#define EVENT_QUEUE_SIZE 10 #define MAX_PORT_DELAY 200 -#define EV_PACKET_RECEIVED 0x20 -#define EV_TRANSMIT_PACKET 0x30 -#define EV_SEND_ACK 0x40 -#define EV_SEND_NACK 0x50 +#define EV_SEND_ACK 0x20 +#define EV_SEND_NACK 0x30 // **************** // Private types -typedef struct { - uint32_t comPort; - UAVTalkConnection UAVTalkCon; - xQueueHandle sendQueue; - xQueueHandle recvQueue; - uint16_t wdg; - bool isGCS; -} UAVTalkComTaskParams; - typedef struct { // The task handles. - xTaskHandle GCSUAVTalkRecvTaskHandle; - xTaskHandle UAVTalkRecvTaskHandle; - xTaskHandle UAVTalkSendTaskHandle; - xTaskHandle transparentCommTaskHandle; - xTaskHandle ppmInputTaskHandle; + xTaskHandle telemetryTxTaskHandle; + xTaskHandle radioRxTaskHandle; + xTaskHandle radioTxTaskHandle; // The UAVTalk connection on the com side. - UAVTalkConnection UAVTalkCon; - UAVTalkConnection GCSUAVTalkCon; + UAVTalkConnection outUAVTalkCon; + UAVTalkConnection inUAVTalkCon; // Queue handles. xQueueHandle gcsEventQueue; @@ -100,44 +82,19 @@ typedef struct { uint32_t UAVTalkErrors; uint32_t droppedPackets; - // The destination ID - uint32_t destination_id; - - // The packet timeout. - portTickType send_timeout; - uint16_t min_packet_size; - - // The RSSI of the last packet received. - int8_t RSSI; - - // Thread parameters. - UAVTalkComTaskParams uavtalk_params; - UAVTalkComTaskParams gcs_uavtalk_params; - } RadioComBridgeData; -typedef struct { - uint32_t com_port; - uint8_t *buffer; - uint16_t length; - uint16_t index; - uint16_t data_length; -} ReadBuffer, *BufferedReadHandle; - // **************** // Private functions -static void UAVTalkRecvTask(void *parameters); -static void UAVTalkSendTask(void *parameters); -static void transparentCommTask(void * parameters); -static void ppmInputTask(void *parameters); -static int32_t UAVTalkSendHandler(uint8_t * data, int32_t length); -static int32_t GCSUAVTalkSendHandler(uint8_t * data, int32_t length); -static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc); -static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid); -static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length); -static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms); -static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port); +static void telemetryTxTask(void *parameters); +static void radioRxTask(void *parameters); +static void radioTxTask(void *parameters); +static void testRxTask(void *parameters); +static void testTxTask(void *parameters); +static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length); +static int32_t RadioSendHandler(uint8_t *buf, int32_t length); +static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type); static void updateSettings(); @@ -155,37 +112,19 @@ static int32_t RadioComBridgeStart(void) { if(data) { - // Register the callbacks with the packet handler - // This has to happen after the Radio module is initialized. - PHRegisterDataHandler(pios_packet_handler, receiveData); + // Set the baudrates, etc. + updateSettings(); // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. - xTaskCreate(UAVTalkRecvTask, (signed char *)"GCSUAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY + 2, &(data->GCSUAVTalkRecvTaskHandle)); - xTaskCreate(UAVTalkSendTask, (signed char *)"GCSUAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle)); - - // If a UAVTalk (non-GCS) com port is set it implies that the com port is connected on the flight side. - // In this case we want to start another com thread on the HID port to talk to the GCS when connected. - if (PIOS_COM_UAVTALK) + xTaskCreate(telemetryTxTask, (signed char *)"telemTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); + xTaskCreate(radioRxTask, (signed char *)"radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle)); + xTaskCreate(radioTxTask, (signed char *)"radioTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle)); + if (0) { - xTaskCreate(UAVTalkRecvTask, (signed char *)"UAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->uavtalk_params), TASK_PRIORITY + 2, &(data->UAVTalkRecvTaskHandle)); - xTaskCreate(UAVTalkSendTask, (signed char *)"UAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle)); + xTaskCreate(testRxTask, (signed char *)"testRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle)); + xTaskCreate(testTxTask, (signed char *)"testTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle)); } - // Start the tasks - if(PIOS_COM_TRANS_COM) - xTaskCreate(transparentCommTask, (signed char *)"transparentComm", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->transparentCommTaskHandle)); - if(PIOS_PPM_RECEIVER) - xTaskCreate(ppmInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->ppmInputTaskHandle)); - -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_COMGCS); - if(PIOS_COM_UAVTALK) - PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK); - if(PIOS_COM_TRANS_COM) - PIOS_WDG_RegisterFlag(PIOS_WDG_TRANSCOMM); - if(PIOS_PPM_RECEIVER) - PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT); -#endif return 0; } @@ -207,315 +146,48 @@ static int32_t RadioComBridgeInitialize(void) // Initialize the UAVObjects that we use GCSReceiverInitialize(); - PipXStatusInitialize(); + OPLinkStatusInitialize(); ObjectPersistenceInitialize(); - updateSettings(); // Initialise UAVTalk - data->GCSUAVTalkCon = UAVTalkInitialize(&GCSUAVTalkSendHandler); - if (PIOS_COM_UAVTALK) - data->UAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); - else - data->UAVTalkCon = 0; + data->outUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); + data->inUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); // Initialize the queues. - data->gcsEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); - if (PIOS_COM_UAVTALK) - data->uavtalkEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); - else - { - data->uavtalkEventQueue = 0; - } + data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); + + // Configure our UAVObjects for updates. + UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); + UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); + //UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); // Initialize the statistics. data->comTxErrors = 0; data->comTxRetries = 0; data->UAVTalkErrors = 0; - data->RSSI = -127; - - // Initialize the packet send timeout - data->send_timeout = 25; // ms - data->min_packet_size = 50; - - // Configure our UAVObjects for updates. - UAVObjConnectQueue(UAVObjGetByID(PIPXSTATUS_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); - UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue ? data->uavtalkEventQueue : data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); - UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); - - // Initialize the UAVTalk comm parameters. - data->gcs_uavtalk_params.isGCS = true; - data->gcs_uavtalk_params.UAVTalkCon = data->GCSUAVTalkCon; - data->gcs_uavtalk_params.sendQueue = data->uavtalkEventQueue; - data->gcs_uavtalk_params.recvQueue = data->gcsEventQueue; - data->gcs_uavtalk_params.wdg = PIOS_WDG_COMGCS; - data->gcs_uavtalk_params.comPort = PIOS_COM_GCS; - if (PIOS_COM_UAVTALK) - { - data->uavtalk_params.isGCS = false; - data->uavtalk_params.UAVTalkCon = data->UAVTalkCon; - data->uavtalk_params.sendQueue = data->gcsEventQueue; - data->uavtalk_params.recvQueue = data->uavtalkEventQueue; - data->uavtalk_params.wdg = PIOS_WDG_COMUAVTALK; - data->uavtalk_params.comPort = PIOS_COM_UAVTALK; - } return 0; } MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart) /** - * Reads UAVTalk messages froma com port and creates packets out of them. + * Telemetry transmit task, regular priority */ -static void UAVTalkRecvTask(void *parameters) +static void telemetryTxTask(void *parameters) { - UAVTalkComTaskParams *params = (UAVTalkComTaskParams *)parameters; - PHPacketHandle p = NULL; - - // Create the buffered reader. - BufferedReadHandle f = BufferedReadInit(params->comPort, TEMP_BUFFER_SIZE); - - while (1) { - bool HID_available = false; - xQueueHandle sendQueue = 0; - -#ifdef PIOS_INCLUDE_WDG - // Update the watchdog timer. - if (params->wdg) - PIOS_WDG_UpdateFlag(params->wdg); -#endif /* PIOS_INCLUDE_WDG */ - -#if defined(PIOS_INCLUDE_USB) - // Is USB plugged in? - if (PIOS_USB_CheckAvailable(0)) - HID_available = true; -#endif /* PIOS_INCLUDE_USB */ - - // Receive from USB HID if available, otherwise UAVTalk com if it's available. - if (params->isGCS && HID_available) - BufferedReadSetCom(f, PIOS_COM_USB_HID); - else - { - if (params->comPort) - BufferedReadSetCom(f, params->comPort); - else - { - vTaskDelay(5); - continue; - } - } - - // Send packets to the UAVTalk port if this is a GCS connction and UAVTalk port is configured - // or to the GCS port if this is a UAVTalk connection and USB is plugged in. - if ((params->isGCS && data->UAVTalkCon) || (!params->isGCS && HID_available)) - sendQueue = params->sendQueue; - - // Read the next byte - uint8_t rx_byte; - if (!BufferedRead(f, &rx_byte, MAX_PORT_DELAY)) - continue; - - // Get a TX packet from the packet handler if required. - if (p == NULL) - { - - // Wait until we receive a sync. - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(params->UAVTalkCon, rx_byte); - if (state != UAVTALK_STATE_TYPE) - continue; - - // Get a packet when we see the sync - p = PHGetTXPacket(pios_packet_handler); - - // No packets available? - if (p == NULL) - { - data->droppedPackets++; - continue; - } - - // Initialize the packet. - p->header.destination_id = data->destination_id; - p->header.type = PACKET_TYPE_DATA; - p->data[0] = rx_byte; - p->header.data_size = 1; - continue; - } - - // Insert this byte. - p->data[p->header.data_size++] = rx_byte; - - // Keep reading until we receive a completed packet. - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(params->UAVTalkCon, rx_byte); - UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(params->UAVTalkCon); - UAVTalkInputProcessor *iproc = &(connection->iproc); - - if (state == UAVTALK_STATE_COMPLETE) - { - // Is this a local UAVObject? - // We only generate GcsReceiver ojects, we don't consume them. - if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) - { - // We treat the ObjectPersistence object differently - if(iproc->objId == OBJECTPERSISTENCE_OBJID) - { - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); - - // Get the ObjectPersistence object. - ObjectPersistenceData obj_per; - ObjectPersistenceGet(&obj_per); - - // Is this concerning or setting object? - if (obj_per.ObjectID == PIPXSETTINGS_OBJID) - { - // Queue up the ACK. - queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); - - // Is this a save, load, or delete? - bool success = true; - switch (obj_per.Operation) - { - case OBJECTPERSISTENCE_OPERATION_LOAD: - { -#if defined(PIOS_INCLUDE_FLASH_EEPROM) - // Load the settings. - PipXSettingsData pipxSettings; - if (PIOS_EEPROM_Load((uint8_t*)&pipxSettings, sizeof(PipXSettingsData)) == 0) - PipXSettingsSet(&pipxSettings); - else - success = false; -#endif - break; - } - case OBJECTPERSISTENCE_OPERATION_SAVE: - { -#if defined(PIOS_INCLUDE_FLASH_EEPROM) - // Save the settings. - PipXSettingsData pipxSettings; - PipXSettingsGet(&pipxSettings); - int32_t ret = PIOS_EEPROM_Save((uint8_t*)&pipxSettings, sizeof(PipXSettingsData)); - if (ret != 0) - success = false; -#endif - break; - } - case OBJECTPERSISTENCE_OPERATION_DELETE: - { -#if defined(PIOS_INCLUDE_FLASH_EEPROM) - // Erase the settings. - PipXSettingsData pipxSettings; - uint8_t *ptr = (uint8_t*)&pipxSettings; - memset(ptr, 0, sizeof(PipXSettingsData)); - int32_t ret = PIOS_EEPROM_Save(ptr, sizeof(PipXSettingsData)); - if (ret != 0) - success = false; -#endif - break; - } - default: - break; - } - if (success == true) - { - obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; - ObjectPersistenceSet(&obj_per); - } - - // Release the packet, since we don't need it. - PHReleaseTXPacket(pios_packet_handler, p); - } - else - { - // Otherwise, queue the packet for transmission. - if (sendQueue) - queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); - else - PHTransmitPacket(PIOS_PACKET_HANDLER, p); - } - } - else - { - switch (iproc->type) - { - case UAVTALK_TYPE_OBJ: - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); - break; - case UAVTALK_TYPE_OBJ_REQ: - // Queue up an object send request. - queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_UPDATE_REQ); - break; - case UAVTALK_TYPE_OBJ_ACK: - if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) - // Queue up an ACK - queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); - break; - } - - // Release the packet, since we don't need it. - PHReleaseTXPacket(pios_packet_handler, p); - } - } - else - { - // Queue the packet for transmission. - if (sendQueue) - queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); - else - PHTransmitPacket(PIOS_PACKET_HANDLER, p); - } - p = NULL; - - } else if(state == UAVTALK_STATE_ERROR) { - data->UAVTalkErrors++; - - // Send a NACK if required. - if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) - { - // Queue up a NACK - queueEvent(params->recvQueue, iproc->obj, iproc->instId, EV_SEND_NACK); - - // Release the packet and start over again. - PHReleaseTXPacket(pios_packet_handler, p); - } - else - { - // Transmit the packet anyway... - if (sendQueue) - queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); - else - PHTransmitPacket(PIOS_PACKET_HANDLER, p); - } - p = NULL; - } - } -} - -/** - * Send packets to the com port. - */ -static void UAVTalkSendTask(void *parameters) -{ - UAVTalkComTaskParams *params = (UAVTalkComTaskParams *)parameters; UAVObjEvent ev; // Loop forever while (1) { -#ifdef PIOS_INCLUDE_WDG - // Update the watchdog timer. - // NOTE: this is temporarily turned off becase PIOS_Com_SendBuffer appears to block for an uncontrollable time, - // and SendBufferNonBlocking doesn't seem to be working in this case. - //PIOS_WDG_UpdateFlag(PIOS_WDG_SENDDATA); -#endif /* PIOS_INCLUDE_WDG */ - // Wait for a packet on the queue. - if (xQueueReceive(params->recvQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { + // Wait for queue message + if (xQueueReceive(data->uavtalkEventQueue, &ev, portMAX_DELAY) == pdTRUE) { if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { // Send update (with retries) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(params->UAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; + success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; if (!success) ++retries; } @@ -527,7 +199,7 @@ static void UAVTalkSendTask(void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendAck(params->UAVTalkCon, ev.obj, ev.instId) == 0; + success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId) == 0; if (!success) ++retries; } @@ -539,187 +211,87 @@ static void UAVTalkSendTask(void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendNack(params->UAVTalkCon, UAVObjGetID(ev.obj)) == 0; + success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj)) == 0; if (!success) ++retries; } data->comTxRetries += retries; } - else if(ev.event == EV_PACKET_RECEIVED) - { - // Receive the packet. - PHReceivePacket(pios_packet_handler, (PHPacketHandle)ev.obj); - } - else if(ev.event == EV_TRANSMIT_PACKET) - { - // Transmit the packet. - PHPacketHandle p = (PHPacketHandle)ev.obj; - UAVTalkSendBuf(params->UAVTalkCon, p->data, p->header.data_size); - PHReleaseTXPacket(pios_packet_handler, p); - } } } } /** - * The com to radio bridge task. + * Radio rx task. Receive data packets from the radio and pass them on. */ -static void transparentCommTask(void * parameters) +static void radioRxTask(void *parameters) { - portTickType packet_start_time = 0; - uint32_t timeout = MAX_PORT_DELAY; - PHPacketHandle p = NULL; - - /* Handle usart/usb -> radio direction */ + // Task loop while (1) { - -#ifdef PIOS_INCLUDE_WDG - // Update the watchdog timer. - PIOS_WDG_UpdateFlag(PIOS_WDG_TRANSCOMM); -#endif /* PIOS_INCLUDE_WDG */ - - // Get a TX packet from the packet handler if required. - if (p == NULL) - { - p = PHGetTXPacket(pios_packet_handler); - - // No packets available? - if (p == NULL) - { - data->droppedPackets++; - // Wait a bit for a packet to come available. - vTaskDelay(5); - continue; - } - - // Initialize the packet. - p->header.destination_id = data->destination_id; - //p->header.type = PACKET_TYPE_ACKED_DATA; - p->header.type = PACKET_TYPE_DATA; - p->header.data_size = 0; - } - - // Receive data from the com port - uint32_t cur_rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_TRANS_COM, p->data + p->header.data_size, - PH_MAX_DATA - p->header.data_size, timeout); - - // Do we have an data to send? - p->header.data_size += cur_rx_bytes; - if (p->header.data_size > 0) { - - // Check how long since last update - portTickType cur_sys_time = xTaskGetTickCount(); - - // Is this the start of a packet? - if(packet_start_time == 0) - packet_start_time = cur_sys_time; - - // Just send the packet on wraparound - bool send_packet = (cur_sys_time < packet_start_time); - if (!send_packet) - { - portTickType dT = (cur_sys_time - packet_start_time) / portTICK_RATE_MS; - if (dT > data->send_timeout) - send_packet = true; - else - timeout = data->send_timeout - dT; - } - - // Also send the packet if the size is over the minimum. - send_packet |= (p->header.data_size > data->min_packet_size); - - // Should we send this packet? - if (send_packet) - { - // Queue the packet for transmission. - PHTransmitPacket(PIOS_PACKET_HANDLER, p); - - // Reset the timeout - timeout = MAX_PORT_DELAY; - p = NULL; - packet_start_time = 0; - } - } + uint8_t serial_data[1]; + uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); + if (bytes_to_process > 0) + for (uint8_t i = 0; i < bytes_to_process; i++) + if (UAVTalkRelayInputStream(data->outUAVTalkCon, serial_data[i]) == UAVTALK_STATE_ERROR) + data->UAVTalkErrors++; } } /** - * The PPM input task. + * Radio rx task. Receive data from a com port and pass it on to the radio. */ -static void ppmInputTask(void *parameters) +static void radioTxTask(void *parameters) { - PHPpmPacket ppm_packet; - PHPacketHandle pph = (PHPacketHandle)&ppm_packet; - + // Task loop while (1) { - -#ifdef PIOS_INCLUDE_WDG - // Update the watchdog timer. - PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT); -#endif /* PIOS_INCLUDE_WDG */ - - // Read the receiver. - bool valid_input_detected = false; - for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i) - { - ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i); - if(ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT) - valid_input_detected = true; - } - - // Send the PPM packet if it's valid - if (valid_input_detected) - { - // Set the GCSReceiver UAVO if we're connected to the FC. - if (data->UAVTalkCon) - { - GCSReceiverData rcvr; - - // Copy the receiver channels into the GCSReceiver object. - for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i) - rcvr.Channel[i] = ppm_packet.channels[i]; - - // Set the GCSReceiverData object. - GCSReceiverSet(&rcvr); - } - else - { - // Otherwise, send a PPM packet over the radio link. - ppm_packet.header.destination_id = data->destination_id; - ppm_packet.header.type = PACKET_TYPE_PPM; - ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet); - PHTransmitPacket(PIOS_PACKET_HANDLER, pph); - } - } - - // Delay until the next update period. - vTaskDelay(PIOS_PPM_PACKET_UPDATE_PERIOD_MS / portTICK_RATE_MS); - } -} - -/** - * Transmit data buffer to the com port. - * \param[in] params The comm parameters. - * \param[in] buf Data buffer to send - * \param[in] length Length of buffer - * \return -1 on failure - * \return number of bytes transmitted on success - */ -static int32_t UAVTalkSend(UAVTalkComTaskParams *params, uint8_t *buf, int32_t length) -{ - uint32_t outputPort = params->comPort; + uint32_t inputPort = PIOS_COM_TELEMETRY; #if defined(PIOS_INCLUDE_USB) - if (params->isGCS) - { // Determine output port (USB takes priority over telemetry port) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) - outputPort = PIOS_COM_USB_HID; - } + if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) + inputPort = PIOS_COM_TELEM_USB; #endif /* PIOS_INCLUDE_USB */ - if(outputPort) - return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); - else - return -1; + if(inputPort) + { + uint8_t serial_data[1]; + uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY); + if (bytes_to_process > 0) + for (uint8_t i = 0; i < bytes_to_process; i++) + ProcessInputStream(data->inUAVTalkCon, serial_data[i]); + } + } +} + +static void testTxTask(void *parameters) +{ + uint8_t buf[100]; + for (uint8_t i = 0; i < 100; ++i) + buf[i] = i; + + // Task loop + uint16_t packets_sent = 0; + while (1) { + PIOS_COM_SendBuffer(PIOS_COM_RADIO, buf, 100); + packets_sent++; + OPLinkStatusTXRateSet(&packets_sent); + vTaskDelay(1000); + } +} + +/** + * Radio rx task. Receive data from a com port and pass it on to the radio. + */ +static void testRxTask(void *parameters) +{ + // Task loop + uint16_t packets_recv = 0; + while (1) { + uint8_t buf[100]; + int16_t bytes_received = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, buf, 100, MAX_PORT_DELAY); + if (bytes_received > 0) { + packets_recv++; + OPLinkStatusRXRateSet(&packets_recv); + } + } } /** @@ -731,7 +303,16 @@ static int32_t UAVTalkSend(UAVTalkComTaskParams *params, uint8_t *buf, int32_t l */ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) { - return UAVTalkSend(&(data->uavtalk_params), buf, length); + uint32_t outputPort = PIOS_COM_TELEMETRY; +#if defined(PIOS_INCLUDE_USB) + // Determine output port (USB takes priority over telemetry port) + if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) + outputPort = PIOS_COM_TELEM_USB; +#endif /* PIOS_INCLUDE_USB */ + if(outputPort) + return PIOS_COM_SendBuffer(outputPort, buf, length); + else + return -1; } /** @@ -741,91 +322,124 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) * \return -1 on failure * \return number of bytes transmitted on success */ -static int32_t GCSUAVTalkSendHandler(uint8_t *buf, int32_t length) +static int32_t RadioSendHandler(uint8_t *buf, int32_t length) { - return UAVTalkSend(&(data->gcs_uavtalk_params), buf, length); -} - -/** - * Receive a packet - * \param[in] buf The received data buffer - * \param[in] length Length of buffer - */ -static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid) -{ -#if defined(PIOS_INCLUDE_USB) - // See if USB is connected if requested. - if(checkHid) - // Determine output port (USB takes priority over telemetry port) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) - outputPort = PIOS_COM_USB_HID; -#endif /* PIOS_INCLUDE_USB */ - if (!outputPort) - return; - - // Send the received data to the com port - if (PIOS_COM_SendBuffer(outputPort, buf, len) != len) - // Error on transmit - data->comTxErrors++; -} - -/** - * Receive a packet - * \param[in] buf The received data buffer - * \param[in] length Length of buffer - */ -static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc) -{ - data->RSSI = rssi; - - // Packet data should go to transparent com if it's configured, - // or just send it through the UAVTalk link. - if (PIOS_COM_TRANS_COM) - transmitData(PIOS_COM_TRANS_COM, buf, len, false); - else if (data->UAVTalkCon) - UAVTalkSendBuf(data->UAVTalkCon, buf, len); + uint32_t outputPort = PIOS_COM_RADIO; + if(outputPort) + return PIOS_COM_SendBuffer(outputPort, buf, length); else - UAVTalkSendBuf(data->GCSUAVTalkCon, buf, len); + return -1; } -static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length) +static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) { - BufferedReadHandle h = (BufferedReadHandle)pvPortMalloc(sizeof(ReadBuffer)); - if (!h) - return NULL; + // Keep reading until we receive a completed packet. + UAVTalkRxState state = UAVTalkRelayInputStream(connectionHandle, rxbyte); + UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(connectionHandle); + UAVTalkInputProcessor *iproc = &(connection->iproc); - h->com_port = com_port; - h->buffer = (uint8_t*)pvPortMalloc(buffer_length); - h->length = buffer_length; - h->index = 0; - h->data_length = 0; + if (state == UAVTALK_STATE_COMPLETE) + { + // Is this a local UAVObject? + // We only generate GcsReceiver ojects, we don't consume them. + if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) + { + // We treat the ObjectPersistence object differently + if(iproc->objId == OBJECTPERSISTENCE_OBJID) + { + // Unpack object, if the instance does not exist it will be created! + UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); - if (h->buffer == NULL) - return NULL; + // Get the ObjectPersistence object. + ObjectPersistenceData obj_per; + ObjectPersistenceGet(&obj_per); - return h; -} + // Is this concerning or setting object? + if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) + { + // Queue up the ACK. + queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); -static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms) -{ - // Read some data if required. - if(h->index == h->data_length) - { - uint32_t rx_bytes = PIOS_COM_ReceiveBuffer(h->com_port, h->buffer, h->length, timeout_ms); - if (rx_bytes == 0) - return false; - h->index = 0; - h->data_length = rx_bytes; - } + // Is this a save, load, or delete? + bool success = true; + switch (obj_per.Operation) + { + case OBJECTPERSISTENCE_OPERATION_LOAD: + { +#if defined(PIOS_INCLUDE_FLASH_EEPROM) + // Load the settings. + OPLinkSettingsData oplinkSettings; + if (PIOS_EEPROM_Load((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)) == 0) + OPLinkSettingsSet(&oplinkSettings); + else + success = false; +#endif + break; + } + case OBJECTPERSISTENCE_OPERATION_SAVE: + { +#if defined(PIOS_INCLUDE_FLASH_EEPROM) + // Save the settings. + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + int32_t ret = PIOS_EEPROM_Save((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)); + if (ret != 0) + success = false; +#endif + break; + } + case OBJECTPERSISTENCE_OPERATION_DELETE: + { +#if defined(PIOS_INCLUDE_FLASH_EEPROM) + // Erase the settings. + OPLinkSettingsData oplinkSettings; + uint8_t *ptr = (uint8_t*)&oplinkSettings; + memset(ptr, 0, sizeof(OPLinkSettingsData)); + int32_t ret = PIOS_EEPROM_Save(ptr, sizeof(OPLinkSettingsData)); + if (ret != 0) + success = false; +#endif + break; + } + default: + break; + } + if (success == true) + { + obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; + ObjectPersistenceSet(&obj_per); + } + } + } + else + { + switch (iproc->type) + { + case UAVTALK_TYPE_OBJ: + // Unpack object, if the instance does not exist it will be created! + UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); + break; + case UAVTALK_TYPE_OBJ_REQ: + // Queue up an object send request. + queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_UPDATE_REQ); + break; + case UAVTALK_TYPE_OBJ_ACK: + if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) + // Queue up an ACK + queueEvent(data->uavtalkEventQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK); + break; + } + } + } - // Return the next byte. - *value = h->buffer[h->index++]; - return true; -} + } else if(state == UAVTALK_STATE_ERROR) { + data->UAVTalkErrors++; -static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port) -{ - h->com_port = com_port; + // Send a NACK if required. + if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) + // Queue up a NACK + queueEvent(data->uavtalkEventQueue, iproc->obj, iproc->instId, EV_SEND_NACK); + } } /** @@ -843,7 +457,7 @@ static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEve xQueueSend(queue, &ev, portMAX_DELAY); } -/** + /** * Update the telemetry settings, called on startup. * FIXME: This should be in the TelemetrySettings object. But objects * have too much overhead yet. Also the telemetry has no any specific @@ -854,85 +468,37 @@ static void updateSettings() { // Get the settings. - PipXSettingsData pipxSettings; - PipXSettingsGet(&pipxSettings); - - // Initialize the destination ID - data->destination_id = pipxSettings.PairID ? pipxSettings.PairID : 0xffffffff; + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); - if (PIOS_COM_TELEMETRY) { - switch (pipxSettings.TelemetrySpeed) { - case PIPXSETTINGS_TELEMETRYSPEED_2400: - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 2400); - break; - case PIPXSETTINGS_TELEMETRYSPEED_4800: - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 4800); - break; - case PIPXSETTINGS_TELEMETRYSPEED_9600: - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 9600); - break; - case PIPXSETTINGS_TELEMETRYSPEED_19200: - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 19200); - break; - case PIPXSETTINGS_TELEMETRYSPEED_38400: - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 38400); - break; - case PIPXSETTINGS_TELEMETRYSPEED_57600: - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 57600); - break; - case PIPXSETTINGS_TELEMETRYSPEED_115200: - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 115200); - break; - } - } - if (PIOS_COM_FLEXI) { - switch (pipxSettings.FlexiSpeed) { - case PIPXSETTINGS_TELEMETRYSPEED_2400: - PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 2400); - break; - case PIPXSETTINGS_TELEMETRYSPEED_4800: - PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 4800); - break; - case PIPXSETTINGS_TELEMETRYSPEED_9600: - PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 9600); - break; - case PIPXSETTINGS_TELEMETRYSPEED_19200: - PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 19200); - break; - case PIPXSETTINGS_TELEMETRYSPEED_38400: - PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 38400); - break; - case PIPXSETTINGS_TELEMETRYSPEED_57600: - PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 57600); - break; - case PIPXSETTINGS_TELEMETRYSPEED_115200: - PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 115200); - break; - } - } - if (PIOS_COM_VCP) { - switch (pipxSettings.VCPSpeed) { - case PIPXSETTINGS_TELEMETRYSPEED_2400: - PIOS_COM_ChangeBaud(PIOS_COM_VCP, 2400); - break; - case PIPXSETTINGS_TELEMETRYSPEED_4800: - PIOS_COM_ChangeBaud(PIOS_COM_VCP, 4800); - break; - case PIPXSETTINGS_TELEMETRYSPEED_9600: - PIOS_COM_ChangeBaud(PIOS_COM_VCP, 9600); - break; - case PIPXSETTINGS_TELEMETRYSPEED_19200: - PIOS_COM_ChangeBaud(PIOS_COM_VCP, 19200); - break; - case PIPXSETTINGS_TELEMETRYSPEED_38400: - PIOS_COM_ChangeBaud(PIOS_COM_VCP, 38400); - break; - case PIPXSETTINGS_TELEMETRYSPEED_57600: - PIOS_COM_ChangeBaud(PIOS_COM_VCP, 57600); - break; - case PIPXSETTINGS_TELEMETRYSPEED_115200: - PIOS_COM_ChangeBaud(PIOS_COM_VCP, 115200); - break; - } + switch (oplinkSettings.ComSpeed) { + case OPLINKSETTINGS_COMSPEED_2400: + if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 2400); + if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 2400); + break; + case OPLINKSETTINGS_COMSPEED_4800: + if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 4800); + if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 4800); + break; + case OPLINKSETTINGS_COMSPEED_9600: + if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 9600); + if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 9600); + break; + case OPLINKSETTINGS_COMSPEED_19200: + if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 19200); + if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 19200); + break; + case OPLINKSETTINGS_COMSPEED_38400: + if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 38400); + if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 38400); + break; + case OPLINKSETTINGS_COMSPEED_57600: + if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 57600); + if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 57600); + break; + case OPLINKSETTINGS_COMSPEED_115200: + if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 115200); + if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 115200); + break; } } diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index 4960aa034..6ca98be32 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -35,10 +35,6 @@ #include "flighttelemetrystats.h" #include "gcstelemetrystats.h" #include "hwsettings.h" -#if defined(PIOS_PACKET_HANDLER) -#include "pipxstatus.h" -#include "packet_handler.h" -#endif // Private constants #define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE @@ -85,9 +81,6 @@ static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); static void updateSettings(); static uint32_t getComPort(); -#ifdef PIOS_PACKET_HANDLER -static void receivePacketData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc); -#endif /** * Initialise the telemetry module @@ -101,13 +94,6 @@ int32_t TelemetryStart(void) // Listen to objects of interest GCSTelemetryStatsConnectQueue(priorityQueue); - - // Register to receive data from the radio packet handler. - // This must be after the radio module is initialized. -#ifdef PIOS_PACKET_HANDLER - if (PIOS_PACKET_HANDLER) - PHRegisterDataHandler(PIOS_PACKET_HANDLER, receivePacketData); -#endif // Start telemetry tasks xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); @@ -261,11 +247,6 @@ static void processObjEvent(UAVObjEvent * ev) retries = 0; success = -1; if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { -#ifdef PIOS_PACKET_HANDLER - // Don't send PipXStatus objects over the radio link. - if (PIOS_PACKET_HANDLER && (ev->obj == PipXStatusHandle()) && (getComPort() == 0)) - return; -#endif // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout @@ -374,14 +355,9 @@ static int32_t transmitData(uint8_t * data, int32_t length) { uint32_t outputPort = getComPort(); - if (outputPort) { + if (outputPort) return PIOS_COM_SendBuffer(outputPort, data, length); - } -#ifdef PIOS_PACKET_HANDLER - if (PIOS_PACKET_HANDLER) - if (PHTransmitData(PIOS_PACKET_HANDLER, data, length)) - return length; -#endif + return -1; } @@ -580,19 +556,6 @@ static uint32_t getComPort() { return telemetryPort; } -#ifdef PIOS_PACKET_HANDLER -/** - * Receive a packet - * \param[in] buf The received data buffer - * \param[in] length Length of buffer - */ -static void receivePacketData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc) -{ - for (uint8_t i = 0; i < len; ++i) - UAVTalkProcessInputStream(uavTalkCon, buf[i]); -} -#endif - /** * @} * @} diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index aaf265c58..7844efa50 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -152,28 +152,15 @@ extern uint32_t pios_i2c_flexi_adapter_id; extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_telemetry_id; -extern uint32_t pios_com_flexi_id; -extern uint32_t pios_com_vcp_id; -extern uint32_t pios_com_uavtalk_com_id; -extern uint32_t pios_com_gcs_com_id; -extern uint32_t pios_com_trans_com_id; -extern uint32_t pios_com_debug_id; extern uint32_t pios_com_rfm22b_id; extern uint32_t pios_ppm_rcvr_id; -#define PIOS_COM_USB_HID (pios_com_telem_usb_id) #define PIOS_COM_TELEMETRY (pios_com_telemetry_id) -#define PIOS_COM_FLEXI (pios_com_flexi_id) -#define PIOS_COM_VCP (pios_com_vcp_id) -#define PIOS_COM_UAVTALK (pios_com_uavtalk_com_id) -#define PIOS_COM_GCS (pios_com_gcs_com_id) -#define PIOS_COM_TRANS_COM (pios_com_trans_com_id) -#define PIOS_COM_DEBUG (pios_com_debug_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) #define PIOS_COM_RADIO (pios_com_rfm22b_id) -#define PIOS_COM_TELEM_USB PIOS_COM_USB_HID #define PIOS_PPM_RECEIVER (pios_ppm_rcvr_id) #define DEBUG_LEVEL 2 -#if DEBUG_LEVEL > 0 +#if DEBUG_LEVEL > 1000 #define DEBUG_PRINTF(level, ...) {if(level <= DEBUG_LEVEL && PIOS_COM_DEBUG > 0) { PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, __VA_ARGS__); }} #else #define DEBUG_PRINTF(...) @@ -268,13 +255,8 @@ extern uint32_t pios_ppm_rcvr_id; //------------------------- #if defined(PIOS_INCLUDE_RFM22B) -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256 -extern uint32_t pios_com_rfm22b_id; -#define PIOS_COM_RADIO (pios_com_rfm22b_id) extern uint32_t pios_spi_rfm22b_id; #define PIOS_RFM22_SPI_PORT (pios_spi_rfm22b_id) -#define RFM22_EXT_INT_USE extern uint32_t pios_rfm22b_id; #endif /* PIOS_INCLUDE_RFM22B */ diff --git a/flight/PiOS/Boards/STM32F4xx_RevoMini.h b/flight/PiOS/Boards/STM32F4xx_RevoMini.h index 1b7caa564..65857b572 100644 --- a/flight/PiOS/Boards/STM32F4xx_RevoMini.h +++ b/flight/PiOS/Boards/STM32F4xx_RevoMini.h @@ -143,10 +143,7 @@ extern uint32_t pios_com_debug_id; #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ #if defined(PIOS_INCLUDE_RFM22B) -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 -extern uint32_t pios_com_rfm22b_id; -#define PIOS_COM_RADIO (pios_com_rfm22b_id) +extern uint32_t pios_rfm22b_id; extern uint32_t pios_spi_telem_flash_id; #define PIOS_RFM22_SPI_PORT (pios_spi_telem_flash_id) #endif /* PIOS_INCLUDE_RFM22B */ diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index d7fca315a..c5eea5941 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -119,157 +119,6 @@ // AFC enabled /* Local type definitions */ -enum pios_rfm22b_dev_magic { - PIOS_RFM22B_DEV_MAGIC = 0x68e971b6, -}; - -enum pios_rfm22b_state { - RFM22B_STATE_UNINITIALIZED, - RFM22B_STATE_INITIALIZING, - RFM22B_STATE_RX_MODE, - RFM22B_STATE_WAIT_PREAMBLE, - RFM22B_STATE_WAIT_SYNC, - RFM22B_STATE_RX_DATA, - RFM22B_STATE_TX_START, - RFM22B_STATE_TX_DATA, - RFM22B_STATE_TIMEOUT, - RFM22B_STATE_ERROR, - RFM22B_STATE_FATAL_ERROR, - - RFM22B_STATE_NUM_STATES // Must be last -}; - -enum pios_rfm22b_event { - RFM22B_EVENT_INITIALIZE, - RFM22B_EVENT_INITIALIZED, - RFM22B_EVENT_INT_RECEIVED, - RFM22B_EVENT_RX_MODE, - RFM22B_EVENT_PREAMBLE_DETECTED, - RFM22B_EVENT_SYNC_DETECTED, - RFM22B_EVENT_RX_COMPLETE, - RFM22B_EVENT_SEND_PACKET, - RFM22B_EVENT_TX_START, - RFM22B_EVENT_TX_STARTED, - RFM22B_EVENT_TX_COMPLETE, - RFM22B_EVENT_TIMEOUT, - RFM22B_EVENT_ERROR, - RFM22B_EVENT_FATAL_ERROR, - - RFM22B_EVENT_NUM_EVENTS // Must be last -}; - -#define RFM22B_RX_PACKET_STATS_LEN 4 -enum pios_rfm22b_rx_packet_status { - RFM22B_GOOD_RX_PACKET = 0x00, - RFM22B_CORRECTED_RX_PACKET = 0x01, - RFM22B_ERROR_RX_PACKET = 0x2, - RFM22B_MISSED_RX_PACKET = 0x3 -}; - -struct pios_rfm22b_dev { - enum pios_rfm22b_dev_magic magic; - struct pios_rfm22b_cfg cfg; - - 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; - - // ISR pending - xSemaphoreHandle isrPending; - - // Receive packet complete - xSemaphoreHandle rxsem; - - // The COM callback functions. - pios_com_callback rx_in_cb; - uint32_t rx_in_context; - pios_com_callback tx_out_cb; - uint32_t tx_out_context; - - // 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 - xQueueHandle eventQueue; - - // device status register - uint8_t device_status; - // interrupt status register 1 - uint8_t int_status1; - // interrupt status register 2 - uint8_t int_status2; - // ezmac status register - uint8_t ezmac_status; - - // The packet transmission counts - uint32_t tx_packet_count; - uint32_t rx_packet_count; - - // The error statistics counters - uint16_t prev_rx_seq_num; - uint32_t rx_packet_stats[RFM22B_RX_PACKET_STATS_LEN]; - - // The packet statistics - struct rfm22b_stats stats; - - // Stats - uint16_t resets; - uint16_t timeouts; - uint16_t errors; - // RSSI in dBm - int8_t rssi_dBm; - - // The packet queue handle - xQueueHandle packetQueue; - - // The current tx packet - PHPacketHandle tx_packet; - // the tx data read index - uint16_t tx_data_rd; - // the tx data write index - uint16_t tx_data_wr; - - // The current rx packet - PHPacketHandle rx_packet; - // The previous rx packet - PHPacketHandle rx_packet_prev; - // The next rx packet - PHPacketHandle rx_packet_next; - // the receive buffer write index - uint16_t rx_buffer_wr; - // the receive buffer write index - uint16_t rx_packet_len; - - // The frequency hopping step size - float frequency_step_size; - // current frequency hop channel - uint8_t frequency_hop_channel; - // the frequency hop step size - uint8_t frequency_hop_step_size_reg; - // afc correction reading (in Hz) - int32_t afc_correction_Hz; - int8_t rx_packet_start_afc_Hz; - - // The status packet - PHStatusPacket status_packet; - - // The maximum time (ms) that it should take to transmit / receive a packet. - uint32_t max_packet_time; - portTickType packet_start_ticks; -}; struct pios_rfm22b_transition { enum pios_rfm22b_event (*entry_fn) (struct pios_rfm22b_dev *rfm22b_dev); @@ -305,11 +154,15 @@ static const uint8_t OUT_FF[64] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, static void PIOS_RFM22B_Task(void *parameters); static void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR); static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_connectionAccepted(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_connectionDeclined(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event); @@ -318,6 +171,7 @@ static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status); +static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len); // SPI read/write functions static void rfm22_assertCs(); @@ -328,22 +182,6 @@ static void rfm22_write(uint8_t addr, uint8_t data); static uint8_t rfm22_read(uint8_t addr); static uint8_t rfm22_read_noclaim(uint8_t addr); -/* Provide a COM driver */ -static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud); -static void PIOS_RFM22B_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context); -static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context); -static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail); -static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail); - -/* Local variables */ -const struct pios_com_driver pios_rfm22b_com_driver = { - .set_baud = PIOS_RFM22B_ChangeBaud, - .tx_start = PIOS_RFM22B_TxStart, - .rx_start = PIOS_RFM22B_RxStart, - .bind_tx_cb = PIOS_RFM22B_RegisterTxCallback, - .bind_rx_cb = PIOS_RFM22B_RegisterRxCallback, -}; - /* Te state transition table */ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_STATES] = { [RFM22B_STATE_UNINITIALIZED] = { @@ -399,6 +237,46 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_rxData, .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA, + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS, + [RFM22B_EVENT_CONNECTION_REQUEST] = RFM22B_STATE_ACCEPTING_CONNECTION, + [RFM22B_EVENT_CONNECTION_ACCEPT] = RFM22B_STATE_CONNECTION_ACCEPTED, + [RFM22B_EVENT_CONNECTION_DECLINED] = RFM22B_STATE_CONNECTION_DECLINED, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_RECEIVING_STATUS] = { + .entry_fn = rfm22_receiveStatus, + .next_state = { + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_ACCEPTING_CONNECTION] = { + .entry_fn = rfm22_acceptConnection, + .next_state = { + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_CONNECTION_ACCEPTED] = { + .entry_fn = rfm22_connectionAccepted, + .next_state = { + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_CONNECTION_DECLINED] = { + .entry_fn = rfm22_connectionDeclined, + .next_state = { [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, @@ -511,7 +389,7 @@ static const uint8_t ss_reg_70[] = { 0x24, 0x2D}; // rfm22_modulation_mode_cont static const uint8_t ss_reg_71[] = { 0x2B, 0x23}; // rfm22_modulation_mode_control2 -static bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev) +bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev) { return (rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC); } @@ -575,11 +453,21 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->int_status2 = 0; rfm22b_dev->ezmac_status = 0; + // Initialize the detected device statistics. + for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) + { + rfm22b_dev->pair_stats[i].pairID = 0; + rfm22b_dev->pair_stats[i].rssi = -127; + rfm22b_dev->pair_stats[i].afc_correction = 0; + rfm22b_dev->pair_stats[i].lastContact = 0; + } + // Initlize the link stats. rfm22b_dev->prev_rx_seq_num = 0; for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) rfm22b_dev->rx_packet_stats[i] = 0; + // Initialize the stats. rfm22b_dev->stats.packets_per_sec = 0; rfm22b_dev->stats.tx_count = 0; rfm22b_dev->stats.rx_count = 0; @@ -593,20 +481,14 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->stats.link_quality = 0; rfm22b_dev->stats.rssi = 0; - // Initialize the stats. - rfm22b_dev->tx_packet_count = 0; - rfm22b_dev->rx_packet_count = 0; - // 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; - rfm22b_dev->rx_packet_next = NULL; - rfm22b_dev->rx_packet_prev = NULL; rfm22b_dev->rx_packet_len = 0; rfm22b_dev->tx_packet = NULL; + rfm22b_dev->tx_seq = 0; *rfm22b_id = (uint32_t)rfm22b_dev; g_rfm22b_dev = rfm22b_dev; @@ -727,6 +609,11 @@ void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id) if(!PIOS_RFM22B_validate(rfm22b_dev)) return; rfm22b_dev->destination_id = (dest_id == 0) ? 0xffffffff : dest_id; + // The first slot is reserved for our current pairID + rfm22b_dev->pair_stats[0].pairID = dest_id; + rfm22b_dev->pair_stats[0].rssi = -127; + rfm22b_dev->pair_stats[0].afc_correction = 0; + rfm22b_dev->pair_stats[0].lastContact = 0; } /** @@ -741,8 +628,6 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { *stats = rfm22b_dev->stats; // Add the RX packet statistics - stats->rx_count = rfm22b_dev->prev_rx_seq_num; - stats->tx_count = rfm22b_dev->tx_packet_count; stats->rx_good = 0; stats->rx_corrected = 0; stats->rx_error = 0; @@ -775,33 +660,44 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { // Using this equation, error and missed packets are counted as -2, and corrected packets are counted as -1. // The rage is 0 (all error or missed packets) to 128 (all good packets). stats->link_quality = 64 + stats->rx_good - stats->rx_error - stats->rx_missed; + + // We are connected if our destination ID is in the pair stats. + stats->connected = false; + if (rfm22b_dev->destination_id != 0xffffffff) + for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) + { + if ((rfm22b_dev->pair_stats[i].pairID == rfm22b_dev->destination_id) && + (rfm22b_dev->pair_stats[i].rssi > -127)) + { + stats->rssi = rfm22b_dev->pair_stats[i].rssi; + stats->afc_correction = rfm22b_dev->pair_stats[i].afc_correction; + stats->connected = true; + break; + } + } } -static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail) + /** + * Get the stats of the oter radio devices that are in range. + * \param[out] device_ids A pointer to the array to store the device IDs. + * \param[out] RSSIs A pointer to the array to store the RSSI values in. + * \param[in] mx_pairs The length of the pdevice_ids and RSSIs arrays. + * \return The number of pair stats returned. + */ +uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs) { - struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) + return 0; - bool valid = PIOS_RFM22B_validate(rfm22b_dev); - PIOS_Assert(valid); + uint8_t mp = (max_pairs >= OPLINKSTATUS_PAIRIDS_NUMELEM) ? max_pairs : OPLINKSTATUS_PAIRIDS_NUMELEM; + for (uint8_t i = 0; i < mp; ++i) + { + device_ids[i] = rfm22b_dev->pair_stats[i].pairID; + RSSIs[i] = rfm22b_dev->pair_stats[i].rssi; + } -} - -static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) -{ - struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - bool valid = PIOS_RFM22B_validate(rfm22b_dev); - PIOS_Assert(valid); - -#ifdef NEVER - // Get some data to send - bool need_yield = false; - if(tx_pre_buffer_size == 0) - tx_pre_buffer_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, tx_pre_buffer, - TX_BUFFER_SIZE, NULL, &need_yield); - - // Inject a send packet event - PIOS_RFM22B_InjectEvent(g_rfm22b_dev, RFM22B_EVENT_TX_START, false); -#endif + return mp; } /** @@ -824,85 +720,12 @@ bool PIOS_RFM22B_Send_Packet(uint32_t rfm22b_id, PHPacketHandle p, uint32_t max_ return false; // Inject a send packet event - PIOS_RFM22B_InjectEvent(g_rfm22b_dev, RFM22B_EVENT_SEND_PACKET, false); + PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_SEND_PACKET, false); // Success return true; } -/** - * Receive a packet from the radio. - * \param[in] rfm22b_id The rfm22b device. - * \param[in] pret A pointer to the packet handle. - * \param[in] max_delay The maximum time to delay waiting for a packet. - * \return The number of bytes received. - */ -uint32_t PIOS_RFM22B_Receive_Packet(uint32_t rfm22b_id, PHPacketHandle *pret, uint32_t max_delay) -{ - struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_validate(rfm22b_dev)) - return 0; - - // Allocate the next Rx packet - if (rfm22b_dev->rx_packet_next == NULL) - rfm22b_dev->rx_packet_next = PHGetRXPacket(pios_packet_handler); - - // Block on the semephore until the a packet has been received. - if (xSemaphoreTake(rfm22b_dev->rxsem, max_delay / portTICK_RATE_MS) != pdTRUE) - return 0; - - // Return the Rx packet if it's available. - uint32_t rx_len = 0; - if (rfm22b_dev->rx_packet_prev) - { - PHPacketHandle p = rfm22b_dev->rx_packet_prev; - uint16_t len = PHPacketSizeECC(p); - uint16_t seq_num = p->header.seq_num; - uint16_t prev_seq_num = rfm22b_dev->prev_rx_seq_num; - uint16_t missed_packets = ((seq_num < prev_seq_num) ? (0xffff - prev_seq_num + seq_num) : (seq_num - prev_seq_num)) - 1; - rfm22b_dev->prev_rx_seq_num = seq_num; - - // Add any missed packets into the stats. - for ( ; missed_packets > 0; --missed_packets) - rfm22b_add_rx_status(rfm22b_dev, RFM22B_MISSED_RX_PACKET); - - // Attempt to correct any errors in the packet. - decode_data((unsigned char*)p, len); - - // Check if there were any errors - bool rx_error = check_syndrome() != 0; - if(rx_error) - { - - // We have an error. Try to correct it. - if (correct_errors_erasures((unsigned char*)p, len, 0, 0) == 0) - { - // We couldn't correct the error, so drop the packet. - rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET); - PHReleaseRXPacket(pios_packet_handler, p); - } - else - { - // We corrected the error. - rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET); - rx_error = false; - } - - } else - rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET); - - // Return the packet if there were not uncorrectable errors. - if (!rx_error) - { - *pret = p; - rx_len = rfm22b_dev->rx_packet_len; - } - rfm22b_dev->rx_packet_prev = NULL; - } - - return rx_len; -} - /** * The task that controls the radio state machine. */ @@ -988,50 +811,6 @@ static void PIOS_RFM22B_Task(void *parameters) } } -/** - * Changes the baud rate of the RFM22B peripheral without re-initialising. - * \param[in] rfm22b_id RFM22B name (GPS, TELEM, AUX) - * \param[in] baud Requested baud rate - */ -static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) -{ - struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - - bool valid = PIOS_RFM22B_validate(rfm22b_dev); - PIOS_Assert(valid); - -} - -static void PIOS_RFM22B_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) -{ - struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - - bool valid = PIOS_RFM22B_validate(rfm22b_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - rfm22b_dev->rx_in_context = context; - rfm22b_dev->rx_in_cb = rx_in_cb; -} - -static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context) -{ - struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - - bool valid = PIOS_RFM22B_validate(rfm22b_dev); - PIOS_Assert(valid); - - /* - * Order is important in these assignments since ISR uses _cb - * field to determine if it's ok to dereference _cb and _context - */ - rfm22b_dev->tx_out_context = context; - rfm22b_dev->tx_out_cb = tx_out_cb; -} - // ************************************ // SPI read/write @@ -1362,7 +1141,7 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) // Add the error correcting code. p->header.source_id = rfm22b_dev->deviceID; - p->header.seq_num = rfm22b_dev->tx_packet_count++; + p->header.seq_num = rfm22b_dev->tx_seq++; encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); rfm22b_dev->tx_packet = p; @@ -1537,16 +1316,14 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de afc_correction |= (uint16_t)rfm22_read(RFM22_ook_counter_value1) & 0x00c0; afc_correction >>= 6; // convert the afc value to Hz - rfm22b_dev->afc_correction_Hz = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f); + int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f); + rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr); // read rx signal strength .. 45 = -100dBm, 205 = -20dBm uint8_t rssi = rfm22_read(RFM22_rssi); // convert to dBm rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122; - // remember the afc value for this packet - rfm22b_dev->rx_packet_start_afc_Hz = rfm22b_dev->afc_correction_Hz; - return RFM22B_EVENT_SYNC_DETECTED; } else if (rfm22b_dev->int_status2 & !RFM22_is2_ipreaval) @@ -1558,17 +1335,53 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de return RFM22B_EVENT_NUM_EVENTS; } +static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len) +{ + static bool first_packet = true; + uint16_t seq_num = p->header.seq_num; + uint16_t prev_seq_num = rfm22b_dev->prev_rx_seq_num; + uint16_t missed_packets = ((seq_num < prev_seq_num) ? (0xffff - prev_seq_num + seq_num) : (seq_num - prev_seq_num)) - 1; + rfm22b_dev->prev_rx_seq_num = seq_num; + if (first_packet) + { + missed_packets = 0; + first_packet = false; + } + + // Add any missed packets into the stats. + for ( ; missed_packets > 0; --missed_packets) + rfm22b_add_rx_status(rfm22b_dev, RFM22B_MISSED_RX_PACKET); + + // Attempt to correct any errors in the packet. + decode_data((unsigned char*)p, rx_len); + + // Check if there were any errors + bool rx_error = check_syndrome() != 0; + if(rx_error) + { + // We have an error. Try to correct it. + if (correct_errors_erasures((unsigned char*)p, rx_len, 0, 0) == 0) + { + // We couldn't correct the error, so drop the packet. + rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET); + } + else + { + // We corrected the error. + rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET); + rx_error = false; + } + + } else + rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET); + + return !rx_error; +} + static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) { // Swap in the next packet buffer if required. - if (rfm22b_dev->rx_packet == NULL) - { - if (rfm22b_dev->rx_packet_next != NULL) - rfm22b_dev->rx_packet = rfm22b_dev->rx_packet_next; - else - return RFM22B_EVENT_ERROR; - } - uint8_t *rx_buffer = (uint8_t*)(rfm22b_dev->rx_packet); + uint8_t *rx_buffer = (uint8_t*)&(rfm22b_dev->rx_packet); // Read the device status registers if (!rfm22_readStatus(rfm22b_dev)) @@ -1630,27 +1443,42 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) return RFM22B_EVENT_ERROR; // we have a valid received packet - + enum pios_rfm22b_event ret_event = RFM22B_EVENT_RX_COMPLETE; if (rfm22b_dev->rx_buffer_wr > 0) { - // Add the rssi and afc to the end of the packet. - rx_buffer[rfm22b_dev->rx_buffer_wr++] = rfm22b_dev->rssi_dBm; - rx_buffer[rfm22b_dev->rx_buffer_wr++] = rfm22b_dev->rx_packet_start_afc_Hz; - // Swap the Rx packets. - if (rfm22b_dev->rx_packet_prev == NULL) + rfm22b_dev->stats.rx_count++; + rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr; + // Check the packet for errors. + if (rfm22_receivePacket(rfm22b_dev, &(rfm22b_dev->rx_packet), rfm22b_dev->rx_buffer_wr)) { - rfm22b_dev->rx_packet_prev = rfm22b_dev->rx_packet; - rfm22b_dev->rx_packet = rfm22b_dev->rx_packet_next; - rfm22b_dev->rx_packet_len = rfm22b_dev->rx_buffer_wr; - // Signal the receive thread. - xSemaphoreGive(rfm22b_dev->rxsem); + switch (rfm22b_dev->rx_packet.header.type) + { + case PACKET_TYPE_STATUS: + ret_event = RFM22B_EVENT_STATUS_RECEIVED; + break; + case PACKET_TYPE_CON_REQUEST: + ret_event = RFM22B_EVENT_CONNECTION_REQUEST; + break; + case PACKET_TYPE_CON_ACCEPT: + ret_event = RFM22B_EVENT_CONNECTION_ACCEPT; + break; + case PACKET_TYPE_DATA: + { + // Send the data to the com port. + bool rx_need_yield; + (rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield); + break; + } + default: + break; + } } rfm22b_dev->rx_buffer_wr = 0; } // Start a new transaction rfm22b_dev->packet_start_ticks = 0; - return RFM22B_EVENT_RX_COMPLETE; + return ret_event; } return RFM22B_EVENT_NUM_EVENTS; @@ -1699,6 +1527,8 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // Packet has been sent else if (rfm22b_dev->int_status1 & RFM22_is1_ipksent) { + rfm22b_dev->stats.tx_count++; + rfm22b_dev->stats.tx_byte_count += rfm22b_dev->tx_packet->header.data_size; // Free the tx packet PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); rfm22b_dev->tx_packet = 0; @@ -1711,6 +1541,74 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) return RFM22B_EVENT_NUM_EVENTS; } +/** + * Receive a status packet + * \param[in] rfm22b_dev The device structure + */ +static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev) +{ + PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet); + int8_t rssi = rfm22b_dev->rssi_dBm; + int8_t afc = rfm22b_dev->afc_correction_Hz; + uint32_t id = status->header.source_id; + bool found = false; + // Have we seen this device recently? + uint8_t id_idx = 0; + for ( ; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) + if(rfm22b_dev->pair_stats[id_idx].pairID == id) + { + found = true; + break; + } + + // If we have seen it, update the RSSI and reset the last contact couter + if(found) + { + rfm22b_dev->pair_stats[id_idx].rssi = rssi; + rfm22b_dev->pair_stats[id_idx].afc_correction = afc; + rfm22b_dev->pair_stats[id_idx].lastContact = 0; + } + + // If we haven't seen it, find a slot to put it in. + if (!found) + { + uint8_t min_idx = 0; + if(id != rfm22b_dev->destination_id) + { + int8_t min_rssi = rfm22b_dev->pair_stats[0].rssi; + for (id_idx = 1; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) + { + if(rfm22b_dev->pair_stats[id_idx].rssi < min_rssi) + { + min_rssi = rfm22b_dev->pair_stats[id_idx].rssi; + min_idx = id_idx; + } + } + } + rfm22b_dev->pair_stats[min_idx].pairID = id; + rfm22b_dev->pair_stats[min_idx].rssi = rssi; + rfm22b_dev->pair_stats[min_idx].afc_correction = afc; + rfm22b_dev->pair_stats[min_idx].lastContact = 0; + } + + return RFM22B_EVENT_RX_COMPLETE; +} + +static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) +{ + return RFM22B_EVENT_RX_COMPLETE; +} + +static enum pios_rfm22b_event rfm22_connectionAccepted(struct pios_rfm22b_dev *rfm22b_dev) +{ + return RFM22B_EVENT_RX_COMPLETE; +} + +static enum pios_rfm22b_event rfm22_connectionDeclined(struct pios_rfm22b_dev *rfm22b_dev) +{ + return RFM22B_EVENT_RX_COMPLETE; +} + // ************************************ // return the current mode diff --git a/flight/PiOS/Common/pios_rfm22b_com.c b/flight/PiOS/Common/pios_rfm22b_com.c new file mode 100644 index 000000000..cccae53db --- /dev/null +++ b/flight/PiOS/Common/pios_rfm22b_com.c @@ -0,0 +1,141 @@ +/** +****************************************************************************** +* @addtogroup PIOS PIOS Core hardware abstraction layer +* @{ +* @addtogroup PIOS_RFM22B Radio Functions +* @brief PIOS COM interface for for the RFM22B radio +* @{ +* +* @file pios_rfm22b_com.c +* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. +* @brief Implements a driver the the RFM22B driver +* @see The GNU Public License (GPL) Version 3 +* +*****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/* Project Includes */ +#include + +#include + +/* Provide a COM driver */ +static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud); +static void PIOS_RFM22B_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context); +static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context); +static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail); +static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail); + +/* Local variables */ +const struct pios_com_driver pios_rfm22b_com_driver = { + .set_baud = PIOS_RFM22B_ChangeBaud, + .tx_start = PIOS_RFM22B_TxStart, + .rx_start = PIOS_RFM22B_RxStart, + .bind_tx_cb = PIOS_RFM22B_RegisterTxCallback, + .bind_rx_cb = PIOS_RFM22B_RegisterRxCallback, +}; + +/** + * Changes the baud rate of the RFM22B peripheral without re-initialising. + * \param[in] rfm22b_id RFM22B name (GPS, TELEM, AUX) + * \param[in] baud Requested baud rate + */ +static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) +{ + // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. + enum rfm22b_datarate datarate = RFM22_datarate_64000; + if (baud <= 1024) + datarate = RFM22_datarate_500; + else if (baud <= 2048) + datarate = RFM22_datarate_1000; + else if (baud <= 4096) + datarate = RFM22_datarate_8000; + else if (baud <= 9600) + datarate = RFM22_datarate_16000; + else if (baud <= 19200) + datarate = RFM22_datarate_32000; + else if (baud <= 38400) + datarate = RFM22_datarate_64000; + else if (baud <= 57600) + datarate = RFM22_datarate_128000; + else if (baud <= 115200) + datarate = RFM22_datarate_192000; + RFM22_SetDatarate(rfm22b_id, datarate, true); +} + +static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail) +{ +} + +static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) + return; + + // Get a packet when we see the sync + PHPacketHandle p = PHGetTXPacket(pios_packet_handler); + if (!p) + return; + + // Get some data to send + bool need_yield = false; + p->header.type = PACKET_TYPE_DATA; + p->header.destination_id = rfm22b_dev->destination_id; + p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, + PH_MAX_DATA, NULL, &need_yield); + if (p->header.data_size == 0) + { + PHReleaseTXPacket(pios_packet_handler, p); + return; + } + + // Send the data packet. + if (!PIOS_RFM22B_Send_Packet(rfm22b_id, p, 5)) + { + rfm22b_dev->stats.tx_dropped++; + PHReleaseTXPacket(pios_packet_handler, p); + } +} + +static void PIOS_RFM22B_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) + return; + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + rfm22b_dev->rx_in_context = context; + rfm22b_dev->rx_in_cb = rx_in_cb; +} + +static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) + return; + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + rfm22b_dev->tx_out_context = context; + rfm22b_dev->tx_out_cb = tx_out_cb; +} diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 7d719f9ed..e63289bf4 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -79,6 +79,8 @@ enum rfm22b_datarate { struct rfm22b_stats { uint16_t packets_per_sec; + uint16_t tx_byte_count; + uint16_t rx_byte_count; uint16_t tx_count; uint16_t rx_count; uint8_t rx_good; @@ -90,6 +92,8 @@ struct rfm22b_stats { uint8_t timeouts; uint8_t link_quality; int8_t rssi; + int8_t afc_correction; + bool connected; }; /* Public Functions */ @@ -100,7 +104,10 @@ extern void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); extern bool PIOS_RFM22B_Send_Packet(uint32_t rfm22b_id, PHPacketHandle p, uint32_t max_delay); -extern uint32_t PIOS_RFM22B_Receive_Packet(uint32_t rfm22b_id, PHPacketHandle *p, uint32_t max_delay); +extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs); + +/* Global Variables */ +extern const struct pios_com_driver pios_rfm22b_com_driver; #endif /* PIOS_RFM22B_H */ diff --git a/flight/Modules/Radio/inc/radio.h b/flight/PiOS/inc/pios_rfm22b_com.h similarity index 67% rename from flight/Modules/Radio/inc/radio.h rename to flight/PiOS/inc/pios_rfm22b_com.h index 58102e061..f48c0af90 100644 --- a/flight/Modules/Radio/inc/radio.h +++ b/flight/PiOS/inc/pios_rfm22b_com.h @@ -1,37 +1,39 @@ /** ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Radio Input / Output Module - * @brief Read and Write packets from/to a radio device. - * @{ + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B COM Functions + * @brief PIOS interface for RFM22B Radio COM interface + * @{ * - * @file radio.h + * @file pios_rfm22b_com.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Include file of the radio module. + * @brief RFM22B functions header. * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef RADIOMODULE_H -#define RADIOMODULE_H +#ifndef PIOS_RFM22B_H +#define PIOS_RFM22B_H -#endif // RADIOMODULE_H +extern const struct pios_com_driver pios_rfm22b_com_driver; + +#endif /* PIOS_RFM22B_H */ /** * @} diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index aa80f1b62..a1fed423d 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -32,11 +32,11 @@ #define PIOS_RFM22B_PRIV_H #include -#include "fifo_buffer.h" +#include +#include +#include #include "pios_rfm22b.h" -extern const struct pios_com_driver pios_rfm22b_com_driver; - // ************************************ #define RFM22_DEVICE_VERSION_V2 0x02 @@ -565,14 +565,184 @@ extern const struct pios_com_driver pios_rfm22b_com_driver; #define RFM22_fifo_access 0x7F // R/W -// ************************************ -typedef int16_t ( *t_rfm22_TxDataByteCallback ) (void); -typedef bool ( *t_rfm22_RxDataCallback ) (void *data, uint8_t len); +// External type definitions -// ************************************ +typedef int16_t (*t_rfm22_TxDataByteCallback) (void); +typedef bool (*t_rfm22_RxDataCallback) (void *data, uint8_t len); +enum pios_rfm22b_dev_magic { + PIOS_RFM22B_DEV_MAGIC = 0x68e971b6, +}; + +enum pios_rfm22b_state { + RFM22B_STATE_UNINITIALIZED, + RFM22B_STATE_INITIALIZING, + RFM22B_STATE_RX_MODE, + RFM22B_STATE_WAIT_PREAMBLE, + RFM22B_STATE_WAIT_SYNC, + RFM22B_STATE_RX_DATA, + RFM22B_STATE_RECEIVING_STATUS, + RFM22B_STATE_ACCEPTING_CONNECTION, + RFM22B_STATE_CONNECTION_ACCEPTED, + RFM22B_STATE_CONNECTION_DECLINED, + RFM22B_STATE_TX_START, + RFM22B_STATE_TX_DATA, + RFM22B_STATE_TIMEOUT, + RFM22B_STATE_ERROR, + RFM22B_STATE_FATAL_ERROR, + + RFM22B_STATE_NUM_STATES // Must be last +}; + +enum pios_rfm22b_event { + RFM22B_EVENT_INITIALIZE, + RFM22B_EVENT_INITIALIZED, + RFM22B_EVENT_INT_RECEIVED, + RFM22B_EVENT_RX_MODE, + RFM22B_EVENT_PREAMBLE_DETECTED, + RFM22B_EVENT_SYNC_DETECTED, + RFM22B_EVENT_RX_COMPLETE, + RFM22B_EVENT_STATUS_RECEIVED, + RFM22B_EVENT_CONNECTION_REQUEST, + RFM22B_EVENT_CONNECTION_ACCEPT, + RFM22B_EVENT_CONNECTION_DECLINED, + RFM22B_EVENT_SEND_PACKET, + RFM22B_EVENT_TX_START, + RFM22B_EVENT_TX_STARTED, + RFM22B_EVENT_TX_COMPLETE, + RFM22B_EVENT_TIMEOUT, + RFM22B_EVENT_ERROR, + RFM22B_EVENT_FATAL_ERROR, + + RFM22B_EVENT_NUM_EVENTS // Must be last +}; + +#define RFM22B_RX_PACKET_STATS_LEN 4 +enum pios_rfm22b_rx_packet_status { + RFM22B_GOOD_RX_PACKET = 0x00, + RFM22B_CORRECTED_RX_PACKET = 0x01, + RFM22B_ERROR_RX_PACKET = 0x2, + RFM22B_MISSED_RX_PACKET = 0x3 +}; + +typedef struct { + uint32_t pairID; + int8_t rssi; + int8_t afc_correction; + uint8_t lastContact; +} rfm22b_pair_stats; + +struct pios_rfm22b_dev { + enum pios_rfm22b_dev_magic magic; + struct pios_rfm22b_cfg cfg; + + 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; + + // The potential paired statistics + rfm22b_pair_stats pair_stats[OPLINKSTATUS_PAIRIDS_NUMELEM]; + + // ISR pending + xSemaphoreHandle isrPending; + + // Receive packet complete + xSemaphoreHandle rxsem; + + // The COM callback functions. + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + + // 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 + xQueueHandle eventQueue; + + // device status register + uint8_t device_status; + // interrupt status register 1 + uint8_t int_status1; + // interrupt status register 2 + uint8_t int_status2; + // ezmac status register + uint8_t ezmac_status; + + // The error statistics counters + uint16_t prev_rx_seq_num; + uint32_t rx_packet_stats[RFM22B_RX_PACKET_STATS_LEN]; + + // The packet statistics + struct rfm22b_stats stats; + + // Stats + uint16_t resets; + uint16_t timeouts; + uint16_t errors; + // RSSI in dBm + int8_t rssi_dBm; + + // The packet queue handle + xQueueHandle packetQueue; + + // The current tx packet + PHPacketHandle tx_packet; + // the tx data read index + uint16_t tx_data_rd; + // the tx data write index + uint16_t tx_data_wr; + // The tx packet sequence number + uint16_t tx_seq; + + // The rx packet + PHPacket rx_packet; + // the receive buffer write index + uint16_t rx_buffer_wr; + // the receive buffer write index + uint16_t rx_packet_len; + + // The frequency hopping step size + float frequency_step_size; + // current frequency hop channel + uint8_t frequency_hop_channel; + // the frequency hop step size + uint8_t frequency_hop_step_size_reg; + // afc correction reading (in Hz) + int8_t afc_correction_Hz; + + // The status packet + PHStatusPacket status_packet; + + // The maximum time (ms) that it should take to transmit / receive a packet. + uint32_t max_packet_time; + portTickType packet_start_ticks; +}; + + +// External function definitions bool PIOS_RFM22_EXT_Int(void); +bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev); + + +// Global variable definitions + +extern const struct pios_com_driver pios_rfm22b_com_driver; #endif /* PIOS_RFM22B_PRIV_H */ diff --git a/flight/PipXtreme/Makefile b/flight/PipXtreme/Makefile index 07954d3b2..31d88ecab 100644 --- a/flight/PipXtreme/Makefile +++ b/flight/PipXtreme/Makefile @@ -73,7 +73,7 @@ FLASH_TOOL = OPENOCD # List of modules to include OPTMODULES = -MODULES = Radio RadioComBridge +MODULES = RadioComBridge # Paths OPSYSTEM = ./System @@ -146,8 +146,8 @@ endif ## UAVOBJECTS ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c -SRC += $(OPUAVSYNTHDIR)/pipxstatus.c -SRC += $(OPUAVSYNTHDIR)/pipxsettings.c +SRC += $(OPUAVSYNTHDIR)/oplinkstatus.c +SRC += $(OPUAVSYNTHDIR)/oplinksettings.c SRC += $(OPUAVSYNTHDIR)/objectpersistence.c endif @@ -189,6 +189,7 @@ SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/printf-stdarg.c SRC += $(PIOSCOMMON)/pios_rfm22b.c +SRC += $(PIOSCOMMON)/pios_rfm22b_com.c ## Libraries for flight calculations SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/CoordinateConversions.c diff --git a/flight/PipXtreme/System/inc/pios_config.h b/flight/PipXtreme/System/inc/pios_config.h index 254d92026..d94355b99 100755 --- a/flight/PipXtreme/System/inc/pios_config.h +++ b/flight/PipXtreme/System/inc/pios_config.h @@ -58,7 +58,7 @@ #define PIOS_INCLUDE_GPIO #define PIOS_INCLUDE_EXTI #define PIOS_INCLUDE_RTC -#define PIOS_INCLUDE_WDG +//#define PIOS_INCLUDE_WDG #define PIOS_INCLUDE_BL_HELPER #define PIOS_INCLUDE_FLASH_EEPROM #define PIOS_INCLUDE_RFM22B diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 863fbb961..58b762a63 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -30,30 +30,26 @@ #include #include -#include +#include #include -#define PIOS_COM_SERIAL_RX_BUF_LEN 256 -#define PIOS_COM_SERIAL_TX_BUF_LEN 256 - -#define PIOS_COM_FLEXI_RX_BUF_LEN 256 -#define PIOS_COM_FLEXI_TX_BUF_LEN 128 +#define PIOS_COM_TELEM_RX_BUF_LEN 256 +#define PIOS_COM_TELEM_TX_BUF_LEN 256 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 256 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 256 -#define PIOS_COM_VCP_USB_RX_BUF_LEN 256 -#define PIOS_COM_VCP_USB_TX_BUF_LEN 256 +#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256 +#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256 uint32_t pios_com_telem_usb_id = 0; -uint32_t pios_com_telemetry_id; -uint32_t pios_com_flexi_id; -uint32_t pios_com_vcp_id; -uint32_t pios_com_uavtalk_com_id = 0; -uint32_t pios_com_gcs_com_id = 0; -uint32_t pios_com_trans_com_id = 0; -uint32_t pios_com_debug_id = 0; +uint32_t pios_com_telemetry_id = 0; uint32_t pios_ppm_rcvr_id = 0; +#if defined(PIOS_INCLUDE_RFM22B) +uint32_t pios_rfm22b_id = 0; +uint32_t pios_com_rfm22b_id = 0; +uint32_t pios_packet_handler = 0; +#endif /** * PIOS_Board_Init() @@ -84,24 +80,24 @@ void PIOS_Board_Init(void) { PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* PIOS_INCLUDE_RTC */ - PipXSettingsInitialize(); + OPLinkSettingsInitialize(); #if defined(PIOS_INCLUDE_LED) PIOS_LED_Init(&pios_led_cfg); #endif /* PIOS_INCLUDE_LED */ - PipXSettingsData pipxSettings; + OPLinkSettingsData oplinkSettings; #if defined(PIOS_INCLUDE_FLASH_EEPROM) PIOS_EEPROM_Init(&pios_eeprom_cfg); /* Read the settings from flash. */ /* NOTE: We probably need to save/restore the objID here incase the object changed but the size doesn't */ - if (PIOS_EEPROM_Load((uint8_t*)&pipxSettings, sizeof(PipXSettingsData)) == 0) - PipXSettingsSet(&pipxSettings); + if (PIOS_EEPROM_Load((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)) == 0) + OPLinkSettingsSet(&oplinkSettings); else - PipXSettingsGet(&pipxSettings); + OPLinkSettingsGet(&oplinkSettings); #else - PipXSettingsGet(&pipxSettings); + OPLinkSettingsGet(&oplinkSettings); #endif /* PIOS_INCLUDE_FLASH_EEPROM */ /* Initialize the task monitor library */ @@ -115,11 +111,9 @@ void PIOS_Board_Init(void) { PIOS_TIM_InitClock(&tim_4_cfg); #endif /* PIOS_INCLUDE_TIM */ -#if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); - /* Flags to determine if various USB interfaces are advertised */ bool usb_cdc_present = false; @@ -134,53 +128,11 @@ void PIOS_Board_Init(void) { } #endif + /*Initialize the USB device */ uint32_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); -#if defined(PIOS_INCLUDE_USB_CDC) - if (!usb_cdc_present) { - /* Force VCP port function to disabled if we haven't advertised VCP in our USB descriptor */ - pipxSettings.VCPConfig = PIPXSETTINGS_VCPCONFIG_DISABLED; - } - - switch (pipxSettings.VCPConfig) - { - case PIPXSETTINGS_VCPCONFIG_SERIAL: - case PIPXSETTINGS_VCPCONFIG_DEBUG: - { - uint32_t pios_usb_cdc_id; - if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_VCP_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_VCP_USB_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_VCP_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_VCP_USB_TX_BUF_LEN)) { - PIOS_Assert(0); - } - switch (pipxSettings.VCPConfig) - { - case PIPXSETTINGS_VCPCONFIG_SERIAL: - pios_com_trans_com_id = pios_com_vcp_id; - break; - case PIPXSETTINGS_VCPCONFIG_DEBUG: - pios_com_debug_id = pios_com_vcp_id; - break; - } - break; - } - case PIPXSETTINGS_VCPCONFIG_DISABLED: - break; - } -#endif - -#if defined(PIOS_INCLUDE_USB_HID) - - /* Configure the usb HID port */ -#if defined(PIOS_INCLUDE_COM) + /* We always onfigure the usb HID port */ { uint32_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { @@ -196,110 +148,146 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); } } -#endif /* PIOS_INCLUDE_COM */ -#endif /* PIOS_INCLUDE_USB_HID */ - -#endif /* PIOS_INCLUDE_USB */ - - /* Configure USART1 (telemetry port) */ - switch (pipxSettings.TelemetryConfig) + /* Configure the requested com port */ + switch (oplinkSettings.InputConnection) { - case PIPXSETTINGS_TELEMETRYCONFIG_SERIAL: - case PIPXSETTINGS_TELEMETRYCONFIG_UAVTALK: - case PIPXSETTINGS_TELEMETRYCONFIG_GCS: - case PIPXSETTINGS_TELEMETRYCONFIG_DEBUG: + case OPLINKSETTINGS_INPUTCONNECTION_HID: + // This is always configured + break; + case OPLINKSETTINGS_INPUTCONNECTION_VCP: { +#if defined(PIOS_INCLUDE_USB_CDC) + if (!usb_cdc_present) + break; + uint32_t pios_usb_cdc_id; + if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { + PIOS_Assert(0); + } + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telemetry_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + PIOS_Assert(0); + } +#endif + break; + } + case OPLINKSETTINGS_INPUTCONNECTION_TELEMETRY: + { + // Configure the telemetry port. uint32_t pios_usart1_id; if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { PIOS_Assert(0); } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_SERIAL_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_SERIAL_TX_BUF_LEN); + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telemetry_id, &pios_usart_com_driver, pios_usart1_id, - rx_buffer, PIOS_COM_SERIAL_RX_BUF_LEN, - tx_buffer, PIOS_COM_SERIAL_TX_BUF_LEN)) { + rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { PIOS_Assert(0); } - switch (pipxSettings.TelemetryConfig) - { - case PIPXSETTINGS_TELEMETRYCONFIG_SERIAL: - pios_com_trans_com_id = pios_com_telemetry_id; - break; - case PIPXSETTINGS_TELEMETRYCONFIG_UAVTALK: - pios_com_uavtalk_com_id = pios_com_telemetry_id; - break; - case PIPXSETTINGS_TELEMETRYCONFIG_GCS: - pios_com_gcs_com_id = pios_com_telemetry_id; - break; - case PIPXSETTINGS_TELEMETRYCONFIG_DEBUG: - pios_com_debug_id = pios_com_telemetry_id; - break; - } break; } - case PIPXSETTINGS_TELEMETRYCONFIG_DISABLED: - break; - } - - /* Configure USART3 */ - switch (pipxSettings.FlexiConfig) - { - case PIPXSETTINGS_FLEXICONFIG_SERIAL: - case PIPXSETTINGS_FLEXICONFIG_UAVTALK: - case PIPXSETTINGS_FLEXICONFIG_GCS: - case PIPXSETTINGS_FLEXICONFIG_DEBUG: + case OPLINKSETTINGS_INPUTCONNECTION_FLEXI: { + // Configure the flexi port. uint32_t pios_usart3_id; if (PIOS_USART_Init(&pios_usart3_id, &pios_usart_telem_flexi_cfg)) { PIOS_Assert(0); } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_FLEXI_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_FLEXI_TX_BUF_LEN); + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_flexi_id, &pios_usart_com_driver, pios_usart3_id, - rx_buffer, PIOS_COM_FLEXI_RX_BUF_LEN, - tx_buffer, PIOS_COM_FLEXI_TX_BUF_LEN)) { + if (PIOS_COM_Init(&pios_com_telemetry_id, &pios_usart_com_driver, pios_usart3_id, + rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { PIOS_Assert(0); } - switch (pipxSettings.FlexiConfig) - { - case PIPXSETTINGS_FLEXICONFIG_SERIAL: - pios_com_trans_com_id = pios_com_flexi_id; - break; - case PIPXSETTINGS_FLEXICONFIG_UAVTALK: - pios_com_uavtalk_com_id = pios_com_flexi_id; - break; - case PIPXSETTINGS_FLEXICONFIG_GCS: - pios_com_gcs_com_id = pios_com_flexi_id; - break; - case PIPXSETTINGS_FLEXICONFIG_DEBUG: - pios_com_debug_id = pios_com_flexi_id; - break; - } break; } - case PIPXSETTINGS_FLEXICONFIG_PPM_IN: -#if defined(PIOS_INCLUDE_PPM) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + } - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - } + /* Configure PPM input */ +#if defined(PIOS_INCLUDE_PPM) + if (oplinkSettings.PPM == OPLINKSETTINGS_PPM_TRUE) + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) + PIOS_Assert(0); + } #endif /* PIOS_INCLUDE_PPM */ - break; - case PIPXSETTINGS_FLEXICONFIG_PPM_OUT: - case PIPXSETTINGS_FLEXICONFIG_RSSI: - case PIPXSETTINGS_FLEXICONFIG_DISABLED: + + /* Initalize the RFM22B radio COM device. */ +#if defined(PIOS_INCLUDE_RFM22B) + { + extern const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision); + const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_rfm22b_id, &pios_rfm22b_com_driver, pios_rfm22b_id, + rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + } + + // Set the maximum radio RF power. + switch (oplinkSettings.MaxRFPower) + { + case OPLINKSETTINGS_MAXRFPOWER_125: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); + break; + case OPLINKSETTINGS_MAXRFPOWER_16: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); + break; + case OPLINKSETTINGS_MAXRFPOWER_316: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); + break; + case OPLINKSETTINGS_MAXRFPOWER_63: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); + break; + case OPLINKSETTINGS_MAXRFPOWER_126: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); + break; + case OPLINKSETTINGS_MAXRFPOWER_25: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); + break; + case OPLINKSETTINGS_MAXRFPOWER_50: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); + break; + case OPLINKSETTINGS_MAXRFPOWER_100: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); break; } + // Set the radio destination ID. + PIOS_RFM22B_SetDestinationId(pios_rfm22b_id, oplinkSettings.PairID); + + // Initialize the packet handler + PacketHandlerConfig pios_ph_cfg = { + .default_destination_id = 0xffffffff, // Broadcast + .source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id), + .win_size = PIOS_PH_WIN_SIZE, + .max_connections = PIOS_PH_MAX_CONNECTIONS, + }; + pios_packet_handler = PHInitialize(&pios_ph_cfg); +#endif /* PIOS_INCLUDE_RFM22B */ + /* Remap AFIO pin */ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); diff --git a/flight/RevoMini/Makefile b/flight/RevoMini/Makefile index 03e91d3ee..42d3f5ec1 100644 --- a/flight/RevoMini/Makefile +++ b/flight/RevoMini/Makefile @@ -58,7 +58,6 @@ MODULES += GPS FirmwareIAP #MODULES += Airspeed/revolution MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner TxPID MODULES += CameraStab -MODULES += Radio MODULES += Telemetry PYMODULES = #FlightPlan @@ -170,6 +169,7 @@ SRC += $(PIOSCOMMON)/pios_ms5611.c SRC += $(PIOSCOMMON)/pios_crc.c SRC += $(PIOSCOMMON)/pios_com.c SRC += $(PIOSCOMMON)/pios_rfm22b.c +SRC += $(PIOSCOMMON)/pios_rfm22b_com.c SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/pios_sbus.c SRC += $(PIOSCOMMON)/pios_flash_jedec.c diff --git a/flight/RevoMini/System/pios_board.c b/flight/RevoMini/System/pios_board.c index 5e065c348..0b092d42b 100644 --- a/flight/RevoMini/System/pios_board.c +++ b/flight/RevoMini/System/pios_board.c @@ -41,6 +41,9 @@ #include #include "hwsettings.h" #include "manualcontrolsettings.h" +#if defined(PIOS_INCLUDE_RFM22B) +#include +#endif /** * Sensor configurations @@ -219,6 +222,9 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_BRIDGE_RX_BUF_LEN 65 #define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 +#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 + #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) #define PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN 40 uint32_t pios_com_debug_id; @@ -229,6 +235,10 @@ uint32_t pios_com_telem_usb_id = 0; uint32_t pios_com_telem_rf_id = 0; uint32_t pios_com_bridge_id = 0; uint32_t pios_com_overo_id = 0; +#if defined(PIOS_INCLUDE_RFM22B) +uint32_t pios_rfm22b_id = 0; +uint32_t pios_packet_handler = 0; +#endif /* * Setup a com port based on the passed cfg, driver and buffer sizes. tx size of -1 make the port rx only @@ -592,9 +602,83 @@ void PIOS_Board_Init(void) { case HWSETTINGS_RM_FLEXIPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_flexi_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; - } /* hwsettings_rm_flexiport */ - - + } /* hwsettings_rv_flexiport */ + + /* Initalize the RFM22B radio COM device. */ +#if defined(PIOS_INCLUDE_RFM22B) + uint8_t hwsettings_radioport; + HwSettingsRadioPortGet(&hwsettings_radioport); + switch (hwsettings_radioport) { + case HWSETTINGS_RADIOPORT_DISABLED: + break; + case HWSETTINGS_RADIOPORT_TELEMETRY: + { + extern const struct pios_rfm22b_cfg * PIOS_BOARD_HW_DEFS_GetRfm22Cfg (uint32_t board_revision); + const struct pios_board_info * bdinfo = &pios_board_info_blob; + const struct pios_rfm22b_cfg *pios_rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); + if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { + PIOS_Assert(0); + } + uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, + rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } + break; + } + } + + // Initalize out UAVOs + OPLinkSettingsInitialize(); + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + + // Set the maximum radio RF power. + switch (oplinkSettings.MaxRFPower) + { + case OPLINKSETTINGS_MAXRFPOWER_125: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); + break; + case OPLINKSETTINGS_MAXRFPOWER_16: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); + break; + case OPLINKSETTINGS_MAXRFPOWER_316: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); + break; + case OPLINKSETTINGS_MAXRFPOWER_63: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); + break; + case OPLINKSETTINGS_MAXRFPOWER_126: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); + break; + case OPLINKSETTINGS_MAXRFPOWER_25: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); + break; + case OPLINKSETTINGS_MAXRFPOWER_50: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); + break; + case OPLINKSETTINGS_MAXRFPOWER_100: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); + break; + } + + // Set the radio destination ID. + PIOS_RFM22B_SetDestinationId(pios_rfm22b_id, oplinkSettings.PairID); + + // Initialize the packet handler + PacketHandlerConfig pios_ph_cfg = { + .default_destination_id = 0xffffffff, // Broadcast + .source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id), + .win_size = PIOS_PH_WIN_SIZE, + .max_connections = PIOS_PH_MAX_CONNECTIONS, + }; + pios_packet_handler = PHInitialize(&pios_ph_cfg); +#endif /* PIOS_INCLUDE_RFM22B */ + /* Configure the receiver port*/ uint8_t hwsettings_rcvrport; HwSettingsRM_RcvrPortGet(&hwsettings_rcvrport); diff --git a/flight/RevoMini/UAVObjects.inc b/flight/RevoMini/UAVObjects.inc index 7b59a2585..4eeab21ac 100644 --- a/flight/RevoMini/UAVObjects.inc +++ b/flight/RevoMini/UAVObjects.inc @@ -98,9 +98,8 @@ UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += txpidsettings #Support for radio module on RM -UAVOBJSRCFILENAMES += pipxstatus -UAVOBJSRCFILENAMES += pipxsettings - +UAVOBJSRCFILENAMES += oplinkstatus +UAVOBJSRCFILENAMES += oplinksettings UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index da24b636d..19a773ae9 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -140,17 +140,17 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) // Connect to the PipXStatus object updates UAVObjectManager *objManager = pm->getObject(); - pipxStatusObj = dynamic_cast(objManager->getObject("PipXStatus")); - if (pipxStatusObj != NULL ) { - connect(pipxStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updatePipXStatus(UAVObject*))); + oplinkStatusObj = dynamic_cast(objManager->getObject("OPLinkStatus")); + if (oplinkStatusObj != NULL ) { + connect(oplinkStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateOPLinkStatus(UAVObject*))); } else { - qDebug() << "Error: Object is unknown (PipXStatus)."; + qDebug() << "Error: Object is unknown (OPLinkStatus)."; } - // Create the timer that is used to timeout the connection to the PipX. - pipxTimeout = new QTimer(this); - connect(pipxTimeout, SIGNAL(timeout()),this,SLOT(onPipxtremeDisconnect())); - pipxConnected = false; + // Create the timer that is used to timeout the connection to the OPLink. + oplinkTimeout = new QTimer(this); + connect(oplinkTimeout, SIGNAL(timeout()),this,SLOT(onOPLinktremeDisconnect())); + oplinkConnected = false; } ConfigGadgetWidget::~ConfigGadgetWidget() @@ -268,29 +268,29 @@ void ConfigGadgetWidget::tabAboutToChange(int i, bool * proceed) } /*! - \brief Called by updates to @PipXStatus + \brief Called by updates to @OPLinkStatus */ -void ConfigGadgetWidget::updatePipXStatus(UAVObject *) +void ConfigGadgetWidget::updateOPLinkStatus(UAVObject *) { // Restart the disconnection timer. - pipxTimeout->start(5000); - if (!pipxConnected) + oplinkTimeout->start(5000); + if (!oplinkConnected) { - qDebug() << "ConfigGadgetWidget onPipxtremeConnect"; + qDebug() << "ConfigGadgetWidget onOPLinkConnect"; QIcon *icon = new QIcon(); icon->addFile(":/configgadget/images/pipx-normal.png", QSize(), QIcon::Normal, QIcon::Off); icon->addFile(":/configgadget/images/pipx-selected.png", QSize(), QIcon::Selected, QIcon::Off); QWidget *qwd = new ConfigPipXtremeWidget(this); - ftw->insertTab(ConfigGadgetWidget::pipxtreme, qwd, *icon, QString("PipXtreme")); - pipxConnected = true; + ftw->insertTab(ConfigGadgetWidget::oplink, qwd, *icon, QString("OPLink")); + oplinkConnected = true; } } -void ConfigGadgetWidget::onPipxtremeDisconnect() { - qDebug()<<"ConfigGadgetWidget onPipxtremeDisconnect"; - pipxTimeout->stop(); - ftw->removeTab(ConfigGadgetWidget::pipxtreme); - pipxConnected = false; +void ConfigGadgetWidget::onOPLinkDisconnect() { + qDebug()<<"ConfigGadgetWidget onOPLinkDisconnect"; + oplinkTimeout->stop(); + ftw->removeTab(ConfigGadgetWidget::oplink); + oplinkConnected = false; } diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index 4f737ff36..304c557b4 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -48,32 +48,32 @@ class ConfigGadgetWidget: public QWidget public: ConfigGadgetWidget(QWidget *parent = 0); ~ConfigGadgetWidget(); - enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, pipxtreme, autotune}; + enum widgetTabs {hardware=0, aircraft, input, output, sensors, stabilization, camerastabilization, txpid, oplink, autotune}; void startInputWizard(); public slots: void onAutopilotConnect(); void onAutopilotDisconnect(); void tabAboutToChange(int i,bool *); - void updatePipXStatus(UAVObject *object); - void onPipxtremeDisconnect(); + void updateOPLinkStatus(UAVObject *object); + void onOPLinkDisconnect(); signals: void autopilotConnected(); void autopilotDisconnected(); - void pipxtremeConnect(); - void pipxtremeDisconnect(); + void oplinkConnect(); + void oplinkDisconnect(); protected: void resizeEvent(QResizeEvent * event); MyTabbedStackWidget *ftw; private: - UAVDataObject* pipxStatusObj; + UAVDataObject* oplinkStatusObj; - // A timer that timesout the connction to the PipX. - QTimer *pipxTimeout; - bool pipxConnected; + // A timer that timesout the connction to the OPLink. + QTimer *oplinkTimeout; + bool oplinkConnected; }; #endif // CONFIGGADGETWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index ded7984e0..428ee52a1 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -27,72 +27,72 @@ #include "configpipxtremewidget.h" -#include -#include +#include +#include ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget(parent) { - m_pipx = new Ui_PipXtremeWidget(); - m_pipx->setupUi(this); + m_oplink = new Ui_PipXtremeWidget(); + m_oplink->setupUi(this); - // Connect to the PipXStatus object updates + // Connect to the OPLinkStatus object updates ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - pipxStatusObj = dynamic_cast(objManager->getObject("PipXStatus")); - if (pipxStatusObj != NULL ) { - connect(pipxStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateStatus(UAVObject*))); + oplinkStatusObj = dynamic_cast(objManager->getObject("OPLinkStatus")); + if (oplinkStatusObj != NULL ) { + connect(oplinkStatusObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateStatus(UAVObject*))); } else { - qDebug() << "Error: Object is unknown (PipXStatus)."; + qDebug() << "Error: Object is unknown (OPLinkStatus)."; } - // Connect to the PipXSettings object updates - pipxSettingsObj = dynamic_cast(objManager->getObject("PipXSettings")); - if (pipxSettingsObj != NULL ) { - connect(pipxSettingsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateSettings(UAVObject*))); + // Connect to the OPLinkSettings object updates + oplinkSettingsObj = dynamic_cast(objManager->getObject("OPLinkSettings")); + if (oplinkSettingsObj != NULL ) { + connect(oplinkSettingsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateSettings(UAVObject*))); } else { - qDebug() << "Error: Object is unknown (PipXSettings)."; + qDebug() << "Error: Object is unknown (OPLinkSettings)."; } autoLoadWidgets(); - addApplySaveButtons(m_pipx->Apply, m_pipx->Save); + addApplySaveButtons(m_oplink->Apply, m_oplink->Save); - addUAVObjectToWidgetRelation("PipXSettings", "TelemetryConfig", m_pipx->TelemPortConfig); - addUAVObjectToWidgetRelation("PipXSettings", "TelemetrySpeed", m_pipx->TelemPortSpeed); - addUAVObjectToWidgetRelation("PipXSettings", "FlexiConfig", m_pipx->FlexiPortConfig); - addUAVObjectToWidgetRelation("PipXSettings", "FlexiSpeed", m_pipx->FlexiPortSpeed); - addUAVObjectToWidgetRelation("PipXSettings", "VCPConfig", m_pipx->VCPConfig); - addUAVObjectToWidgetRelation("PipXSettings", "VCPSpeed", m_pipx->VCPSpeed); - addUAVObjectToWidgetRelation("PipXSettings", "RFSpeed", m_pipx->MaxRFDatarate); - addUAVObjectToWidgetRelation("PipXSettings", "MaxRFPower", m_pipx->MaxRFTxPower); - addUAVObjectToWidgetRelation("PipXSettings", "SendTimeout", m_pipx->SendTimeout); - addUAVObjectToWidgetRelation("PipXSettings", "MinPacketSize", m_pipx->MinPacketSize); - addUAVObjectToWidgetRelation("PipXSettings", "FrequencyCalibration", m_pipx->FrequencyCalibration); - addUAVObjectToWidgetRelation("PipXSettings", "Frequency", m_pipx->Frequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "Coordinator", m_oplink->Coordinator); + addUAVObjectToWidgetRelation("OPLinkSettings", "PPM", m_oplink->PPM); + addUAVObjectToWidgetRelation("OPLinkSettings", "UAVTalk", m_oplink->UAVTalk); + addUAVObjectToWidgetRelation("OPLinkSettings", "InputConnection", m_oplink->InputConnection); + addUAVObjectToWidgetRelation("OPLinkSettings", "OutputConnection", m_oplink->OutputConnection); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed); + addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); + addUAVObjectToWidgetRelation("OPLinkSettings", "SendTimeout", m_oplink->SendTimeout); + addUAVObjectToWidgetRelation("OPLinkSettings", "MinPacketSize", m_oplink->MinPacketSize); + addUAVObjectToWidgetRelation("OPLinkSettings", "FrequencyCalibration", m_oplink->FrequencyCalibration); + addUAVObjectToWidgetRelation("OPLinkSettings", "Frequency", m_oplink->Frequency); - addUAVObjectToWidgetRelation("PipXStatus", "RxGood", m_pipx->Good); - addUAVObjectToWidgetRelation("PipXStatus", "RxCorrected", m_pipx->Corrected); - addUAVObjectToWidgetRelation("PipXStatus", "RxErrors", m_pipx->Errors); - addUAVObjectToWidgetRelation("PipXStatus", "RxMissed", m_pipx->Missed); - addUAVObjectToWidgetRelation("PipXStatus", "UAVTalkErrors", m_pipx->UAVTalkErrors); - addUAVObjectToWidgetRelation("PipXStatus", "TxDropped", m_pipx->Dropped); - addUAVObjectToWidgetRelation("PipXStatus", "Resets", m_pipx->Resets); - addUAVObjectToWidgetRelation("PipXStatus", "Timeouts", m_pipx->Timeouts); - addUAVObjectToWidgetRelation("PipXStatus", "RSSI", m_pipx->RSSI); - addUAVObjectToWidgetRelation("PipXStatus", "LinkQuality", m_pipx->LinkQuality); - addUAVObjectToWidgetRelation("PipXStatus", "RXRate", m_pipx->RXRate); - addUAVObjectToWidgetRelation("PipXStatus", "TXRate", m_pipx->TXRate); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxCorrected", m_oplink->Corrected); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxErrors", m_oplink->Errors); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxMissed", m_oplink->Missed); + addUAVObjectToWidgetRelation("OPLinkStatus", "UAVTalkErrors", m_oplink->UAVTalkErrors); + addUAVObjectToWidgetRelation("OPLinkStatus", "TxDropped", m_oplink->Dropped); + addUAVObjectToWidgetRelation("OPLinkStatus", "Resets", m_oplink->Resets); + addUAVObjectToWidgetRelation("OPLinkStatus", "Timeouts", m_oplink->Timeouts); + addUAVObjectToWidgetRelation("OPLinkStatus", "RSSI", m_oplink->RSSI); + addUAVObjectToWidgetRelation("OPLinkStatus", "AFCCorrection", m_oplink->AFCCorrection); + addUAVObjectToWidgetRelation("OPLinkStatus", "LinkQuality", m_oplink->LinkQuality); + addUAVObjectToWidgetRelation("OPLinkStatus", "RXRate", m_oplink->RXRate); + addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate); // Connect to the pair ID radio buttons. - connect(m_pipx->PairSelectB, SIGNAL(toggled(bool)), this, SLOT(pairBToggled(bool))); - connect(m_pipx->PairSelect1, SIGNAL(toggled(bool)), this, SLOT(pair1Toggled(bool))); - connect(m_pipx->PairSelect2, SIGNAL(toggled(bool)), this, SLOT(pair2Toggled(bool))); - connect(m_pipx->PairSelect3, SIGNAL(toggled(bool)), this, SLOT(pair3Toggled(bool))); - connect(m_pipx->PairSelect4, SIGNAL(toggled(bool)), this, SLOT(pair4Toggled(bool))); + connect(m_oplink->PairSelectB, SIGNAL(toggled(bool)), this, SLOT(pairBToggled(bool))); + connect(m_oplink->PairSelect1, SIGNAL(toggled(bool)), this, SLOT(pair1Toggled(bool))); + connect(m_oplink->PairSelect2, SIGNAL(toggled(bool)), this, SLOT(pair2Toggled(bool))); + connect(m_oplink->PairSelect3, SIGNAL(toggled(bool)), this, SLOT(pair3Toggled(bool))); + connect(m_oplink->PairSelect4, SIGNAL(toggled(bool)), this, SLOT(pair4Toggled(bool))); //Add scroll bar when necessary QScrollArea *scroll = new QScrollArea; - scroll->setWidget(m_pipx->frame_3); + scroll->setWidget(m_oplink->frame_3); scroll->setWidgetResizable(true); - m_pipx->verticalLayout_3->addWidget(scroll); + m_oplink->verticalLayout_3->addWidget(scroll); // Request and update of the setting object. settingsUpdated = false; @@ -111,83 +111,83 @@ void ConfigPipXtremeWidget::refreshValues() void ConfigPipXtremeWidget::applySettings() { - PipXSettings *pipxSettings = PipXSettings::GetInstance(getObjectManager()); - PipXSettings::DataFields pipxSettingsData = pipxSettings->getData(); + OPLinkSettings *oplinkSettings = OPLinkSettings::GetInstance(getObjectManager()); + OPLinkSettings::DataFields oplinkSettingsData = oplinkSettings->getData(); // Get the pair ID. quint32 pairID = 0; bool okay; - if (m_pipx->PairSelect1->isChecked()) - pairID = m_pipx->PairID1->text().toUInt(&okay, 16); - else if (m_pipx->PairSelect2->isChecked()) - pairID = m_pipx->PairID2->text().toUInt(&okay, 16); - else if (m_pipx->PairSelect3->isChecked()) - pairID = m_pipx->PairID3->text().toUInt(&okay, 16); - else if (m_pipx->PairSelect4->isChecked()) - pairID = m_pipx->PairID4->text().toUInt(&okay, 16); - pipxSettingsData.PairID = pairID; - pipxSettings->setData(pipxSettingsData); + if (m_oplink->PairSelect1->isChecked()) + pairID = m_oplink->PairID1->text().toUInt(&okay, 16); + else if (m_oplink->PairSelect2->isChecked()) + pairID = m_oplink->PairID2->text().toUInt(&okay, 16); + else if (m_oplink->PairSelect3->isChecked()) + pairID = m_oplink->PairID3->text().toUInt(&okay, 16); + else if (m_oplink->PairSelect4->isChecked()) + pairID = m_oplink->PairID4->text().toUInt(&okay, 16); + oplinkSettingsData.PairID = pairID; + oplinkSettings->setData(oplinkSettingsData); } void ConfigPipXtremeWidget::saveSettings() { //applySettings(); - UAVObject *obj = PipXSettings::GetInstance(getObjectManager()); + UAVObject *obj = OPLinkSettings::GetInstance(getObjectManager()); saveObjectToSD(obj); } /*! - \brief Called by updates to @PipXStatus + \brief Called by updates to @OPLinkStatus */ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) { // Request and update of the setting object if we haven't received it yet. if (!settingsUpdated) - pipxSettingsObj->requestUpdate(); + oplinkSettingsObj->requestUpdate(); // Get the current pairID - PipXSettings *pipxSettings = PipXSettings::GetInstance(getObjectManager()); + OPLinkSettings *oplinkSettings = OPLinkSettings::GetInstance(getObjectManager()); quint32 pairID = 0; - if (pipxSettings) - pairID = pipxSettings->getPairID(); + if (oplinkSettings) + pairID = oplinkSettings->getPairID(); // Update the detected devices. UAVObjectField* pairIdField = object->getField("PairIDs"); if (pairIdField) { quint32 pairid1 = pairIdField->getValue(0).toUInt(); - m_pipx->PairID1->setText(QString::number(pairid1, 16).toUpper()); - m_pipx->PairID1->setEnabled(false); - m_pipx->PairSelect1->setChecked(pairID && (pairID == pairid1)); - m_pipx->PairSelect1->setEnabled(pairid1); + m_oplink->PairID1->setText(QString::number(pairid1, 16).toUpper()); + m_oplink->PairID1->setEnabled(false); + m_oplink->PairSelect1->setChecked(pairID && (pairID == pairid1)); + m_oplink->PairSelect1->setEnabled(pairid1); quint32 pairid2 = pairIdField->getValue(1).toUInt(); - m_pipx->PairID2->setText(QString::number(pairIdField->getValue(1).toUInt(), 16).toUpper()); - m_pipx->PairID2->setEnabled(false); - m_pipx->PairSelect2->setChecked(pairID && (pairID == pairid2)); - m_pipx->PairSelect2->setEnabled(pairid2); + m_oplink->PairID2->setText(QString::number(pairIdField->getValue(1).toUInt(), 16).toUpper()); + m_oplink->PairID2->setEnabled(false); + m_oplink->PairSelect2->setChecked(pairID && (pairID == pairid2)); + m_oplink->PairSelect2->setEnabled(pairid2); quint32 pairid3 = pairIdField->getValue(2).toUInt(); - m_pipx->PairID3->setText(QString::number(pairIdField->getValue(2).toUInt(), 16).toUpper()); - m_pipx->PairID3->setEnabled(false); - m_pipx->PairSelect3->setChecked(pairID && (pairID == pairid3)); - m_pipx->PairSelect3->setEnabled(pairid3); + m_oplink->PairID3->setText(QString::number(pairIdField->getValue(2).toUInt(), 16).toUpper()); + m_oplink->PairID3->setEnabled(false); + m_oplink->PairSelect3->setChecked(pairID && (pairID == pairid3)); + m_oplink->PairSelect3->setEnabled(pairid3); quint32 pairid4 = pairIdField->getValue(3).toUInt(); - m_pipx->PairID4->setText(QString::number(pairIdField->getValue(3).toUInt(), 16).toUpper()); - m_pipx->PairID4->setEnabled(false); - m_pipx->PairSelect4->setChecked(pairID && (pairID == pairid4)); - m_pipx->PairSelect4->setEnabled(pairid4); + m_oplink->PairID4->setText(QString::number(pairIdField->getValue(3).toUInt(), 16).toUpper()); + m_oplink->PairID4->setEnabled(false); + m_oplink->PairSelect4->setChecked(pairID && (pairID == pairid4)); + m_oplink->PairSelect4->setEnabled(pairid4); } else { qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; } UAVObjectField* pairRssiField = object->getField("PairSignalStrengths"); if (pairRssiField) { - m_pipx->PairSignalStrengthBar1->setValue(pairRssiField->getValue(0).toInt()); - m_pipx->PairSignalStrengthBar2->setValue(pairRssiField->getValue(1).toInt()); - m_pipx->PairSignalStrengthBar3->setValue(pairRssiField->getValue(2).toInt()); - m_pipx->PairSignalStrengthBar4->setValue(pairRssiField->getValue(3).toInt()); - m_pipx->PairSignalStrengthLabel1->setText(QString("%1dB").arg(pairRssiField->getValue(0).toInt())); - m_pipx->PairSignalStrengthLabel2->setText(QString("%1dB").arg(pairRssiField->getValue(1).toInt())); - m_pipx->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); - m_pipx->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); + m_oplink->PairSignalStrengthBar1->setValue(pairRssiField->getValue(0).toInt()); + m_oplink->PairSignalStrengthBar2->setValue(pairRssiField->getValue(1).toInt()); + m_oplink->PairSignalStrengthBar3->setValue(pairRssiField->getValue(2).toInt()); + m_oplink->PairSignalStrengthBar4->setValue(pairRssiField->getValue(3).toInt()); + m_oplink->PairSignalStrengthLabel1->setText(QString("%1dB").arg(pairRssiField->getValue(0).toInt())); + m_oplink->PairSignalStrengthLabel2->setText(QString("%1dB").arg(pairRssiField->getValue(1).toInt())); + m_oplink->PairSignalStrengthLabel3->setText(QString("%1dB").arg(pairRssiField->getValue(2).toInt())); + m_oplink->PairSignalStrengthLabel4->setText(QString("%1dB").arg(pairRssiField->getValue(3).toInt())); } else { qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; } @@ -206,7 +206,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) * 20 bytes: SHA1 sum of the firmware. * 40 bytes: free for now. */ - char buf[PipXStatus::DESCRIPTION_NUMELEM]; + char buf[OPLinkStatus::DESCRIPTION_NUMELEM]; for (unsigned int i = 0; i < 26; ++i) buf[i] = descField->getValue(i + 14).toChar().toAscii(); buf[26] = '\0'; @@ -217,7 +217,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) gitDate += descField->getValue(11-i).toChar().toAscii() & 0xFF; } QString date = QDateTime::fromTime_t(gitDate).toUTC().toString("yyyy-MM-dd HH:mm"); - m_pipx->FirmwareVersion->setText(descstr + " " + date); + m_oplink->FirmwareVersion->setText(descstr + " " + date); } else { qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; } @@ -225,16 +225,16 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) // Update the serial number field UAVObjectField* serialField = object->getField("CPUSerial"); if (serialField) { - char buf[PipXStatus::CPUSERIAL_NUMELEM * 2 + 1]; - for (unsigned int i = 0; i < PipXStatus::CPUSERIAL_NUMELEM; ++i) + char buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2 + 1]; + for (unsigned int i = 0; i < OPLinkStatus::CPUSERIAL_NUMELEM; ++i) { unsigned char val = serialField->getValue(i).toUInt() >> 4; buf[i * 2] = ((val < 10) ? '0' : '7') + val; val = serialField->getValue(i).toUInt() & 0xf; buf[i * 2 + 1] = ((val < 10) ? '0' : '7') + val; } - buf[PipXStatus::CPUSERIAL_NUMELEM * 2] = '\0'; - m_pipx->SerialNumber->setText(buf); + buf[OPLinkStatus::CPUSERIAL_NUMELEM * 2] = '\0'; + m_oplink->SerialNumber->setText(buf); } else { qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; } @@ -242,25 +242,25 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) // Update the DeviceID field UAVObjectField* idField = object->getField("DeviceID"); if (idField) { - m_pipx->DeviceID->setText(QString::number(idField->getValue().toUInt(), 16).toUpper()); + m_oplink->DeviceID->setText(QString::number(idField->getValue().toUInt(), 16).toUpper()); } else { qDebug() << "PipXtremeGadgetWidget: Count not read DeviceID field."; } // Update the PairID field - m_pipx->PairID->setText(QString::number(pairID, 16).toUpper()); + m_oplink->PairID->setText(QString::number(pairID, 16).toUpper()); // Update the link state UAVObjectField* linkField = object->getField("LinkState"); if (linkField) { - m_pipx->LinkState->setText(linkField->getValue().toString()); + m_oplink->LinkState->setText(linkField->getValue().toString()); } else { qDebug() << "PipXtremeGadgetWidget: Count not read link state field."; } } /*! - \brief Called by updates to @PipXSettings + \brief Called by updates to @OPLinkSettings */ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) { @@ -286,20 +286,20 @@ void ConfigPipXtremeWidget::pairIDToggled(bool checked, quint8 idx) { if(checked) { - PipXStatus *pipxStatus = PipXStatus::GetInstance(getObjectManager()); - PipXSettings *pipxSettings = PipXSettings::GetInstance(getObjectManager()); + OPLinkStatus *oplinkStatus = OPLinkStatus::GetInstance(getObjectManager()); + OPLinkSettings *oplinkSettings = OPLinkSettings::GetInstance(getObjectManager()); - if (pipxStatus && pipxSettings) + if (oplinkStatus && oplinkSettings) { if (idx == 4) { - pipxSettings->setPairID(0); + oplinkSettings->setPairID(0); } else { - quint32 pairID = pipxStatus->getPairIDs(idx); + quint32 pairID = oplinkStatus->getPairIDs(idx); if (pairID) - pipxSettings->setPairID(pairID); + oplinkSettings->setPairID(pairID); } } } diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index 28db67b74..1243c5b65 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -43,13 +43,13 @@ public slots: void updateSettings(UAVObject *object1); private: - Ui_PipXtremeWidget *m_pipx; + Ui_PipXtremeWidget *m_oplink; - // The PipXtreme status UAVObject - UAVDataObject* pipxStatusObj; + // The OPLink status UAVObject + UAVDataObject* oplinkStatusObj; - // The PipXtreme ssettins UAVObject - UAVDataObject* pipxSettingsObj; + // The OPLink ssettins UAVObject + UAVDataObject* oplinkSettingsObj; bool settingsUpdated; diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 25461372b..2ab148060 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -750,7 +750,7 @@ - Rx AFC + AFC Corr. Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -758,7 +758,7 @@ - + 101 @@ -1305,18 +1305,39 @@ Configuration - - + + + + Coordinator + + + + + + + PPM + + + + + + + UAVTalk + + + + + - Telemetry Port Config. + Input Connection Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - + + 16777215 @@ -1324,22 +1345,45 @@ - Set the telemetry port configuration + Choose which port to communicate over on this modem - - + + - Telemetry Port Speed + Output Connection Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - + + + + + 16777215 + 16777215 + + + + Choose which port to communicate over on the remote modem + + + + + + + COM Speed + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + 16777215 @@ -1351,122 +1395,7 @@ - - - - Flexi Port Configuration - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Set the flexi port configuration - - - - - - - Flexi Port Speed - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Set the flexi port speed - - - - - - - VCP Configuration - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Set the virtual serial port configuration - - - - - - - VCP Speed - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Set the virtual serial port speed - - - - - - Max RF Datarate (bits/s) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Set the maximum RF datarate/channel bandwidth the modem will use - - - - Max RF Tx Power(mW) @@ -1476,7 +1405,7 @@ - + @@ -1495,7 +1424,7 @@ - + Send Timeout (ms) @@ -1505,7 +1434,7 @@ - + @@ -1530,7 +1459,7 @@ - + Min Packet Size @@ -1540,7 +1469,7 @@ - + @@ -1565,7 +1494,7 @@ - + Frequency Calibration @@ -1575,7 +1504,7 @@ - + @@ -1600,7 +1529,7 @@ - + Frequency (Hz) @@ -1610,7 +1539,7 @@ - + @@ -1644,7 +1573,7 @@ - + AES Encryption @@ -1713,7 +1642,7 @@ - + Qt::Vertical @@ -1726,16 +1655,6 @@ - - - - Scan whole band to see where their is interference and/or used channels - - - Scan Spectrum - - - diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d28dd07be..826978c2c 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -95,8 +95,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/txpidsettings.h \ $$UAVOBJECT_SYNTHETICS/cameradesired.h \ $$UAVOBJECT_SYNTHETICS/faultsettings.h \ - $$UAVOBJECT_SYNTHETICS/pipxsettings.h \ - $$UAVOBJECT_SYNTHETICS/pipxstatus.h \ + $$UAVOBJECT_SYNTHETICS/oplinksettings.h \ + $$UAVOBJECT_SYNTHETICS/oplinkstatus.h \ $$UAVOBJECT_SYNTHETICS/osdsettings.h \ $$UAVOBJECT_SYNTHETICS/waypoint.h \ $$UAVOBJECT_SYNTHETICS/waypointactive.h @@ -174,8 +174,8 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/txpidsettings.cpp \ $$UAVOBJECT_SYNTHETICS/cameradesired.cpp \ $$UAVOBJECT_SYNTHETICS/faultsettings.cpp \ - $$UAVOBJECT_SYNTHETICS/pipxsettings.cpp \ - $$UAVOBJECT_SYNTHETICS/pipxstatus.cpp \ + $$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \ + $$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \ $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/waypoint.cpp \ $$UAVOBJECT_SYNTHETICS/waypointactive.cpp diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 1560980d0..b88b19de3 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp @@ -27,7 +27,7 @@ #include "telemetry.h" #include "qxtlogger.h" -#include "pipxsettings.h" +#include "oplinksettings.h" #include "objectpersistence.h" #include #include @@ -376,7 +376,7 @@ void Telemetry::processObjectQueue() if ( gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED ) { objQueue.clear(); - if ( objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != PipXSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID ) + if ( objInfo.obj->getObjID() != GCSTelemetryStats::OBJID && objInfo.obj->getObjID() != OPLinkSettings::OBJID && objInfo.obj->getObjID() != ObjectPersistence::OBJID ) { objInfo.obj->emitTransactionCompleted(false); return; diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index d40a47d23..2d3ed3d83 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -15,6 +15,8 @@ + + @@ -22,7 +24,7 @@ - + diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml new file mode 100644 index 000000000..f1633fd7a --- /dev/null +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -0,0 +1,23 @@ + + + OPLink configurations options. + + + + + + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/pipxstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml similarity index 90% rename from shared/uavobjectdefinition/pipxstatus.xml rename to shared/uavobjectdefinition/oplinkstatus.xml index 88ea3d3b3..e93052cf0 100755 --- a/shared/uavobjectdefinition/pipxstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -1,6 +1,6 @@ - - PipXtreme device status. + + OPLink device status. @@ -15,6 +15,7 @@ + diff --git a/shared/uavobjectdefinition/pipxsettings.xml b/shared/uavobjectdefinition/pipxsettings.xml deleted file mode 100644 index af0927add..000000000 --- a/shared/uavobjectdefinition/pipxsettings.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - PipXtreme configurations options. - - - - - - - - - - - - - - - - - - - - - From bd42083376da1c101ea7ad7343c2595c5bc2238f Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 20 Oct 2012 18:02:49 -0400 Subject: [PATCH 04/31] RFM22B: Changed the way the frequencies and power settings are set for the rfm22b device. --- .../Modules/RadioComBridge/RadioComBridge.c | 35 +++++ flight/PiOS/Common/pios_rfm22b.c | 137 ++++++++++-------- flight/PiOS/inc/pios_rfm22b.h | 6 +- flight/PiOS/inc/pios_rfm22b_priv.h | 26 +++- flight/PipXtreme/System/pios_board.c | 32 ---- .../board_hw_defs/pipxtreme/board_hw_defs.c | 5 - flight/board_hw_defs/revomini/board_hw_defs.c | 10 -- .../plugins/config/configpipxtremewidget.cpp | 3 +- .../src/plugins/config/pipxtreme.ui | 60 +++++++- shared/uavobjectdefinition/oplinksettings.xml | 3 +- 10 files changed, 190 insertions(+), 127 deletions(-) diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index ba246d44e..aff437525 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -501,4 +501,39 @@ static void updateSettings() if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 115200); break; } + + // Set the frequencies. + PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency); + + // Set the maximum radio RF power. + switch (oplinkSettings.MaxRFPower) + { + case OPLINKSETTINGS_MAXRFPOWER_125: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); + break; + case OPLINKSETTINGS_MAXRFPOWER_16: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); + break; + case OPLINKSETTINGS_MAXRFPOWER_316: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); + break; + case OPLINKSETTINGS_MAXRFPOWER_63: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); + break; + case OPLINKSETTINGS_MAXRFPOWER_126: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); + break; + case OPLINKSETTINGS_MAXRFPOWER_25: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); + break; + case OPLINKSETTINGS_MAXRFPOWER_50: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); + break; + case OPLINKSETTINGS_MAXRFPOWER_100: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); + break; + } + + // Set the radio destination ID. + PIOS_RFM22B_SetDestinationId(pios_rfm22b_id, oplinkSettings.PairID); } diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index c5eea5941..565414053 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -63,6 +63,10 @@ #define EVENT_QUEUE_SIZE 5 #define PACKET_QUEUE_SIZE 3 #define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_64000 +#define RFM22B_DEFAULT_FREQUENCY 434000000 +#define RFM22B_DEFAULT_MIN_FREQUENCY (RFM22B_DEFAULT_FREQUENCY - 2000000) +#define RFM22B_DEFAULT_MAX_FREQUENCY (RFM22B_DEFAULT_FREQUENCY + 2000000) +#define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_7 // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms @@ -160,6 +164,7 @@ static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22 static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_connectionAccepted(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_connectionDeclined(struct pios_rfm22b_dev *rfm22b_dev); @@ -172,6 +177,7 @@ static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_d static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status); static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len); +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz); // SPI read/write functions static void rfm22_assertCs(); @@ -194,11 +200,48 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_INITIALIZING] = { .entry_fn = rfm22_init, .next_state = { + //[RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_REQUESTING_CONNECTION, [RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_TX_START, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, + [RFM22B_STATE_REQUESTING_CONNECTION] = { + .entry_fn = rfm22_requestConnection, + .next_state = { + [RFM22B_EVENT_REQUESTED_CONNECTION] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_ACCEPTING_CONNECTION] = { + .entry_fn = rfm22_acceptConnection, + .next_state = { + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_CONNECTION_ACCEPTED] = { + .entry_fn = rfm22_connectionAccepted, + .next_state = { + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_CONNECTION_DECLINED] = { + .entry_fn = rfm22_connectionDeclined, + .next_state = { + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, [RFM22B_STATE_RX_MODE] = { .entry_fn = rfm22_setRxMode, .next_state = { @@ -256,33 +299,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, - [RFM22B_STATE_ACCEPTING_CONNECTION] = { - .entry_fn = rfm22_acceptConnection, - .next_state = { - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_CONNECTION_ACCEPTED] = { - .entry_fn = rfm22_connectionAccepted, - .next_state = { - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, - [RFM22B_STATE_CONNECTION_DECLINED] = { - .entry_fn = rfm22_connectionDeclined, - .next_state = { - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, [RFM22B_STATE_TX_START] = { .entry_fn = rfm22_txStart, .next_state = { @@ -480,6 +496,8 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->stats.timeouts = 0; rfm22b_dev->stats.link_quality = 0; rfm22b_dev->stats.rssi = 0; + rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; + rfm22b_dev->destination_id = 0xffffffff; // Bind the configuration to the device instance rfm22b_dev->cfg = *cfg; @@ -505,9 +523,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu // Create the packet queue. rfm22b_dev->packetQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle)); - // Initialize the max tx power level. - PIOS_RFM22B_SetTxPower(*rfm22b_id, cfg->maxTxPower); - // Create our (hopefully) unique 32 bit id from the processor serial number. uint8_t crcs[] = { 0, 0, 0, 0 }; { @@ -595,6 +610,11 @@ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) return 0; } +/** + * Sets the radio device transmit power. + * \param[in] rfm22b_id The RFM22B device index. + * \param[in] tx_pwr The transmit power. + */ 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; @@ -603,6 +623,22 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) rfm22b_dev->tx_power = tx_pwr; } +/** + * Sets the radio frequency range and value. + * \param[in] rfm22b_id The RFM22B device index. + * \param[in] min_frequency The minimum frequency. + * \param[in] max_frequency The maximum frequency. + */ +void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_frequency, uint32_t max_frequency) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_validate(rfm22b_dev)) + return; + rfm22b_dev->min_frequency = min_frequency; + rfm22b_dev->max_frequency = max_frequency; + rfm22_setNominalCarrierFrequency(rfm22b_dev, (max_frequency - min_frequency) / 2); +} + void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; @@ -950,12 +986,10 @@ static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz) { - uint32_t min_frequency_hz = rfm22b_dev->cfg.minFrequencyHz; - uint32_t max_frequency_hz = rfm22b_dev->cfg.maxFrequencyHz; - if (frequency_hz < min_frequency_hz) - frequency_hz = min_frequency_hz; - else if (frequency_hz > max_frequency_hz) - frequency_hz = max_frequency_hz; + if (frequency_hz < rfm22b_dev->min_frequency) + frequency_hz = rfm22b_dev->min_frequency; + else if (frequency_hz > rfm22b_dev->max_frequency) + frequency_hz = rfm22b_dev->max_frequency; // holds the hbsel (1 or 2) uint8_t hbsel; @@ -1594,6 +1628,12 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b return RFM22B_EVENT_RX_COMPLETE; } +static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev) +{ + // Generate and set a connection request message. + return RFM22B_EVENT_RX_COMPLETE; +} + static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) { return RFM22B_EVENT_RX_COMPLETE; @@ -1647,8 +1687,6 @@ void rfm22_setFreqCalibration(uint8_t value) static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) { uint32_t id = rfm22b_dev->deviceID; - uint32_t min_frequency_hz = rfm22b_dev->cfg.minFrequencyHz; - uint32_t max_frequency_hz = rfm22b_dev->cfg.maxFrequencyHz; uint32_t freq_hop_step_size = 50000; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) @@ -1726,21 +1764,9 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // **************** // set the minimum and maximum carrier frequency allowed - - if (min_frequency_hz < RFM22_MIN_CARRIER_FREQUENCY_HZ) min_frequency_hz = RFM22_MIN_CARRIER_FREQUENCY_HZ; - else - if (min_frequency_hz > RFM22_MAX_CARRIER_FREQUENCY_HZ) min_frequency_hz = RFM22_MAX_CARRIER_FREQUENCY_HZ; - - if (max_frequency_hz < RFM22_MIN_CARRIER_FREQUENCY_HZ) max_frequency_hz = RFM22_MIN_CARRIER_FREQUENCY_HZ; - else - if (max_frequency_hz > RFM22_MAX_CARRIER_FREQUENCY_HZ) max_frequency_hz = RFM22_MAX_CARRIER_FREQUENCY_HZ; - - if (min_frequency_hz > max_frequency_hz) - { // swap them over - uint32_t tmp = min_frequency_hz; - min_frequency_hz = max_frequency_hz; - max_frequency_hz = tmp; - } + rfm22b_dev->min_frequency = RFM22B_DEFAULT_MIN_FREQUENCY; + rfm22b_dev->max_frequency = RFM22B_DEFAULT_MAX_FREQUENCY; + rfm22b_dev->frequency_hz = RFM22B_DEFAULT_FREQUENCY; // **************** // calibrate our RF module to be exactly on frequency .. different for every module @@ -1855,9 +1881,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // set frequency hopping channel step size (multiples of 10kHz) rfm22_write(RFM22_frequency_hopping_step_size, rfm22b_dev->frequency_hop_step_size_reg); - // set our nominal carrier frequency - rfm22_setNominalCarrierFrequency(rfm22b_dev, (min_frequency_hz + max_frequency_hz) / 2); - // set the tx power rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); @@ -1871,7 +1894,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_write(RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); rfm22_setFreqCalibration(rfm22b_dev->cfg.RFXtalCap); - rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->cfg.frequencyHz); + rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->frequency_hz); 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 e63289bf4..8ee2354ab 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -39,12 +39,7 @@ enum gpio_direction {GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX}; struct pios_rfm22b_cfg { const struct pios_spi_cfg * spi_cfg; /* Pointer to SPI interface configuration */ const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ - uint32_t frequencyHz; - uint32_t minFrequencyHz; - uint32_t maxFrequencyHz; uint8_t RFXtalCap; - uint32_t maxRFBandwidth; - uint8_t maxTxPower; uint8_t slave_num; enum gpio_direction gpio_direction; }; @@ -98,6 +93,7 @@ struct rfm22b_stats { /* 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_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_frequency, uint32_t max_frequency); 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); diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index a1fed423d..70cf2196b 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -577,14 +577,16 @@ enum pios_rfm22b_dev_magic { enum pios_rfm22b_state { RFM22B_STATE_UNINITIALIZED, RFM22B_STATE_INITIALIZING, + RFM22B_STATE_INITIALIZED, + RFM22B_STATE_REQUESTING_CONNECTION, + RFM22B_STATE_ACCEPTING_CONNECTION, + RFM22B_STATE_CONNECTION_ACCEPTED, + RFM22B_STATE_CONNECTION_DECLINED, RFM22B_STATE_RX_MODE, RFM22B_STATE_WAIT_PREAMBLE, RFM22B_STATE_WAIT_SYNC, RFM22B_STATE_RX_DATA, RFM22B_STATE_RECEIVING_STATUS, - RFM22B_STATE_ACCEPTING_CONNECTION, - RFM22B_STATE_CONNECTION_ACCEPTED, - RFM22B_STATE_CONNECTION_DECLINED, RFM22B_STATE_TX_START, RFM22B_STATE_TX_DATA, RFM22B_STATE_TIMEOUT, @@ -595,17 +597,19 @@ enum pios_rfm22b_state { }; enum pios_rfm22b_event { + RFM22B_EVENT_INT_RECEIVED, RFM22B_EVENT_INITIALIZE, RFM22B_EVENT_INITIALIZED, - RFM22B_EVENT_INT_RECEIVED, + RFM22B_EVENT_REQUEST_CONNECTION, + RFM22B_EVENT_REQUESTED_CONNECTION, + RFM22B_EVENT_CONNECTION_REQUEST, + RFM22B_EVENT_CONNECTION_ACCEPT, + RFM22B_EVENT_CONNECTION_DECLINED, RFM22B_EVENT_RX_MODE, RFM22B_EVENT_PREAMBLE_DETECTED, RFM22B_EVENT_SYNC_DETECTED, RFM22B_EVENT_RX_COMPLETE, RFM22B_EVENT_STATUS_RECEIVED, - RFM22B_EVENT_CONNECTION_REQUEST, - RFM22B_EVENT_CONNECTION_ACCEPT, - RFM22B_EVENT_CONNECTION_DECLINED, RFM22B_EVENT_SEND_PACKET, RFM22B_EVENT_TX_START, RFM22B_EVENT_TX_STARTED, @@ -715,7 +719,13 @@ struct pios_rfm22b_dev { uint16_t rx_buffer_wr; // the receive buffer write index uint16_t rx_packet_len; - + + // The minimum frequency + uint32_t min_frequency; + // The maximum frequency + uint32_t max_frequency; + // The current nominal frequency + uint32_t frequency_hz; // The frequency hopping step size float frequency_step_size; // current frequency hop channel diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 58b762a63..e69e0ed33 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -246,38 +246,6 @@ void PIOS_Board_Init(void) { } } - // Set the maximum radio RF power. - switch (oplinkSettings.MaxRFPower) - { - case OPLINKSETTINGS_MAXRFPOWER_125: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); - break; - case OPLINKSETTINGS_MAXRFPOWER_16: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); - break; - case OPLINKSETTINGS_MAXRFPOWER_316: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); - break; - case OPLINKSETTINGS_MAXRFPOWER_63: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); - break; - case OPLINKSETTINGS_MAXRFPOWER_126: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); - break; - case OPLINKSETTINGS_MAXRFPOWER_25: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); - break; - case OPLINKSETTINGS_MAXRFPOWER_50: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); - break; - case OPLINKSETTINGS_MAXRFPOWER_100: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); - break; - } - - // Set the radio destination ID. - PIOS_RFM22B_SetDestinationId(pios_rfm22b_id, oplinkSettings.PairID); - // Initialize the packet handler PacketHandlerConfig pios_ph_cfg = { .default_destination_id = 0xffffffff, // Broadcast diff --git a/flight/board_hw_defs/pipxtreme/board_hw_defs.c b/flight/board_hw_defs/pipxtreme/board_hw_defs.c index c43658bde..4b5c17264 100644 --- a/flight/board_hw_defs/pipxtreme/board_hw_defs.c +++ b/flight/board_hw_defs/pipxtreme/board_hw_defs.c @@ -222,12 +222,7 @@ static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = { struct pios_rfm22b_cfg pios_rfm22b_pipx_cfg = { .spi_cfg = &pios_spi_rfm22b_cfg, .exti_cfg = &pios_exti_rfm22b_cfg, - .frequencyHz = 434000000, - .minFrequencyHz = 434000000 - 2000000, - .maxFrequencyHz = 434000000 + 2000000, .RFXtalCap = 0x7f, - .maxRFBandwidth = 64000, - .maxTxPower = RFM22_tx_pwr_txpow_7, // +20dBm .. 100mW .slave_num = 0, .gpio_direction = GPIO0_TX_GPIO1_RX, }; diff --git a/flight/board_hw_defs/revomini/board_hw_defs.c b/flight/board_hw_defs/revomini/board_hw_defs.c index 7d9d8fe19..aab997f0a 100644 --- a/flight/board_hw_defs/revomini/board_hw_defs.c +++ b/flight/board_hw_defs/revomini/board_hw_defs.c @@ -395,12 +395,7 @@ static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = { const struct pios_rfm22b_cfg pios_rfm22b_rm1_cfg = { .spi_cfg = &pios_spi_telem_flash_cfg, .exti_cfg = &pios_exti_rfm22b_cfg, - .frequencyHz = 434000000, - .minFrequencyHz = 434000000 - 2000000, - .maxFrequencyHz = 434000000 + 2000000, .RFXtalCap = 0x7f, - .maxRFBandwidth = 64000, - .maxTxPower = RFM22_tx_pwr_txpow_7, // +20dBm .. 100mW .slave_num = 0, .gpio_direction = GPIO0_RX_GPIO1_TX, }; @@ -408,12 +403,7 @@ const struct pios_rfm22b_cfg pios_rfm22b_rm1_cfg = { const struct pios_rfm22b_cfg pios_rfm22b_rm2_cfg = { .spi_cfg = &pios_spi_telem_flash_cfg, .exti_cfg = &pios_exti_rfm22b_cfg, - .frequencyHz = 434000000, - .minFrequencyHz = 434000000 - 2000000, - .maxFrequencyHz = 434000000 + 2000000, .RFXtalCap = 0x7f, - .maxRFBandwidth = 64000, - .maxTxPower = RFM22_tx_pwr_txpow_7, // +20dBm .. 100mW .slave_num = 0, .gpio_direction = GPIO0_TX_GPIO1_RX, }; diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 428ee52a1..801d79325 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -65,7 +65,8 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkSettings", "SendTimeout", m_oplink->SendTimeout); addUAVObjectToWidgetRelation("OPLinkSettings", "MinPacketSize", m_oplink->MinPacketSize); addUAVObjectToWidgetRelation("OPLinkSettings", "FrequencyCalibration", m_oplink->FrequencyCalibration); - addUAVObjectToWidgetRelation("OPLinkSettings", "Frequency", m_oplink->Frequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "MinFrequency", m_oplink->MinFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaxFrequency); addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); addUAVObjectToWidgetRelation("OPLinkStatus", "RxCorrected", m_oplink->Corrected); diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 2ab148060..ca6ee6eb0 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -1449,7 +1449,7 @@ - Calibrate the modems RF carrier frequency + Set the send timeout true @@ -1484,7 +1484,7 @@ - Calibrate the modems RF carrier frequency + Set the minimum packet size true @@ -1532,7 +1532,7 @@ - Frequency (Hz) + Min. Frequency (Hz) Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -1540,7 +1540,7 @@ - + 0 @@ -1554,7 +1554,7 @@ - Set the modems RF carrier frequency + Set the modems minimum RF carrier frequency Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter @@ -1563,7 +1563,7 @@ true - 0 + 400000000 1000000000 @@ -1573,7 +1573,51 @@ - + + + + Max. Frequency (Hz) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Set the modems maximum RF carrier frequency + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 400000000 + + + 1000000000 + + + 100000 + + + + AES Encryption @@ -1642,7 +1686,7 @@ - + Qt::Vertical diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index f1633fd7a..2ee54b5a6 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -12,7 +12,8 @@ - + + From ca31fe99aca4e95a3fd0b0e94f0f051ee4991b6f Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 21 Oct 2012 19:49:58 -0400 Subject: [PATCH 05/31] RFM22B: Fixed removal of the OPLink configuration window when the OPLink is disconnected. --- ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index 19a773ae9..6aea1ce17 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -149,7 +149,7 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) // Create the timer that is used to timeout the connection to the OPLink. oplinkTimeout = new QTimer(this); - connect(oplinkTimeout, SIGNAL(timeout()),this,SLOT(onOPLinktremeDisconnect())); + connect(oplinkTimeout, SIGNAL(timeout()), this, SLOT(onOPLinkDisconnect())); oplinkConnected = false; } From 15fc29560a824166ae0d9e8aa28c1c6226dd5e66 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 21 Oct 2012 19:50:49 -0400 Subject: [PATCH 06/31] RM: Remove configuration of the RFM22B from the RevoMini pios_board.c. This is in preparation for auto-configuration. --- flight/RevoMini/System/pios_board.c | 40 ----------------------------- 1 file changed, 40 deletions(-) diff --git a/flight/RevoMini/System/pios_board.c b/flight/RevoMini/System/pios_board.c index 0b092d42b..fd5af508e 100644 --- a/flight/RevoMini/System/pios_board.c +++ b/flight/RevoMini/System/pios_board.c @@ -41,9 +41,6 @@ #include #include "hwsettings.h" #include "manualcontrolsettings.h" -#if defined(PIOS_INCLUDE_RFM22B) -#include -#endif /** * Sensor configurations @@ -632,43 +629,6 @@ void PIOS_Board_Init(void) { } } - // Initalize out UAVOs - OPLinkSettingsInitialize(); - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - - // Set the maximum radio RF power. - switch (oplinkSettings.MaxRFPower) - { - case OPLINKSETTINGS_MAXRFPOWER_125: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); - break; - case OPLINKSETTINGS_MAXRFPOWER_16: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); - break; - case OPLINKSETTINGS_MAXRFPOWER_316: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); - break; - case OPLINKSETTINGS_MAXRFPOWER_63: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); - break; - case OPLINKSETTINGS_MAXRFPOWER_126: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); - break; - case OPLINKSETTINGS_MAXRFPOWER_25: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); - break; - case OPLINKSETTINGS_MAXRFPOWER_50: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); - break; - case OPLINKSETTINGS_MAXRFPOWER_100: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); - break; - } - - // Set the radio destination ID. - PIOS_RFM22B_SetDestinationId(pios_rfm22b_id, oplinkSettings.PairID); - // Initialize the packet handler PacketHandlerConfig pios_ph_cfg = { .default_destination_id = 0xffffffff, // Broadcast From 9f43e1151c50f482c2dae3abaae42d0047597009 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 21 Oct 2012 19:51:27 -0400 Subject: [PATCH 07/31] RFM22B: Fixed status reporting and connection status for OPLink. --- flight/Modules/PipXtreme/pipxtrememod.c | 7 +- flight/PiOS/Common/pios_rfm22b.c | 276 +++++++++++++----------- 2 files changed, 161 insertions(+), 122 deletions(-) diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c index 8402574d7..2bbc7dc49 100644 --- a/flight/Modules/PipXtreme/pipxtrememod.c +++ b/flight/Modules/PipXtreme/pipxtrememod.c @@ -192,11 +192,16 @@ static void systemTask(void *parameters) prev_tx_count = tx_count; prev_rx_count = rx_count; } - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED; if (radio_stats.connected) + { + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_CONNECTED; LINK_LED_ON; + } else + { + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED; LINK_LED_OFF; + } // Update the object OPLinkStatusSet(&oplinkStatus); diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 565414053..bb7d266a8 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -178,6 +178,7 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status); static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len); static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz); +static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); // SPI read/write functions static void rfm22_assertCs(); @@ -661,56 +662,25 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if(!PIOS_RFM22B_validate(rfm22b_dev)) return; - *stats = rfm22b_dev->stats; - // Add the RX packet statistics - stats->rx_good = 0; - stats->rx_corrected = 0; - stats->rx_error = 0; - stats->rx_missed = 0; - for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) - { - uint32_t val = rfm22b_dev->rx_packet_stats[i]; - for (uint8_t j = 0; j < 16; ++j) - { - switch ((val >> (j * 2)) & 0x3) - { - case RFM22B_GOOD_RX_PACKET: - stats->rx_good++; - break; - case RFM22B_CORRECTED_RX_PACKET: - stats->rx_corrected++; - break; - case RFM22B_ERROR_RX_PACKET: - stats->rx_error++; - break; - case RFM22B_MISSED_RX_PACKET: - stats->rx_missed++; - break; - } - } - } - - // Calculate the link quality metric, which is related to the number of good packets in relation to the number of bad packets. - // Note: This assumes that the number of packets sampled for the stats is 64. - // Using this equation, error and missed packets are counted as -2, and corrected packets are counted as -1. - // The rage is 0 (all error or missed packets) to 128 (all good packets). - stats->link_quality = 64 + stats->rx_good - stats->rx_error - stats->rx_missed; + // Calculate the current link quality + rfm22_calculateLinkQuality(rfm22b_dev); // We are connected if our destination ID is in the pair stats. - stats->connected = false; + rfm22b_dev->stats.connected = false; if (rfm22b_dev->destination_id != 0xffffffff) for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { if ((rfm22b_dev->pair_stats[i].pairID == rfm22b_dev->destination_id) && (rfm22b_dev->pair_stats[i].rssi > -127)) { - stats->rssi = rfm22b_dev->pair_stats[i].rssi; - stats->afc_correction = rfm22b_dev->pair_stats[i].afc_correction; - stats->connected = true; + rfm22b_dev->stats.rssi = rfm22b_dev->pair_stats[i].rssi; + rfm22b_dev->stats.afc_correction = rfm22b_dev->pair_stats[i].afc_correction; + rfm22b_dev->stats.connected = true; break; } } + *stats = rfm22b_dev->stats; } /** @@ -847,6 +817,90 @@ static void PIOS_RFM22B_Task(void *parameters) } } +// ************************************ +// radio datarate about 19200 Baud +// radio frequency deviation 45kHz +// radio receiver bandwidth 67kHz. +// +// Carson's rule: +// The signal bandwidth is about 2(Delta-f + fm) .. +// +// Delta-f = frequency deviation +// fm = maximum frequency of the signal +// +// This gives 2(45 + 9.6) = 109.2kHz. + +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; + + 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[datarate]); + + // rfm22_afc_loop_gearshift_override + rfm22_write(0x1D, reg_1D[datarate]); + // RFM22_afc_timing_control + rfm22_write(0x1E, reg_1E[datarate]); + + // RFM22_clk_recovery_gearshift_override + rfm22_write(0x1F, reg_1F[datarate]); + // rfm22_clk_recovery_oversampling_ratio + rfm22_write(0x20, reg_20[datarate]); + // rfm22_clk_recovery_offset2 + rfm22_write(0x21, reg_21[datarate]); + // rfm22_clk_recovery_offset1 + rfm22_write(0x22, reg_22[datarate]); + // rfm22_clk_recovery_offset0 + rfm22_write(0x23, reg_23[datarate]); + // rfm22_clk_recovery_timing_loop_gain1 + rfm22_write(0x24, reg_24[datarate]); + // rfm22_clk_recovery_timing_loop_gain0 + rfm22_write(0x25, reg_25[datarate]); + + // rfm22_afc_limiter + rfm22_write(0x2A, reg_2A[datarate]); + +/* This breaks all bit rates < 100000! + if (datarate_bps < 100000) + // rfm22_chargepump_current_trimming_override + rfm22_write(0x58, 0x80); + else + // rfm22_chargepump_current_trimming_override + rfm22_write(0x58, 0xC0); +*/ + + // rfm22_tx_data_rate1 + rfm22_write(0x6E, reg_6E[datarate]); + // rfm22_tx_data_rate0 + rfm22_write(0x6F, reg_6F[datarate]); + + // Enable data whitening + // uint8_t txdtrtscale_bit = rfm22_read(RFM22_modulation_mode_control1) & RFM22_mmc1_txdtrtscale; + // rfm22_write(RFM22_modulation_mode_control1, txdtrtscale_bit | RFM22_mmc1_enwhite); + + if (!data_whitening) + // rfm22_modulation_mode_control1 + rfm22_write(0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); + else + // rfm22_modulation_mode_control1 + rfm22_write(0x70, reg_70[datarate] | RFM22_mmc1_enwhite); + + // rfm22_modulation_mode_control2 + rfm22_write(0x71, reg_71[datarate]); + + // rfm22_frequency_deviation + rfm22_write(0x72, reg_72[datarate]); + + rfm22_write(RFM22_ook_counter_value1, 0x00); + rfm22_write(RFM22_ook_counter_value2, 0x00); +} + // ************************************ // SPI read/write @@ -1026,99 +1080,54 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, rfm22_write(RFM22_nominal_carrier_frequency0, fc & 0xff); } -void rfm22_setFreqHopChannel(uint8_t channel) +/* +static void rfm22_setFreqHopChannel(uint8_t channel) { // set the frequency hopping channel g_rfm22b_dev->frequency_hop_channel = channel; rfm22_write(RFM22_frequency_hopping_channel_select, channel); } -uint32_t rfm22_freqHopSize(void) +static uint32_t rfm22_freqHopSize(void) { // return the frequency hopping step size return ((uint32_t)g_rfm22b_dev->frequency_hop_step_size_reg * 10000); } - -// ************************************ -// radio datarate about 19200 Baud -// radio frequency deviation 45kHz -// radio receiver bandwidth 67kHz. -// -// Carson's rule: -// The signal bandwidth is about 2(Delta-f + fm) .. -// -// Delta-f = frequency deviation -// fm = maximum frequency of the signal -// -// This gives 2(45 + 9.6) = 109.2kHz. - -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; - - 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[datarate]); - - // rfm22_afc_loop_gearshift_override - rfm22_write(0x1D, reg_1D[datarate]); - // RFM22_afc_timing_control - rfm22_write(0x1E, reg_1E[datarate]); - - // RFM22_clk_recovery_gearshift_override - rfm22_write(0x1F, reg_1F[datarate]); - // rfm22_clk_recovery_oversampling_ratio - rfm22_write(0x20, reg_20[datarate]); - // rfm22_clk_recovery_offset2 - rfm22_write(0x21, reg_21[datarate]); - // rfm22_clk_recovery_offset1 - rfm22_write(0x22, reg_22[datarate]); - // rfm22_clk_recovery_offset0 - rfm22_write(0x23, reg_23[datarate]); - // rfm22_clk_recovery_timing_loop_gain1 - rfm22_write(0x24, reg_24[datarate]); - // rfm22_clk_recovery_timing_loop_gain0 - rfm22_write(0x25, reg_25[datarate]); - - // rfm22_afc_limiter - rfm22_write(0x2A, reg_2A[datarate]); - -/* This breaks all bit rates < 100000! - if (datarate_bps < 100000) - // rfm22_chargepump_current_trimming_override - rfm22_write(0x58, 0x80); - else - // rfm22_chargepump_current_trimming_override - rfm22_write(0x58, 0xC0); */ - // rfm22_tx_data_rate1 - rfm22_write(0x6E, reg_6E[datarate]); - // rfm22_tx_data_rate0 - rfm22_write(0x6F, reg_6F[datarate]); +static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) +{ + // Add the RX packet statistics + rfm22b_dev->stats.rx_good = 0; + rfm22b_dev->stats.rx_corrected = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.rx_missed = 0; + for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) + { + uint32_t val = rfm22b_dev->rx_packet_stats[i]; + for (uint8_t j = 0; j < 16; ++j) + { + switch ((val >> (j * 2)) & 0x3) + { + case RFM22B_GOOD_RX_PACKET: + rfm22b_dev->stats.rx_good++; + break; + case RFM22B_CORRECTED_RX_PACKET: + rfm22b_dev->stats.rx_corrected++; + break; + case RFM22B_ERROR_RX_PACKET: + rfm22b_dev->stats.rx_error++; + break; + case RFM22B_MISSED_RX_PACKET: + rfm22b_dev->stats.rx_missed++; + break; + } + } + } - // Enable data whitening - // uint8_t txdtrtscale_bit = rfm22_read(RFM22_modulation_mode_control1) & RFM22_mmc1_txdtrtscale; - // rfm22_write(RFM22_modulation_mode_control1, txdtrtscale_bit | RFM22_mmc1_enwhite); - - if (!data_whitening) - // rfm22_modulation_mode_control1 - rfm22_write(0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); - else - // rfm22_modulation_mode_control1 - rfm22_write(0x70, reg_70[datarate] | RFM22_mmc1_enwhite); - - // rfm22_modulation_mode_control2 - rfm22_write(0x71, reg_71[datarate]); - - // rfm22_frequency_deviation - rfm22_write(0x72, reg_72[datarate]); - - rfm22_write(RFM22_ook_counter_value1, 0x00); - rfm22_write(RFM22_ook_counter_value2, 0x00); + // Calculate the link quality metric, which is related to the number of good packets in relation to the number of bad packets. + // Note: This assumes that the number of packets sampled for the stats is 64. + // Using this equation, error and missed packets are counted as -2, and corrected packets are counted as -1. + // The rage is 0 (all error or missed packets) to 128 (all good packets). + rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.rx_missed; } // ************************************ @@ -1248,10 +1257,15 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) { PHPacketHandle sph = (PHPacketHandle)&(rfm22b_dev->status_packet); + // Update the link quality metric. + rfm22_calculateLinkQuality(rfm22b_dev); + // Queue the status message rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); + rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; + rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; if (xQueueSend(rfm22b_dev->packetQueue, &sph, 0) != pdTRUE) return false; @@ -1406,7 +1420,8 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand rx_error = false; } - } else + } + else rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET); return !rx_error; @@ -1625,6 +1640,9 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b rfm22b_dev->pair_stats[min_idx].lastContact = 0; } + rfm22b_dev->stats.link_quality = status->link_quality; + rfm22b_dev->stats.rssi = status->received_rssi; + return RFM22B_EVENT_RX_COMPLETE; } @@ -1904,6 +1922,14 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) { rfm22b_dev->resets++; rfm22b_dev->packet_start_ticks = 0; + // Release the Tx packet if it's set. + if (rfm22b_dev->tx_packet != 0) + { + PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); + rfm22b_dev->tx_packet = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + } + rfm22b_dev->rx_buffer_wr = 0; return RFM22B_EVENT_TX_START; } @@ -1911,6 +1937,14 @@ static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev) { rfm22b_dev->resets++; rfm22b_dev->packet_start_ticks = 0; + // Release the Tx packet if it's set. + if (rfm22b_dev->tx_packet != 0) + { + PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); + rfm22b_dev->tx_packet = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + } + rfm22b_dev->rx_buffer_wr = 0; return RFM22B_EVENT_INITIALIZE; } From e36bd678e1a3c78815cc6407853786192ec108cf Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 21 Oct 2012 20:25:23 -0400 Subject: [PATCH 08/31] RFM22B: Allow a receive to happen between transmitts. --- flight/PiOS/Common/pios_rfm22b.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index bb7d266a8..b5df8a9f9 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -314,7 +314,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_txData, .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA, - [RFM22B_EVENT_TX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_COMPLETE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, @@ -785,6 +785,15 @@ static void PIOS_RFM22B_Task(void *parameters) while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) ; lastEventTicks = xTaskGetTickCount(); + } else + { + if (rfm22b_dev->state == RFM22B_STATE_RX_MODE) + { + // Try to start a transmission + enum pios_rfm22b_event event = RFM22B_EVENT_TX_START; + while(event != RFM22B_EVENT_NUM_EVENTS) + event = rfm22_process_state_transition(rfm22b_dev, event); + } } } From 04da5d2377d04467e27032796b3167219dbd66e6 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 26 Oct 2012 14:25:41 -0700 Subject: [PATCH 09/31] USB/COM/RFM22B: Added a standard com callback to test for link availability. This replaces the PIOS_USB_CheckAvailable function with a generic PIOS_COM_Available function. This is now used by the RFM22B com driver to test if a good link is up. This was originally written by James, and Brian merged it into the latest RFM22B branch. --- flight/Modules/Telemetry/telemetry.c | 9 ++++-- flight/PiOS/Common/pios_com.c | 21 +++++++++++++ flight/PiOS/Common/pios_rfm22b.c | 15 ++++++++- flight/PiOS/Common/pios_rfm22b_com.c | 37 ++++++++++++++--------- flight/PiOS/STM32F10x/pios_usb.c | 2 +- flight/PiOS/STM32F10x/pios_usb_cdc.c | 1 + flight/PiOS/STM32F10x/pios_usb_hid.c | 1 + flight/PiOS/STM32F4xx/pios_usb.c | 2 +- flight/PiOS/STM32F4xx/pios_usb_cdc.c | 1 + flight/PiOS/STM32F4xx/pios_usb_hid.c | 1 + flight/PiOS/inc/pios_com.h | 2 ++ flight/PiOS/inc/pios_rfm22b.h | 1 + flight/PiOS/inc/pios_usb.h | 2 +- flight/PiOS/pios.h | 7 +++++ flight/PipXtreme/System/inc/pios_config.h | 1 + flight/RevoMini/System/inc/pios_config.h | 2 +- 16 files changed, 82 insertions(+), 23 deletions(-) diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index 6ca98be32..dc8aa63a6 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -545,15 +545,18 @@ static void updateSettings() } /** - * Determine input/output com port (USB takes priority over telemetry port) + * Determine input/output com port as highest priority available */ static uint32_t getComPort() { #if defined(PIOS_INCLUDE_USB) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) + if ( PIOS_COM_Available(PIOS_COM_TELEM_USB) ) return PIOS_COM_TELEM_USB; else #endif /* PIOS_INCLUDE_USB */ - return telemetryPort; + if ( PIOS_COM_Available(telemetryPort) ) + return telemetryPort; + else + return 0; } /** diff --git a/flight/PiOS/Common/pios_com.c b/flight/PiOS/Common/pios_com.c index ba54c1e61..422f3ecc9 100644 --- a/flight/PiOS/Common/pios_com.c +++ b/flight/PiOS/Common/pios_com.c @@ -503,6 +503,27 @@ uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len return (bytes_from_fifo); } +/** + * Query if a com port is available for use. That can be + * used to check a link is established even if the device + * is valid. + */ +bool PIOS_COM_Available(uint32_t com_id) +{ + struct pios_com_dev * com_dev = (struct pios_com_dev *)com_id; + + if (!PIOS_COM_validate(com_dev)) { + return false; + } + + // If a driver does not provide a query method assume always + // available if valid + if (com_dev->driver->available == NULL) + return true; + + return (com_dev->driver->available)(com_dev->lower_id); +} + #endif /** diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index b5df8a9f9..0f747cd62 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -67,7 +67,7 @@ #define RFM22B_DEFAULT_MIN_FREQUENCY (RFM22B_DEFAULT_FREQUENCY - 2000000) #define RFM22B_DEFAULT_MAX_FREQUENCY (RFM22B_DEFAULT_FREQUENCY + 2000000) #define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_7 - +#define RFM22B_LINK_QUALITY_THRESHOLD 20 // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms @@ -732,6 +732,19 @@ bool PIOS_RFM22B_Send_Packet(uint32_t rfm22b_id, PHPacketHandle p, uint32_t max_ return true; } +/** + * Check the radio device for a valid connection + * \param[in] rfm22b_id The rfm22b device. + * Returns true if there is a valid connection to paired radio, false otherwise. + */ +bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_validate(rfm22b_dev)) + return false; + return rfm22b_dev->stats.connected && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD); +} + /** * The task that controls the radio state machine. */ diff --git a/flight/PiOS/Common/pios_rfm22b_com.c b/flight/PiOS/Common/pios_rfm22b_com.c index cccae53db..478376c17 100644 --- a/flight/PiOS/Common/pios_rfm22b_com.c +++ b/flight/PiOS/Common/pios_rfm22b_com.c @@ -34,19 +34,21 @@ #include /* Provide a COM driver */ -static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud); -static void PIOS_RFM22B_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context); -static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context); -static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail); -static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail); +static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud); +static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context); +static void PIOS_RFM22B_COM_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context); +static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail); +static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail); +static bool PIOS_RFM22B_COM_Available(uint32_t rfm22b_com_id); /* Local variables */ const struct pios_com_driver pios_rfm22b_com_driver = { - .set_baud = PIOS_RFM22B_ChangeBaud, - .tx_start = PIOS_RFM22B_TxStart, - .rx_start = PIOS_RFM22B_RxStart, - .bind_tx_cb = PIOS_RFM22B_RegisterTxCallback, - .bind_rx_cb = PIOS_RFM22B_RegisterRxCallback, + .set_baud = PIOS_RFM22B_COM_ChangeBaud, + .tx_start = PIOS_RFM22B_COM_TxStart, + .rx_start = PIOS_RFM22B_COM_RxStart, + .bind_tx_cb = PIOS_RFM22B_COM_RegisterTxCallback, + .bind_rx_cb = PIOS_RFM22B_COM_RegisterRxCallback, + .available = PIOS_RFM22B_COM_Available }; /** @@ -54,7 +56,7 @@ const struct pios_com_driver pios_rfm22b_com_driver = { * \param[in] rfm22b_id RFM22B name (GPS, TELEM, AUX) * \param[in] baud Requested baud rate */ -static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) +static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) { // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. enum rfm22b_datarate datarate = RFM22_datarate_64000; @@ -77,11 +79,11 @@ static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) RFM22_SetDatarate(rfm22b_id, datarate, true); } -static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail) +static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail) { } -static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) +static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (!PIOS_RFM22B_validate(rfm22b_dev)) @@ -112,7 +114,7 @@ static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) } } -static void PIOS_RFM22B_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) +static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (!PIOS_RFM22B_validate(rfm22b_dev)) @@ -126,7 +128,7 @@ static void PIOS_RFM22B_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rfm22b_dev->rx_in_cb = rx_in_cb; } -static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context) +static void PIOS_RFM22B_COM_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback tx_out_cb, uint32_t context) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (!PIOS_RFM22B_validate(rfm22b_dev)) @@ -139,3 +141,8 @@ static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback rfm22b_dev->tx_out_context = context; rfm22b_dev->tx_out_cb = tx_out_cb; } + +static bool PIOS_RFM22B_COM_Available(uint32_t rfm22b_id) +{ + return PIOS_RFM22B_LinkStatus(rfm22b_id); +} diff --git a/flight/PiOS/STM32F10x/pios_usb.c b/flight/PiOS/STM32F10x/pios_usb.c index edac4611a..bdbc0adc7 100644 --- a/flight/PiOS/STM32F10x/pios_usb.c +++ b/flight/PiOS/STM32F10x/pios_usb.c @@ -223,7 +223,7 @@ bool PIOS_USB_CableConnected(uint8_t id) * \return 0: interface not available * \note Applications shouldn't call this function directly, instead please use \ref PIOS_COM layer functions */ -bool PIOS_USB_CheckAvailable(uint8_t id) +bool PIOS_USB_CheckAvailable(uint32_t id) { struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_com_id; diff --git a/flight/PiOS/STM32F10x/pios_usb_cdc.c b/flight/PiOS/STM32F10x/pios_usb_cdc.c index 22d422077..abe4b7d3c 100644 --- a/flight/PiOS/STM32F10x/pios_usb_cdc.c +++ b/flight/PiOS/STM32F10x/pios_usb_cdc.c @@ -51,6 +51,7 @@ const struct pios_com_driver pios_usb_cdc_com_driver = { .rx_start = PIOS_USB_CDC_RxStart, .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_cdc_dev_magic { diff --git a/flight/PiOS/STM32F10x/pios_usb_hid.c b/flight/PiOS/STM32F10x/pios_usb_hid.c index 72a08b7a7..0047c7196 100644 --- a/flight/PiOS/STM32F10x/pios_usb_hid.c +++ b/flight/PiOS/STM32F10x/pios_usb_hid.c @@ -51,6 +51,7 @@ const struct pios_com_driver pios_usb_hid_com_driver = { .rx_start = PIOS_USB_HID_RxStart, .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_hid_dev_magic { diff --git a/flight/PiOS/STM32F4xx/pios_usb.c b/flight/PiOS/STM32F4xx/pios_usb.c index c040fb0e5..ae74d0e89 100644 --- a/flight/PiOS/STM32F4xx/pios_usb.c +++ b/flight/PiOS/STM32F4xx/pios_usb.c @@ -155,7 +155,7 @@ int32_t PIOS_USB_ChangeConnectionState(bool connected) * \return 0: interface not available */ uint32_t usb_found; -bool PIOS_USB_CheckAvailable(uint8_t id) +bool PIOS_USB_CheckAvailable(uint32_t id) { struct pios_usb_dev * usb_dev = (struct pios_usb_dev *) pios_usb_id; diff --git a/flight/PiOS/STM32F4xx/pios_usb_cdc.c b/flight/PiOS/STM32F4xx/pios_usb_cdc.c index cb468b6d1..c26997ccd 100644 --- a/flight/PiOS/STM32F4xx/pios_usb_cdc.c +++ b/flight/PiOS/STM32F4xx/pios_usb_cdc.c @@ -48,6 +48,7 @@ const struct pios_com_driver pios_usb_cdc_com_driver = { .rx_start = PIOS_USB_CDC_RxStart, .bind_tx_cb = PIOS_USB_CDC_RegisterTxCallback, .bind_rx_cb = PIOS_USB_CDC_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_cdc_dev_magic { diff --git a/flight/PiOS/STM32F4xx/pios_usb_hid.c b/flight/PiOS/STM32F4xx/pios_usb_hid.c index eb03debec..4fa50960c 100644 --- a/flight/PiOS/STM32F4xx/pios_usb_hid.c +++ b/flight/PiOS/STM32F4xx/pios_usb_hid.c @@ -52,6 +52,7 @@ const struct pios_com_driver pios_usb_hid_com_driver = { .rx_start = PIOS_USB_HID_RxStart, .bind_tx_cb = PIOS_USB_HID_RegisterTxCallback, .bind_rx_cb = PIOS_USB_HID_RegisterRxCallback, + .available = PIOS_USB_CheckAvailable, }; enum pios_usb_hid_dev_magic { diff --git a/flight/PiOS/inc/pios_com.h b/flight/PiOS/inc/pios_com.h index 10591f116..176a870b6 100644 --- a/flight/PiOS/inc/pios_com.h +++ b/flight/PiOS/inc/pios_com.h @@ -43,6 +43,7 @@ struct pios_com_driver { void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); + bool (*available)(uint32_t id); }; /* Public Functions */ @@ -56,6 +57,7 @@ extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...); extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms); +extern bool PIOS_COM_Available(uint32_t com_id); #endif /* PIOS_COM_H */ diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 8ee2354ab..51745dea9 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -101,6 +101,7 @@ extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); extern bool PIOS_RFM22B_Send_Packet(uint32_t rfm22b_id, PHPacketHandle p, uint32_t max_delay); extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs); +extern bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id); /* Global Variables */ extern const struct pios_com_driver pios_rfm22b_com_driver; diff --git a/flight/PiOS/inc/pios_usb.h b/flight/PiOS/inc/pios_usb.h index ad63ae3a8..24d1a0e3e 100644 --- a/flight/PiOS/inc/pios_usb.h +++ b/flight/PiOS/inc/pios_usb.h @@ -36,7 +36,7 @@ extern int32_t PIOS_USB_Reenumerate(); extern int32_t PIOS_USB_ChangeConnectionState(bool connected); extern bool PIOS_USB_CableConnected(uint8_t id); -extern bool PIOS_USB_CheckAvailable(uint8_t id); +extern bool PIOS_USB_CheckAvailable(uint32_t id); #endif /* PIOS_USB_H */ diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index 23c2b598a..de641011b 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -169,6 +169,13 @@ #include #endif +#if defined(PIOS_INCLUDE_RFM22B) +#include +#ifdef PIOS_INCLUDE_RFM22B_COM +#include +#endif +#endif + #include #define NELEMENTS(x) (sizeof(x) / sizeof(*(x))) diff --git a/flight/PipXtreme/System/inc/pios_config.h b/flight/PipXtreme/System/inc/pios_config.h index d94355b99..70c0768fa 100755 --- a/flight/PipXtreme/System/inc/pios_config.h +++ b/flight/PipXtreme/System/inc/pios_config.h @@ -39,6 +39,7 @@ #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_RFM22B +#define PIOS_INCLUDE_RFM22B_COM #define PIOS_INCLUDE_RCVR #define PIOS_INCLUDE_TIM diff --git a/flight/RevoMini/System/inc/pios_config.h b/flight/RevoMini/System/inc/pios_config.h index a4e18652c..dc2ec3c98 100644 --- a/flight/RevoMini/System/inc/pios_config.h +++ b/flight/RevoMini/System/inc/pios_config.h @@ -54,10 +54,10 @@ //#define PIOS_INCLUDE_GPIO #define PIOS_INCLUDE_EXTI #define PIOS_INCLUDE_RTC -#define PIOS_INCLUDE_WDG /* Variables related to the RFM22B functionality */ #define PIOS_INCLUDE_RFM22B +#define PIOS_INCLUDE_RFM22B_COM #define RFM22_EXT_INT_USE /* Select the sensors to include */ From 98026966f7b7eed0a30fc3e1a372c2ebaa714f4a Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 27 Oct 2012 08:32:01 -0700 Subject: [PATCH 10/31] RFM22B: Added PPM packets to rfm22 driver. --- .../PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 3 +- flight/PiOS/Common/pios_rfm22b.c | 109 +++++++++++++----- flight/PiOS/inc/pios_rfm22b_priv.h | 5 + 3 files changed, 83 insertions(+), 34 deletions(-) diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index 7844efa50..87f14d951 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -217,8 +217,7 @@ extern uint32_t pios_ppm_rcvr_id; // Receiver PPM input //------------------------- #define PIOS_PPM_MAX_DEVS 1 -#define PIOS_PPM_NUM_INPUTS 12 -#define PIOS_PPM_PACKET_UPDATE_PERIOD_MS 25 +#define PIOS_PPM_NUM_INPUTS 8 //------------------------- // Servo outputs diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 0f747cd62..5fb0dd571 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -59,7 +59,7 @@ /* Local Defines */ #define STACK_SIZE_BYTES 200 #define TASK_PRIORITY (tskIDLE_PRIORITY + 2) -#define ISR_TIMEOUT 5 // ms +#define ISR_TIMEOUT 2 // ms #define EVENT_QUEUE_SIZE 5 #define PACKET_QUEUE_SIZE 3 #define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_64000 @@ -71,9 +71,12 @@ // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms -// The time between updates over the radio link. +// The time between updates for sending stats the radio link. #define RADIOSTATS_UPDATE_PERIOD_MS 250 +// The time between PPM updates +#define PPM_UPDATE_PERIOD_MS 40 + // this is too adjust the RF module so that it is on frequency #define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default #define OSC_LOAD_CAP_1 0x7D // board 1 @@ -171,10 +174,12 @@ static enum pios_rfm22b_event rfm22_connectionDeclined(struct pios_rfm22b_dev *r static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event); +static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event); static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev); +static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status); static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len); static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz); @@ -605,7 +610,7 @@ static void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pio uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(PIOS_RFM22B_validate(rfm22b_dev)) + if (PIOS_RFM22B_validate(rfm22b_dev)) return rfm22b_dev->deviceID; else return 0; @@ -619,7 +624,7 @@ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) 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)) + if (!PIOS_RFM22B_validate(rfm22b_dev)) return; rfm22b_dev->tx_power = tx_pwr; } @@ -633,17 +638,22 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_frequency, uint32_t max_frequency) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_validate(rfm22b_dev)) + if (!PIOS_RFM22B_validate(rfm22b_dev)) return; rfm22b_dev->min_frequency = min_frequency; rfm22b_dev->max_frequency = max_frequency; rfm22_setNominalCarrierFrequency(rfm22b_dev, (max_frequency - min_frequency) / 2); } +/** + * Sets the radio destination ID. + * \param[in] rfm22b_id The RFM22B device index. + * \param[in] dest_id The destination ID. + */ 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)) + if (!PIOS_RFM22B_validate(rfm22b_dev)) return; rfm22b_dev->destination_id = (dest_id == 0) ? 0xffffffff : dest_id; // The first slot is reserved for our current pairID @@ -755,6 +765,7 @@ static void PIOS_RFM22B_Task(void *parameters) return; portTickType lastEventTicks = xTaskGetTickCount(); portTickType lastStatusTicks = lastEventTicks; + portTickType lastPPMTicks = lastEventTicks; while(1) { @@ -774,10 +785,7 @@ static void PIOS_RFM22B_Task(void *parameters) if ((event == RFM22B_EVENT_INT_RECEIVED) && ((rfm22b_dev->state == RFM22B_STATE_UNINITIALIZED) || (rfm22b_dev->state == RFM22B_STATE_INITIALIZING))) continue; - - // Process all state transitions. - while(event != RFM22B_EVENT_NUM_EVENTS) - event = rfm22_process_state_transition(rfm22b_dev, event); + rfm22_process_event(rfm22b_dev, event); } } else @@ -790,24 +798,17 @@ static void PIOS_RFM22B_Task(void *parameters) if ((ticksSinceEvent / portTICK_RATE_MS) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) { // Transsition through an error event. - enum pios_rfm22b_event event = RFM22B_EVENT_ERROR; - while(event != RFM22B_EVENT_NUM_EVENTS) - event = rfm22_process_state_transition(rfm22b_dev, event); + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); // Clear the event queue. + enum pios_rfm22b_event event; while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) ; lastEventTicks = xTaskGetTickCount(); - } else - { - if (rfm22b_dev->state == RFM22B_STATE_RX_MODE) - { - // Try to start a transmission - enum pios_rfm22b_event event = RFM22B_EVENT_TX_START; - while(event != RFM22B_EVENT_NUM_EVENTS) - event = rfm22_process_state_transition(rfm22b_dev, event); - } } + else if (rfm22b_dev->state == RFM22B_STATE_RX_MODE) + // Try to start a transmission + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); } // Have we locked up sending / receiving a packet? @@ -821,18 +822,21 @@ static void PIOS_RFM22B_Task(void *parameters) // Have we been sending this packet too long? if (((cur_ticks - rfm22b_dev->packet_start_ticks) / portTICK_RATE_MS) > (rfm22b_dev->max_packet_time * 3)) - { - enum pios_rfm22b_event event = RFM22B_EVENT_TIMEOUT; - while(event != RFM22B_EVENT_NUM_EVENTS) - event = rfm22_process_state_transition(rfm22b_dev, event); - } + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT); } - // Queue up a status packet if it's time. + // Queue up a ppm packet if it's time. portTickType curTicks = xTaskGetTickCount(); // Rollover + if (curTicks < lastPPMTicks) + lastPPMTicks = curTicks; + if (((curTicks - lastPPMTicks) / portTICK_RATE_MS) > PPM_UPDATE_PERIOD_MS) + if (rfm22_sendPPM(rfm22b_dev)) + lastPPMTicks = curTicks; + // Rollover if (curTicks < lastStatusTicks) lastStatusTicks = curTicks; + // Queue up a status packet if it's time. if (((curTicks - lastStatusTicks) / portTICK_RATE_MS) > RADIOSTATS_UPDATE_PERIOD_MS) if (rfm22_sendStatus(rfm22b_dev)) lastStatusTicks = curTicks; @@ -1058,6 +1062,13 @@ static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_ return RFM22B_EVENT_NUM_EVENTS; } +static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event) +{ + // Process all state transitions. + while(event != RFM22B_EVENT_NUM_EVENTS) + event = rfm22_process_state_transition(rfm22b_dev, event); +} + // ************************************ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz) @@ -1292,9 +1303,43 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) return false; // Process a SEND_PACKT event. - enum pios_rfm22b_event event = RFM22B_EVENT_SEND_PACKET; - while(event != RFM22B_EVENT_NUM_EVENTS) - event = rfm22_process_state_transition(rfm22b_dev, event); + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_SEND_PACKET); + + return true; +} + +static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) +{ +#ifdef PIOS_PPM_RECEIVER + // Just return if the PPM receiver is not configured. + if (PIOS_PPM_RECEIVER == 0) + return true; + + // See if we have any valid channels. + bool valid_input_detected = false; + for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i) + { + rfm22b_dev->ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i); + if(rfm22b_dev->ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT) + valid_input_detected = true; + } + + // Send the PPM packet if it's valid + if (valid_input_detected) + { + rfm22b_dev->ppm_packet.header.destination_id = rfm22b_dev->destination_id; + rfm22b_dev->ppm_packet.header.type = PACKET_TYPE_PPM; + rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); + + // Queue the packet + PHPacketHandle pph = (PHPacketHandle)&(rfm22b_dev->ppm_packet); + if (xQueueSend(rfm22b_dev->packetQueue, &pph, 0) != pdTRUE) + return false; + + // Process a SEND_PACKT event. + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_SEND_PACKET); + } +#endif return true; } @@ -1599,7 +1644,7 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) else if (rfm22b_dev->int_status1 & RFM22_is1_ipksent) { rfm22b_dev->stats.tx_count++; - rfm22b_dev->stats.tx_byte_count += rfm22b_dev->tx_packet->header.data_size; + rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet); // Free the tx packet PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); rfm22b_dev->tx_packet = 0; diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 70cf2196b..9aeb86845 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -738,6 +738,11 @@ struct pios_rfm22b_dev { // The status packet PHStatusPacket status_packet; +#ifdef PIOS_PPM_RECEIVER + // The PPM packet + PHPpmPacket ppm_packet; +#endif + // The maximum time (ms) that it should take to transmit / receive a packet. uint32_t max_packet_time; portTickType packet_start_ticks; From fa5f7a8fdd501a08da7e1068862316e23bbf1aff Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 3 Nov 2012 09:18:40 -0700 Subject: [PATCH 11/31] RFM22B: All packets are now ACKed, and added a formal connection request / accept that will be used or auto-configuring the remote modem. --- flight/Libraries/inc/packet_handler.h | 21 +- flight/Modules/PipXtreme/pipxtrememod.c | 9 +- .../Modules/RadioComBridge/RadioComBridge.c | 8 +- flight/PiOS/Common/pios_rfm22b.c | 434 ++++++++++++++---- flight/PiOS/Common/pios_rfm22b_com.c | 25 +- flight/PiOS/inc/pios_rfm22b.h | 4 +- flight/PiOS/inc/pios_rfm22b_priv.h | 56 ++- flight/PipXtreme/System/pios_board.c | 3 + 8 files changed, 411 insertions(+), 149 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 789807a04..744a73b97 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -42,8 +42,6 @@ typedef enum { PACKET_TYPE_NONE = 0, PACKET_TYPE_STATUS, // broadcasts status of this modem PACKET_TYPE_CON_REQUEST, // request a connection to another modem - PACKET_TYPE_CON_ACCEPT, // accecpt a connection to another modem - PACKET_TYPE_CON_DECLINE, // decline a connection to another modem PACKET_TYPE_DATA, // data packet (packet contains user data) PACKET_TYPE_PPM, // PPM relay values PACKET_TYPE_ACK, @@ -59,12 +57,19 @@ typedef struct { } PHPacketHeader; #define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY) -#define PH_PACKET_SIZE(p) (p->data + p->header.data_size - (uint8_t*)p + RS_ECC_NPARITY) +#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t*)(p) + RS_ECC_NPARITY) typedef struct { PHPacketHeader header; uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY]; } PHPacket, *PHPacketHandle; +typedef struct { + PHPacketHeader header; + uint16_t seq_num; + bool ready_to_send; + uint8_t ecc[RS_ECC_NPARITY]; +} PHAckNackPacket, *PHAckNackPacketHandle; + #define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; @@ -83,11 +88,11 @@ typedef struct { #define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - uint32_t datarate; - uint32_t frequencyHz; - uint32_t minFrequencyHz; - uint32_t maxFrequencyHz; - uint8_t maxTxPower; + uint8_t datarate; + uint32_t frequency_hz; + uint32_t min_frequency; + uint32_t max_frequency; + uint8_t max_tx_power; uint8_t ecc[RS_ECC_NPARITY]; } PHConnectionPacket, *PHConnectionPacketHandle; diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c index 2bbc7dc49..fe3a4fb32 100644 --- a/flight/Modules/PipXtreme/pipxtrememod.c +++ b/flight/Modules/PipXtreme/pipxtrememod.c @@ -192,16 +192,11 @@ static void systemTask(void *parameters) prev_tx_count = tx_count; prev_rx_count = rx_count; } - if (radio_stats.connected) - { - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_CONNECTED; + oplinkStatus.LinkState = radio_stats.link_state; + if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) LINK_LED_ON; - } else - { - oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISCONNECTED; LINK_LED_OFF; - } // Update the object OPLinkStatusSet(&oplinkStatus); diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index aff437525..ca3556e83 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -306,7 +306,7 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) uint32_t outputPort = PIOS_COM_TELEMETRY; #if defined(PIOS_INCLUDE_USB) // Determine output port (USB takes priority over telemetry port) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) + if (PIOS_COM_TELEM_USB && PIOS_COM_Available(PIOS_COM_TELEM_USB)) outputPort = PIOS_COM_TELEM_USB; #endif /* PIOS_INCLUDE_USB */ if(outputPort) @@ -325,10 +325,12 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) static int32_t RadioSendHandler(uint8_t *buf, int32_t length) { uint32_t outputPort = PIOS_COM_RADIO; - if(outputPort) + // Don't send any data unless the radio port is available. + if(outputPort && PIOS_COM_Available(outputPort)) return PIOS_COM_SendBuffer(outputPort, buf, length); else - return -1; + // For some reason, if this function returns failure, it prevents saving settings. + return length; } static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 5fb0dd571..bc5ecf9af 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -71,6 +71,9 @@ // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms +// The time between connection attempts when not connected +#define CONNECT_ATTEMPT_PERIOD_MS 250 // ms + // The time between updates for sending stats the radio link. #define RADIOSTATS_UPDATE_PERIOD_MS 250 @@ -159,7 +162,6 @@ static const uint8_t OUT_FF[64] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, /* Local function forwared declarations */ static void PIOS_RFM22B_Task(void *parameters); -static void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR); static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev); @@ -167,10 +169,14 @@ static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22 static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_initConnection(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_connectionAccepted(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_rfm22b_event rfm22_connectionDeclined(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event); @@ -196,6 +202,8 @@ static uint8_t rfm22_read_noclaim(uint8_t addr); /* Te state transition table */ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_STATES] = { + + // Initialization thread [RFM22B_STATE_UNINITIALIZED] = { .entry_fn = 0, .next_state = { @@ -206,27 +214,40 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_INITIALIZING] = { .entry_fn = rfm22_init, .next_state = { - //[RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_REQUESTING_CONNECTION, - [RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_INITIALIZED] = RFM22B_STATE_INITIATING_CONNECTION, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, + [RFM22B_STATE_INITIATING_CONNECTION] = { + .entry_fn = rfm22_initConnection, + .next_state = { + [RFM22B_EVENT_REQUEST_CONNECTION] = RFM22B_STATE_REQUESTING_CONNECTION, + [RFM22B_EVENT_WAIT_FOR_CONNECTION] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, [RFM22B_STATE_REQUESTING_CONNECTION] = { .entry_fn = rfm22_requestConnection, .next_state = { - [RFM22B_EVENT_REQUESTED_CONNECTION] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, [RFM22B_STATE_ACCEPTING_CONNECTION] = { .entry_fn = rfm22_acceptConnection, .next_state = { - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_CONNECTION_ACCEPTED] = RFM22B_STATE_SENDING_ACK, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, @@ -236,38 +257,35 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, - [RFM22B_STATE_CONNECTION_DECLINED] = { - .entry_fn = rfm22_connectionDeclined, - .next_state = { - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, + [RFM22B_STATE_RX_MODE] = { .entry_fn = rfm22_setRxMode, .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, [RFM22B_EVENT_SEND_PACKET] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, [RFM22B_STATE_WAIT_PREAMBLE] = { .entry_fn = rfm22_detectPreamble, .next_state = { - [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, [RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC, + [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, [RFM22B_EVENT_SEND_PACKET] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, @@ -276,9 +294,9 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_SYNC, [RFM22B_EVENT_SYNC_DETECTED] = RFM22B_STATE_RX_DATA, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, @@ -286,22 +304,46 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_rxData, .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA, - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK, [RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS, - [RFM22B_EVENT_CONNECTION_REQUEST] = RFM22B_STATE_ACCEPTING_CONNECTION, - [RFM22B_EVENT_CONNECTION_ACCEPT] = RFM22B_STATE_CONNECTION_ACCEPTED, - [RFM22B_EVENT_CONNECTION_DECLINED] = RFM22B_STATE_CONNECTION_DECLINED, + [RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION, + [RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK, + [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_RECEIVING_ACK] = { + .entry_fn = rfm22_receiveAck, + .next_state = { + [RFM22B_EVENT_CONNECTION_ACCEPTED] = RFM22B_STATE_CONNECTION_ACCEPTED, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_RECEIVING_NACK] = { + .entry_fn = rfm22_receiveNack, + .next_state = { + [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, [RFM22B_STATE_RECEIVING_STATUS] = { .entry_fn = rfm22_receiveStatus, .next_state = { - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, @@ -312,6 +354,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, @@ -319,18 +362,41 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_txData, .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA, - [RFM22B_EVENT_TX_COMPLETE] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_SENDING_ACK] = { + .entry_fn = rfm22_sendAck, + .next_state = { + [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_SENDING_NACK] = { + .entry_fn = rfm22_sendNack, + .next_state = { + [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, [RFM22B_STATE_TIMEOUT] = { .entry_fn = rfm22_timeout, .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, @@ -339,14 +405,13 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .next_state = { [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, [RFM22B_STATE_FATAL_ERROR] = { .entry_fn = rfm22_fatal_error, .next_state = { - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, }; @@ -411,6 +476,12 @@ static const uint8_t ss_reg_70[] = { 0x24, 0x2D}; // rfm22_modulation_mode_cont static const uint8_t ss_reg_71[] = { 0x2B, 0x23}; // rfm22_modulation_mode_control2 +static inline uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { + if(end_time > start_time) + return (end_time - start_time) * portTICK_RATE_MS; + return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; +} + bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev) { return (rfm22b_dev != NULL && rfm22b_dev->magic == PIOS_RFM22B_DEV_MAGIC); @@ -498,12 +569,18 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->stats.rx_error = 0; rfm22b_dev->stats.rx_missed = 0; rfm22b_dev->stats.tx_dropped = 0; + rfm22b_dev->stats.tx_resent = 0; rfm22b_dev->stats.resets = 0; rfm22b_dev->stats.timeouts = 0; rfm22b_dev->stats.link_quality = 0; rfm22b_dev->stats.rssi = 0; + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; rfm22b_dev->destination_id = 0xffffffff; + rfm22b_dev->coordinator = false; + rfm22b_dev->send_status = false; + rfm22b_dev->send_ppm = false; + rfm22b_dev->send_connection_request = false; // Bind the configuration to the device instance rfm22b_dev->cfg = *cfg; @@ -512,6 +589,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu // Initialize the packets. rfm22b_dev->rx_packet_len = 0; rfm22b_dev->tx_packet = NULL; + rfm22b_dev->prev_tx_packet = NULL; rfm22b_dev->tx_seq = 0; *rfm22b_id = (uint32_t)rfm22b_dev; @@ -577,7 +655,7 @@ bool PIOS_RFM22_EXT_Int(void) * \param[in] event The event to inject * \param[in] inISR Is this being called from an interrrup service routine? */ -static void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR) +void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR) { // Store the event. @@ -663,6 +741,22 @@ void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id) rfm22b_dev->pair_stats[0].lastContact = 0; } +/** + * Configures the radio as a coordinator or not. + * \param[in] rfm22b_id The RFM22B device index. + * \param[in] coordinator Sets as coordinator if true. + */ +void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) + return; + rfm22b_dev->coordinator = coordinator; + + // Re-initialize the radio device. + PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); +} + /** * Returns the device statistics RFM22B device. * \param[in] rfm22b_id The RFM22B device index. @@ -677,7 +771,6 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { rfm22_calculateLinkQuality(rfm22b_dev); // We are connected if our destination ID is in the pair stats. - rfm22b_dev->stats.connected = false; if (rfm22b_dev->destination_id != 0xffffffff) for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { @@ -686,10 +779,10 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { { rfm22b_dev->stats.rssi = rfm22b_dev->pair_stats[i].rssi; rfm22b_dev->stats.afc_correction = rfm22b_dev->pair_stats[i].afc_correction; - rfm22b_dev->stats.connected = true; break; } } + rfm22b_dev->stats.tx_resent = rfm22b_dev->state; *stats = rfm22b_dev->stats; } @@ -752,7 +845,7 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if(!PIOS_RFM22B_validate(rfm22b_dev)) return false; - return rfm22b_dev->stats.connected && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD); + return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD); } /** @@ -766,6 +859,7 @@ static void PIOS_RFM22B_Task(void *parameters) portTickType lastEventTicks = xTaskGetTickCount(); portTickType lastStatusTicks = lastEventTicks; portTickType lastPPMTicks = lastEventTicks; + portTickType lastConnectTicks = lastEventTicks; while(1) { @@ -792,10 +886,7 @@ static void PIOS_RFM22B_Task(void *parameters) { // Has it been too long since the last event? portTickType curTicks = xTaskGetTickCount(); - if (curTicks < lastEventTicks) - lastEventTicks = curTicks; - portTickType ticksSinceEvent = curTicks - lastEventTicks; - if ((ticksSinceEvent / portTICK_RATE_MS) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) + if (timeDifferenceMs(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) { // Transsition through an error event. rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); @@ -806,40 +897,33 @@ static void PIOS_RFM22B_Task(void *parameters) ; lastEventTicks = xTaskGetTickCount(); } - else if (rfm22b_dev->state == RFM22B_STATE_RX_MODE) - // Try to start a transmission - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); } - // Have we locked up sending / receiving a packet? - if (rfm22b_dev->packet_start_ticks > 0) - { - portTickType cur_ticks = xTaskGetTickCount(); - - // Did the clock wrap around? - if (cur_ticks < rfm22b_dev->packet_start_ticks) - rfm22b_dev->packet_start_ticks = (cur_ticks > 0) ? cur_ticks : 1; - - // Have we been sending this packet too long? - if (((cur_ticks - rfm22b_dev->packet_start_ticks) / portTICK_RATE_MS) > (rfm22b_dev->max_packet_time * 3)) - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT); - } - - // Queue up a ppm packet if it's time. portTickType curTicks = xTaskGetTickCount(); - // Rollover - if (curTicks < lastPPMTicks) - lastPPMTicks = curTicks; - if (((curTicks - lastPPMTicks) / portTICK_RATE_MS) > PPM_UPDATE_PERIOD_MS) - if (rfm22_sendPPM(rfm22b_dev)) + // Have we been sending this packet too long? + if ((rfm22b_dev->packet_start_ticks > 0) && (timeDifferenceMs(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT); + + // Queue up a connection request packet if it's necessary + else if (rfm22b_dev->coordinator && (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) && (timeDifferenceMs(lastConnectTicks, curTicks) > CONNECT_ATTEMPT_PERIOD_MS)) + { + lastConnectTicks = curTicks; + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE); + } + else + { + // Queue up a PPM packet if it's time. + if ((timeDifferenceMs(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) && rfm22_sendPPM(rfm22b_dev)) lastPPMTicks = curTicks; - // Rollover - if (curTicks < lastStatusTicks) - lastStatusTicks = curTicks; - // Queue up a status packet if it's time. - if (((curTicks - lastStatusTicks) / portTICK_RATE_MS) > RADIOSTATS_UPDATE_PERIOD_MS) - if (rfm22_sendStatus(rfm22b_dev)) + + // Queue up a status packet if it's time. + if ((timeDifferenceMs(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) && rfm22_sendStatus(rfm22b_dev)) lastStatusTicks = curTicks; + + // Can we kick of a transmit? + if (rfm22b_dev->prev_tx_packet && timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_PACKET_NACKED); + } } } @@ -865,6 +949,7 @@ void RFM22_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, bool d 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); + rfm22b_dev->max_ack_delay = 25; // rfm22_if_filter_bandwidth rfm22_write(0x1C, reg_1C[datarate]); @@ -1206,14 +1291,45 @@ static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) { - // See if there's a packet on the packet queue. - PHPacketHandle p; - if (xQueueReceive(rfm22b_dev->packetQueue, &p, 0) != pdTRUE) + PHPacketHandle p = NULL; + + // See if there's a packet ready to send. + if (rfm22b_dev->tx_packet) + p = rfm22b_dev->tx_packet; + + // Are we waiting for an ACK? + else { - // Clear the TX buffer. - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; - return RFM22B_EVENT_RX_MODE; + + // Don't send a packet if we're waiting for an ACK + if (rfm22b_dev->prev_tx_packet) + return RFM22B_EVENT_RX_MODE; + + // Is there a packet in the queue to send? + if (xQueueReceive(rfm22b_dev->packetQueue, &p, 0) != pdTRUE) + p = NULL; + + if (!p) + { + + // Try to get some data to send + bool need_yield = false; + p = &rfm22b_dev->data_packet; + p->header.type = PACKET_TYPE_DATA; + p->header.destination_id = rfm22b_dev->destination_id; + p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, + PH_MAX_DATA, NULL, &need_yield); + if (p->header.data_size == 0) + p = NULL; + + // Don't send any data until we're connected. + else if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) + return RFM22B_EVENT_RX_MODE; + } + } + if (!p) + return RFM22B_EVENT_RX_MODE; // Add the error correcting code. p->header.source_id = rfm22b_dev->deviceID; @@ -1288,22 +1404,27 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) { - PHPacketHandle sph = (PHPacketHandle)&(rfm22b_dev->status_packet); + // We don't send status if we're a coordinator + if (rfm22b_dev->coordinator) + return true; // Update the link quality metric. rfm22_calculateLinkQuality(rfm22b_dev); // Queue the status message - rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast + PHPacketHandle sph = (PHPacketHandle)&(rfm22b_dev->status_packet); + if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) + rfm22b_dev->status_packet.header.destination_id = rfm22b_dev->destination_id; + else + rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; + //rfm22b_dev->send_status = true; if (xQueueSend(rfm22b_dev->packetQueue, &sph, 0) != pdTRUE) return false; - - // Process a SEND_PACKT event. - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_SEND_PACKET); + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_START_TRANSFER); return true; } @@ -1444,7 +1565,7 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de else if (rfm22b_dev->int_status2 & !RFM22_is2_ipreaval) { // Waiting for sync timed out. - return RFM22B_EVENT_TX_START; + return RFM22B_EVENT_START_TRANSFER; } return RFM22B_EVENT_NUM_EVENTS; @@ -1573,10 +1694,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) ret_event = RFM22B_EVENT_STATUS_RECEIVED; break; case PACKET_TYPE_CON_REQUEST: - ret_event = RFM22B_EVENT_CONNECTION_REQUEST; - break; - case PACKET_TYPE_CON_ACCEPT: - ret_event = RFM22B_EVENT_CONNECTION_ACCEPT; + ret_event = RFM22B_EVENT_CONNECTION_REQUESTED; break; case PACKET_TYPE_DATA: { @@ -1585,6 +1703,12 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) (rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield); break; } + case PACKET_TYPE_ACK: + ret_event = RFM22B_EVENT_PACKET_ACKED; + break; + case PACKET_TYPE_NACK: + ret_event = RFM22B_EVENT_PACKET_NACKED; + break; default: break; } @@ -1602,6 +1726,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) { + enum pios_rfm22b_event ret_event = RFM22B_EVENT_NUM_EVENTS; // Read the device status registers if (!rfm22_readStatus(rfm22b_dev)) @@ -1645,16 +1770,93 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) { rfm22b_dev->stats.tx_count++; rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet); - // Free the tx packet - PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); + ret_event = (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) ? RFM22B_EVENT_TX_START : RFM22B_EVENT_RX_MODE; + + // Do we require an ACK? + if ((rfm22b_dev->tx_packet->header.type != PACKET_TYPE_ACK) && (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK)) + { + rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet; + rfm22b_dev->tx_complete_ticks = xTaskGetTickCount(); + } + else + // Free the tx packet + PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); + rfm22b_dev->tx_packet = 0; rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; // Start a new transaction rfm22b_dev->packet_start_ticks = 0; - return RFM22B_EVENT_TX_COMPLETE; } - return RFM22B_EVENT_NUM_EVENTS; + return ret_event; +} + +/** + * Send an ACK to a received packet. + * \param[in] rfm22b_dev The device structure + */ +static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) +{ + PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); + aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id; + aph->header.type = PACKET_TYPE_ACK; + aph->header.data_size = 0; + aph->seq_num = rfm22b_dev->rx_packet.header.seq_num; + //aph->ready_to_send = (uxQueueMessagesWaiting(rfm22b_dev->packetQueue) > 0); + aph->ready_to_send = false; + rfm22b_dev->tx_packet = (PHPacketHandle)aph; + return RFM22B_EVENT_START_TRANSFER; +} + +/** + * Send an NACK to a received packet. + * \param[in] rfm22b_dev The device structure + */ +static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) +{ + PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); + aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id; + aph->header.type = PACKET_TYPE_NACK; + aph->header.data_size = 0; + aph->seq_num = rfm22b_dev->rx_packet.header.seq_num; + rfm22b_dev->tx_packet = (PHPacketHandle)aph; + return RFM22B_EVENT_START_TRANSFER; +} + +/** + * Receive an ACK. + * \param[in] rfm22b_dev The device structure + */ +static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev) +{ + PHPacketHandle prev = rfm22b_dev->prev_tx_packet; + PHAckNackPacketHandle aph = (PHAckNackPacketHandle)&(rfm22b_dev->rx_packet); + + // Clear the previous TX packet. + rfm22b_dev->prev_tx_packet = NULL; + + // Was this a connection request? + if (prev->header.type == PACKET_TYPE_CON_REQUEST) + return RFM22B_EVENT_CONNECTION_ACCEPTED; + + // Should we try to start another TX? + if (aph->ready_to_send) + return RFM22B_EVENT_RX_MODE; + else + return RFM22B_EVENT_TX_START; +} + +/** + * Receive an MACK. + * \param[in] rfm22b_dev The device structure + */ +static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev) +{ + // Resend the previous TX packet. + rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; + rfm22b_dev->prev_tx_packet = NULL; + //rfm22b_dev->stats.tx_resent++; + return RFM22B_EVENT_START_TRANSFER; } /** @@ -1713,24 +1915,70 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b return RFM22B_EVENT_RX_COMPLETE; } +static enum pios_rfm22b_event rfm22_initConnection(struct pios_rfm22b_dev *rfm22b_dev) +{ + if (rfm22b_dev->coordinator) + return RFM22B_EVENT_REQUEST_CONNECTION; + else + return RFM22B_EVENT_WAIT_FOR_CONNECTION; +} + static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev) { - // Generate and set a connection request message. - return RFM22B_EVENT_RX_COMPLETE; + PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); + + // Set our connection state to requesting connection. + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; + + // Fill in the connection request + cph->header.destination_id = rfm22b_dev->destination_id; + cph->header.type = PACKET_TYPE_CON_REQUEST; + cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet)); + cph->datarate = rfm22b_dev->datarate; + cph->frequency_hz = rfm22b_dev->frequency_hz; + cph->min_frequency = rfm22b_dev->min_frequency; + cph->max_frequency = rfm22b_dev->max_frequency; + cph->max_tx_power = rfm22b_dev->tx_power; + rfm22b_dev->tx_packet = (PHPacketHandle)cph; + + return RFM22B_EVENT_START_TRANSFER; } static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) { - return RFM22B_EVENT_RX_COMPLETE; + // Set our connection state to connected + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + + // Copy the connection packet + PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->rx_packet)); + PHConnectionPacketHandle lcph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); + memcpy((uint8_t*)lcph, (uint8_t*)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); + + // Configure this modem from the connection request message. + PIOS_RFM22B_SetDestinationId((uint32_t)rfm22b_dev, cph->header.source_id); + RFM22_SetDatarate((uint32_t)rfm22b_dev, cph->datarate, true); + PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power); + rfm22b_dev->min_frequency = cph->min_frequency; + rfm22b_dev->max_frequency = cph->max_frequency; + rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->frequency_hz); + + return RFM22B_EVENT_CONNECTION_ACCEPTED; } static enum pios_rfm22b_event rfm22_connectionAccepted(struct pios_rfm22b_dev *rfm22b_dev) { - return RFM22B_EVENT_RX_COMPLETE; -} + PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); + + // Set our connection state to connected + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + + // Configure this modem from the connection request message. + RFM22_SetDatarate((uint32_t)rfm22b_dev, cph->datarate, true); + PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power); + rfm22b_dev->min_frequency = cph->min_frequency; + rfm22b_dev->max_frequency = cph->max_frequency; + rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->frequency_hz); -static enum pios_rfm22b_event rfm22_connectionDeclined(struct pios_rfm22b_dev *rfm22b_dev) -{ return RFM22B_EVENT_RX_COMPLETE; } @@ -1997,7 +2245,7 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; } rfm22b_dev->rx_buffer_wr = 0; - return RFM22B_EVENT_TX_START; + return RFM22B_EVENT_START_TRANSFER; } static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev) diff --git a/flight/PiOS/Common/pios_rfm22b_com.c b/flight/PiOS/Common/pios_rfm22b_com.c index 478376c17..2833c73f1 100644 --- a/flight/PiOS/Common/pios_rfm22b_com.c +++ b/flight/PiOS/Common/pios_rfm22b_com.c @@ -89,29 +89,8 @@ static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) if (!PIOS_RFM22B_validate(rfm22b_dev)) return; - // Get a packet when we see the sync - PHPacketHandle p = PHGetTXPacket(pios_packet_handler); - if (!p) - return; - - // Get some data to send - bool need_yield = false; - p->header.type = PACKET_TYPE_DATA; - p->header.destination_id = rfm22b_dev->destination_id; - p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, - PH_MAX_DATA, NULL, &need_yield); - if (p->header.data_size == 0) - { - PHReleaseTXPacket(pios_packet_handler, p); - return; - } - - // Send the data packet. - if (!PIOS_RFM22B_Send_Packet(rfm22b_id, p, 5)) - { - rfm22b_dev->stats.tx_dropped++; - PHReleaseTXPacket(pios_packet_handler, p); - } + // Send a signal to the radio to start a transmit. + PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_SEND_PACKET, false); } static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 51745dea9..20b617f54 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -83,12 +83,13 @@ struct rfm22b_stats { uint8_t rx_error; uint8_t rx_missed; uint8_t tx_dropped; + uint8_t tx_resent; uint8_t resets; uint8_t timeouts; uint8_t link_quality; int8_t rssi; int8_t afc_correction; - bool connected; + uint8_t link_state; }; /* Public Functions */ @@ -97,6 +98,7 @@ extern void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_frequ 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 void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); 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 9aeb86845..06f22a576 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -578,10 +578,11 @@ enum pios_rfm22b_state { RFM22B_STATE_UNINITIALIZED, RFM22B_STATE_INITIALIZING, RFM22B_STATE_INITIALIZED, + RFM22B_STATE_INITIATING_CONNECTION, + RFM22B_STATE_WAIT_FOR_CONNECTION, RFM22B_STATE_REQUESTING_CONNECTION, RFM22B_STATE_ACCEPTING_CONNECTION, RFM22B_STATE_CONNECTION_ACCEPTED, - RFM22B_STATE_CONNECTION_DECLINED, RFM22B_STATE_RX_MODE, RFM22B_STATE_WAIT_PREAMBLE, RFM22B_STATE_WAIT_SYNC, @@ -589,6 +590,10 @@ enum pios_rfm22b_state { RFM22B_STATE_RECEIVING_STATUS, RFM22B_STATE_TX_START, RFM22B_STATE_TX_DATA, + RFM22B_STATE_SENDING_ACK, + RFM22B_STATE_SENDING_NACK, + RFM22B_STATE_RECEIVING_ACK, + RFM22B_STATE_RECEIVING_NACK, RFM22B_STATE_TIMEOUT, RFM22B_STATE_ERROR, RFM22B_STATE_FATAL_ERROR, @@ -601,16 +606,18 @@ enum pios_rfm22b_event { RFM22B_EVENT_INITIALIZE, RFM22B_EVENT_INITIALIZED, RFM22B_EVENT_REQUEST_CONNECTION, - RFM22B_EVENT_REQUESTED_CONNECTION, - RFM22B_EVENT_CONNECTION_REQUEST, - RFM22B_EVENT_CONNECTION_ACCEPT, - RFM22B_EVENT_CONNECTION_DECLINED, + RFM22B_EVENT_WAIT_FOR_CONNECTION, + RFM22B_EVENT_CONNECTION_REQUESTED, + RFM22B_EVENT_CONNECTION_ACCEPTED, + RFM22B_EVENT_PACKET_ACKED, + RFM22B_EVENT_PACKET_NACKED, RFM22B_EVENT_RX_MODE, RFM22B_EVENT_PREAMBLE_DETECTED, RFM22B_EVENT_SYNC_DETECTED, RFM22B_EVENT_RX_COMPLETE, RFM22B_EVENT_STATUS_RECEIVED, RFM22B_EVENT_SEND_PACKET, + RFM22B_EVENT_START_TRANSFER, RFM22B_EVENT_TX_START, RFM22B_EVENT_TX_STARTED, RFM22B_EVENT_TX_COMPLETE, @@ -649,6 +656,9 @@ struct pios_rfm22b_dev { // The destination ID uint32_t destination_id; + // Is this device a coordinator? + bool coordinator; + // The task handle xTaskHandle taskHandle; @@ -704,8 +714,12 @@ struct pios_rfm22b_dev { // The packet queue handle xQueueHandle packetQueue; + // The tx data packet + PHPacket data_packet; // The current tx packet PHPacketHandle tx_packet; + // The previous tx packet (waiting for an ACK) + PHPacketHandle prev_tx_packet; // the tx data read index uint16_t tx_data_rd; // the tx data write index @@ -713,13 +727,32 @@ struct pios_rfm22b_dev { // The tx packet sequence number uint16_t tx_seq; - // The rx packet + // The rx data packet PHPacket rx_packet; // the receive buffer write index uint16_t rx_buffer_wr; // the receive buffer write index uint16_t rx_packet_len; + // The status packet + PHStatusPacket status_packet; + + // The ACK/NACK packet + PHAckNackPacket ack_nack_packet; + +#ifdef PIOS_PPM_RECEIVER + // The PPM packet + PHPpmPacket ppm_packet; +#endif + + // The connection packet. + PHConnectionPacket con_packet; + + // Send flags + bool send_status; + bool send_ppm; + bool send_connection_request; + // The minimum frequency uint32_t min_frequency; // The maximum frequency @@ -735,17 +768,11 @@ struct pios_rfm22b_dev { // afc correction reading (in Hz) int8_t afc_correction_Hz; - // The status packet - PHStatusPacket status_packet; - -#ifdef PIOS_PPM_RECEIVER - // The PPM packet - PHPpmPacket ppm_packet; -#endif - // The maximum time (ms) that it should take to transmit / receive a packet. uint32_t max_packet_time; portTickType packet_start_ticks; + portTickType tx_complete_ticks; + uint8_t max_ack_delay; }; @@ -753,6 +780,7 @@ struct pios_rfm22b_dev { bool PIOS_RFM22_EXT_Int(void); bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev); +void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event, bool inISR); // Global variable definitions diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index e69e0ed33..01cbedfab 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -246,6 +246,9 @@ void PIOS_Board_Init(void) { } } + /* Configure the RFM22B device as coordinator or not */ + PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); + // Initialize the packet handler PacketHandlerConfig pios_ph_cfg = { .default_destination_id = 0xffffffff, // Broadcast From 7aaa02268fee038a03325ffc956534453ac941ff Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 3 Nov 2012 16:03:23 -0700 Subject: [PATCH 12/31] RFM22B: Added TX Resent to OPLink status. Also removed the need for the packet queue. --- flight/Modules/PipXtreme/pipxtrememod.c | 1 + flight/PiOS/Common/pios_rfm22b.c | 79 ++++++------------- flight/PiOS/inc/pios_rfm22b.h | 1 - .../plugins/config/configpipxtremewidget.cpp | 1 + .../src/plugins/config/pipxtreme.ui | 61 ++++++++++++-- shared/uavobjectdefinition/oplinkstatus.xml | 1 + 6 files changed, 82 insertions(+), 62 deletions(-) diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c index fe3a4fb32..eb571600a 100644 --- a/flight/Modules/PipXtreme/pipxtrememod.c +++ b/flight/Modules/PipXtreme/pipxtrememod.c @@ -174,6 +174,7 @@ static void systemTask(void *parameters) oplinkStatus.RxErrors = radio_stats.rx_error; oplinkStatus.RxMissed = radio_stats.rx_missed; oplinkStatus.TxDropped = radio_stats.tx_dropped; // + data->droppedPackets; + oplinkStatus.TxResent = radio_stats.tx_resent; oplinkStatus.Resets = radio_stats.resets; oplinkStatus.Timeouts = radio_stats.timeouts; oplinkStatus.RSSI = radio_stats.rssi; diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index bc5ecf9af..d212d2c16 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -61,7 +61,6 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 2) #define ISR_TIMEOUT 2 // ms #define EVENT_QUEUE_SIZE 5 -#define PACKET_QUEUE_SIZE 3 #define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_64000 #define RFM22B_DEFAULT_FREQUENCY 434000000 #define RFM22B_DEFAULT_MIN_FREQUENCY (RFM22B_DEFAULT_FREQUENCY - 2000000) @@ -604,9 +603,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu // Create a semaphore to know when an rx packet is available vSemaphoreCreateBinary( rfm22b_dev->rxsem ); - // Create the packet queue. - rfm22b_dev->packetQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle)); - // Create our (hopefully) unique 32 bit id from the processor serial number. uint8_t crcs[] = { 0, 0, 0, 0 }; { @@ -782,7 +778,6 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { break; } } - rfm22b_dev->stats.tx_resent = rfm22b_dev->state; *stats = rfm22b_dev->stats; } @@ -809,32 +804,6 @@ uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t return mp; } -/** - * Insert a packet on the packet queue for sending. - * Note: If this finction succedds, the packet will be released by the driver, so no release is necessary. - * If this function doesn't success, the caller is still responsible for the packet. - * \param[in] rfm22b_id The rfm22b device. - * \param[in] p The packet handle. - * \param[in] max_delay The maximum time to delay waiting to queue the packet. - * \return true on success, false on failue to queue the packet. - */ -bool PIOS_RFM22B_Send_Packet(uint32_t rfm22b_id, PHPacketHandle p, uint32_t max_delay) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if(!PIOS_RFM22B_validate(rfm22b_dev)) - return false; - - // Store the packet handle in the packet queue - if (xQueueSend(rfm22b_dev->packetQueue, &p, max_delay) != pdTRUE) - return false; - - // Inject a send packet event - PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_SEND_PACKET, false); - - // Success - return true; -} - /** * Check the radio device for a valid connection * \param[in] rfm22b_id The rfm22b device. @@ -1305,9 +1274,25 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) if (rfm22b_dev->prev_tx_packet) return RFM22B_EVENT_RX_MODE; - // Is there a packet in the queue to send? - if (xQueueReceive(rfm22b_dev->packetQueue, &p, 0) != pdTRUE) - p = NULL; + if (!p && rfm22b_dev->send_connection_request) + { + p = (PHPacketHandle)&(rfm22b_dev->con_packet); + rfm22b_dev->send_connection_request = false; + } + +#ifdef PIOS_PPM_RECEIVER + if (!p && rfm22b_dev->send_ppm) + { + p = (PHPacketHandle)&(rfm22b_dev->ppm_packet); + rfm22b_dev->send_ppm = false; + } +#endif + + if (!p && rfm22b_dev->send_status) + { + p = (PHPacketHandle)&(rfm22b_dev->status_packet); + rfm22b_dev->send_status = false; + } if (!p) { @@ -1412,7 +1397,6 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) rfm22_calculateLinkQuality(rfm22b_dev); // Queue the status message - PHPacketHandle sph = (PHPacketHandle)&(rfm22b_dev->status_packet); if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) rfm22b_dev->status_packet.header.destination_id = rfm22b_dev->destination_id; else @@ -1421,9 +1405,7 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; - //rfm22b_dev->send_status = true; - if (xQueueSend(rfm22b_dev->packetQueue, &sph, 0) != pdTRUE) - return false; + rfm22b_dev->send_status = true; rfm22_process_event(rfm22b_dev, RFM22B_EVENT_START_TRANSFER); return true; @@ -1451,13 +1433,7 @@ static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->ppm_packet.header.destination_id = rfm22b_dev->destination_id; rfm22b_dev->ppm_packet.header.type = PACKET_TYPE_PPM; rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); - - // Queue the packet - PHPacketHandle pph = (PHPacketHandle)&(rfm22b_dev->ppm_packet); - if (xQueueSend(rfm22b_dev->packetQueue, &pph, 0) != pdTRUE) - return false; - - // Process a SEND_PACKT event. + rfm22b_dev->send_ppm = true; rfm22_process_event(rfm22b_dev, RFM22B_EVENT_SEND_PACKET); } #endif @@ -1731,8 +1707,6 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // Read the device status registers if (!rfm22_readStatus(rfm22b_dev)) { - // Free the tx packet - PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); rfm22b_dev->tx_packet = 0; rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; return RFM22B_EVENT_ERROR; @@ -1741,8 +1715,6 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // FIFO under/over flow error. Back to RX mode. if (rfm22b_dev->device_status & (RFM22_ds_ffunfl | RFM22_ds_ffovfl)) { - // Free the tx packet - PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); rfm22b_dev->tx_packet = 0; rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; return RFM22B_EVENT_ERROR; @@ -1778,9 +1750,6 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet; rfm22b_dev->tx_complete_ticks = xTaskGetTickCount(); } - else - // Free the tx packet - PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); rfm22b_dev->tx_packet = 0; rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; @@ -1855,7 +1824,7 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d // Resend the previous TX packet. rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; rfm22b_dev->prev_tx_packet = NULL; - //rfm22b_dev->stats.tx_resent++; + rfm22b_dev->stats.tx_resent++; return RFM22B_EVENT_START_TRANSFER; } @@ -1939,7 +1908,7 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf cph->min_frequency = rfm22b_dev->min_frequency; cph->max_frequency = rfm22b_dev->max_frequency; cph->max_tx_power = rfm22b_dev->tx_power; - rfm22b_dev->tx_packet = (PHPacketHandle)cph; + rfm22b_dev->send_connection_request = true; return RFM22B_EVENT_START_TRANSFER; } @@ -2240,7 +2209,6 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) // Release the Tx packet if it's set. if (rfm22b_dev->tx_packet != 0) { - PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); rfm22b_dev->tx_packet = 0; rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; } @@ -2255,7 +2223,6 @@ static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev) // Release the Tx packet if it's set. if (rfm22b_dev->tx_packet != 0) { - PHReleaseTXPacket(pios_packet_handler, rfm22b_dev->tx_packet); rfm22b_dev->tx_packet = 0; rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; } diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 20b617f54..81619f6ac 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -101,7 +101,6 @@ extern void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id); extern void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); -extern bool PIOS_RFM22B_Send_Packet(uint32_t rfm22b_id, PHPacketHandle p, uint32_t max_delay); extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs); extern bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id); diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 801d79325..353d0fc16 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -74,6 +74,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkStatus", "RxMissed", m_oplink->Missed); addUAVObjectToWidgetRelation("OPLinkStatus", "UAVTalkErrors", m_oplink->UAVTalkErrors); addUAVObjectToWidgetRelation("OPLinkStatus", "TxDropped", m_oplink->Dropped); + addUAVObjectToWidgetRelation("OPLinkStatus", "TxResent", m_oplink->Resent); addUAVObjectToWidgetRelation("OPLinkStatus", "Resets", m_oplink->Resets); addUAVObjectToWidgetRelation("OPLinkStatus", "Timeouts", m_oplink->Timeouts); addUAVObjectToWidgetRelation("OPLinkStatus", "RSSI", m_oplink->RSSI); diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index ca6ee6eb0..09e83bdc5 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -1150,6 +1150,57 @@ + + + TX Resent + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + The number of packets that were unable to be transmitted + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + UAVTalk Errors @@ -1159,7 +1210,7 @@ - + @@ -1191,7 +1242,7 @@ - + Resets @@ -1201,7 +1252,7 @@ - + @@ -1239,7 +1290,7 @@ - + Timeouts @@ -1249,7 +1300,7 @@ - + diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml index e93052cf0..a8d23fa35 100755 --- a/shared/uavobjectdefinition/oplinkstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -12,6 +12,7 @@ + From 0dce12e984b730c02d01319fca663f44f27df070 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 4 Nov 2012 17:50:12 -0700 Subject: [PATCH 13/31] RFM22B: Finised handshacking on ACK and changed the link quality metric to use resent packets rather than missed packets. --- flight/Libraries/inc/packet_handler.h | 2 +- flight/PiOS/Common/pios_rfm22b.c | 145 ++++++++++++++++---------- flight/PiOS/inc/pios_rfm22b_priv.h | 3 +- 3 files changed, 92 insertions(+), 58 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 744a73b97..e0237023e 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -63,9 +63,9 @@ typedef struct { uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY]; } PHPacket, *PHPacketHandle; +#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - uint16_t seq_num; bool ready_to_send; uint8_t ecc[RS_ECC_NPARITY]; } PHAckNackPacket, *PHAckNackPacketHandle; diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index d212d2c16..15af429f6 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -189,6 +189,7 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len); static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz); static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); +static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev); // SPI read/write functions static void rfm22_assertCs(); @@ -304,6 +305,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_RX_DATA, [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_SENDING_ACK, + [RFM22B_EVENT_RX_ERROR] = RFM22B_STATE_SENDING_NACK, [RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS, [RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION, [RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK, @@ -555,7 +557,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu } // Initlize the link stats. - rfm22b_dev->prev_rx_seq_num = 0; for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) rfm22b_dev->rx_packet_stats[i] = 0; @@ -590,6 +591,8 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->tx_packet = NULL; rfm22b_dev->prev_tx_packet = NULL; rfm22b_dev->tx_seq = 0; + rfm22b_dev->prev_rx_seq_num = 0; + rfm22b_dev->data_packet.header.data_size = 0; *rfm22b_id = (uint32_t)rfm22b_dev; g_rfm22b_dev = rfm22b_dev; @@ -828,7 +831,6 @@ static void PIOS_RFM22B_Task(void *parameters) portTickType lastEventTicks = xTaskGetTickCount(); portTickType lastStatusTicks = lastEventTicks; portTickType lastPPMTicks = lastEventTicks; - portTickType lastConnectTicks = lastEventTicks; while(1) { @@ -873,12 +875,6 @@ static void PIOS_RFM22B_Task(void *parameters) if ((rfm22b_dev->packet_start_ticks > 0) && (timeDifferenceMs(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT); - // Queue up a connection request packet if it's necessary - else if (rfm22b_dev->coordinator && (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) && (timeDifferenceMs(lastConnectTicks, curTicks) > CONNECT_ATTEMPT_PERIOD_MS)) - { - lastConnectTicks = curTicks; - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE); - } else { // Queue up a PPM packet if it's time. @@ -891,7 +887,10 @@ static void PIOS_RFM22B_Task(void *parameters) // Can we kick of a transmit? if (rfm22b_dev->prev_tx_packet && timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) + { + rfm22b_dev->tx_complete_ticks = curTicks; rfm22_process_event(rfm22b_dev, RFM22B_EVENT_PACKET_NACKED); + } } } } @@ -1186,7 +1185,7 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->stats.rx_good = 0; rfm22b_dev->stats.rx_corrected = 0; rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.rx_missed = 0; + rfm22b_dev->stats.tx_resent = 0; for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) { uint32_t val = rfm22b_dev->rx_packet_stats[i]; @@ -1203,8 +1202,8 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) case RFM22B_ERROR_RX_PACKET: rfm22b_dev->stats.rx_error++; break; - case RFM22B_MISSED_RX_PACKET: - rfm22b_dev->stats.rx_missed++; + case RFM22B_RESENT_TX_PACKET: + rfm22b_dev->stats.tx_resent++; break; } } @@ -1212,9 +1211,9 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) // Calculate the link quality metric, which is related to the number of good packets in relation to the number of bad packets. // Note: This assumes that the number of packets sampled for the stats is 64. - // Using this equation, error and missed packets are counted as -2, and corrected packets are counted as -1. - // The rage is 0 (all error or missed packets) to 128 (all good packets). - rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.rx_missed; + // Using this equation, error and resent packets are counted as -2, and corrected packets are counted as -1. + // The range is 0 (all error or resent packets) to 128 (all good packets). + rfm22b_dev->stats.link_quality = 64 + rfm22b_dev->stats.rx_good - rfm22b_dev->stats.rx_error - rfm22b_dev->stats.tx_resent; } // ************************************ @@ -1258,6 +1257,26 @@ static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev // ************************************ +static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev) +{ + // Is there a status of PPM packet ready to send? + if (rfm22b_dev->send_ppm || rfm22b_dev->send_status) + return true; + + // Is there some data ready to sent? + PHPacketHandle dp = &rfm22b_dev->data_packet; + if (dp->header.data_size > 0) + return true; + bool need_yield = false; + dp->header.data_size = + (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, dp->data, + PH_MAX_DATA, NULL, &need_yield); + if (dp->header.data_size > 0) + return true; + + return false; +} + static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) { PHPacketHandle p = NULL; @@ -1302,23 +1321,28 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) p = &rfm22b_dev->data_packet; p->header.type = PACKET_TYPE_DATA; p->header.destination_id = rfm22b_dev->destination_id; - p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, - PH_MAX_DATA, NULL, &need_yield); + if (p->header.data_size == 0) + p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, + PH_MAX_DATA, NULL, &need_yield); if (p->header.data_size == 0) p = NULL; // Don't send any data until we're connected. else if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) + { + p->header.data_size = 0; return RFM22B_EVENT_RX_MODE; + } } + if (p) + p->header.seq_num = rfm22b_dev->tx_seq++; } if (!p) return RFM22B_EVENT_RX_MODE; // Add the error correcting code. p->header.source_id = rfm22b_dev->deviceID; - p->header.seq_num = rfm22b_dev->tx_seq++; encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); rfm22b_dev->tx_packet = p; @@ -1549,46 +1573,47 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len) { - static bool first_packet = true; - uint16_t seq_num = p->header.seq_num; - uint16_t prev_seq_num = rfm22b_dev->prev_rx_seq_num; - uint16_t missed_packets = ((seq_num < prev_seq_num) ? (0xffff - prev_seq_num + seq_num) : (seq_num - prev_seq_num)) - 1; - rfm22b_dev->prev_rx_seq_num = seq_num; - if (first_packet) - { - missed_packets = 0; - first_packet = false; - } - - // Add any missed packets into the stats. - for ( ; missed_packets > 0; --missed_packets) - rfm22b_add_rx_status(rfm22b_dev, RFM22B_MISSED_RX_PACKET); // Attempt to correct any errors in the packet. decode_data((unsigned char*)p, rx_len); - // Check if there were any errors - bool rx_error = check_syndrome() != 0; - if(rx_error) + bool good_packet = check_syndrome() == 0; + bool corrected_packet = false; + // We have an error. Try to correct it. + if(!good_packet && (correct_errors_erasures((unsigned char*)p, rx_len, 0, 0) != 0)) + // We corrected it + corrected_packet = true; + + // Add any missed packets into the stats. + bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_NACK)); + if (!ack_nack_packet && (good_packet || corrected_packet) && (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED)) { - // We have an error. Try to correct it. - if (correct_errors_erasures((unsigned char*)p, rx_len, 0, 0) == 0) - { - // We couldn't correct the error, so drop the packet. - rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET); - } + static bool first_time = true; + uint16_t seq_num = p->header.seq_num; + uint16_t missed_packets = 0; + if (first_time) + first_time = false; else { - // We corrected the error. - rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET); - rx_error = false; + uint16_t prev_seq_num = rfm22b_dev->prev_rx_seq_num; + if (seq_num > prev_seq_num) + missed_packets = seq_num - prev_seq_num - 1; } - + rfm22b_dev->stats.rx_missed += missed_packets; + rfm22b_dev->prev_rx_seq_num = seq_num; } - else - rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET); - return !rx_error; + // Set the packet status + if (good_packet) + rfm22b_add_rx_status(rfm22b_dev, RFM22B_GOOD_RX_PACKET); + else if(corrected_packet) + // We corrected the error. + rfm22b_add_rx_status(rfm22b_dev, RFM22B_CORRECTED_RX_PACKET); + else + // We couldn't correct the error, so drop the packet. + rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET); + + return good_packet || corrected_packet; } static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) @@ -1689,6 +1714,8 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) break; } } + else + ret_event = RFM22B_EVENT_RX_ERROR; rfm22b_dev->rx_buffer_wr = 0; } @@ -1769,10 +1796,9 @@ static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id; aph->header.type = PACKET_TYPE_ACK; - aph->header.data_size = 0; - aph->seq_num = rfm22b_dev->rx_packet.header.seq_num; - //aph->ready_to_send = (uxQueueMessagesWaiting(rfm22b_dev->packetQueue) > 0); - aph->ready_to_send = false; + aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); + aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; + aph->ready_to_send = rfm22_ready_to_send(rfm22b_dev); rfm22b_dev->tx_packet = (PHPacketHandle)aph; return RFM22B_EVENT_START_TRANSFER; } @@ -1786,8 +1812,8 @@ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id; aph->header.type = PACKET_TYPE_NACK; - aph->header.data_size = 0; - aph->seq_num = rfm22b_dev->rx_packet.header.seq_num; + aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); + aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; rfm22b_dev->tx_packet = (PHPacketHandle)aph; return RFM22B_EVENT_START_TRANSFER; } @@ -1799,16 +1825,22 @@ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev) { PHPacketHandle prev = rfm22b_dev->prev_tx_packet; - PHAckNackPacketHandle aph = (PHAckNackPacketHandle)&(rfm22b_dev->rx_packet); // Clear the previous TX packet. rfm22b_dev->prev_tx_packet = NULL; // Was this a connection request? - if (prev->header.type == PACKET_TYPE_CON_REQUEST) + switch (prev->header.type) + { + case PACKET_TYPE_CON_REQUEST: return RFM22B_EVENT_CONNECTION_ACCEPTED; + case PACKET_TYPE_DATA: + rfm22b_dev->data_packet.header.data_size = 0; + break; + } // Should we try to start another TX? + PHAckNackPacketHandle aph = (PHAckNackPacketHandle)&(rfm22b_dev->rx_packet); if (aph->ready_to_send) return RFM22B_EVENT_RX_MODE; else @@ -1824,7 +1856,8 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d // Resend the previous TX packet. rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; rfm22b_dev->prev_tx_packet = NULL; - rfm22b_dev->stats.tx_resent++; + if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) + rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET); return RFM22B_EVENT_START_TRANSFER; } diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 06f22a576..7abd9ded1 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -615,6 +615,7 @@ enum pios_rfm22b_event { RFM22B_EVENT_PREAMBLE_DETECTED, RFM22B_EVENT_SYNC_DETECTED, RFM22B_EVENT_RX_COMPLETE, + RFM22B_EVENT_RX_ERROR, RFM22B_EVENT_STATUS_RECEIVED, RFM22B_EVENT_SEND_PACKET, RFM22B_EVENT_START_TRANSFER, @@ -633,7 +634,7 @@ enum pios_rfm22b_rx_packet_status { RFM22B_GOOD_RX_PACKET = 0x00, RFM22B_CORRECTED_RX_PACKET = 0x01, RFM22B_ERROR_RX_PACKET = 0x2, - RFM22B_MISSED_RX_PACKET = 0x3 + RFM22B_RESENT_TX_PACKET = 0x3 }; typedef struct { From 6ed9b63da91a52eab67cb91e3692d00891dc8610 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 11 Nov 2012 08:52:53 -0700 Subject: [PATCH 14/31] RFM22B: Changing configuration parameters over-the-air working. Added tracking of Tx/Rx sequence number. Still seeing too many resent packets. --- flight/Modules/PipXtreme/pipxtrememod.c | 2 + flight/PiOS/Common/pios_rfm22b.c | 274 +++++++++--------- flight/PiOS/Common/pios_rfm22b_com.c | 8 +- flight/PiOS/inc/pios_rfm22b.h | 6 +- flight/PiOS/inc/pios_rfm22b_priv.h | 1 + flight/PipXtreme/System/pios_board.c | 7 +- .../plugins/config/configpipxtremewidget.cpp | 2 + .../src/plugins/config/pipxtreme.ui | 104 ++++++- shared/uavobjectdefinition/oplinkstatus.xml | 2 + 9 files changed, 259 insertions(+), 147 deletions(-) diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c index eb571600a..d834af320 100644 --- a/flight/Modules/PipXtreme/pipxtrememod.c +++ b/flight/Modules/PipXtreme/pipxtrememod.c @@ -193,6 +193,8 @@ static void systemTask(void *parameters) prev_tx_count = tx_count; prev_rx_count = rx_count; } + oplinkStatus.TXSeq = radio_stats.tx_seq; + oplinkStatus.RXSeq = radio_stats.rx_seq; oplinkStatus.LinkState = radio_stats.link_state; if (radio_stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) LINK_LED_ON; diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 15af429f6..01e912afd 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -61,7 +61,7 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 2) #define ISR_TIMEOUT 2 // ms #define EVENT_QUEUE_SIZE 5 -#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_64000 +#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_32000 #define RFM22B_DEFAULT_FREQUENCY 434000000 #define RFM22B_DEFAULT_MIN_FREQUENCY (RFM22B_DEFAULT_FREQUENCY - 2000000) #define RFM22B_DEFAULT_MAX_FREQUENCY (RFM22B_DEFAULT_FREQUENCY + 2000000) @@ -76,6 +76,9 @@ // The time between updates for sending stats the radio link. #define RADIOSTATS_UPDATE_PERIOD_MS 250 +// The number of stats updates that a modem can miss before it's considered disconnected +#define MAX_RADIOSTATS_MISS_COUNT 3 + // The time between PPM updates #define PPM_UPDATE_PERIOD_MS 40 @@ -162,6 +165,7 @@ static const uint8_t OUT_FF[64] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, /* Local function forwared declarations */ static void PIOS_RFM22B_Task(void *parameters); static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening); static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev); @@ -175,7 +179,6 @@ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_initConnection(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_rfm22b_event rfm22_connectionAccepted(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event); @@ -190,6 +193,7 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz); static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev); // SPI read/write functions static void rfm22_assertCs(); @@ -251,16 +255,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, - [RFM22B_STATE_CONNECTION_ACCEPTED] = { - .entry_fn = rfm22_connectionAccepted, - .next_state = { - [RFM22B_EVENT_RX_COMPLETE] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, [RFM22B_STATE_RX_MODE] = { .entry_fn = rfm22_setRxMode, @@ -319,7 +313,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_RECEIVING_ACK] = { .entry_fn = rfm22_receiveAck, .next_state = { - [RFM22B_EVENT_CONNECTION_ACCEPTED] = RFM22B_STATE_CONNECTION_ACCEPTED, [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, @@ -348,6 +341,15 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, + [RFM22B_STATE_TX_DELAY] = { + .next_state = { + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, [RFM22B_STATE_TX_START] = { .entry_fn = rfm22_txStart, .next_state = { @@ -562,8 +564,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu // Initialize the stats. rfm22b_dev->stats.packets_per_sec = 0; - rfm22b_dev->stats.tx_count = 0; - rfm22b_dev->stats.rx_count = 0; rfm22b_dev->stats.rx_good = 0; rfm22b_dev->stats.rx_corrected = 0; rfm22b_dev->stats.rx_error = 0; @@ -590,8 +590,8 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->rx_packet_len = 0; rfm22b_dev->tx_packet = NULL; rfm22b_dev->prev_tx_packet = NULL; - rfm22b_dev->tx_seq = 0; - rfm22b_dev->prev_rx_seq_num = 0; + rfm22b_dev->stats.tx_seq = 0; + rfm22b_dev->stats.rx_seq = 0; rfm22b_dev->data_packet.header.data_size = 0; *rfm22b_id = (uint32_t)rfm22b_dev; @@ -875,22 +875,35 @@ static void PIOS_RFM22B_Task(void *parameters) if ((rfm22b_dev->packet_start_ticks > 0) && (timeDifferenceMs(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT); + else if (rfm22b_dev->state == RFM22B_STATE_TX_DELAY) + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); + else { - // Queue up a PPM packet if it's time. - if ((timeDifferenceMs(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) && rfm22_sendPPM(rfm22b_dev)) - lastPPMTicks = curTicks; - - // Queue up a status packet if it's time. - if ((timeDifferenceMs(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) && rfm22_sendStatus(rfm22b_dev)) - lastStatusTicks = curTicks; - - // Can we kick of a transmit? - if (rfm22b_dev->prev_tx_packet && timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) + + // Are we waiting for an ACK? + if (rfm22b_dev->prev_tx_packet) { - rfm22b_dev->tx_complete_ticks = curTicks; - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_PACKET_NACKED); + + // Should be resend the packet? + if (timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) + { + rfm22b_dev->tx_complete_ticks = curTicks; + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_PACKET_NACKED); + } } + else + { + + // Queue up a PPM packet if it's time. + if ((timeDifferenceMs(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) && rfm22_sendPPM(rfm22b_dev)) + lastPPMTicks = curTicks; + + // Queue up a status packet if it's time. + if ((timeDifferenceMs(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) && rfm22_sendStatus(rfm22b_dev)) + lastStatusTicks = curTicks; + } + } } } @@ -908,16 +921,15 @@ static void PIOS_RFM22B_Task(void *parameters) // // This gives 2(45 + 9.6) = 109.2kHz. -void RFM22_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, bool data_whitening) +static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, 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; - 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); - rfm22b_dev->max_ack_delay = 25; + if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) + //rfm22b_dev->max_ack_delay = (uint16_t)((float)(sizeof(PHPacketHeader) * 8 * 1000) / (float)(datarate_bps) + 0.5) * 4; + rfm22b_dev->max_ack_delay = 50; + else + rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS; // rfm22_if_filter_bandwidth rfm22_write(0x1C, reg_1C[datarate]); @@ -945,24 +957,11 @@ void RFM22_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, bool d // rfm22_afc_limiter rfm22_write(0x2A, reg_2A[datarate]); -/* This breaks all bit rates < 100000! - if (datarate_bps < 100000) - // rfm22_chargepump_current_trimming_override - rfm22_write(0x58, 0x80); - else - // rfm22_chargepump_current_trimming_override - rfm22_write(0x58, 0xC0); -*/ - // rfm22_tx_data_rate1 rfm22_write(0x6E, reg_6E[datarate]); // rfm22_tx_data_rate0 rfm22_write(0x6F, reg_6F[datarate]); - // Enable data whitening - // uint8_t txdtrtscale_bit = rfm22_read(RFM22_modulation_mode_control1) & RFM22_mmc1_txdtrtscale; - // rfm22_write(RFM22_modulation_mode_control1, txdtrtscale_bit | RFM22_mmc1_enwhite); - if (!data_whitening) // rfm22_modulation_mode_control1 rfm22_write(0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); @@ -980,6 +979,17 @@ void RFM22_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, bool d rfm22_write(RFM22_ook_counter_value2, 0x00); } +void PIOS_RFM22B_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; + rfm22b_dev->datarate = datarate; + + // Re-initialize the radio device. + PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); +} + // ************************************ // SPI read/write @@ -1260,7 +1270,7 @@ static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev) { // Is there a status of PPM packet ready to send? - if (rfm22b_dev->send_ppm || rfm22b_dev->send_status) + if (rfm22b_dev->prev_tx_packet || rfm22b_dev->send_ppm || rfm22b_dev->send_status) return true; // Is there some data ready to sent? @@ -1315,7 +1325,6 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) if (!p) { - // Try to get some data to send bool need_yield = false; p = &rfm22b_dev->data_packet; @@ -1324,19 +1333,16 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) if (p->header.data_size == 0) p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield); - if (p->header.data_size == 0) - p = NULL; // Don't send any data until we're connected. - else if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) - { + if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) p->header.data_size = 0; - return RFM22B_EVENT_RX_MODE; - } + if (p->header.data_size == 0) + p = NULL; } if (p) - p->header.seq_num = rfm22b_dev->tx_seq++; + p->header.seq_num = rfm22b_dev->stats.tx_seq++; } if (!p) return RFM22B_EVENT_RX_MODE; @@ -1413,9 +1419,29 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) { - // We don't send status if we're a coordinator - if (rfm22b_dev->coordinator) - return true; + // Remove any modems that have been disconnected too long. + bool disconnected = false; + uint8_t id_idx; + for (id_idx = 0; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) + { + if ((rfm22b_dev->pair_stats[id_idx].pairID != 0) && (rfm22b_dev->pair_stats[id_idx].lastContact++ > MAX_RADIOSTATS_MISS_COUNT)) + { + if (id_idx > 0) + rfm22b_dev->pair_stats[id_idx].pairID = 0; + else + disconnected = true; + rfm22b_dev->pair_stats[id_idx].rssi = -127; + rfm22b_dev->pair_stats[id_idx].afc_correction = 0; + rfm22b_dev->pair_stats[id_idx].lastContact = 0; + } + } + + // We need to re-connect if we lost connection to our remote modem. + if (disconnected) + { + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE); + return false; + } // Update the link quality metric. rfm22_calculateLinkQuality(rfm22b_dev); @@ -1586,21 +1612,24 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand // Add any missed packets into the stats. bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_NACK)); - if (!ack_nack_packet && (good_packet || corrected_packet) && (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED)) + if (!ack_nack_packet && (good_packet || corrected_packet)) { - static bool first_time = true; uint16_t seq_num = p->header.seq_num; - uint16_t missed_packets = 0; - if (first_time) - first_time = false; - else + if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { - uint16_t prev_seq_num = rfm22b_dev->prev_rx_seq_num; - if (seq_num > prev_seq_num) - missed_packets = seq_num - prev_seq_num - 1; + static bool first_time = true; + uint16_t missed_packets = 0; + if (first_time) + first_time = false; + else + { + uint16_t prev_seq_num = rfm22b_dev->stats.rx_seq; + if (seq_num > prev_seq_num) + missed_packets = seq_num - prev_seq_num - 1; + } + rfm22b_dev->stats.rx_missed += missed_packets; } - rfm22b_dev->stats.rx_missed += missed_packets; - rfm22b_dev->prev_rx_seq_num = seq_num; + rfm22b_dev->stats.rx_seq = seq_num; } // Set the packet status @@ -1684,7 +1713,6 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) enum pios_rfm22b_event ret_event = RFM22B_EVENT_RX_COMPLETE; if (rfm22b_dev->rx_buffer_wr > 0) { - rfm22b_dev->stats.rx_count++; rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr; // Check the packet for errors. if (rfm22_receivePacket(rfm22b_dev, &(rfm22b_dev->rx_packet), rfm22b_dev->rx_buffer_wr)) @@ -1767,13 +1795,22 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // Packet has been sent else if (rfm22b_dev->int_status1 & RFM22_is1_ipksent) { - rfm22b_dev->stats.tx_count++; rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet); - ret_event = (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) ? RFM22B_EVENT_TX_START : RFM22B_EVENT_RX_MODE; - // Do we require an ACK? - if ((rfm22b_dev->tx_packet->header.type != PACKET_TYPE_ACK) && (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK)) + // Is this an ACK? + bool is_ack = (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK); + ret_event = is_ack ? RFM22B_EVENT_TX_START : RFM22B_EVENT_RX_MODE; + if (is_ack) { + // If this is an ACK for a connection request message we need to + // configure this modem from the connection request message. + if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) + rfm22_setConnectionParameters(rfm22b_dev); + + } + else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) + { + // We need to wait for an ACK if this packet it not an ACK or NACK. rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet; rfm22b_dev->tx_complete_ticks = xTaskGetTickCount(); } @@ -1833,7 +1870,8 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de switch (prev->header.type) { case PACKET_TYPE_CON_REQUEST: - return RFM22B_EVENT_CONNECTION_ACCEPTED; + rfm22_setConnectionParameters(rfm22b_dev); + break; case PACKET_TYPE_DATA: rfm22b_dev->data_packet.header.data_size = 0; break; @@ -1946,6 +1984,21 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf return RFM22B_EVENT_START_TRANSFER; } +static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) +{ + PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); + + // Set our connection state to connected + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + + // Configure this modem from the connection request message. + rfm22_setDatarate(rfm22b_dev, cph->datarate, true); + PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power); + rfm22b_dev->min_frequency = cph->min_frequency; + rfm22b_dev->max_frequency = cph->max_frequency; + rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->frequency_hz); +} + static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) { // Set our connection state to connected @@ -1956,66 +2009,12 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm PHConnectionPacketHandle lcph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); memcpy((uint8_t*)lcph, (uint8_t*)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); - // Configure this modem from the connection request message. + // Set the destination ID to the source of the connection request message. PIOS_RFM22B_SetDestinationId((uint32_t)rfm22b_dev, cph->header.source_id); - RFM22_SetDatarate((uint32_t)rfm22b_dev, cph->datarate, true); - PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power); - rfm22b_dev->min_frequency = cph->min_frequency; - rfm22b_dev->max_frequency = cph->max_frequency; - rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->frequency_hz); return RFM22B_EVENT_CONNECTION_ACCEPTED; } -static enum pios_rfm22b_event rfm22_connectionAccepted(struct pios_rfm22b_dev *rfm22b_dev) -{ - PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); - - // Set our connection state to connected - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; - - // Configure this modem from the connection request message. - RFM22_SetDatarate((uint32_t)rfm22b_dev, cph->datarate, true); - PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power); - rfm22b_dev->min_frequency = cph->min_frequency; - rfm22b_dev->max_frequency = cph->max_frequency; - rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->frequency_hz); - - return RFM22B_EVENT_RX_COMPLETE; -} - -// ************************************ - -// return the current mode -int8_t rfm22_currentMode(void) -{ - return g_rfm22b_dev->state; -} - -// return true if we are transmitting -bool rfm22_transmitting(void) -{ - return (g_rfm22b_dev->state == RFM22B_STATE_TX_DATA); -} - -// return true if the channel is clear to transmit on -bool rfm22_channelIsClear(void) -{ - if (g_rfm22b_dev->state != RFM22B_STATE_RX_MODE && g_rfm22b_dev->state != RFM22B_STATE_WAIT_PREAMBLE && g_rfm22b_dev->state != RFM22B_STATE_WAIT_SYNC) - // we are receiving something or we are transmitting or we are scanning the spectrum - return false; - - return true; -} - -// ************************************ -// set/get the frequency calibration value - -void rfm22_setFreqCalibration(uint8_t value) -{ - rfm22_write(RFM22_xtal_osc_load_cap, value); -} - // ************************************ // Initialise this hardware layer module and the rf module @@ -2228,9 +2227,12 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // RX FIFO Almost Full Threshold (0 - 63) rfm22_write(RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); - rfm22_setFreqCalibration(rfm22b_dev->cfg.RFXtalCap); - rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->frequency_hz); - RFM22_SetDatarate((uint32_t)rfm22b_dev, rfm22b_dev->datarate, true); + // Set the frequency calibration + rfm22_write(RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap); + + // Initialize the frequency and datarate to te default. + rfm22_setNominalCarrierFrequency(rfm22b_dev, RFM22B_DEFAULT_FREQUENCY); + rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); return RFM22B_EVENT_INITIALIZED; } diff --git a/flight/PiOS/Common/pios_rfm22b_com.c b/flight/PiOS/Common/pios_rfm22b_com.c index 2833c73f1..4798711d3 100644 --- a/flight/PiOS/Common/pios_rfm22b_com.c +++ b/flight/PiOS/Common/pios_rfm22b_com.c @@ -58,6 +58,12 @@ const struct pios_com_driver pios_rfm22b_com_driver = { */ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) { + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) + return; + // This is only allowed for coordinators. + if (!rfm22b_dev->coordinator) + return; // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. enum rfm22b_datarate datarate = RFM22_datarate_64000; if (baud <= 1024) @@ -76,7 +82,7 @@ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) datarate = RFM22_datarate_128000; else if (baud <= 115200) datarate = RFM22_datarate_192000; - RFM22_SetDatarate(rfm22b_id, datarate, true); + PIOS_RFM22B_SetDatarate(rfm22b_id, datarate, true); } static void PIOS_RFM22B_COM_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail) diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 81619f6ac..55290ce5b 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -76,8 +76,8 @@ struct rfm22b_stats { uint16_t packets_per_sec; uint16_t tx_byte_count; uint16_t rx_byte_count; - uint16_t tx_count; - uint16_t rx_count; + uint16_t tx_seq; + uint16_t rx_seq; uint8_t rx_good; uint8_t rx_corrected; uint8_t rx_error; @@ -96,7 +96,7 @@ struct rfm22b_stats { 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_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_frequency, uint32_t max_frequency); 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_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 void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 7abd9ded1..bdf2be40a 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -588,6 +588,7 @@ enum pios_rfm22b_state { RFM22B_STATE_WAIT_SYNC, RFM22B_STATE_RX_DATA, RFM22B_STATE_RECEIVING_STATUS, + RFM22B_STATE_TX_DELAY, RFM22B_STATE_TX_START, RFM22B_STATE_TX_DATA, RFM22B_STATE_SENDING_ACK, diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 01cbedfab..5f4688359 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -235,6 +235,10 @@ void PIOS_Board_Init(void) { if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { PIOS_Assert(0); } + + /* Configure the RFM22B device as coordinator or not */ + PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); + uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); @@ -246,9 +250,6 @@ void PIOS_Board_Init(void) { } } - /* Configure the RFM22B device as coordinator or not */ - PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); - // Initialize the packet handler PacketHandlerConfig pios_ph_cfg = { .default_destination_id = 0xffffffff, // Broadcast diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 353d0fc16..ff1aba611 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -80,6 +80,8 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkStatus", "RSSI", m_oplink->RSSI); addUAVObjectToWidgetRelation("OPLinkStatus", "AFCCorrection", m_oplink->AFCCorrection); addUAVObjectToWidgetRelation("OPLinkStatus", "LinkQuality", m_oplink->LinkQuality); + addUAVObjectToWidgetRelation("OPLinkStatus", "RXSeq", m_oplink->RXSeq); + addUAVObjectToWidgetRelation("OPLinkStatus", "TXSeq", m_oplink->TXSeq); addUAVObjectToWidgetRelation("OPLinkStatus", "RXRate", m_oplink->RXRate); addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate); diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 09e83bdc5..f7c628cbd 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -787,9 +787,9 @@ - + - TX Rate (B/s) + TX Seq. No. Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -797,7 +797,7 @@ - + 0 @@ -835,6 +835,102 @@ + + + TX Rate (B/s) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + false + + + true + + + + + + + RX Seq. No. + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + false + + + true + + + + RX Rate (B/s) @@ -844,7 +940,7 @@ - + diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml index a8d23fa35..0d048c139 100755 --- a/shared/uavobjectdefinition/oplinkstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -20,6 +20,8 @@ + + From 7a930807aa706720c8162cabe1951accac0bf8b4 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Tue, 13 Nov 2012 21:02:43 -0700 Subject: [PATCH 15/31] RM: Fixed initialization of the debug console. --- flight/PiOS/Boards/STM32F4xx_RevoMini.h | 4 ++-- flight/RevoMini/System/inc/pios_config.h | 3 ++- flight/RevoMini/System/pios_board.c | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/flight/PiOS/Boards/STM32F4xx_RevoMini.h b/flight/PiOS/Boards/STM32F4xx_RevoMini.h index 65857b572..6dd7041dc 100644 --- a/flight/PiOS/Boards/STM32F4xx_RevoMini.h +++ b/flight/PiOS/Boards/STM32F4xx_RevoMini.h @@ -32,8 +32,8 @@ #include #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -#define DEBUG_LEVEL 0 -#define DEBUG_PRINTF(level, ...) {if(level <= DEBUG_LEVEL && pios_com_aux_id > 0) { PIOS_COM_SendFormattedStringNonBlocking(pios_com_aux_id, __VA_ARGS__); }} +#define DEBUG_LEVEL 2 +#define DEBUG_PRINTF(level, ...) {if(level <= DEBUG_LEVEL && pios_com_debug_id > 0) { PIOS_COM_SendFormattedStringNonBlocking(pios_com_debug_id, __VA_ARGS__); }} #else #define DEBUG_PRINTF(level, ...) #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ diff --git a/flight/RevoMini/System/inc/pios_config.h b/flight/RevoMini/System/inc/pios_config.h index dc2ec3c98..1ae25020c 100644 --- a/flight/RevoMini/System/inc/pios_config.h +++ b/flight/RevoMini/System/inc/pios_config.h @@ -58,7 +58,6 @@ /* Variables related to the RFM22B functionality */ #define PIOS_INCLUDE_RFM22B #define PIOS_INCLUDE_RFM22B_COM -#define RFM22_EXT_INT_USE /* Select the sensors to include */ #define PIOS_INCLUDE_HMC5883 @@ -92,6 +91,8 @@ /* A really shitty setting saving implementation */ #define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS +//#define PIOS_INCLUDE_DEBUG_CONSOLE + /* Other Interfaces */ //#define PIOS_INCLUDE_I2C_ESC diff --git a/flight/RevoMini/System/pios_board.c b/flight/RevoMini/System/pios_board.c index fd5af508e..8feac9fa3 100644 --- a/flight/RevoMini/System/pios_board.c +++ b/flight/RevoMini/System/pios_board.c @@ -529,7 +529,7 @@ void PIOS_Board_Init(void) { case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { - PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; @@ -592,7 +592,7 @@ void PIOS_Board_Init(void) { case HWSETTINGS_RM_FLEXIPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { - PIOS_Board_configure_com(&pios_usart_flexi_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_aux_id); + PIOS_Board_configure_com(&pios_usart_main_cfg, 0, PIOS_COM_DEBUGCONSOLE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_debug_id); } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; From 4cb311538a80d2844bf129af07c9033abd20866a Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Tue, 13 Nov 2012 21:08:33 -0700 Subject: [PATCH 16/31] RFM22B: Improved reliability, especially with reconnection after a timeout, although there is still too many resent packets. --- flight/Libraries/inc/packet_handler.h | 1 + flight/PiOS/Common/pios_rfm22b.c | 183 ++++++++++++-------------- flight/PiOS/inc/pios_rfm22b_priv.h | 7 +- 3 files changed, 87 insertions(+), 104 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index e0237023e..517463555 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -43,6 +43,7 @@ typedef enum { PACKET_TYPE_STATUS, // broadcasts status of this modem PACKET_TYPE_CON_REQUEST, // request a connection to another modem PACKET_TYPE_DATA, // data packet (packet contains user data) + PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet PACKET_TYPE_PPM, // PPM relay values PACKET_TYPE_ACK, PACKET_TYPE_NACK, diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 01e912afd..e952b19fe 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -61,12 +61,16 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 2) #define ISR_TIMEOUT 2 // ms #define EVENT_QUEUE_SIZE 5 -#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_32000 +#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 #define RFM22B_DEFAULT_FREQUENCY 434000000 #define RFM22B_DEFAULT_MIN_FREQUENCY (RFM22B_DEFAULT_FREQUENCY - 2000000) #define RFM22B_DEFAULT_MAX_FREQUENCY (RFM22B_DEFAULT_FREQUENCY + 2000000) #define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_7 #define RFM22B_LINK_QUALITY_THRESHOLD 20 + +// The maximum amount of time since the last message received to consider the connection broken. +#define DISCONNECT_TIMEOUT_MS 1000 // ms + // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms @@ -533,79 +537,27 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu struct pios_rfm22b_dev * rfm22b_dev = (struct pios_rfm22b_dev *) PIOS_RFM22B_alloc(); if (!rfm22b_dev) return(-1); + *rfm22b_id = (uint32_t)rfm22b_dev; + g_rfm22b_dev = rfm22b_dev; // Store the SPI handle rfm22b_dev->slave_num = slave_num; rfm22b_dev->spi_id = spi_id; - // Set the state to initializing. - rfm22b_dev->state = RFM22B_STATE_UNINITIALIZED; + // Initialize our configuration parameters + rfm22b_dev->coordinator = false; + rfm22b_dev->send_ppm = false; + rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; + // Create the event queue rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_rfm22b_event)); - // Initialize the register values. - rfm22b_dev->device_status = 0; - rfm22b_dev->int_status1 = 0; - rfm22b_dev->int_status2 = 0; - rfm22b_dev->ezmac_status = 0; - - // Initialize the detected device statistics. - for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) - { - rfm22b_dev->pair_stats[i].pairID = 0; - rfm22b_dev->pair_stats[i].rssi = -127; - rfm22b_dev->pair_stats[i].afc_correction = 0; - rfm22b_dev->pair_stats[i].lastContact = 0; - } - - // Initlize the link stats. - for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) - rfm22b_dev->rx_packet_stats[i] = 0; - - // Initialize the stats. - rfm22b_dev->stats.packets_per_sec = 0; - rfm22b_dev->stats.rx_good = 0; - rfm22b_dev->stats.rx_corrected = 0; - rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.rx_missed = 0; - rfm22b_dev->stats.tx_dropped = 0; - rfm22b_dev->stats.tx_resent = 0; - rfm22b_dev->stats.resets = 0; - rfm22b_dev->stats.timeouts = 0; - rfm22b_dev->stats.link_quality = 0; - rfm22b_dev->stats.rssi = 0; - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; - rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; - rfm22b_dev->destination_id = 0xffffffff; - rfm22b_dev->coordinator = false; - rfm22b_dev->send_status = false; - rfm22b_dev->send_ppm = false; - rfm22b_dev->send_connection_request = false; - // 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_len = 0; - rfm22b_dev->tx_packet = NULL; - rfm22b_dev->prev_tx_packet = NULL; - rfm22b_dev->stats.tx_seq = 0; - rfm22b_dev->stats.rx_seq = 0; - rfm22b_dev->data_packet.header.data_size = 0; - - *rfm22b_id = (uint32_t)rfm22b_dev; - g_rfm22b_dev = rfm22b_dev; - - // Calculate the (approximate) maximum amount of time that it should take to transmit / receive a packet. - rfm22b_dev->packet_start_ticks = 0; // Create a semaphore to know if an ISR needs responding to vSemaphoreCreateBinary( rfm22b_dev->isrPending ); - // Create a semaphore to know when an rx packet is available - vSemaphoreCreateBinary( rfm22b_dev->rxsem ); - // Create our (hopefully) unique 32 bit id from the processor serial number. uint8_t crcs[] = { 0, 0, 0, 0 }; { @@ -626,12 +578,15 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu PIOS_WDG_RegisterFlag(PIOS_WDG_RFM22B); #endif /* PIOS_WDG_RFM22B */ - // Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler. - xTaskCreate(PIOS_RFM22B_Task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle)); + // Set the state to initializing. + rfm22b_dev->state = RFM22B_STATE_UNINITIALIZED; // Initialize the radio device. PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); + // Start the driver task. This task controls the radio state machine and removed all of the IO from the IRQ handler. + xTaskCreate(PIOS_RFM22B_Task, (signed char *)"PIOS_RFM22B_Task", STACK_SIZE_BYTES, (void*)rfm22b_dev, TASK_PRIORITY, &(rfm22b_dev->taskHandle)); + return(0); } @@ -878,6 +833,10 @@ static void PIOS_RFM22B_Task(void *parameters) else if (rfm22b_dev->state == RFM22B_STATE_TX_DELAY) rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); + // Have it been too long since we received a packet + else if ((rfm22b_dev->rx_complete_ticks > 0) && (timeDifferenceMs(rfm22b_dev->rx_complete_ticks, curTicks) > DISCONNECT_TIMEOUT_MS)) + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); + else { @@ -1419,29 +1378,6 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) { - // Remove any modems that have been disconnected too long. - bool disconnected = false; - uint8_t id_idx; - for (id_idx = 0; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) - { - if ((rfm22b_dev->pair_stats[id_idx].pairID != 0) && (rfm22b_dev->pair_stats[id_idx].lastContact++ > MAX_RADIOSTATS_MISS_COUNT)) - { - if (id_idx > 0) - rfm22b_dev->pair_stats[id_idx].pairID = 0; - else - disconnected = true; - rfm22b_dev->pair_stats[id_idx].rssi = -127; - rfm22b_dev->pair_stats[id_idx].afc_correction = 0; - rfm22b_dev->pair_stats[id_idx].lastContact = 0; - } - } - - // We need to re-connect if we lost connection to our remote modem. - if (disconnected) - { - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_INITIALIZE); - return false; - } // Update the link quality metric. rfm22_calculateLinkQuality(rfm22b_dev); @@ -1626,6 +1562,8 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand uint16_t prev_seq_num = rfm22b_dev->stats.rx_seq; if (seq_num > prev_seq_num) missed_packets = seq_num - prev_seq_num - 1; + else if((seq_num == prev_seq_num) && (p->header.type == PACKET_TYPE_DATA)) + p->header.type = PACKET_TYPE_DUPLICATE_DATA; } rfm22b_dev->stats.rx_missed += missed_packets; } @@ -1642,7 +1580,7 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand // We couldn't correct the error, so drop the packet. rfm22b_add_rx_status(rfm22b_dev, RFM22B_ERROR_RX_PACKET); - return good_packet || corrected_packet; + return (good_packet || corrected_packet); } static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) @@ -1727,11 +1665,13 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) break; case PACKET_TYPE_DATA: { - // Send the data to the com port. + // Send the data to the com port bool rx_need_yield; (rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield); break; } + case PACKET_TYPE_DUPLICATE_DATA: + break; case PACKET_TYPE_ACK: ret_event = RFM22B_EVENT_PACKET_ACKED; break; @@ -1745,6 +1685,9 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) else ret_event = RFM22B_EVENT_RX_ERROR; rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); + if (rfm22b_dev->rx_complete_ticks == 0) + rfm22b_dev->rx_complete_ticks = 1; } // Start a new transaction @@ -2020,8 +1963,54 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) { - uint32_t id = rfm22b_dev->deviceID; - uint32_t freq_hop_step_size = 50000; + + // Initialize the register values. + rfm22b_dev->device_status = 0; + rfm22b_dev->int_status1 = 0; + rfm22b_dev->int_status2 = 0; + rfm22b_dev->ezmac_status = 0; + + // Initialize the detected device statistics. + for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) + { + rfm22b_dev->pair_stats[i].pairID = 0; + rfm22b_dev->pair_stats[i].rssi = -127; + rfm22b_dev->pair_stats[i].afc_correction = 0; + rfm22b_dev->pair_stats[i].lastContact = 0; + } + + // Initlize the link stats. + for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) + rfm22b_dev->rx_packet_stats[i] = 0; + + // Initialize the stats. + rfm22b_dev->stats.packets_per_sec = 0; + rfm22b_dev->stats.rx_good = 0; + rfm22b_dev->stats.rx_corrected = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.rx_missed = 0; + rfm22b_dev->stats.tx_dropped = 0; + rfm22b_dev->stats.tx_resent = 0; + rfm22b_dev->stats.resets = 0; + rfm22b_dev->stats.timeouts = 0; + rfm22b_dev->stats.link_quality = 0; + rfm22b_dev->stats.rssi = 0; + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; + rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; + rfm22b_dev->destination_id = 0xffffffff; + rfm22b_dev->send_status = false; + rfm22b_dev->send_connection_request = false; + + // Initialize the packets. + rfm22b_dev->rx_packet_len = 0; + rfm22b_dev->tx_packet = NULL; + rfm22b_dev->prev_tx_packet = NULL; + rfm22b_dev->stats.tx_seq = 0; + rfm22b_dev->stats.rx_seq = 0; + rfm22b_dev->data_packet.header.data_size = 0; + + // Calculate the (approximate) maximum amount of time that it should take to transmit / receive a packet. + rfm22b_dev->packet_start_ticks = 0; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); @@ -2065,6 +2054,8 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->afc_correction_Hz = 0; rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->tx_complete_ticks = 0; + rfm22b_dev->rx_complete_ticks = 0; // **************** // read the RF chip ID bytes @@ -2132,6 +2123,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // **************** // initialize the frequency hopping step size + uint32_t freq_hop_step_size = 50000; freq_hop_step_size /= 10000; // in 10kHz increments if (freq_hop_step_size > 255) freq_hop_step_size = 255; rfm22b_dev->frequency_hop_step_size_reg = freq_hop_step_size; @@ -2193,6 +2185,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_write(RFM22_header_enable2, 0xff); rfm22_write(RFM22_header_enable3, 0xff); // Set the ID to be checked + uint32_t id = rfm22b_dev->deviceID; rfm22_write(RFM22_check_header0, id & 0xff); rfm22_write(RFM22_check_header1, (id >> 8) & 0xff); rfm22_write(RFM22_check_header2, (id >> 16) & 0xff); @@ -2239,7 +2232,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) { - rfm22b_dev->resets++; + rfm22b_dev->stats.timeouts++; rfm22b_dev->packet_start_ticks = 0; // Release the Tx packet if it's set. if (rfm22b_dev->tx_packet != 0) @@ -2253,15 +2246,7 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev) { - rfm22b_dev->resets++; - rfm22b_dev->packet_start_ticks = 0; - // Release the Tx packet if it's set. - if (rfm22b_dev->tx_packet != 0) - { - rfm22b_dev->tx_packet = 0; - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; - } - rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->stats.resets++; return RFM22B_EVENT_INITIALIZE; } diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index bdf2be40a..6182f58e3 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -670,9 +670,6 @@ struct pios_rfm22b_dev { // ISR pending xSemaphoreHandle isrPending; - // Receive packet complete - xSemaphoreHandle rxsem; - // The COM callback functions. pios_com_callback rx_in_cb; uint32_t rx_in_context; @@ -707,9 +704,8 @@ struct pios_rfm22b_dev { struct rfm22b_stats stats; // Stats - uint16_t resets; - uint16_t timeouts; uint16_t errors; + // RSSI in dBm int8_t rssi_dBm; @@ -774,6 +770,7 @@ struct pios_rfm22b_dev { uint32_t max_packet_time; portTickType packet_start_ticks; portTickType tx_complete_ticks; + portTickType rx_complete_ticks; uint8_t max_ack_delay; }; From 6ffe5185096bd9b582ee3bd60a4e9e0005702819 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 17 Nov 2012 13:18:34 -0700 Subject: [PATCH 17/31] RFM22B: Improved link stability, and added tracking of error encountered in the processes of transmitting and receiving. --- flight/Libraries/inc/packet_handler.h | 6 +- flight/Modules/PipXtreme/pipxtrememod.c | 5 +- flight/PiOS/Common/pios_rfm22b.c | 166 +++++++++++------- flight/PiOS/Common/pios_rfm22b_com.c | 2 +- flight/PiOS/inc/pios_rfm22b.h | 2 + flight/PiOS/inc/pios_rfm22b_priv.h | 6 +- .../plugins/config/configpipxtremewidget.cpp | 2 + .../src/plugins/config/pipxtreme.ui | 117 +++++++++++- shared/uavobjectdefinition/oplinkstatus.xml | 4 +- 9 files changed, 227 insertions(+), 83 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 517463555..288f98e91 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -45,8 +45,9 @@ typedef enum { PACKET_TYPE_DATA, // data packet (packet contains user data) PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet PACKET_TYPE_PPM, // PPM relay values - PACKET_TYPE_ACK, - PACKET_TYPE_NACK, + PACKET_TYPE_ACK, // Acknowlege the receipt of a packet + PACKET_TYPE_ACK_RTS, // Acknowlege the receipt of a packet and indicate that the receiving side has data to send (ready to send) + PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet } PHPacketType; typedef struct { @@ -67,7 +68,6 @@ typedef struct { #define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - bool ready_to_send; uint8_t ecc[RS_ECC_NPARITY]; } PHAckNackPacket, *PHAckNackPacketHandle; diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c index d834af320..77f61109f 100644 --- a/flight/Modules/PipXtreme/pipxtrememod.c +++ b/flight/Modules/PipXtreme/pipxtrememod.c @@ -168,13 +168,14 @@ static void systemTask(void *parameters) // Update the status oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); - //oplinkStatus.UAVTalkErrors = data->UAVTalkErrors; oplinkStatus.RxGood = radio_stats.rx_good; oplinkStatus.RxCorrected = radio_stats.rx_corrected; oplinkStatus.RxErrors = radio_stats.rx_error; oplinkStatus.RxMissed = radio_stats.rx_missed; - oplinkStatus.TxDropped = radio_stats.tx_dropped; // + data->droppedPackets; + oplinkStatus.RxFailure = radio_stats.rx_failure; + oplinkStatus.TxDropped = radio_stats.tx_dropped; oplinkStatus.TxResent = radio_stats.tx_resent; + oplinkStatus.TxFailure = radio_stats.tx_failure; oplinkStatus.Resets = radio_stats.resets; oplinkStatus.Timeouts = radio_stats.timeouts; oplinkStatus.RSSI = radio_stats.rssi; diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index e952b19fe..d11d4dddd 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -61,7 +61,7 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 2) #define ISR_TIMEOUT 2 // ms #define EVENT_QUEUE_SIZE 5 -#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 +#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_32000 #define RFM22B_DEFAULT_FREQUENCY 434000000 #define RFM22B_DEFAULT_MIN_FREQUENCY (RFM22B_DEFAULT_FREQUENCY - 2000000) #define RFM22B_DEFAULT_MAX_FREQUENCY (RFM22B_DEFAULT_FREQUENCY + 2000000) @@ -175,6 +175,7 @@ static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev); @@ -185,6 +186,7 @@ static enum pios_rfm22b_event rfm22_initConnection(struct pios_rfm22b_dev *rfm22 static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev); +static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_process_state_transition(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event); static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_event event); static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev); @@ -242,7 +244,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_REQUESTING_CONNECTION] = { .entry_fn = rfm22_requestConnection, .next_state = { - [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -264,9 +266,10 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_setRxMode, .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, - [RFM22B_EVENT_SEND_PACKET] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, + [RFM22B_EVENT_SEND_DATA] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -277,10 +280,11 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_detectPreamble, .next_state = { [RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC, - [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, - [RFM22B_EVENT_SEND_PACKET] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, + [RFM22B_EVENT_SEND_DATA] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, + [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -292,6 +296,9 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_SYNC, [RFM22B_EVENT_SYNC_DETECTED] = RFM22B_STATE_RX_DATA, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, + [RFM22B_EVENT_SEND_DATA] = RFM22B_STATE_TX_DELAY, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -308,6 +315,17 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION, [RFM22B_EVENT_PACKET_ACKED] = RFM22B_STATE_RECEIVING_ACK, [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_RX_FAILURE] = { + .entry_fn = rfm22_rxFailure, + .next_state = { + [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -317,7 +335,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_RECEIVING_ACK] = { .entry_fn = rfm22_receiveAck, .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, @@ -328,7 +346,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_RECEIVING_NACK] = { .entry_fn = rfm22_receiveNack, .next_state = { - [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -369,8 +387,19 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_txData, .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, + [RFM22B_EVENT_FAILURE] = RFM22B_STATE_TX_FAILURE, + [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, + [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, + [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, + }, + }, + [RFM22B_STATE_TX_FAILURE] = { + .entry_fn = rfm22_txFailure, + .next_state = { + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -380,7 +409,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_SENDING_ACK] = { .entry_fn = rfm22_sendAck, .next_state = { - [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -390,7 +419,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_SENDING_NACK] = { .entry_fn = rfm22_sendNack, .next_state = { - [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -400,7 +429,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_TIMEOUT] = { .entry_fn = rfm22_timeout, .next_state = { - [RFM22B_EVENT_START_TRANSFER] = RFM22B_STATE_TX_START, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -549,6 +578,19 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->send_ppm = false; rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; + // Initialize the stats. + rfm22b_dev->stats.packets_per_sec = 0; + rfm22b_dev->stats.rx_good = 0; + rfm22b_dev->stats.rx_corrected = 0; + rfm22b_dev->stats.rx_error = 0; + rfm22b_dev->stats.rx_missed = 0; + rfm22b_dev->stats.tx_dropped = 0; + rfm22b_dev->stats.tx_resent = 0; + rfm22b_dev->stats.resets = 0; + rfm22b_dev->stats.timeouts = 0; + rfm22b_dev->stats.link_quality = 0; + rfm22b_dev->stats.rssi = 0; + // Create the event queue rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_rfm22b_event)); @@ -1392,7 +1434,7 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; rfm22b_dev->send_status = true; - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_START_TRANSFER); + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); return true; } @@ -1420,7 +1462,7 @@ static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->ppm_packet.header.type = PACKET_TYPE_PPM; rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); rfm22b_dev->send_ppm = true; - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_SEND_PACKET); + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); } #endif @@ -1477,10 +1519,9 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22b_dev) { - // Read the device status registers if (!rfm22_readStatus(rfm22b_dev)) - return RFM22B_EVENT_ERROR; + return RFM22B_EVENT_FAILURE; // Valid preamble detected if (rfm22b_dev->int_status2 & RFM22_is2_ipreaval) @@ -1500,7 +1541,7 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de // Read the device status registers if (!rfm22_readStatus(rfm22b_dev)) - return RFM22B_EVENT_ERROR; + return RFM22B_EVENT_FAILURE; // Sync word detected if (rfm22b_dev->int_status2 & RFM22_is2_iswdet) @@ -1525,10 +1566,8 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de return RFM22B_EVENT_SYNC_DETECTED; } else if (rfm22b_dev->int_status2 & !RFM22_is2_ipreaval) - { // Waiting for sync timed out. - return RFM22B_EVENT_START_TRANSFER; - } + return RFM22B_EVENT_FAILURE; return RFM22B_EVENT_NUM_EVENTS; } @@ -1547,7 +1586,7 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand corrected_packet = true; // Add any missed packets into the stats. - bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_NACK)); + bool ack_nack_packet = ((p->header.type == PACKET_TYPE_ACK) || (p->header.type == PACKET_TYPE_ACK_RTS) || (p->header.type == PACKET_TYPE_NACK)); if (!ack_nack_packet && (good_packet || corrected_packet)) { uint16_t seq_num = p->header.seq_num; @@ -1590,11 +1629,11 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) // Read the device status registers if (!rfm22_readStatus(rfm22b_dev)) - return RFM22B_EVENT_ERROR; + return RFM22B_EVENT_FAILURE; // FIFO under/over flow error. Restart RX mode. - if (rfm22b_dev->device_status & (RFM22_ds_ffunfl | RFM22_ds_ffovfl)) - return RFM22B_EVENT_ERROR; + if (rfm22b_dev->int_status1 & RFM22_is1_ifferr) + return RFM22B_EVENT_FAILURE; // RX FIFO almost full, it needs emptying if (rfm22b_dev->int_status1 & RFM22_is1_irxffafull) @@ -1605,11 +1644,11 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) // The received packet is going to be larger than the specified length if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) - return RFM22B_EVENT_ERROR; + return RFM22B_EVENT_FAILURE; // Another packet length error. if (((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) >= len) && !(rfm22b_dev->int_status1 & RFM22_is1_ipkvalid)) - return RFM22B_EVENT_ERROR; + return RFM22B_EVENT_FAILURE; // Fetch the data from the RX FIFO rfm22_claimBus(); @@ -1622,7 +1661,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) // CRC error .. discard the received data if (rfm22b_dev->int_status1 & RFM22_is1_icrerror) - return RFM22B_EVENT_ERROR; + return RFM22B_EVENT_FAILURE; // Valid packet received if (rfm22b_dev->int_status1 & RFM22_is1_ipkvalid) @@ -1645,7 +1684,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) } if (rfm22b_dev->rx_buffer_wr != len) - return RFM22B_EVENT_ERROR; + return RFM22B_EVENT_FAILURE; // we have a valid received packet enum pios_rfm22b_event ret_event = RFM22B_EVENT_RX_COMPLETE; @@ -1673,6 +1712,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) case PACKET_TYPE_DUPLICATE_DATA: break; case PACKET_TYPE_ACK: + case PACKET_TYPE_ACK_RTS: ret_event = RFM22B_EVENT_PACKET_ACKED; break; case PACKET_TYPE_NACK: @@ -1698,25 +1738,27 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) return RFM22B_EVENT_NUM_EVENTS; } +static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) +{ + rfm22b_dev->stats.rx_failure++; + rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); + if (rfm22b_dev->rx_complete_ticks == 0) + rfm22b_dev->rx_complete_ticks = 1; + return RFM22B_EVENT_RX_MODE; +} + static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) { enum pios_rfm22b_event ret_event = RFM22B_EVENT_NUM_EVENTS; // Read the device status registers if (!rfm22_readStatus(rfm22b_dev)) - { - rfm22b_dev->tx_packet = 0; - rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; - return RFM22B_EVENT_ERROR; - } + return RFM22B_EVENT_FAILURE; - // FIFO under/over flow error. Back to RX mode. - if (rfm22b_dev->device_status & (RFM22_ds_ffunfl | RFM22_ds_ffovfl)) - { - rfm22b_dev->tx_packet = 0; - rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; - return RFM22B_EVENT_ERROR; - } + // FIFO under/over flow error. + //if (rfm22b_dev->int_status1 & RFM22_is1_ifferr) + //return RFM22B_EVENT_FAILURE; // TX FIFO almost empty, it needs filling up if (rfm22b_dev->int_status1 & RFM22_is1_ixtffaem) @@ -1741,7 +1783,7 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet); // Is this an ACK? - bool is_ack = (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK); + bool is_ack = ((rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) || (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS)); ret_event = is_ack ? RFM22B_EVENT_TX_START : RFM22B_EVENT_RX_MODE; if (is_ack) { @@ -1767,6 +1809,13 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) return ret_event; } +static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev) +{ + rfm22b_dev->stats.tx_failure++; + rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; + return RFM22B_EVENT_TX_START; +} + /** * Send an ACK to a received packet. * \param[in] rfm22b_dev The device structure @@ -1775,12 +1824,11 @@ static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) { PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id; - aph->header.type = PACKET_TYPE_ACK; + aph->header.type = rfm22_ready_to_send(rfm22b_dev) ? PACKET_TYPE_ACK_RTS : PACKET_TYPE_ACK; aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; - aph->ready_to_send = rfm22_ready_to_send(rfm22b_dev); rfm22b_dev->tx_packet = (PHPacketHandle)aph; - return RFM22B_EVENT_START_TRANSFER; + return RFM22B_EVENT_TX_START; } /** @@ -1795,7 +1843,7 @@ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; rfm22b_dev->tx_packet = (PHPacketHandle)aph; - return RFM22B_EVENT_START_TRANSFER; + return RFM22B_EVENT_TX_START; } /** @@ -1821,11 +1869,10 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de } // Should we try to start another TX? - PHAckNackPacketHandle aph = (PHAckNackPacketHandle)&(rfm22b_dev->rx_packet); - if (aph->ready_to_send) - return RFM22B_EVENT_RX_MODE; - else + if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) return RFM22B_EVENT_TX_START; + else + return RFM22B_EVENT_RX_MODE; } /** @@ -1839,7 +1886,7 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d rfm22b_dev->prev_tx_packet = NULL; if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET); - return RFM22B_EVENT_START_TRANSFER; + return RFM22B_EVENT_TX_START; } /** @@ -1924,7 +1971,7 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf cph->max_tx_power = rfm22b_dev->tx_power; rfm22b_dev->send_connection_request = true; - return RFM22B_EVENT_START_TRANSFER; + return RFM22B_EVENT_TX_START; } static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) @@ -1983,18 +2030,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) for (uint8_t i = 0; i < RFM22B_RX_PACKET_STATS_LEN; ++i) rfm22b_dev->rx_packet_stats[i] = 0; - // Initialize the stats. - rfm22b_dev->stats.packets_per_sec = 0; - rfm22b_dev->stats.rx_good = 0; - rfm22b_dev->stats.rx_corrected = 0; - rfm22b_dev->stats.rx_error = 0; - rfm22b_dev->stats.rx_missed = 0; - rfm22b_dev->stats.tx_dropped = 0; - rfm22b_dev->stats.tx_resent = 0; - rfm22b_dev->stats.resets = 0; - rfm22b_dev->stats.timeouts = 0; - rfm22b_dev->stats.link_quality = 0; - rfm22b_dev->stats.rssi = 0; + // Initialize the state rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; rfm22b_dev->destination_id = 0xffffffff; @@ -2241,7 +2277,7 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; } rfm22b_dev->rx_buffer_wr = 0; - return RFM22B_EVENT_START_TRANSFER; + return RFM22B_EVENT_TX_START; } static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev) diff --git a/flight/PiOS/Common/pios_rfm22b_com.c b/flight/PiOS/Common/pios_rfm22b_com.c index 4798711d3..f000b1dd9 100644 --- a/flight/PiOS/Common/pios_rfm22b_com.c +++ b/flight/PiOS/Common/pios_rfm22b_com.c @@ -96,7 +96,7 @@ static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) return; // Send a signal to the radio to start a transmit. - PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_SEND_PACKET, false); + PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_SEND_DATA, false); } static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 55290ce5b..522e037a6 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -82,8 +82,10 @@ struct rfm22b_stats { uint8_t rx_corrected; uint8_t rx_error; uint8_t rx_missed; + uint8_t rx_failure; uint8_t tx_dropped; uint8_t tx_resent; + uint8_t tx_failure; uint8_t resets; uint8_t timeouts; uint8_t link_quality; diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 6182f58e3..a7de949d1 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -587,10 +587,12 @@ enum pios_rfm22b_state { RFM22B_STATE_WAIT_PREAMBLE, RFM22B_STATE_WAIT_SYNC, RFM22B_STATE_RX_DATA, + RFM22B_STATE_RX_FAILURE, RFM22B_STATE_RECEIVING_STATUS, RFM22B_STATE_TX_DELAY, RFM22B_STATE_TX_START, RFM22B_STATE_TX_DATA, + RFM22B_STATE_TX_FAILURE, RFM22B_STATE_SENDING_ACK, RFM22B_STATE_SENDING_NACK, RFM22B_STATE_RECEIVING_ACK, @@ -618,11 +620,11 @@ enum pios_rfm22b_event { RFM22B_EVENT_RX_COMPLETE, RFM22B_EVENT_RX_ERROR, RFM22B_EVENT_STATUS_RECEIVED, - RFM22B_EVENT_SEND_PACKET, - RFM22B_EVENT_START_TRANSFER, + RFM22B_EVENT_SEND_DATA, RFM22B_EVENT_TX_START, RFM22B_EVENT_TX_STARTED, RFM22B_EVENT_TX_COMPLETE, + RFM22B_EVENT_FAILURE, RFM22B_EVENT_TIMEOUT, RFM22B_EVENT_ERROR, RFM22B_EVENT_FATAL_ERROR, diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index ff1aba611..8208e6c41 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -72,9 +72,11 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkStatus", "RxCorrected", m_oplink->Corrected); addUAVObjectToWidgetRelation("OPLinkStatus", "RxErrors", m_oplink->Errors); addUAVObjectToWidgetRelation("OPLinkStatus", "RxMissed", m_oplink->Missed); + addUAVObjectToWidgetRelation("OPLinkStatus", "RxFailure", m_oplink->RxFailure); addUAVObjectToWidgetRelation("OPLinkStatus", "UAVTalkErrors", m_oplink->UAVTalkErrors); addUAVObjectToWidgetRelation("OPLinkStatus", "TxDropped", m_oplink->Dropped); addUAVObjectToWidgetRelation("OPLinkStatus", "TxResent", m_oplink->Resent); + addUAVObjectToWidgetRelation("OPLinkStatus", "TxFailure", m_oplink->TxFailure); addUAVObjectToWidgetRelation("OPLinkStatus", "Resets", m_oplink->Resets); addUAVObjectToWidgetRelation("OPLinkStatus", "Timeouts", m_oplink->Timeouts); addUAVObjectToWidgetRelation("OPLinkStatus", "RSSI", m_oplink->RSSI); diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index f7c628cbd..06e199394 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -1195,6 +1195,63 @@ + + + RX Failure + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + The percentage of packets that were not received at all + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + TX Dropped @@ -1204,7 +1261,7 @@ - + @@ -1245,7 +1302,7 @@ - + TX Resent @@ -1255,7 +1312,7 @@ - + @@ -1296,7 +1353,49 @@ - + + + + Tx Failure + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + false + + + true + + + + UAVTalk Errors @@ -1306,7 +1405,7 @@ - + @@ -1338,7 +1437,7 @@ - + Resets @@ -1348,7 +1447,7 @@ - + @@ -1386,7 +1485,7 @@ - + Timeouts @@ -1396,7 +1495,7 @@ - + diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml index 0d048c139..619ab1e81 100755 --- a/shared/uavobjectdefinition/oplinkstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -10,9 +10,11 @@ + - + + From fecc23eb499e7db342a3544001c86aaebfe96dc4 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 25 Nov 2012 19:40:49 -0700 Subject: [PATCH 18/31] RFM22B/OPLink: Auto-configuration of remote com port now working. --- flight/Libraries/inc/packet_handler.h | 3 + .../Modules/RadioComBridge/RadioComBridge.c | 185 +++++++++++++----- .../PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 12 +- flight/PiOS/Common/pios_rfm22b.c | 35 ++++ flight/PiOS/inc/pios_rfm22b.h | 6 + flight/PiOS/inc/pios_rfm22b_priv.h | 3 + flight/PipXtreme/System/pios_board.c | 63 +++--- 7 files changed, 222 insertions(+), 85 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 288f98e91..6bbbb1d88 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -32,6 +32,7 @@ #include #include +#include // Public defines / macros #define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p) @@ -94,6 +95,8 @@ typedef struct { uint32_t min_frequency; uint32_t max_frequency; uint8_t max_tx_power; + OPLinkSettingsOutputConnectionOptions port; + OPLinkSettingsComSpeedOptions com_speed; uint8_t ecc[RS_ECC_NPARITY]; } PHConnectionPacket, *PHConnectionPacketHandle; diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index ca3556e83..dee6d7afa 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -96,6 +96,7 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length); static int32_t RadioSendHandler(uint8_t *buf, int32_t length); static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type); +static void configureComCallback(OPLinkSettingsOutputConnectionOptions com_port, OPLinkSettingsComSpeedOptions com_speed); static void updateSettings(); // **************** @@ -112,6 +113,9 @@ static int32_t RadioComBridgeStart(void) { if(data) { + // Configure the com port configuration callback + PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); + // Set the baudrates, etc. updateSettings(); @@ -459,12 +463,54 @@ static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEve xQueueSend(queue, &ev, portMAX_DELAY); } - /** - * Update the telemetry settings, called on startup. - * FIXME: This should be in the TelemetrySettings object. But objects - * have too much overhead yet. Also the telemetry has no any specific - * settings, etc. Thus the HwSettings object which contains the - * telemetry port speed is used for now. +/** + * Configure the output port based on a configuration event from the remote coordinator. + * \param[in] com_port The com port to configure + * \param[in] com_speed The com port speed + */ +static void configureComCallback(OPLinkSettingsOutputConnectionOptions com_port, OPLinkSettingsComSpeedOptions com_speed) +{ + + // Get the settings. + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + + // Set the output telemetry port and speed. + switch (com_port) + { + case OPLINKSETTINGS_OUTPUTCONNECTION_REMOTEHID: + oplinkSettings.InputConnection = OPLINKSETTINGS_INPUTCONNECTION_HID; + break; + case OPLINKSETTINGS_OUTPUTCONNECTION_REMOTEVCP: + oplinkSettings.InputConnection = OPLINKSETTINGS_INPUTCONNECTION_VCP; + break; + case OPLINKSETTINGS_OUTPUTCONNECTION_REMOTETELEMETRY: + oplinkSettings.InputConnection = OPLINKSETTINGS_INPUTCONNECTION_TELEMETRY; + break; + case OPLINKSETTINGS_OUTPUTCONNECTION_REMOTEFLEXI: + oplinkSettings.InputConnection = OPLINKSETTINGS_INPUTCONNECTION_FLEXI; + break; + case OPLINKSETTINGS_OUTPUTCONNECTION_TELEMETRY: + oplinkSettings.InputConnection = OPLINKSETTINGS_INPUTCONNECTION_HID; + break; + case OPLINKSETTINGS_OUTPUTCONNECTION_FLEXI: + oplinkSettings.InputConnection = OPLINKSETTINGS_INPUTCONNECTION_HID; + break; + } + oplinkSettings.ComSpeed = com_speed; + + // A non-coordinator modem should not set a remote telemetry connection. + oplinkSettings.OutputConnection = OPLINKSETTINGS_OUTPUTCONNECTION_REMOTEHID; + + // Update the OPLinkSettings object. + OPLinkSettingsSet(&oplinkSettings); + + // Perform the update. + updateSettings(); +} + +/** + * Update the oplink settings, called on startup. */ static void updateSettings() { @@ -472,70 +518,111 @@ static void updateSettings() // Get the settings. OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - + + bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); + if (is_coordinator) + { + // Set the remote com configuration parameters + PIOS_RFM22B_SetRemoteComConfig(pios_rfm22b_id, oplinkSettings.OutputConnection, oplinkSettings.ComSpeed); + + // Configure the RFM22B device as coordinator or not + PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, true); + + // Set the frequencies. + PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency); + + // Set the maximum radio RF power. + switch (oplinkSettings.MaxRFPower) + { + case OPLINKSETTINGS_MAXRFPOWER_125: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); + break; + case OPLINKSETTINGS_MAXRFPOWER_16: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); + break; + case OPLINKSETTINGS_MAXRFPOWER_316: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); + break; + case OPLINKSETTINGS_MAXRFPOWER_63: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); + break; + case OPLINKSETTINGS_MAXRFPOWER_126: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); + break; + case OPLINKSETTINGS_MAXRFPOWER_25: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); + break; + case OPLINKSETTINGS_MAXRFPOWER_50: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); + break; + case OPLINKSETTINGS_MAXRFPOWER_100: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); + break; + } + + // Set the radio destination ID. + PIOS_RFM22B_SetDestinationId(pios_rfm22b_id, oplinkSettings.PairID); + } + + // Determine what com ports we're using. + switch (oplinkSettings.InputConnection) + { + case OPLINKSETTINGS_INPUTCONNECTION_VCP: + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_VCP; + break; + case OPLINKSETTINGS_INPUTCONNECTION_TELEMETRY: + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_TELEM; + break; + case OPLINKSETTINGS_INPUTCONNECTION_FLEXI: + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; + break; + default: + PIOS_COM_TELEMETRY = 0; + break; + } + + switch (oplinkSettings.OutputConnection) + { + case OPLINKSETTINGS_OUTPUTCONNECTION_FLEXI: + PIOS_COM_RADIO = PIOS_COM_TELEM_UART_FLEXI; + break; + case OPLINKSETTINGS_OUTPUTCONNECTION_TELEMETRY: + PIOS_COM_RADIO = PIOS_COM_TELEM_UART_TELEM; + break; + default: + PIOS_COM_RADIO = PIOS_COM_RFM22B; + break; + } + + // Configure the com port speeds. switch (oplinkSettings.ComSpeed) { case OPLINKSETTINGS_COMSPEED_2400: - if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 2400); + if (is_coordinator && PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 2400); if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 2400); break; case OPLINKSETTINGS_COMSPEED_4800: - if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 4800); + if (is_coordinator && PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 4800); if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 4800); break; case OPLINKSETTINGS_COMSPEED_9600: - if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 9600); + if (is_coordinator && PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 9600); if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 9600); break; case OPLINKSETTINGS_COMSPEED_19200: - if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 19200); + if (is_coordinator && PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 19200); if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 19200); break; case OPLINKSETTINGS_COMSPEED_38400: - if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 38400); + if (is_coordinator && PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 38400); if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 38400); break; case OPLINKSETTINGS_COMSPEED_57600: - if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 57600); + if (is_coordinator && PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 57600); if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 57600); break; case OPLINKSETTINGS_COMSPEED_115200: - if (PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 115200); + if (is_coordinator && PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 115200); if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 115200); break; } - - // Set the frequencies. - PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency); - - // Set the maximum radio RF power. - switch (oplinkSettings.MaxRFPower) - { - case OPLINKSETTINGS_MAXRFPOWER_125: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); - break; - case OPLINKSETTINGS_MAXRFPOWER_16: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); - break; - case OPLINKSETTINGS_MAXRFPOWER_316: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); - break; - case OPLINKSETTINGS_MAXRFPOWER_63: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); - break; - case OPLINKSETTINGS_MAXRFPOWER_126: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); - break; - case OPLINKSETTINGS_MAXRFPOWER_25: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); - break; - case OPLINKSETTINGS_MAXRFPOWER_50: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); - break; - case OPLINKSETTINGS_MAXRFPOWER_100: - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); - break; - } - - // Set the radio destination ID. - PIOS_RFM22B_SetDestinationId(pios_rfm22b_id, oplinkSettings.PairID); } diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index 87f14d951..fc1cabb84 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -151,12 +151,20 @@ extern uint32_t pios_i2c_flexi_adapter_id; #define PIOS_COM_MAX_DEVS 5 extern uint32_t pios_com_telem_usb_id; +extern uint32_t pios_com_telem_vcp_id; +extern uint32_t pios_com_telem_uart_telem_id; +extern uint32_t pios_com_telem_uart_flexi_id; extern uint32_t pios_com_telemetry_id; extern uint32_t pios_com_rfm22b_id; +extern uint32_t pios_com_radio_id; extern uint32_t pios_ppm_rcvr_id; -#define PIOS_COM_TELEMETRY (pios_com_telemetry_id) #define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_RADIO (pios_com_rfm22b_id) +#define PIOS_COM_TELEM_VCP (pios_com_telem_vcp_id) +#define PIOS_COM_TELEM_UART_FLEXI (pios_com_telem_uart_flexi_id) +#define PIOS_COM_TELEM_UART_TELEM (pios_com_telem_uart_telem_id) +#define PIOS_COM_TELEMETRY (pios_com_telemetry_id) +#define PIOS_COM_RFM22B (pios_com_rfm22b_id) +#define PIOS_COM_RADIO (pios_com_radio_id) #define PIOS_PPM_RECEIVER (pios_ppm_rcvr_id) #define DEBUG_LEVEL 2 diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index d11d4dddd..e736794c2 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -578,6 +578,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->send_ppm = false; rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; + // Initialize the com configuration callback. + rfm22b_dev->com_config_cb = NULL; + // Initialize the stats. rfm22b_dev->stats.packets_per_sec = 0; rfm22b_dev->stats.rx_good = 0; @@ -753,6 +756,34 @@ void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator) PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); } +/** + * Set the remote com port configuration parameters. + * \param[in] rfm22b_id The rfm22b device. + * \param[in] com_port The remote com port + * \param[in] com_speed The remote com port speed + */ +void PIOS_RFM22B_SetRemoteComConfig(uint32_t rfm22b_id, OPLinkSettingsOutputConnectionOptions com_port, OPLinkSettingsComSpeedOptions com_speed) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_validate(rfm22b_dev)) + return; + rfm22b_dev->con_packet.port = com_port; + rfm22b_dev->con_packet.com_speed = com_speed; +} + +/** + * Set the com port configuration callback (to receive com configuration over the air) + * \param[in] rfm22b_id The rfm22b device. + * \param[in] cb A pointer to the callback function + */ +void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_validate(rfm22b_dev)) + return; + rfm22b_dev->com_config_cb = cb; +} + /** * Returns the device statistics RFM22B device. * \param[in] rfm22b_id The RFM22B device index. @@ -1987,6 +2018,10 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->min_frequency = cph->min_frequency; rfm22b_dev->max_frequency = cph->max_frequency; rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->frequency_hz); + + // Call the com port configuration function + if (rfm22b_dev->com_config_cb && !rfm22b_dev->coordinator) + rfm22b_dev->com_config_cb(cph->port, cph->com_speed); } static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 522e037a6..1511c9884 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -32,6 +32,7 @@ #define PIOS_RFM22B_H #include +#include enum gpio_direction {GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX}; @@ -94,6 +95,9 @@ struct rfm22b_stats { uint8_t link_state; }; +/* Callback function prototypes */ +typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsOutputConnectionOptions com_port, OPLinkSettingsComSpeedOptions com_speed); + /* 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_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_frequency, uint32_t max_frequency); @@ -101,6 +105,8 @@ extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_p extern void PIOS_RFM22B_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 void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator); +extern void PIOS_RFM22B_SetRemoteComConfig(uint32_t rfm22b_id, OPLinkSettingsOutputConnectionOptions com_port, OPLinkSettingsComSpeedOptions com_speed); +extern void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs); diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index a7de949d1..672112d49 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -672,6 +672,9 @@ struct pios_rfm22b_dev { // ISR pending xSemaphoreHandle isrPending; + // The com configuration callback + PIOS_RFM22B_ComConfigCallback com_config_cb; + // The COM callback functions. pios_com_callback rx_in_cb; uint32_t rx_in_context; diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 5f4688359..47b08dd95 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -39,15 +39,24 @@ #define PIOS_COM_TELEM_USB_RX_BUF_LEN 256 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 256 +#define PIOS_COM_TELEM_VCP_RX_BUF_LEN 256 +#define PIOS_COM_TELEM_VCP_TX_BUF_LEN 256 + #define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256 #define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256 uint32_t pios_com_telem_usb_id = 0; +uint32_t pios_com_telem_vcp_id = 0; +uint32_t pios_com_telem_uart_telem_id = 0; +uint32_t pios_com_telem_uart_flexi_id = 0; uint32_t pios_com_telemetry_id = 0; +#if defined(PIOS_INCLUDE_PPM) uint32_t pios_ppm_rcvr_id = 0; +#endif #if defined(PIOS_INCLUDE_RFM22B) uint32_t pios_rfm22b_id = 0; uint32_t pios_com_rfm22b_id = 0; +uint32_t pios_com_radio_id = 0; uint32_t pios_packet_handler = 0; #endif @@ -132,14 +141,14 @@ void PIOS_Board_Init(void) { uint32_t pios_usb_id; PIOS_USB_Init(&pios_usb_id, &pios_usb_main_cfg); - /* We always onfigure the usb HID port */ + /* Configure the USB HID port */ { uint32_t pios_usb_hid_id; if (PIOS_USB_HID_Init(&pios_usb_hid_id, &pios_usb_hid_cfg, pios_usb_id)) { PIOS_Assert(0); } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_USB_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_usb_id, &pios_usb_hid_com_driver, pios_usb_hid_id, @@ -149,54 +158,45 @@ void PIOS_Board_Init(void) { } } - /* Configure the requested com port */ - switch (oplinkSettings.InputConnection) - { - case OPLINKSETTINGS_INPUTCONNECTION_HID: - // This is always configured - break; - case OPLINKSETTINGS_INPUTCONNECTION_VCP: - { + /* Configure the USB virtual com port (VCP) */ #if defined(PIOS_INCLUDE_USB_CDC) - if (!usb_cdc_present) - break; + if (usb_cdc_present) + { uint32_t pios_usb_cdc_id; if (PIOS_USB_CDC_Init(&pios_usb_cdc_id, &pios_usb_cdc_cfg, pios_usb_id)) { PIOS_Assert(0); } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_VCP_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telemetry_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, - rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + if (PIOS_COM_Init(&pios_com_telem_vcp_id, &pios_usb_cdc_com_driver, pios_usb_cdc_id, + rx_buffer, PIOS_COM_TELEM_VCP_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_VCP_TX_BUF_LEN)) { PIOS_Assert(0); } -#endif - break; } - case OPLINKSETTINGS_INPUTCONNECTION_TELEMETRY: +#endif + + /* Configure the telemetry serial port */ { - // Configure the telemetry port. uint32_t pios_usart1_id; if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { PIOS_Assert(0); } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); + uint8_t *rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); + uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telemetry_id, &pios_usart_com_driver, pios_usart1_id, + if (PIOS_COM_Init(&pios_com_telem_uart_telem_id, &pios_usart_com_driver, pios_usart1_id, rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { PIOS_Assert(0); } - break; } - case OPLINKSETTINGS_INPUTCONNECTION_FLEXI: + + /* Configure the flexi serial port */ { - // Configure the flexi port. uint32_t pios_usart3_id; if (PIOS_USART_Init(&pios_usart3_id, &pios_usart_telem_flexi_cfg)) { PIOS_Assert(0); @@ -205,13 +205,11 @@ void PIOS_Board_Init(void) { uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telemetry_id, &pios_usart_com_driver, pios_usart3_id, + if (PIOS_COM_Init(&pios_com_telem_uart_flexi_id, &pios_usart_com_driver, pios_usart3_id, rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { PIOS_Assert(0); } - break; - } } /* Configure PPM input */ @@ -236,9 +234,6 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); } - /* Configure the RFM22B device as coordinator or not */ - PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); - uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); From 7f6a71835453a2083e83c950953135bcd804218c Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 2 Dec 2012 09:41:48 -0700 Subject: [PATCH 19/31] RFM22B: Added optional debug signals on the telemetry port of the PipX. Also fixed a couple of bugs that were causing excessive re-transmissions (found using the debug signals). --- .../PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 24 ++++++ flight/PiOS/Common/pios_rfm22b.c | 74 ++++++++++++++++--- flight/PiOS/inc/pios_rfm22b_priv.h | 1 + flight/PipXtreme/System/inc/pios_config.h | 4 +- flight/PipXtreme/System/pios_board.c | 2 + .../board_hw_defs/pipxtreme/board_hw_defs.c | 42 +++++++++++ 6 files changed, 133 insertions(+), 14 deletions(-) diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index fc1cabb84..2c9b0062c 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -90,6 +90,12 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 #define PIOS_LED_LINK 1 #define PIOS_LED_RX 2 #define PIOS_LED_TX 3 +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM +#define PIOS_LED_D1 4 +#define PIOS_LED_D2 5 +#define PIOS_LED_D3 6 +#define PIOS_LED_D4 7 +#endif #define PIOS_LED_HEARTBEAT PIOS_LED_USB #define PIOS_LED_ALARM PIOS_LED_TX @@ -110,6 +116,24 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 #define TX_LED_OFF PIOS_LED_Off(PIOS_LED_TX) #define TX_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_TX) +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM +#define D1_LED_ON PIOS_LED_On(PIOS_LED_D1) +#define D1_LED_OFF PIOS_LED_Off(PIOS_LED_D1) +#define D1_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D1) + +#define D2_LED_ON PIOS_LED_On(PIOS_LED_D2) +#define D2_LED_OFF PIOS_LED_Off(PIOS_LED_D2) +#define D2_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D2) + +#define D3_LED_ON PIOS_LED_On(PIOS_LED_D3) +#define D3_LED_OFF PIOS_LED_Off(PIOS_LED_D3) +#define D3_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D3) + +#define D4_LED_ON PIOS_LED_On(PIOS_LED_D4) +#define D4_LED_OFF PIOS_LED_Off(PIOS_LED_D4) +#define D4_LED_TOGGLE PIOS_LED_Toggle(PIOS_LED_D4) +#endif + //------------------------- // System Settings //------------------------- diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index e736794c2..0ecb112f6 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -200,6 +200,7 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_clearLEDs(); // SPI read/write functions static void rfm22_assertCs(); @@ -268,7 +269,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_SEND_DATA] = RFM22B_STATE_TX_DELAY, - [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, + [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK, [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, @@ -282,7 +283,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC, [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_SEND_DATA] = RFM22B_STATE_TX_DELAY, - [RFM22B_EVENT_PACKET_NACKED] = RFM22B_STATE_RECEIVING_NACK, + [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK, [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, @@ -512,10 +513,12 @@ static const uint8_t ss_reg_70[] = { 0x24, 0x2D}; // rfm22_modulation_mode_cont static const uint8_t ss_reg_71[] = { 0x2B, 0x23}; // rfm22_modulation_mode_control2 -static inline uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { - if(end_time > start_time) +static inline uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) +{ + if(end_time >= start_time) return (end_time - start_time) * portTICK_RATE_MS; - return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; + // Rollover + return ((portMAX_DELAY - start_time) + end_time) * portTICK_RATE_MS; } bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev) @@ -921,7 +924,7 @@ static void PIOS_RFM22B_Task(void *parameters) if (timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) { rfm22b_dev->tx_complete_ticks = curTicks; - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_PACKET_NACKED); + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ACK_TIMEOUT); } } else @@ -958,8 +961,11 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_d uint32_t datarate_bps = data_rate[datarate]; rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5); if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) - //rfm22b_dev->max_ack_delay = (uint16_t)((float)(sizeof(PHPacketHeader) * 8 * 1000) / (float)(datarate_bps) + 0.5) * 4; - rfm22b_dev->max_ack_delay = 50; + { + // Generate a pseudo-random number from 0-8 to add to the delay + uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03; + rfm22b_dev->max_ack_delay = (uint16_t)((float)(sizeof(PHPacketHeader) * 8 * 1000) / (float)(datarate_bps) + 0.5) * 4 + random; + } else rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS; @@ -1263,6 +1269,9 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) { rfm22b_dev->packet_start_ticks = 0; +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D2_LED_ON; +#endif // disable interrupts rfm22_write(RFM22_interrupt_enable1, 0x00); @@ -1379,6 +1388,11 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) if (!p) return RFM22B_EVENT_RX_MODE; +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D1_LED_ON; + D2_LED_OFF; +#endif + // Add the error correcting code. p->header.source_id = rfm22b_dev->deviceID; encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); @@ -1759,6 +1773,9 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); if (rfm22b_dev->rx_complete_ticks == 0) rfm22b_dev->rx_complete_ticks = 1; +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D2_LED_OFF; +#endif } // Start a new transaction @@ -1835,6 +1852,12 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; // Start a new transaction rfm22b_dev->packet_start_ticks = 0; + +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D1_LED_OFF; + D3_LED_OFF; + D4_LED_OFF; +#endif } return ret_event; @@ -1859,6 +1882,9 @@ static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; rfm22b_dev->tx_packet = (PHPacketHandle)aph; +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D3_LED_ON; +#endif return RFM22B_EVENT_TX_START; } @@ -1874,6 +1900,9 @@ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; rfm22b_dev->tx_packet = (PHPacketHandle)aph; +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D4_LED_ON; +#endif return RFM22B_EVENT_TX_START; } @@ -2052,6 +2081,9 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->int_status2 = 0; rfm22b_dev->ezmac_status = 0; + // Clean the LEDs + rfm22_clearLEDs(); + // Initialize the detected device statistics. for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { @@ -2080,9 +2112,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->stats.rx_seq = 0; rfm22b_dev->data_packet.header.data_size = 0; - // Calculate the (approximate) maximum amount of time that it should take to transmit / receive a packet. - rfm22b_dev->packet_start_ticks = 0; - // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); @@ -2301,6 +2330,18 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) return RFM22B_EVENT_INITIALIZED; } +static void rfm22_clearLEDs() { + LINK_LED_OFF; + RX_LED_OFF; + TX_LED_OFF; +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D1_LED_OFF; + D2_LED_OFF; + D3_LED_OFF; + D4_LED_OFF; +#endif +} + static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) { rfm22b_dev->stats.timeouts++; @@ -2312,12 +2353,21 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; } rfm22b_dev->rx_buffer_wr = 0; + TX_LED_OFF; + RX_LED_OFF; +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D1_LED_OFF; + D2_LED_OFF; + D3_LED_OFF; + D4_LED_OFF; +#endif return RFM22B_EVENT_TX_START; } static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev) { rfm22b_dev->stats.resets++; + rfm22_clearLEDs(); return RFM22B_EVENT_INITIALIZE; } @@ -2329,8 +2379,8 @@ static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev) */ static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev) { - // RF module error .. flash the LED's + rfm22_clearLEDs(); for(unsigned int j = 0; j < 16; j++) { USB_LED_ON; diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 672112d49..1afc30425 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -614,6 +614,7 @@ enum pios_rfm22b_event { RFM22B_EVENT_CONNECTION_ACCEPTED, RFM22B_EVENT_PACKET_ACKED, RFM22B_EVENT_PACKET_NACKED, + RFM22B_EVENT_ACK_TIMEOUT, RFM22B_EVENT_RX_MODE, RFM22B_EVENT_PREAMBLE_DETECTED, RFM22B_EVENT_SYNC_DETECTED, diff --git a/flight/PipXtreme/System/inc/pios_config.h b/flight/PipXtreme/System/inc/pios_config.h index 70c0768fa..62a76aa00 100755 --- a/flight/PipXtreme/System/inc/pios_config.h +++ b/flight/PipXtreme/System/inc/pios_config.h @@ -99,8 +99,8 @@ /* PIOS Initcall infrastructure */ #define PIOS_INCLUDE_INITCALL -/* Always include the radio module */ -#define RADIO_BUILTIN +/* Turn on debugging signals on the telemetry port */ +//#define PIOS_RFM22B_DEBUG_ON_TELEM #endif /* PIOS_CONFIG_H */ /** diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 47b08dd95..2cf3677ae 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -179,6 +179,7 @@ void PIOS_Board_Init(void) { #endif /* Configure the telemetry serial port */ +#ifndef PIOS_RFM22B_DEBUG_ON_TELEM { uint32_t pios_usart1_id; if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { @@ -194,6 +195,7 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); } } +#endif /* Configure the flexi serial port */ { diff --git a/flight/board_hw_defs/pipxtreme/board_hw_defs.c b/flight/board_hw_defs/pipxtreme/board_hw_defs.c index 4b5c17264..a205cab27 100644 --- a/flight/board_hw_defs/pipxtreme/board_hw_defs.c +++ b/flight/board_hw_defs/pipxtreme/board_hw_defs.c @@ -45,6 +45,48 @@ static const struct pios_led pios_leds[] = { }, }, }, +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + [PIOS_LED_D1] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_D2] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_D3] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, + [PIOS_LED_D4] = { + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_Out_PP, + .GPIO_Speed = GPIO_Speed_50MHz, + }, + }, + }, +#endif }; static const struct pios_led_cfg pios_led_cfg = { From 8ead1200b9024b83cacea8ab39728cb71752a51f Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 10 Dec 2012 20:47:35 -0700 Subject: [PATCH 20/31] RFM22B: Added timing synchronization on transmit to try to eliminate both sides transmitting at the same time. This now achieves virtually 0 re-transmissions for close range transmission while transmitting both full telemety and PPM at 64k. --- flight/PiOS/Common/pios_rfm22b.c | 107 +++++++++++++++++---------- flight/PiOS/Common/pios_rfm22b_com.c | 3 - flight/PiOS/inc/pios_rfm22b_priv.h | 20 ++--- 3 files changed, 80 insertions(+), 50 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 0ecb112f6..67c17ae0c 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -267,8 +267,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_setRxMode, .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, - [RFM22B_EVENT_SEND_DATA] = RFM22B_STATE_TX_DELAY, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK, [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, @@ -281,8 +280,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_detectPreamble, .next_state = { [RFM22B_EVENT_PREAMBLE_DETECTED] = RFM22B_STATE_WAIT_SYNC, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, - [RFM22B_EVENT_SEND_DATA] = RFM22B_STATE_TX_DELAY, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_ACK_TIMEOUT] = RFM22B_STATE_RECEIVING_NACK, [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_PREAMBLE, [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, @@ -297,8 +295,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_WAIT_SYNC, [RFM22B_EVENT_SYNC_DETECTED] = RFM22B_STATE_RX_DATA, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, - [RFM22B_EVENT_SEND_DATA] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_FAILURE] = RFM22B_STATE_RX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, @@ -336,7 +332,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_RECEIVING_ACK] = { .entry_fn = rfm22_receiveAck, .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, @@ -364,15 +360,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, }, }, - [RFM22B_STATE_TX_DELAY] = { - .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, - [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, - [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, - [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, - }, - }, [RFM22B_STATE_TX_START] = { .entry_fn = rfm22_txStart, .next_state = { @@ -388,7 +375,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_txData, .next_state = { [RFM22B_EVENT_INT_RECEIVED] = RFM22B_STATE_TX_DATA, - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_FAILURE] = RFM22B_STATE_TX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, @@ -410,7 +396,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_SENDING_ACK] = { .entry_fn = rfm22_sendAck, .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -420,7 +406,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_SENDING_NACK] = { .entry_fn = rfm22_sendNack, .next_state = { - [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_DELAY, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -871,7 +857,7 @@ static void PIOS_RFM22B_Task(void *parameters) #endif /* PIOS_WDG_RFM22B */ // Wait for a signal indicating an external interrupt or a pending send/receive request. - if ( xSemaphoreTake(g_rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE ) { + if (xSemaphoreTake(g_rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { lastEventTicks = xTaskGetTickCount(); // Process events through the state machine. @@ -906,9 +892,6 @@ static void PIOS_RFM22B_Task(void *parameters) if ((rfm22b_dev->packet_start_ticks > 0) && (timeDifferenceMs(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TIMEOUT); - else if (rfm22b_dev->state == RFM22B_STATE_TX_DELAY) - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); - // Have it been too long since we received a packet else if ((rfm22b_dev->rx_complete_ticks > 0) && (timeDifferenceMs(rfm22b_dev->rx_complete_ticks, curTicks) > DISCONNECT_TIMEOUT_MS)) rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); @@ -920,7 +903,7 @@ static void PIOS_RFM22B_Task(void *parameters) if (rfm22b_dev->prev_tx_packet) { - // Should be resend the packet? + // Should we resend the packet? if (timeDifferenceMs(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) { rfm22b_dev->tx_complete_ticks = curTicks; @@ -940,6 +923,17 @@ static void PIOS_RFM22B_Task(void *parameters) } } + + // Send a packet if it's our time slice + rfm22b_dev->time_to_send = (((curTicks - rfm22b_dev->time_to_send_offset) & 0xC) == 0); +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + if (rfm22b_dev->time_to_send) + D4_LED_ON; + else + D4_LED_OFF; +#endif + if (rfm22b_dev->time_to_send) + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); } } @@ -1268,9 +1262,14 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) { + // Are we already in Rx mode? + if (rfm22b_dev->in_rx_mode) + return RFM22B_EVENT_NUM_EVENTS; + rfm22b_dev->packet_start_ticks = 0; #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D2_LED_ON; + D3_LED_TOGGLE; #endif // disable interrupts @@ -1302,6 +1301,9 @@ static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev // enable the receiver rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon); + // Indicate that we're in RX mode. + rfm22b_dev->in_rx_mode = true; + // No event generated return RFM22B_EVENT_NUM_EVENTS; } @@ -1332,6 +1334,10 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) { PHPacketHandle p = NULL; + // Don't send if it's not our turn. + if (!rfm22b_dev->time_to_send) + return RFM22B_EVENT_RX_MODE; + // See if there's a packet ready to send. if (rfm22b_dev->tx_packet) p = rfm22b_dev->tx_packet; @@ -1388,9 +1394,13 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) if (!p) return RFM22B_EVENT_RX_MODE; + // We're transitioning out of Rx mode. + rfm22b_dev->in_rx_mode = false; + #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D1_LED_ON; D2_LED_OFF; + D3_LED_TOGGLE; #endif // Add the error correcting code. @@ -1460,7 +1470,7 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) TX_LED_ON; - return RFM22B_EVENT_TX_STARTED; + return RFM22B_EVENT_NUM_EVENTS; } static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) @@ -1479,7 +1489,6 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; rfm22b_dev->send_status = true; - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); return true; } @@ -1499,6 +1508,7 @@ static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) if(rfm22b_dev->ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT) valid_input_detected = true; } + valid_input_detected = true; // Send the PPM packet if it's valid if (valid_input_detected) @@ -1507,7 +1517,6 @@ static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->ppm_packet.header.type = PACKET_TYPE_PPM; rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); rfm22b_dev->send_ppm = true; - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); } #endif @@ -1575,6 +1584,10 @@ static enum pios_rfm22b_event rfm22_detectPreamble(struct pios_rfm22b_dev *rfm22 if (rfm22b_dev->packet_start_ticks == 0) rfm22b_dev->packet_start_ticks = 1; RX_LED_ON; + +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D3_LED_TOGGLE; +#endif return RFM22B_EVENT_PREAMBLE_DETECTED; } @@ -1775,9 +1788,13 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->rx_complete_ticks = 1; #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D2_LED_OFF; + D3_LED_TOGGLE; #endif } + // We're finished with Rx mode + rfm22b_dev->in_rx_mode = false; + // Start a new transaction rfm22b_dev->packet_start_ticks = 0; return ret_event; @@ -1791,6 +1808,7 @@ static enum pios_rfm22b_event rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev rfm22b_dev->stats.rx_failure++; rfm22b_dev->rx_buffer_wr = 0; rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); + rfm22b_dev->in_rx_mode = false; if (rfm22b_dev->rx_complete_ticks == 0) rfm22b_dev->rx_complete_ticks = 1; return RFM22B_EVENT_RX_MODE; @@ -1832,7 +1850,8 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // Is this an ACK? bool is_ack = ((rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) || (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS)); - ret_event = is_ack ? RFM22B_EVENT_TX_START : RFM22B_EVENT_RX_MODE; + //ret_event = is_ack ? RFM22B_EVENT_TX_START : RFM22B_EVENT_RX_MODE; + ret_event = RFM22B_EVENT_RX_MODE; if (is_ack) { // If this is an ACK for a connection request message we need to @@ -1847,7 +1866,12 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->prev_tx_packet = rfm22b_dev->tx_packet; rfm22b_dev->tx_complete_ticks = xTaskGetTickCount(); } - + // Set the Tx period + portTickType curTicks = xTaskGetTickCount(); + if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) + rfm22b_dev->time_to_send_offset = curTicks + 0x8; + else if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS) + rfm22b_dev->time_to_send_offset = curTicks; rfm22b_dev->tx_packet = 0; rfm22b_dev->tx_data_wr = rfm22b_dev->tx_data_rd = 0; // Start a new transaction @@ -1855,8 +1879,7 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D1_LED_OFF; - D3_LED_OFF; - D4_LED_OFF; + D3_LED_TOGGLE; #endif } @@ -1882,9 +1905,7 @@ static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; rfm22b_dev->tx_packet = (PHPacketHandle)aph; -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM - D3_LED_ON; -#endif + rfm22b_dev->time_to_send = true; return RFM22B_EVENT_TX_START; } @@ -1900,9 +1921,7 @@ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; rfm22b_dev->tx_packet = (PHPacketHandle)aph; -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM - D4_LED_ON; -#endif + rfm22b_dev->time_to_send = true; return RFM22B_EVENT_TX_START; } @@ -1913,6 +1932,7 @@ static enum pios_rfm22b_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev) { PHPacketHandle prev = rfm22b_dev->prev_tx_packet; + portTickType curTicks = xTaskGetTickCount(); // Clear the previous TX packet. rfm22b_dev->prev_tx_packet = NULL; @@ -1930,9 +1950,16 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de // Should we try to start another TX? if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) + { + rfm22b_dev->time_to_send_offset = curTicks; + rfm22b_dev->time_to_send = true; return RFM22B_EVENT_TX_START; + } else + { + rfm22b_dev->time_to_send_offset = curTicks + 0x8; return RFM22B_EVENT_RX_MODE; + } } /** @@ -1946,6 +1973,7 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d rfm22b_dev->prev_tx_packet = NULL; if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET); + rfm22b_dev->time_to_send = true; return RFM22B_EVENT_TX_START; } @@ -2029,6 +2057,7 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf cph->min_frequency = rfm22b_dev->min_frequency; cph->max_frequency = rfm22b_dev->max_frequency; cph->max_tx_power = rfm22b_dev->tx_power; + rfm22b_dev->time_to_send = true; rfm22b_dev->send_connection_request = true; return RFM22B_EVENT_TX_START; @@ -2101,6 +2130,8 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; rfm22b_dev->destination_id = 0xffffffff; + rfm22b_dev->time_to_send = false; + rfm22b_dev->time_to_send_offset = 0; rfm22b_dev->send_status = false; rfm22b_dev->send_connection_request = false; @@ -2111,6 +2142,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->stats.tx_seq = 0; rfm22b_dev->stats.rx_seq = 0; rfm22b_dev->data_packet.header.data_size = 0; + rfm22b_dev->in_rx_mode = false; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); @@ -2349,7 +2381,6 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) // Release the Tx packet if it's set. if (rfm22b_dev->tx_packet != 0) { - rfm22b_dev->tx_packet = 0; rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; } rfm22b_dev->rx_buffer_wr = 0; diff --git a/flight/PiOS/Common/pios_rfm22b_com.c b/flight/PiOS/Common/pios_rfm22b_com.c index f000b1dd9..e8fb4a6db 100644 --- a/flight/PiOS/Common/pios_rfm22b_com.c +++ b/flight/PiOS/Common/pios_rfm22b_com.c @@ -94,9 +94,6 @@ static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail) struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (!PIOS_RFM22B_validate(rfm22b_dev)) return; - - // Send a signal to the radio to start a transmit. - PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_SEND_DATA, false); } static void PIOS_RFM22B_COM_RegisterRxCallback(uint32_t rfm22b_id, pios_com_callback rx_in_cb, uint32_t context) diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 1afc30425..fdd57a83e 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -589,7 +589,6 @@ enum pios_rfm22b_state { RFM22B_STATE_RX_DATA, RFM22B_STATE_RX_FAILURE, RFM22B_STATE_RECEIVING_STATUS, - RFM22B_STATE_TX_DELAY, RFM22B_STATE_TX_START, RFM22B_STATE_TX_DATA, RFM22B_STATE_TX_FAILURE, @@ -621,10 +620,7 @@ enum pios_rfm22b_event { RFM22B_EVENT_RX_COMPLETE, RFM22B_EVENT_RX_ERROR, RFM22B_EVENT_STATUS_RECEIVED, - RFM22B_EVENT_SEND_DATA, RFM22B_EVENT_TX_START, - RFM22B_EVENT_TX_STARTED, - RFM22B_EVENT_TX_COMPLETE, RFM22B_EVENT_FAILURE, RFM22B_EVENT_TIMEOUT, RFM22B_EVENT_ERROR, @@ -652,6 +648,7 @@ struct pios_rfm22b_dev { enum pios_rfm22b_dev_magic magic; struct pios_rfm22b_cfg cfg; + // The SPI bus information uint32_t spi_id; uint32_t slave_num; @@ -670,7 +667,7 @@ struct pios_rfm22b_dev { // The potential paired statistics rfm22b_pair_stats pair_stats[OPLINKSTATUS_PAIRIDS_NUMELEM]; - // ISR pending + // ISR pending semaphore xSemaphoreHandle isrPending; // The com configuration callback @@ -690,6 +687,7 @@ struct pios_rfm22b_dev { // The state machine state and the current event enum pios_rfm22b_state state; + // The event queue handle xQueueHandle eventQueue; @@ -724,19 +722,21 @@ struct pios_rfm22b_dev { PHPacketHandle tx_packet; // The previous tx packet (waiting for an ACK) PHPacketHandle prev_tx_packet; - // the tx data read index + // The tx data read index uint16_t tx_data_rd; - // the tx data write index + // The tx data write index uint16_t tx_data_wr; // The tx packet sequence number uint16_t tx_seq; // The rx data packet PHPacket rx_packet; - // the receive buffer write index + // The receive buffer write index uint16_t rx_buffer_wr; - // the receive buffer write index + // The receive buffer write index uint16_t rx_packet_len; + // Is the modem currently in Rx mode? + bool in_rx_mode; // The status packet PHStatusPacket status_packet; @@ -756,6 +756,8 @@ struct pios_rfm22b_dev { bool send_status; bool send_ppm; bool send_connection_request; + bool time_to_send; + uint8_t time_to_send_offset; // The minimum frequency uint32_t min_frequency; From 1d7e4e0fc2ac99b9d9e165c6dbbf5bdd79e14e5f Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Tue, 11 Dec 2012 17:42:22 -0700 Subject: [PATCH 21/31] Reduced length of transmit window period to 8 ms from 16 ms. --- flight/PiOS/Common/pios_rfm22b.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 67c17ae0c..d6ab31d73 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -925,7 +925,7 @@ static void PIOS_RFM22B_Task(void *parameters) } // Send a packet if it's our time slice - rfm22b_dev->time_to_send = (((curTicks - rfm22b_dev->time_to_send_offset) & 0xC) == 0); + rfm22b_dev->time_to_send = (((curTicks - rfm22b_dev->time_to_send_offset) & 0x6) == 0); #ifdef PIOS_RFM22B_DEBUG_ON_TELEM if (rfm22b_dev->time_to_send) D4_LED_ON; @@ -1869,7 +1869,7 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // Set the Tx period portTickType curTicks = xTaskGetTickCount(); if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK) - rfm22b_dev->time_to_send_offset = curTicks + 0x8; + rfm22b_dev->time_to_send_offset = curTicks + 0x4; else if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS) rfm22b_dev->time_to_send_offset = curTicks; rfm22b_dev->tx_packet = 0; @@ -1957,7 +1957,7 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de } else { - rfm22b_dev->time_to_send_offset = curTicks + 0x8; + rfm22b_dev->time_to_send_offset = curTicks + 0x4; return RFM22B_EVENT_RX_MODE; } } From 90e0746ade533c57a4e6d61055ac526eb81e670f Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Tue, 11 Dec 2012 19:23:59 -0700 Subject: [PATCH 22/31] RFM22B: Changed default (initial) air baud rate to 9600. Also some code cleanup. --- flight/PiOS/Common/pios_rfm22b.c | 366 +++++++++++++++---------------- 1 file changed, 179 insertions(+), 187 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index d6ab31d73..ea53ac9c5 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -61,7 +61,7 @@ #define TASK_PRIORITY (tskIDLE_PRIORITY + 2) #define ISR_TIMEOUT 2 // ms #define EVENT_QUEUE_SIZE 5 -#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_32000 +#define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 #define RFM22B_DEFAULT_FREQUENCY 434000000 #define RFM22B_DEFAULT_MIN_FREQUENCY (RFM22B_DEFAULT_FREQUENCY - 2000000) #define RFM22B_DEFAULT_MAX_FREQUENCY (RFM22B_DEFAULT_FREQUENCY + 2000000) @@ -203,13 +203,13 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_clearLEDs(); // SPI read/write functions -static void rfm22_assertCs(); -static void rfm22_deassertCs(); -static void rfm22_claimBus(); -static void rfm22_releaseBus(); -static void rfm22_write(uint8_t addr, uint8_t data); -static uint8_t rfm22_read(uint8_t addr); -static uint8_t rfm22_read_noclaim(uint8_t addr); +static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data); +static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); +static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); /* Te state transition table */ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_STATES] = { @@ -857,7 +857,7 @@ static void PIOS_RFM22B_Task(void *parameters) #endif /* PIOS_WDG_RFM22B */ // Wait for a signal indicating an external interrupt or a pending send/receive request. - if (xSemaphoreTake(g_rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { + if (xSemaphoreTake(rfm22b_dev->isrPending, ISR_TIMEOUT / portTICK_RATE_MS) == pdTRUE) { lastEventTicks = xTaskGetTickCount(); // Process events through the state machine. @@ -964,51 +964,51 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_d rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS; // rfm22_if_filter_bandwidth - rfm22_write(0x1C, reg_1C[datarate]); + rfm22_write(rfm22b_dev, 0x1C, reg_1C[datarate]); // rfm22_afc_loop_gearshift_override - rfm22_write(0x1D, reg_1D[datarate]); + rfm22_write(rfm22b_dev, 0x1D, reg_1D[datarate]); // RFM22_afc_timing_control - rfm22_write(0x1E, reg_1E[datarate]); + rfm22_write(rfm22b_dev, 0x1E, reg_1E[datarate]); // RFM22_clk_recovery_gearshift_override - rfm22_write(0x1F, reg_1F[datarate]); + rfm22_write(rfm22b_dev, 0x1F, reg_1F[datarate]); // rfm22_clk_recovery_oversampling_ratio - rfm22_write(0x20, reg_20[datarate]); + rfm22_write(rfm22b_dev, 0x20, reg_20[datarate]); // rfm22_clk_recovery_offset2 - rfm22_write(0x21, reg_21[datarate]); + rfm22_write(rfm22b_dev, 0x21, reg_21[datarate]); // rfm22_clk_recovery_offset1 - rfm22_write(0x22, reg_22[datarate]); + rfm22_write(rfm22b_dev, 0x22, reg_22[datarate]); // rfm22_clk_recovery_offset0 - rfm22_write(0x23, reg_23[datarate]); + rfm22_write(rfm22b_dev, 0x23, reg_23[datarate]); // rfm22_clk_recovery_timing_loop_gain1 - rfm22_write(0x24, reg_24[datarate]); + rfm22_write(rfm22b_dev, 0x24, reg_24[datarate]); // rfm22_clk_recovery_timing_loop_gain0 - rfm22_write(0x25, reg_25[datarate]); + rfm22_write(rfm22b_dev, 0x25, reg_25[datarate]); // rfm22_afc_limiter - rfm22_write(0x2A, reg_2A[datarate]); + rfm22_write(rfm22b_dev, 0x2A, reg_2A[datarate]); // rfm22_tx_data_rate1 - rfm22_write(0x6E, reg_6E[datarate]); + rfm22_write(rfm22b_dev, 0x6E, reg_6E[datarate]); // rfm22_tx_data_rate0 - rfm22_write(0x6F, reg_6F[datarate]); + rfm22_write(rfm22b_dev, 0x6F, reg_6F[datarate]); if (!data_whitening) // rfm22_modulation_mode_control1 - rfm22_write(0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); + rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] & ~RFM22_mmc1_enwhite); else // rfm22_modulation_mode_control1 - rfm22_write(0x70, reg_70[datarate] | RFM22_mmc1_enwhite); + rfm22_write(rfm22b_dev, 0x70, reg_70[datarate] | RFM22_mmc1_enwhite); // rfm22_modulation_mode_control2 - rfm22_write(0x71, reg_71[datarate]); + rfm22_write(rfm22b_dev, 0x71, reg_71[datarate]); // rfm22_frequency_deviation - rfm22_write(0x72, reg_72[datarate]); + rfm22_write(rfm22b_dev, 0x72, reg_72[datarate]); - rfm22_write(RFM22_ook_counter_value1, 0x00); - rfm22_write(RFM22_ook_counter_value2, 0x00); + rfm22_write(rfm22b_dev, RFM22_ook_counter_value1, 0x00); + rfm22_write(rfm22b_dev, RFM22_ook_counter_value2, 0x00); } void PIOS_RFM22B_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, bool data_whitening) @@ -1026,32 +1026,32 @@ void PIOS_RFM22B_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, // SPI read/write //! Assert the CS line -static void rfm22_assertCs() +static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev) { PIOS_DELAY_WaituS(1); - if(PIOS_RFM22B_validate(g_rfm22b_dev) && g_rfm22b_dev->spi_id != 0) - PIOS_SPI_RC_PinSet(g_rfm22b_dev->spi_id, g_rfm22b_dev->slave_num, 0); + if(rfm22b_dev->spi_id != 0) + PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 0); } //! Deassert the CS line -static void rfm22_deassertCs() +static void rfm22_deassertCs(struct pios_rfm22b_dev *rfm22b_dev) { - if(PIOS_RFM22B_validate(g_rfm22b_dev) && g_rfm22b_dev->spi_id != 0) - PIOS_SPI_RC_PinSet(g_rfm22b_dev->spi_id, g_rfm22b_dev->slave_num, 1); + if(rfm22b_dev->spi_id != 0) + PIOS_SPI_RC_PinSet(rfm22b_dev->spi_id, rfm22b_dev->slave_num, 1); } //! Claim the SPI bus semaphore -static void rfm22_claimBus() +static void rfm22_claimBus(struct pios_rfm22b_dev *rfm22b_dev) { - if(PIOS_RFM22B_validate(g_rfm22b_dev) && g_rfm22b_dev->spi_id != 0) - PIOS_SPI_ClaimBus(g_rfm22b_dev->spi_id); + if(rfm22b_dev->spi_id != 0) + PIOS_SPI_ClaimBus(rfm22b_dev->spi_id); } //! Release the SPI bus semaphore -static void rfm22_releaseBus() +static void rfm22_releaseBus(struct pios_rfm22b_dev *rfm22b_dev) { - if(PIOS_RFM22B_validate(g_rfm22b_dev) && g_rfm22b_dev->spi_id != 0) - PIOS_SPI_ReleaseBus(g_rfm22b_dev->spi_id); + if(rfm22b_dev->spi_id != 0) + PIOS_SPI_ReleaseBus(rfm22b_dev->spi_id); } /** @@ -1059,16 +1059,14 @@ static void rfm22_releaseBus() * @param[in] addr The address to write to * @param[in] data The datat to write to that address */ -static void rfm22_write(uint8_t addr, uint8_t data) +static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data) { - if(PIOS_RFM22B_validate(g_rfm22b_dev)) { - rfm22_claimBus(); - rfm22_assertCs(); - uint8_t buf[2] = {addr | 0x80, data}; - PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); - rfm22_deassertCs(); - rfm22_releaseBus(); - } + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + uint8_t buf[2] = {addr | 0x80, data}; + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); } /** @@ -1076,14 +1074,12 @@ static void rfm22_write(uint8_t addr, uint8_t data) * toggle the NSS line * @param[in] addr The address of the RFM22b register to write * @param[in] data The data to write to that register -static void rfm22_write_noclaim(uint8_t addr, uint8_t data) +static void rfm22_write_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data) { uint8_t buf[2] = {addr | 0x80, data}; - if(PIOS_RFM22B_validate(g_rfm22b_dev)) { - rfm22_assertCs(); - PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); - rfm22_deassertCs(); - } + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); + rfm22_deassertCs(rfm22b_dev); } */ @@ -1093,17 +1089,15 @@ static void rfm22_write_noclaim(uint8_t addr, uint8_t data) * @param[in] addr The address to read from * @return Returns the result of the register read */ -static uint8_t rfm22_read(uint8_t addr) +static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr) { uint8_t in[2]; uint8_t out[2] = {addr & 0x7f, 0xFF}; - if(PIOS_RFM22B_validate(g_rfm22b_dev)) { - rfm22_claimBus(); - rfm22_assertCs(); - PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, out, in, sizeof(out), NULL); - rfm22_deassertCs(); - rfm22_releaseBus(); - } + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL); + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); return in[1]; } @@ -1112,15 +1106,13 @@ static uint8_t rfm22_read(uint8_t addr) * @param[in] addr The address to read from * @return Returns the result of the register read */ -static uint8_t rfm22_read_noclaim(uint8_t addr) +static uint8_t rfm22_read_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr) { uint8_t out[2] = {addr & 0x7F, 0xFF}; uint8_t in[2]; - if (PIOS_RFM22B_validate(g_rfm22b_dev)) { - rfm22_assertCs(); - PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, out, in, sizeof(out), NULL); - rfm22_deassertCs(); - } + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, out, in, sizeof(out), NULL); + rfm22_deassertCs(rfm22b_dev); return in[1]; } @@ -1195,24 +1187,24 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, rfm22b_dev->frequency_step_size = 156.25f * hbsel; // frequency hopping channel (0-255) - rfm22_write(RFM22_frequency_hopping_channel_select, rfm22b_dev->frequency_hop_channel); + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, rfm22b_dev->frequency_hop_channel); // no frequency offset - rfm22_write(RFM22_frequency_offset1, 0); + rfm22_write(rfm22b_dev, RFM22_frequency_offset1, 0); // no frequency offset - rfm22_write(RFM22_frequency_offset2, 0); + rfm22_write(rfm22b_dev, RFM22_frequency_offset2, 0); // set the carrier frequency - rfm22_write(RFM22_frequency_band_select, fb); - rfm22_write(RFM22_nominal_carrier_frequency1, fc >> 8); - rfm22_write(RFM22_nominal_carrier_frequency0, fc & 0xff); + rfm22_write(rfm22b_dev, RFM22_frequency_band_select, fb); + rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency1, fc >> 8); + rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency0, fc & 0xff); } /* static void rfm22_setFreqHopChannel(uint8_t channel) { // set the frequency hopping channel g_rfm22b_dev->frequency_hop_channel = channel; - rfm22_write(RFM22_frequency_hopping_channel_select, channel); + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); } static uint32_t rfm22_freqHopSize(void) @@ -1273,11 +1265,11 @@ static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev #endif // disable interrupts - rfm22_write(RFM22_interrupt_enable1, 0x00); - rfm22_write(RFM22_interrupt_enable2, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); // Switch to TUNE mode - rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); RX_LED_OFF; TX_LED_OFF; @@ -1289,17 +1281,17 @@ static enum pios_rfm22b_event rfm22_setRxMode(struct pios_rfm22b_dev *rfm22b_dev rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; // clear FIFOs - rfm22_write(RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); - rfm22_write(RFM22_op_and_func_ctrl2, 0x00); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); // enable RX interrupts - rfm22_write(RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid | + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_encrcerror | RFM22_ie1_enpkvalid | RFM22_ie1_enrxffafull | RFM22_ie1_enfferr); - rfm22_write(RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval | + rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, RFM22_ie2_enpreainval | RFM22_ie2_enpreaval | RFM22_ie2_enswdet); // enable the receiver - rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon); // Indicate that we're in RX mode. rfm22b_dev->in_rx_mode = true; @@ -1413,11 +1405,11 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->packet_start_ticks = 1; // disable interrupts - rfm22_write(RFM22_interrupt_enable1, 0x00); - rfm22_write(RFM22_interrupt_enable2, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); // TUNE mode - rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); // Queue the data up for sending rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet); @@ -1427,46 +1419,46 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) // Set the destination address in the transmit header. // The destination address is the first 4 bytes of the message. uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); - rfm22_write(RFM22_transmit_header0, tx_buffer[0]); - rfm22_write(RFM22_transmit_header1, tx_buffer[1]); - rfm22_write(RFM22_transmit_header2, tx_buffer[2]); - rfm22_write(RFM22_transmit_header3, tx_buffer[3]); + rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]); + rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]); + rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]); + rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]); // FIFO mode, GFSK modulation - uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd; - rfm22_write(RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo | + uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; + rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo | RFM22_mmc2_modtyp_gfsk); // set the tx power - rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_1 | - RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | g_rfm22b_dev->tx_power); + rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_1 | + RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); // clear FIFOs - rfm22_write(RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); - rfm22_write(RFM22_op_and_func_ctrl2, 0x00); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); // ******************* // add some data to the chips TX FIFO before enabling the transmitter // set the total number of data bytes we are going to transmit - rfm22_write(RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr); + rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr); // add some data - rfm22_claimBus(); - rfm22_assertCs(); - PIOS_SPI_TransferByte(g_rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); bytes_to_write = (bytes_to_write > FIFO_SIZE) ? FIFO_SIZE: bytes_to_write; - PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); rfm22b_dev->tx_data_rd += bytes_to_write; - rfm22_deassertCs(); - rfm22_releaseBus(); + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); // enable TX interrupts - rfm22_write(RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, RFM22_ie1_enpksent | RFM22_ie1_entxffaem); // enable the transmitter - rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_txon); TX_LED_ON; @@ -1531,23 +1523,23 @@ static bool rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev) { // 1. Read the interrupt statuses with burst read - rfm22_claimBus(); // Set RC and the semaphore + rfm22_claimBus(rfm22b_dev); // Set RC and the semaphore uint8_t write_buf[3] = {RFM22_interrupt_status1 & 0x7f, 0xFF, 0xFF}; uint8_t read_buf[3]; - rfm22_assertCs(); - PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL); - rfm22_deassertCs(); + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, write_buf, read_buf, sizeof(write_buf), NULL); + rfm22_deassertCs(rfm22b_dev); rfm22b_dev->int_status1 = read_buf[1]; rfm22b_dev->int_status2 = read_buf[2]; // Device status - rfm22b_dev->device_status = rfm22_read_noclaim(RFM22_device_status); + rfm22b_dev->device_status = rfm22_read_noclaim(rfm22b_dev, RFM22_device_status); // EzMAC status - rfm22b_dev->ezmac_status = rfm22_read_noclaim(RFM22_ezmac_status); + rfm22b_dev->ezmac_status = rfm22_read_noclaim(rfm22b_dev, RFM22_ezmac_status); // Release the bus - rfm22_releaseBus(); + rfm22_releaseBus(rfm22b_dev); // the RF module has gone and done a reset - we need to re-initialize the rf module if (rfm22b_dev->int_status2 & RFM22_is2_ipor) @@ -1608,16 +1600,16 @@ static enum pios_rfm22b_event rfm22_detectSync(struct pios_rfm22b_dev *rfm22b_de // read the 10-bit signed afc correction value // bits 9 to 2 - uint16_t afc_correction = (uint16_t)rfm22_read(RFM22_afc_correction_read) << 8; + uint16_t afc_correction = (uint16_t)rfm22_read(rfm22b_dev, RFM22_afc_correction_read) << 8; // bits 1 & 0 - afc_correction |= (uint16_t)rfm22_read(RFM22_ook_counter_value1) & 0x00c0; + afc_correction |= (uint16_t)rfm22_read(rfm22b_dev, RFM22_ook_counter_value1) & 0x00c0; afc_correction >>= 6; // convert the afc value to Hz int32_t afc_corr = (int32_t)(rfm22b_dev->frequency_step_size * afc_correction + 0.5f); rfm22b_dev->afc_correction_Hz = (afc_corr < -127) ? -127 : ((afc_corr > 127) ? 127 : afc_corr); // read rx signal strength .. 45 = -100dBm, 205 = -20dBm - uint8_t rssi = rfm22_read(RFM22_rssi); + uint8_t rssi = rfm22_read(rfm22b_dev, RFM22_rssi); // convert to dBm rfm22b_dev->rssi_dBm = (int8_t)(rssi >> 1) - 122; @@ -1698,7 +1690,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) { // read data from the rf chips FIFO buffer // read the total length of the packet data - uint16_t len = rfm22_read(RFM22_received_packet_length); + uint16_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length); // The received packet is going to be larger than the specified length if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) @@ -1709,12 +1701,12 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) return RFM22B_EVENT_FAILURE; // Fetch the data from the RX FIFO - rfm22_claimBus(); - rfm22_assertCs(); + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id ,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], RX_FIFO_HI_WATERMARK, NULL) == 0) ? RX_FIFO_HI_WATERMARK : 0; - rfm22_deassertCs(); - rfm22_releaseBus(); + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); } // CRC error .. discard the received data @@ -1726,19 +1718,19 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) { // read the total length of the packet data - uint32_t len = rfm22_read(RFM22_received_packet_length); + uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length); // their must still be data in the RX FIFO we need to get if (rfm22b_dev->rx_buffer_wr < len) { int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr; // Fetch the data from the RX FIFO - rfm22_claimBus(); - rfm22_assertCs(); + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id,RFM22_fifo_access & 0x7F); rfm22b_dev->rx_buffer_wr += (PIOS_SPI_TransferBlock(rfm22b_dev->spi_id,OUT_FF, (uint8_t *)&rx_buffer[rfm22b_dev->rx_buffer_wr], bytes_to_read, NULL) == 0) ? bytes_to_read : 0; - rfm22_deassertCs(); - rfm22_releaseBus(); + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); } if (rfm22b_dev->rx_buffer_wr != len) @@ -1832,15 +1824,15 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // top-up the rf chips TX FIFO buffer uint8_t *tx_buffer = (uint8_t*)(rfm22b_dev->tx_packet); uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1; - rfm22_claimBus(); - rfm22_assertCs(); - PIOS_SPI_TransferByte(g_rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); + rfm22_claimBus(rfm22b_dev); + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); bytes_to_write = (bytes_to_write > max_bytes) ? max_bytes: bytes_to_write; - PIOS_SPI_TransferBlock(g_rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, &tx_buffer[rfm22b_dev->tx_data_rd], NULL, bytes_to_write, NULL); rfm22b_dev->tx_data_rd += bytes_to_write; - rfm22_deassertCs(); - rfm22_releaseBus(); + rfm22_deassertCs(rfm22b_dev); + rfm22_releaseBus(rfm22b_dev); } // Packet has been sent @@ -2145,7 +2137,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->in_rx_mode = false; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) - rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); // wait 26ms PIOS_DELAY_WaitmS(26); @@ -2156,22 +2148,22 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) PIOS_DELAY_WaitmS(1); // read the status registers - rfm22b_dev->int_status1 = rfm22_read(RFM22_interrupt_status1); - rfm22b_dev->int_status2 = rfm22_read(RFM22_interrupt_status2); + rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); + rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); if (rfm22b_dev->int_status2 & RFM22_is2_ichiprdy) break; } // **************** // read status - clears interrupt - rfm22b_dev->device_status = rfm22_read(RFM22_device_status); - rfm22b_dev->int_status1 = rfm22_read(RFM22_interrupt_status1); - rfm22b_dev->int_status2 = rfm22_read(RFM22_interrupt_status2); - rfm22b_dev->ezmac_status = rfm22_read(RFM22_ezmac_status); + rfm22b_dev->device_status = rfm22_read(rfm22b_dev, RFM22_device_status); + rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); + rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); + rfm22b_dev->ezmac_status = rfm22_read(rfm22b_dev, RFM22_ezmac_status); // disable all interrupts - rfm22_write(RFM22_interrupt_enable1, 0x00); - rfm22_write(RFM22_interrupt_enable2, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); + rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); // **************** @@ -2193,9 +2185,9 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // read the RF chip ID bytes // read the device type - uint8_t device_type = rfm22_read(RFM22_DEVICE_TYPE) & RFM22_DT_MASK; + uint8_t device_type = rfm22_read(rfm22b_dev, RFM22_DEVICE_TYPE) & RFM22_DT_MASK; // read the device version - uint8_t device_version = rfm22_read(RFM22_DEVICE_VERSION) & RFM22_DV_MASK; + uint8_t device_version = rfm22_read(rfm22b_dev, RFM22_DEVICE_VERSION) & RFM22_DV_MASK; #if defined(RFM22_DEBUG) DEBUG_PRINTF(2, "rf device type: %d\n\r", device_type); @@ -2227,30 +2219,30 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // **************** // calibrate our RF module to be exactly on frequency .. different for every module - rfm22_write(RFM22_xtal_osc_load_cap, OSC_LOAD_CAP); + rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, OSC_LOAD_CAP); // **************** // disable Low Duty Cycle Mode - rfm22_write(RFM22_op_and_func_ctrl2, 0x00); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); // 1MHz clock output - rfm22_write(RFM22_cpu_output_clk, RFM22_coc_1MHz); + rfm22_write(rfm22b_dev, RFM22_cpu_output_clk, RFM22_coc_1MHz); // READY mode - rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_xton); + rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_xton); // choose the 3 GPIO pin functions // GPIO port use default value - rfm22_write(RFM22_io_port_config, RFM22_io_port_default); + rfm22_write(rfm22b_dev, RFM22_io_port_config, RFM22_io_port_default); if (rfm22b_dev->cfg.gpio_direction == GPIO0_TX_GPIO1_RX) { - rfm22_write(RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); // GPIO0 = TX State (to control RF Switch) - rfm22_write(RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); // GPIO1 = RX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); // GPIO0 = TX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); // GPIO1 = RX State (to control RF Switch) } else { - rfm22_write(RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); // GPIO0 = TX State (to control RF Switch) - rfm22_write(RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); // GPIO1 = RX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); // GPIO0 = TX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); // GPIO1 = RX State (to control RF Switch) } - rfm22_write(RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); // GPIO2 = Clear Channel Assessment + rfm22_write(rfm22b_dev, RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); // GPIO2 = Clear Channel Assessment // **************** @@ -2261,48 +2253,48 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->frequency_hop_step_size_reg = freq_hop_step_size; // FIFO mode, GFSK modulation - uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd; - rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk); + uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; + rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk); // setup to read the internal temperature sensor // ADC used to sample the temperature sensor uint8_t adc_config = RFM22_ac_adcsel_temp_sensor | RFM22_ac_adcref_bg; - rfm22_write(RFM22_adc_config, adc_config); + rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config); // adc offset - rfm22_write(RFM22_adc_sensor_amp_offset, 0); + rfm22_write(rfm22b_dev, RFM22_adc_sensor_amp_offset, 0); // temp sensor calibration .. �40C to +64C 0.5C resolution - rfm22_write(RFM22_temp_sensor_calib, RFM22_tsc_tsrange0 | RFM22_tsc_entsoffs); + rfm22_write(rfm22b_dev, RFM22_temp_sensor_calib, RFM22_tsc_tsrange0 | RFM22_tsc_entsoffs); // temp sensor offset - rfm22_write(RFM22_temp_value_offset, 0); + rfm22_write(rfm22b_dev, RFM22_temp_value_offset, 0); // start an ADC conversion - rfm22_write(RFM22_adc_config, adc_config | RFM22_ac_adcstartbusy); + rfm22_write(rfm22b_dev, RFM22_adc_config, adc_config | RFM22_ac_adcstartbusy); // set the RSSI threshold interrupt to about -90dBm - rfm22_write(RFM22_rssi_threshold_clear_chan_indicator, (-90 + 122) * 2); + rfm22_write(rfm22b_dev, RFM22_rssi_threshold_clear_chan_indicator, (-90 + 122) * 2); // enable the internal Tx & Rx packet handlers (without CRC) - rfm22_write(RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_enpactx); + rfm22_write(rfm22b_dev, RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_enpactx); // x-nibbles tx preamble - rfm22_write(RFM22_preamble_length, TX_PREAMBLE_NIBBLES); + rfm22_write(rfm22b_dev, RFM22_preamble_length, TX_PREAMBLE_NIBBLES); // x-nibbles rx preamble detection - rfm22_write(RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); + rfm22_write(rfm22b_dev, RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); #ifdef PIOS_RFM22_NO_HEADER // header control - we are not using the header - rfm22_write(RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none); + rfm22_write(rfm22b_dev, RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none); // no header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. - rfm22_write(RFM22_header_control2, RFM22_header_cntl2_hdlen_none | + rfm22_write(rfm22b_dev, RFM22_header_control2, RFM22_header_cntl2_hdlen_none | RFM22_header_cntl2_synclen_3210 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); #else // header control - using a 4 by header with broadcast of 0xffffffff - rfm22_write(RFM22_header_control1, + rfm22_write(rfm22b_dev, RFM22_header_control1, RFM22_header_cntl1_bcen_0 | RFM22_header_cntl1_bcen_1 | RFM22_header_cntl1_bcen_2 | @@ -2312,48 +2304,48 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) RFM22_header_cntl1_hdch_2 | RFM22_header_cntl1_hdch_3); // Check all bit of all bytes of the header - rfm22_write(RFM22_header_enable0, 0xff); - rfm22_write(RFM22_header_enable1, 0xff); - rfm22_write(RFM22_header_enable2, 0xff); - rfm22_write(RFM22_header_enable3, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable2, 0xff); + rfm22_write(rfm22b_dev, RFM22_header_enable3, 0xff); // Set the ID to be checked uint32_t id = rfm22b_dev->deviceID; - rfm22_write(RFM22_check_header0, id & 0xff); - rfm22_write(RFM22_check_header1, (id >> 8) & 0xff); - rfm22_write(RFM22_check_header2, (id >> 16) & 0xff); - rfm22_write(RFM22_check_header3, (id >> 24) & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header0, id & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header1, (id >> 8) & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header2, (id >> 16) & 0xff); + rfm22_write(rfm22b_dev, RFM22_check_header3, (id >> 24) & 0xff); // 4 header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. - rfm22_write(RFM22_header_control2, + rfm22_write(rfm22b_dev, RFM22_header_control2, RFM22_header_cntl2_hdlen_3210 | RFM22_header_cntl2_synclen_3210 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); #endif // sync word - rfm22_write(RFM22_sync_word3, SYNC_BYTE_1); - rfm22_write(RFM22_sync_word2, SYNC_BYTE_2); - rfm22_write(RFM22_sync_word1, SYNC_BYTE_3); - rfm22_write(RFM22_sync_word0, SYNC_BYTE_4); + rfm22_write(rfm22b_dev, RFM22_sync_word3, SYNC_BYTE_1); + rfm22_write(rfm22b_dev, RFM22_sync_word2, SYNC_BYTE_2); + rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3); + rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4); - rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_agcen); + rfm22_write(rfm22b_dev, RFM22_agc_override1, RFM22_agc_ovr1_agcen); // set frequency hopping channel step size (multiples of 10kHz) - rfm22_write(RFM22_frequency_hopping_step_size, rfm22b_dev->frequency_hop_step_size_reg); + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, rfm22b_dev->frequency_hop_step_size_reg); // set the tx power - rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); + rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); // TX FIFO Almost Full Threshold (0 - 63) - rfm22_write(RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); + rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); // TX FIFO Almost Empty Threshold (0 - 63) - rfm22_write(RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK); + rfm22_write(rfm22b_dev, RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK); // RX FIFO Almost Full Threshold (0 - 63) - rfm22_write(RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); + rfm22_write(rfm22b_dev, RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); // Set the frequency calibration - rfm22_write(RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap); + rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, rfm22b_dev->cfg.RFXtalCap); // Initialize the frequency and datarate to te default. rfm22_setNominalCarrierFrequency(rfm22b_dev, RFM22B_DEFAULT_FREQUENCY); From 6e929d7a9204f9dbae828d7439567f9bab561376 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 12 Dec 2012 20:27:08 -0700 Subject: [PATCH 23/31] Fixed configuration of PPM input. --- flight/PiOS/Common/pios_rfm22b.c | 1 - flight/PipXtreme/System/pios_board.c | 25 +++++++++++++------------ 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index ea53ac9c5..c6b101ee4 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -1500,7 +1500,6 @@ static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) if(rfm22b_dev->ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT) valid_input_detected = true; } - valid_input_detected = true; // Send the PPM packet if it's valid if (valid_input_detected) diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 2cf3677ae..3467706be 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -197,6 +197,19 @@ void PIOS_Board_Init(void) { } #endif + /* Configure PPM input */ +#if defined(PIOS_INCLUDE_PPM) + if (oplinkSettings.PPM == OPLINKSETTINGS_PPM_TRUE) + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) + PIOS_Assert(0); + } + else +#endif /* PIOS_INCLUDE_PPM */ + /* Configure the flexi serial port */ { uint32_t pios_usart3_id; @@ -214,18 +227,6 @@ void PIOS_Board_Init(void) { } } - /* Configure PPM input */ -#if defined(PIOS_INCLUDE_PPM) - if (oplinkSettings.PPM == OPLINKSETTINGS_PPM_TRUE) - { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); - - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) - PIOS_Assert(0); - } -#endif /* PIOS_INCLUDE_PPM */ - /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) { From 3a1803b7a1197d424cdec7cefc9d062ec3406167 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Thu, 13 Dec 2012 21:07:19 -0700 Subject: [PATCH 24/31] Added RFM22B (OPLink) receiver. --- flight/Libraries/inc/packet_handler.h | 3 +- flight/Modules/ManualControl/manualcontrol.c | 3 + flight/PiOS/Boards/STM32F4xx_RevoMini.h | 1 + flight/PiOS/Common/pios_rfm22b.c | 8 ++ flight/PiOS/Common/pios_rfm22b_rcvr.c | 99 +++++++++++++++++++ flight/PiOS/inc/pios_rfm22b_priv.h | 6 ++ flight/PiOS/inc/pios_rfm22b_rcvr.h | 40 ++++++++ flight/PiOS/pios.h | 3 + flight/RevoMini/Makefile | 1 + flight/RevoMini/System/inc/pios_config.h | 3 +- flight/RevoMini/System/pios_board.c | 13 ++- .../src/plugins/config/inputchannelform.cpp | 3 +- .../manualcontrolsettings.xml | 2 +- .../uavobjectdefinition/receiveractivity.xml | 2 +- 14 files changed, 180 insertions(+), 7 deletions(-) create mode 100644 flight/PiOS/Common/pios_rfm22b_rcvr.c create mode 100644 flight/PiOS/inc/pios_rfm22b_rcvr.h diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 6bbbb1d88..54f55e63e 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -33,6 +33,7 @@ #include #include #include +#include // Public defines / macros #define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p) @@ -75,7 +76,7 @@ typedef struct { #define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - uint16_t channels[GCSRECEIVER_CHANNEL_NUMELEM]; + uint16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS]; uint8_t ecc[RS_ECC_NPARITY]; } PHPpmPacket, *PHPpmPacketHandle; diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index ffea27457..16beac710 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -498,6 +498,9 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: + group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; + break; default: PIOS_Assert(0); break; diff --git a/flight/PiOS/Boards/STM32F4xx_RevoMini.h b/flight/PiOS/Boards/STM32F4xx_RevoMini.h index 6dd7041dc..3421afd9a 100644 --- a/flight/PiOS/Boards/STM32F4xx_RevoMini.h +++ b/flight/PiOS/Boards/STM32F4xx_RevoMini.h @@ -214,6 +214,7 @@ extern uint32_t pios_packet_handler; #define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 +#define PIOS_RFM22B_RCVR_TIMEOUT_MS 100 //------------------------- // Receiver PPM input diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index c6b101ee4..0ae6ac2a3 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -1767,6 +1767,14 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) case PACKET_TYPE_NACK: ret_event = RFM22B_EVENT_PACKET_NACKED; break; + case PACKET_TYPE_PPM: + { + PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet); + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) + rfm22b_dev->ppm_channel[i] = ppmp->channels[i]; + rfm22b_dev->ppm_fresh = true; + break; + } default: break; } diff --git a/flight/PiOS/Common/pios_rfm22b_rcvr.c b/flight/PiOS/Common/pios_rfm22b_rcvr.c new file mode 100644 index 000000000..0878da0f1 --- /dev/null +++ b/flight/PiOS/Common/pios_rfm22b_rcvr.c @@ -0,0 +1,99 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_RFM22B_RCVR RFM22B Receiver Input Functions + * @brief Code to output the PPM signal from the RFM22B + * @{ + * + * @file pios_rfm22b_rcvr.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Implements a receiver interface to the RFM22B device + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_RFM22B_RCVR) + +#include "pios_rfm22b_priv.h" + +/* Provide a RCVR driver */ +static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel); +static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id); + +const struct pios_rcvr_driver pios_rfm22b_rcvr_driver = { + .read = PIOS_RFM22B_RCVR_Get, +}; + +int32_t PIOS_RFM22B_RCVR_Init(uint32_t rcvr_id) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) + return -1; + + // Initialize + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) + rfm22b_dev->ppm_channel[i] = PIOS_RCVR_TIMEOUT; + rfm22b_dev->ppm_supv_timer = 0; + + // Register the failsafe timer callback. + if (!PIOS_RTC_RegisterTickCallback(PIOS_RFM22B_RCVR_Supervisor, rcvr_id)) + PIOS_DEBUG_Assert(0); + + return 0; +} + +static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) + return -1; + + if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) + /* channel is out of range */ + return -1; + + return rfm22b_dev->ppm_channel[channel]; +} + +static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id) { + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) + return; + + // RTC runs at 625Hz. + if (++(rfm22b_dev->ppm_supv_timer) < (PIOS_RFM22B_RCVR_TIMEOUT_MS * 1000 / 625)) + return; + rfm22b_dev->ppm_supv_timer = 0; + + // Have we received fresh values since the last update? + if (!rfm22b_dev->ppm_fresh) + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) + rfm22b_dev->ppm_channel[i] = 0; + rfm22b_dev->ppm_fresh = false; +} + +#endif /* PIOS_INCLUDE_GCSRCVR */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index fdd57a83e..27883311c 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -36,6 +36,7 @@ #include #include #include "pios_rfm22b.h" +#include "pios_rfm22b_rcvr.h" // ************************************ @@ -780,6 +781,11 @@ struct pios_rfm22b_dev { portTickType tx_complete_ticks; portTickType rx_complete_ticks; uint8_t max_ack_delay; + + // The PPM channel values + uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS]; + uint8_t ppm_supv_timer; + bool ppm_fresh; }; diff --git a/flight/PiOS/inc/pios_rfm22b_rcvr.h b/flight/PiOS/inc/pios_rfm22b_rcvr.h new file mode 100644 index 000000000..74d176c4e --- /dev/null +++ b/flight/PiOS/inc/pios_rfm22b_rcvr.h @@ -0,0 +1,40 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS RFM22B Receiver Input Functions + * @{ + * + * @file pios_rfm22b_rcvr.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief RFM22B Receiver Input functions header. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_RFM22B_RCVR_H + +#define PIOS_RFM22B_RCVR_MAX_CHANNELS 12 + +extern const struct pios_rcvr_driver pios_rfm22b_rcvr_driver; + +extern int32_t PIOS_RFM22B_RCVR_Init(uint32_t rcvr_id); + +#define PIOS_RFM22B_RCVR_H + +#endif /* PIOS_RFM22B_RCVR_H */ diff --git a/flight/PiOS/pios.h b/flight/PiOS/pios.h index de641011b..5d2fa8d6e 100644 --- a/flight/PiOS/pios.h +++ b/flight/PiOS/pios.h @@ -174,6 +174,9 @@ #ifdef PIOS_INCLUDE_RFM22B_COM #include #endif +#ifdef PIOS_INCLUDE_RFM22B_RCVR +#include +#endif #endif #include diff --git a/flight/RevoMini/Makefile b/flight/RevoMini/Makefile index 42d3f5ec1..5f74a9f4e 100644 --- a/flight/RevoMini/Makefile +++ b/flight/RevoMini/Makefile @@ -170,6 +170,7 @@ SRC += $(PIOSCOMMON)/pios_crc.c SRC += $(PIOSCOMMON)/pios_com.c SRC += $(PIOSCOMMON)/pios_rfm22b.c SRC += $(PIOSCOMMON)/pios_rfm22b_com.c +SRC += $(PIOSCOMMON)/pios_rfm22b_rcvr.c SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/pios_sbus.c SRC += $(PIOSCOMMON)/pios_flash_jedec.c diff --git a/flight/RevoMini/System/inc/pios_config.h b/flight/RevoMini/System/inc/pios_config.h index 1ae25020c..2507b0cc6 100644 --- a/flight/RevoMini/System/inc/pios_config.h +++ b/flight/RevoMini/System/inc/pios_config.h @@ -58,7 +58,8 @@ /* Variables related to the RFM22B functionality */ #define PIOS_INCLUDE_RFM22B #define PIOS_INCLUDE_RFM22B_COM - +#define PIOS_INCLUDE_RFM22B_RCVR + /* Select the sensors to include */ #define PIOS_INCLUDE_HMC5883 #define PIOS_HMC5883_HAS_GPIOS diff --git a/flight/RevoMini/System/pios_board.c b/flight/RevoMini/System/pios_board.c index 8feac9fa3..3cc1fb105 100644 --- a/flight/RevoMini/System/pios_board.c +++ b/flight/RevoMini/System/pios_board.c @@ -616,15 +616,24 @@ void PIOS_Board_Init(void) { if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, pios_rfm22b_cfg->slave_num, pios_rfm22b_cfg)) { PIOS_Assert(0); } +#ifdef PIOS_INCLUDE_RFM22B_COM uint8_t *rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t *tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id, rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { + tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) PIOS_Assert(0); - } +#endif +#ifdef PIOS_INCLUDE_RFM22B_RCVR + if (PIOS_RFM22B_RCVR_Init(pios_rfm22b_id) != 0) + PIOS_Assert(0); + uint32_t pios_rfm22b_rcvr_id; + if (PIOS_RCVR_Init(&pios_rfm22b_rcvr_id, &pios_rfm22b_rcvr_driver, pios_rfm22b_id)) + PIOS_Assert(0); + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_rfm22b_rcvr_id; +#endif break; } } diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp index bd2d5830e..e5fcbcaa5 100755 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp @@ -109,6 +109,7 @@ void inputChannelForm::groupUpdated() count = 8; // Need to make this 6 for CC break; case ManualControlSettings::CHANNELGROUPS_PPM: + case ManualControlSettings::CHANNELGROUPS_OPLINK: case ManualControlSettings::CHANNELGROUPS_DSMMAINPORT: case ManualControlSettings::CHANNELGROUPS_DSMFLEXIPORT: count = 12; @@ -117,7 +118,7 @@ void inputChannelForm::groupUpdated() count = 18; break; case ManualControlSettings::CHANNELGROUPS_GCS: - count = GCSReceiver::CHANNEL_NUMELEM; + count = 8; break; case ManualControlSettings::CHANNELGROUPS_NONE: count = 0; diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 7dc2711a6..46aa8353a 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -3,7 +3,7 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. + options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,GCS,OPLink,None" defaultvalue="None"/> Monitors which receiver channels have been active within the last second. From 43f6b4150fbd37e7f69e35d950794e32baac3b4d Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 15 Dec 2012 09:56:27 -0700 Subject: [PATCH 25/31] RFM22: Don't send status until the link is connected. --- flight/PiOS/Common/pios_rfm22b.c | 34 ++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 0ae6ac2a3..a99098949 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -192,8 +192,8 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rf static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev); -static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev); -static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status); static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len); static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz); @@ -914,12 +914,18 @@ static void PIOS_RFM22B_Task(void *parameters) { // Queue up a PPM packet if it's time. - if ((timeDifferenceMs(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) && rfm22_sendPPM(rfm22b_dev)) + if (timeDifferenceMs(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) + { + rfm22_sendPPM(rfm22b_dev); lastPPMTicks = curTicks; + } // Queue up a status packet if it's time. - if ((timeDifferenceMs(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) && rfm22_sendStatus(rfm22b_dev)) + if (timeDifferenceMs(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) + { + rfm22_sendStatus(rfm22b_dev); lastStatusTicks = curTicks; + } } } @@ -1465,8 +1471,11 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) return RFM22B_EVENT_NUM_EVENTS; } -static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) +static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) { + // Only send status if we're connected + if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) + return; // Update the link quality metric. rfm22_calculateLinkQuality(rfm22b_dev); @@ -1482,15 +1491,19 @@ static bool rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; rfm22b_dev->send_status = true; - return true; + return; } -static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) +static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) { #ifdef PIOS_PPM_RECEIVER + // Only send PPM if we're connected + if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) + return; + // Just return if the PPM receiver is not configured. if (PIOS_PPM_RECEIVER == 0) - return true; + return; // See if we have any valid channels. bool valid_input_detected = false; @@ -1510,8 +1523,6 @@ static bool rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->send_ppm = true; } #endif - - return true; } /** @@ -2026,9 +2037,6 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b rfm22b_dev->pair_stats[min_idx].lastContact = 0; } - rfm22b_dev->stats.link_quality = status->link_quality; - rfm22b_dev->stats.rssi = status->received_rssi; - return RFM22B_EVENT_RX_COMPLETE; } From e0acb413795ddb60d56388b1acf0506e47898ccb Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 15 Dec 2012 10:35:51 -0700 Subject: [PATCH 26/31] Removed some unintended changes to RevoMini. --- flight/PiOS/Boards/STM32F4xx_RevoMini.h | 2 +- flight/RevoMini/System/inc/pios_config.h | 1 + ground/openpilotgcs/src/plugins/config/inputchannelform.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/PiOS/Boards/STM32F4xx_RevoMini.h b/flight/PiOS/Boards/STM32F4xx_RevoMini.h index 3421afd9a..680b26276 100644 --- a/flight/PiOS/Boards/STM32F4xx_RevoMini.h +++ b/flight/PiOS/Boards/STM32F4xx_RevoMini.h @@ -32,7 +32,7 @@ #include #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) -#define DEBUG_LEVEL 2 +#define DEBUG_LEVEL 0 #define DEBUG_PRINTF(level, ...) {if(level <= DEBUG_LEVEL && pios_com_debug_id > 0) { PIOS_COM_SendFormattedStringNonBlocking(pios_com_debug_id, __VA_ARGS__); }} #else #define DEBUG_PRINTF(level, ...) diff --git a/flight/RevoMini/System/inc/pios_config.h b/flight/RevoMini/System/inc/pios_config.h index 2507b0cc6..8f8fbe9ba 100644 --- a/flight/RevoMini/System/inc/pios_config.h +++ b/flight/RevoMini/System/inc/pios_config.h @@ -54,6 +54,7 @@ //#define PIOS_INCLUDE_GPIO #define PIOS_INCLUDE_EXTI #define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_WDG /* Variables related to the RFM22B functionality */ #define PIOS_INCLUDE_RFM22B diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp index e5fcbcaa5..b8393b19c 100755 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp @@ -118,7 +118,7 @@ void inputChannelForm::groupUpdated() count = 18; break; case ManualControlSettings::CHANNELGROUPS_GCS: - count = 8; + count = GCSReceiver::CHANNEL_NUMELEM; break; case ManualControlSettings::CHANNELGROUPS_NONE: count = 0; From 0dd8caeec0b37fb935fbe1b143f8a8458ce26066 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 15 Dec 2012 15:39:11 -0700 Subject: [PATCH 27/31] Removed oplink UAVObjects from the RevoMini target. --- flight/RevoMini/UAVObjects.inc | 5 ----- 1 file changed, 5 deletions(-) diff --git a/flight/RevoMini/UAVObjects.inc b/flight/RevoMini/UAVObjects.inc index 4eeab21ac..5423a6b2a 100644 --- a/flight/RevoMini/UAVObjects.inc +++ b/flight/RevoMini/UAVObjects.inc @@ -97,10 +97,5 @@ UAVOBJSRCFILENAMES += waypointactive UAVOBJSRCFILENAMES += txpidsettings -#Support for radio module on RM -UAVOBJSRCFILENAMES += oplinkstatus -UAVOBJSRCFILENAMES += oplinksettings - - UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) From c4c58331e51c5d1aa52d94167f643ea7ab2a7976 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 16 Dec 2012 09:16:19 -0700 Subject: [PATCH 28/31] RM: Now doesn't crash when the radio is disabled. --- flight/PiOS/Common/pios_rfm22b.c | 17 +++++++++-------- flight/RevoMini/System/pios_board.c | 9 --------- 2 files changed, 9 insertions(+), 17 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index a99098949..92bb260b6 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -567,8 +567,10 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->send_ppm = false; rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; - // Initialize the com configuration callback. + // Initialize the com callbacks. rfm22b_dev->com_config_cb = NULL; + rfm22b_dev->rx_in_cb = NULL; + rfm22b_dev->tx_out_cb = NULL; // Initialize the stats. rfm22b_dev->stats.packets_per_sec = 0; @@ -1319,9 +1321,8 @@ static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev) if (dp->header.data_size > 0) return true; bool need_yield = false; - dp->header.data_size = - (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, dp->data, - PH_MAX_DATA, NULL, &need_yield); + if (rfm22b_dev->tx_out_cb) + dp->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, dp->data, PH_MAX_DATA, NULL, &need_yield); if (dp->header.data_size > 0) return true; @@ -1375,9 +1376,8 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) p = &rfm22b_dev->data_packet; p->header.type = PACKET_TYPE_DATA; p->header.destination_id = rfm22b_dev->destination_id; - if (p->header.data_size == 0) - p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, - PH_MAX_DATA, NULL, &need_yield); + if (rfm22b_dev->tx_out_cb && (p->header.data_size == 0)) + p->header.data_size = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield); // Don't send any data until we're connected. if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) @@ -1766,7 +1766,8 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) { // Send the data to the com port bool rx_need_yield; - (rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield); + if (rfm22b_dev->rx_in_cb) + (rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, rfm22b_dev->rx_packet.data, rfm22b_dev->rx_packet.header.data_size, NULL, &rx_need_yield); break; } case PACKET_TYPE_DUPLICATE_DATA: diff --git a/flight/RevoMini/System/pios_board.c b/flight/RevoMini/System/pios_board.c index 3cc1fb105..101a37e64 100644 --- a/flight/RevoMini/System/pios_board.c +++ b/flight/RevoMini/System/pios_board.c @@ -637,15 +637,6 @@ void PIOS_Board_Init(void) { break; } } - - // Initialize the packet handler - PacketHandlerConfig pios_ph_cfg = { - .default_destination_id = 0xffffffff, // Broadcast - .source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id), - .win_size = PIOS_PH_WIN_SIZE, - .max_connections = PIOS_PH_MAX_CONNECTIONS, - }; - pios_packet_handler = PHInitialize(&pios_ph_cfg); #endif /* PIOS_INCLUDE_RFM22B */ /* Configure the receiver port*/ From 97652447534044555196bd8987527309a1b04362 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 16 Dec 2012 09:31:21 -0700 Subject: [PATCH 29/31] Removed (now unused) packet_handler.c --- flight/Libraries/inc/packet_handler.h | 28 --- flight/Libraries/packet_handler.c | 335 -------------------------- flight/PipXtreme/Makefile | 1 - flight/PipXtreme/System/pios_board.c | 10 - flight/RevoMini/Makefile | 1 - flight/RevoMini/System/pios_board.c | 1 - 6 files changed, 376 deletions(-) delete mode 100644 flight/Libraries/packet_handler.c diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 54f55e63e..8bdfb4eb0 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -101,34 +101,6 @@ typedef struct { uint8_t ecc[RS_ECC_NPARITY]; } PHConnectionPacket, *PHConnectionPacketHandle; -typedef struct { - uint32_t default_destination_id; - uint32_t source_id; - uint16_t max_connections; - uint8_t win_size; -} PacketHandlerConfig; - -typedef int32_t (*PHOutputStream)(PHPacketHandle packet); -typedef void (*PHDataHandler)(uint8_t *data, uint8_t len, int8_t rssi, int8_t afc); -typedef void (*PHStatusHandler)(PHStatusPacketHandle s, int8_t rssi, int8_t afc); -typedef void (*PHPPMHandler)(uint16_t *channels); - -typedef uint32_t PHInstHandle; - -// Public functions -PHInstHandle PHInitialize(PacketHandlerConfig *cfg); -void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f); -void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f); -void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f); -void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f); -uint32_t PHConnect(PHInstHandle h, uint32_t dest_id); -PHPacketHandle PHGetRXPacket(PHInstHandle h); -void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p); -PHPacketHandle PHGetTXPacket(PHInstHandle h); -void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p); -uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p); -uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len); - #endif // __PACKET_HANDLER_H__ /** diff --git a/flight/Libraries/packet_handler.c b/flight/Libraries/packet_handler.c deleted file mode 100644 index b417100de..000000000 --- a/flight/Libraries/packet_handler.c +++ /dev/null @@ -1,335 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotLibraries OpenPilot System Libraries - * @{ - * - * @file packet_handler.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief A packet handler for handeling radio packet transmission. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "openpilot.h" -#include "packet_handler.h" -#include "aes.h" -#include "ecc.h" - -extern char *debug_msg; - -// Private types and constants -typedef struct { - PacketHandlerConfig cfg; - PHPacket *tx_packets; - uint8_t tx_win_start; - uint8_t tx_win_end; - PHPacket *rx_packets; - uint8_t rx_win_start; - uint8_t rx_win_end; - xSemaphoreHandle lock; - PHOutputStream output_stream; - PHDataHandler data_handler; - PHStatusHandler status_handler; - PHPPMHandler ppm_handler; -} PHPacketData, *PHPacketDataHandle; - -// Private functions -static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p); - -/** - * Initialize the Packet Handler library - * \param[in] txWinSize The transmission window size (number of tx packet buffers). - * \param[in] streme A callback function for transmitting the packet. - * \param[in] id The source ID of transmitter. - * \return PHInstHandle The Pachet Handler instance data. - * \return 0 Failure - */ -PHInstHandle PHInitialize(PacketHandlerConfig *cfg) -{ - // Allocate the primary structure - PHPacketDataHandle data = pvPortMalloc(sizeof(PHPacketData)); - if (!data) - return 0; - data->cfg = *cfg; - - // Allocate the packet windows - data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.win_size); - data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.win_size); - - // Initialize the windows - data->tx_win_start = data->tx_win_end = 0; - data->rx_win_start = data->rx_win_end = 0; - for (uint8_t i = 0; i < data->cfg.win_size; ++i) - { - data->tx_packets[i].header.type = PACKET_TYPE_NONE; - data->rx_packets[i].header.type = PACKET_TYPE_NONE; - } - - // Create the lock - data->lock = xSemaphoreCreateRecursiveMutex(); - - // Initialize the ECC library. - initialize_ecc(); - - // Initialize the handlers - data->output_stream = 0; - data->data_handler = 0; - data->status_handler = 0; - data->ppm_handler = 0; - - // Return the structure. - return (PHInstHandle)data; -} - -/** - * Register an output stream handler - * \param[in] h The packet handler instance data pointer. - * \param[in] f The output stream handler function - */ -void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - data->output_stream = f; -} - -/** - * Register a data handler - * \param[in] h The packet handler instance data pointer. - * \param[in] f The data handler function - */ -void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - data->data_handler = f; -} - -/** - * Register a PPM packet handler - * \param[in] h The packet handler instance data pointer. - * \param[in] f The PPM handler function - */ -void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - data->status_handler = f; -} - -/** - * Register a PPM packet handler - * \param[in] h The packet handler instance data pointer. - * \param[in] f The PPM handler function - */ -void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - data->ppm_handler = f; -} - -/** - * Get a packet out of the transmit buffer. - * \param[in] h The packet handler instance data pointer. - * \param[in] dest_id The destination ID of this connection - * \return PHPacketHandle A pointer to the packet buffer. - * \return 0 No packets buffers avaiable in the transmit window. - */ -uint32_t PHConnect(PHInstHandle h, uint32_t dest_id) -{ - return 1; -} - -/** - * Get a packet out of the transmit buffer. - * \param[in] h The packet handler instance data pointer. - * \return PHPacketHandle A pointer to the packet buffer. - * \return 0 No packets buffers avaiable in the transmit window. - */ -PHPacketHandle PHGetTXPacket(PHInstHandle h) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - // Lock - xSemaphoreTakeRecursive(data->lock, portMAX_DELAY); - - // Find a free packet. - PHPacketHandle p = NULL; - for (uint8_t i = 0; i < data->cfg.win_size; ++i) - if (data->tx_packets[i].header.type == PACKET_TYPE_NONE) - { - p = data->tx_packets + i; - break; - } - - // Release lock - xSemaphoreGiveRecursive(data->lock); - - // Return a pointer to the packet at the end of the TX window. - return p; -} - -/** - * Release a packet from the transmit packet buffer window. - * \param[in] h The packet handler instance data pointer. - * \param[in] p A pointer to the packet buffer. - * \return Nothing - */ -void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - // Lock - xSemaphoreTakeRecursive(data->lock, portMAX_DELAY); - - // Change the packet type so we know this packet is unused. - p->header.type = PACKET_TYPE_NONE; - - // If this packet is at the start of the window, increment the start index. - while ((data->tx_win_start != data->tx_win_end) && - (data->tx_packets[data->tx_win_start].header.type == PACKET_TYPE_NONE)) - data->tx_win_start = (data->tx_win_start + 1) % data->cfg.win_size; - - // Release lock - xSemaphoreGiveRecursive(data->lock); -} - -/** - * Get a packet out of the receive buffer. - * \param[in] h The packet handler instance data pointer. - * \return PHPacketHandle A pointer to the packet buffer. - * \return 0 No packets buffers avaiable in the transmit window. - */ -PHPacketHandle PHGetRXPacket(PHInstHandle h) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - // Lock - xSemaphoreTakeRecursive(data->lock, portMAX_DELAY); - - // Find a free packet. - PHPacketHandle p = NULL; - for (uint8_t i = 0; i < data->cfg.win_size; ++i) - if (data->rx_packets[i].header.type == PACKET_TYPE_NONE) - { - p = data->rx_packets + i; - break; - } - - // Release lock - xSemaphoreGiveRecursive(data->lock); - - // Return a pointer to the packet at the end of the TX window. - return p; -} - -/** - * Release a packet from the receive packet buffer window. - * \param[in] h The packet handler instance data pointer. - * \param[in] p A pointer to the packet buffer. - * \return Nothing - */ -void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - // Lock - xSemaphoreTakeRecursive(data->lock, portMAX_DELAY); - - // Change the packet type so we know this packet is unused. - p->header.type = PACKET_TYPE_NONE; - - // If this packet is at the start of the window, increment the start index. - while ((data->rx_win_start != data->rx_win_end) && - (data->rx_packets[data->rx_win_start].header.type == PACKET_TYPE_NONE)) - data->rx_win_start = (data->rx_win_start + 1) % data->cfg.win_size; - - // Release lock - xSemaphoreGiveRecursive(data->lock); -} - -/** - * Transmit a packet from the transmit packet buffer window. - * \param[in] h The packet handler instance data pointer. - * \param[in] p A pointer to the packet buffer. - * \return 1 Success - * \return 0 Failure - */ -uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - // Try to transmit the packet. - if (!PHLTransmitPacket(data, p)) - return 0; - - return 1; -} - -/** - * Transmit a packet of data. - * \param[in] h The packet handler instance data pointer. - * \param[in] p A pointer to the data buffer. - * \param[in] len The length of the data buffer. - * \return 1 Success - * \return 0 Failure - */ -uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len) -{ - PHPacketDataHandle data = (PHPacketDataHandle)h; - - // Get a packet from the packet handler. - PHPacketHandle p = PHGetTXPacket(pios_packet_handler); - if (!p) - return 0; - - // Initialize the packet. - p->header.destination_id = data->cfg.default_destination_id; - p->header.source_id = data->cfg.source_id; - p->header.type = PACKET_TYPE_DATA; - p->header.data_size = len; - - // Copy the data into the packet. - memcpy(p->data, buf, len); - - // Send the packet. - return PHLTransmitPacket(data, p); -} - -/** - * Transmit a packet from the transmit packet buffer window. - * \param[in] data The packet handler instance data pointer. - * \param[in] p A pointer to the packet buffer. - * \return 1 Success - * \return 0 Failure - */ -static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p) -{ - - if(!data->output_stream) - return 0; - - // Transmit the packet using the output stream. - if(data->output_stream(p) == -1) - return 0; - - return 1; -} diff --git a/flight/PipXtreme/Makefile b/flight/PipXtreme/Makefile index 31d88ecab..13f885a8e 100644 --- a/flight/PipXtreme/Makefile +++ b/flight/PipXtreme/Makefile @@ -195,7 +195,6 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(FLIGHTLIB)/taskmonitor.c SRC += $(FLIGHTLIB)/aes.c -SRC += $(FLIGHTLIB)/packet_handler.c ## The Reed-Solomon FEC library SRC += $(FLIGHTLIB)/rscode/rs.c SRC += $(FLIGHTLIB)/rscode/berlekamp.c diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 3467706be..acfca665f 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -57,7 +57,6 @@ uint32_t pios_ppm_rcvr_id = 0; uint32_t pios_rfm22b_id = 0; uint32_t pios_com_rfm22b_id = 0; uint32_t pios_com_radio_id = 0; -uint32_t pios_packet_handler = 0; #endif /** @@ -247,15 +246,6 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); } } - - // Initialize the packet handler - PacketHandlerConfig pios_ph_cfg = { - .default_destination_id = 0xffffffff, // Broadcast - .source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id), - .win_size = PIOS_PH_WIN_SIZE, - .max_connections = PIOS_PH_MAX_CONNECTIONS, - }; - pios_packet_handler = PHInitialize(&pios_ph_cfg); #endif /* PIOS_INCLUDE_RFM22B */ /* Remap AFIO pin */ diff --git a/flight/RevoMini/Makefile b/flight/RevoMini/Makefile index 5f74a9f4e..6643dc907 100644 --- a/flight/RevoMini/Makefile +++ b/flight/RevoMini/Makefile @@ -149,7 +149,6 @@ SRC += $(MATHLIB)/sin_lookup.c SRC += $(MATHLIB)/pid.c ## For RFM22b -SRC += $(FLIGHTLIB)/packet_handler.c SRC += $(RSCODE)/berlekamp.c SRC += $(RSCODE)/crcgen.c SRC += $(RSCODE)/galois.c diff --git a/flight/RevoMini/System/pios_board.c b/flight/RevoMini/System/pios_board.c index 101a37e64..6f772acdf 100644 --- a/flight/RevoMini/System/pios_board.c +++ b/flight/RevoMini/System/pios_board.c @@ -234,7 +234,6 @@ uint32_t pios_com_bridge_id = 0; uint32_t pios_com_overo_id = 0; #if defined(PIOS_INCLUDE_RFM22B) uint32_t pios_rfm22b_id = 0; -uint32_t pios_packet_handler = 0; #endif /* From 4c2de5a140f7e77618b3438957d637acbd661f6e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 15 Dec 2012 23:53:00 -0600 Subject: [PATCH 30/31] PiOS simulation: Implement pios_com->available for simulators --- flight/PiOS.osx/inc/pios_com.h | 3 ++- flight/PiOS.osx/osx/pios_com.c | 21 ++++++++++++--------- flight/PiOS.posix/inc/pios_com.h | 3 ++- flight/PiOS.posix/posix/pios_com.c | 21 ++++++++++++--------- 4 files changed, 28 insertions(+), 20 deletions(-) diff --git a/flight/PiOS.osx/inc/pios_com.h b/flight/PiOS.osx/inc/pios_com.h index cbee33735..badba0128 100644 --- a/flight/PiOS.osx/inc/pios_com.h +++ b/flight/PiOS.osx/inc/pios_com.h @@ -41,6 +41,7 @@ struct pios_com_driver { void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); + bool (*available)(uint32_t id); }; /* Public Functions */ @@ -55,7 +56,7 @@ extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...); extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms); -extern int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id); +extern bool PIOS_COM_Available(uint32_t com_id); #endif /* PIOS_COM_H */ diff --git a/flight/PiOS.osx/osx/pios_com.c b/flight/PiOS.osx/osx/pios_com.c index 77c2e2595..4f2e1e3c9 100644 --- a/flight/PiOS.osx/osx/pios_com.c +++ b/flight/PiOS.osx/osx/pios_com.c @@ -497,21 +497,24 @@ uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len } /** -* Get the number of bytes waiting in the buffer -* \param[in] port COM port -* \return Number of bytes used in buffer -*/ -int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id) + * Query if a com port is available for use. That can be + * used to check a link is established even if the device + * is valid. + */ +bool PIOS_COM_Available(uint32_t com_id) { struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - PIOS_Assert(0); + return false; } - PIOS_Assert(com_dev->has_rx); - return (fifoBuf_getUsed(&com_dev->rx)); + // If a driver does not provide a query method assume always + // available if valid + if (com_dev->driver->available == NULL) + return true; + + return (com_dev->driver->available)(com_dev->lower_id); } #endif diff --git a/flight/PiOS.posix/inc/pios_com.h b/flight/PiOS.posix/inc/pios_com.h index cbee33735..358b19f83 100644 --- a/flight/PiOS.posix/inc/pios_com.h +++ b/flight/PiOS.posix/inc/pios_com.h @@ -41,6 +41,7 @@ struct pios_com_driver { void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); + bool (*available)(uint32_t id); }; /* Public Functions */ @@ -55,7 +56,7 @@ extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str); extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...); extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms); -extern int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id); +extern bool PIOS_COM_Available(uint32_t com_id); #endif /* PIOS_COM_H */ diff --git a/flight/PiOS.posix/posix/pios_com.c b/flight/PiOS.posix/posix/pios_com.c index 77c2e2595..4f2e1e3c9 100644 --- a/flight/PiOS.posix/posix/pios_com.c +++ b/flight/PiOS.posix/posix/pios_com.c @@ -497,21 +497,24 @@ uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len } /** -* Get the number of bytes waiting in the buffer -* \param[in] port COM port -* \return Number of bytes used in buffer -*/ -int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id) + * Query if a com port is available for use. That can be + * used to check a link is established even if the device + * is valid. + */ +bool PIOS_COM_Available(uint32_t com_id) { struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); if (!PIOS_COM_validate(com_dev)) { - /* Undefined COM port for this board (see pios_board.c) */ - PIOS_Assert(0); + return false; } - PIOS_Assert(com_dev->has_rx); - return (fifoBuf_getUsed(&com_dev->rx)); + // If a driver does not provide a query method assume always + // available if valid + if (com_dev->driver->available == NULL) + return true; + + return (com_dev->driver->available)(com_dev->lower_id); } #endif From 9225debdc1a0762766167c76058bb367e0e1e096 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 17 Dec 2012 19:33:42 -0700 Subject: [PATCH 31/31] Added initialization of the ECC, which was removed with the removal of the packet handler. Removed a couple of test functions from RadioComBridge module. Turned on watchdog timers in RadioComBridge (and RFM22B driver). --- .../Modules/RadioComBridge/RadioComBridge.c | 61 ++++++------------- .../PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 10 ++- flight/PiOS/Common/pios_rfm22b.c | 3 + flight/PipXtreme/System/inc/pios_config.h | 2 +- 4 files changed, 26 insertions(+), 50 deletions(-) diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index dee6d7afa..adda6cd60 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -90,8 +90,6 @@ typedef struct { static void telemetryTxTask(void *parameters); static void radioRxTask(void *parameters); static void radioTxTask(void *parameters); -static void testRxTask(void *parameters); -static void testTxTask(void *parameters); static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length); static int32_t RadioSendHandler(uint8_t *buf, int32_t length); static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); @@ -123,11 +121,13 @@ static int32_t RadioComBridgeStart(void) xTaskCreate(telemetryTxTask, (signed char *)"telemTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); xTaskCreate(radioRxTask, (signed char *)"radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle)); xTaskCreate(radioTxTask, (signed char *)"radioTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle)); - if (0) - { - xTaskCreate(testRxTask, (signed char *)"testRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle)); - xTaskCreate(testTxTask, (signed char *)"testTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle)); - } + + // Register the watchdog timers. +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRY); + PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORX); + PIOS_WDG_RegisterFlag(PIOS_WDG_RADIOTX); +#endif return 0; } @@ -163,7 +163,6 @@ static int32_t RadioComBridgeInitialize(void) // Configure our UAVObjects for updates. UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); - //UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); // Initialize the statistics. data->comTxErrors = 0; @@ -183,8 +182,11 @@ static void telemetryTxTask(void *parameters) // Loop forever while (1) { +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_TELEMETRY); +#endif // Wait for queue message - if (xQueueReceive(data->uavtalkEventQueue, &ev, portMAX_DELAY) == pdTRUE) { + if (xQueueReceive(data->uavtalkEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { // Send update (with retries) @@ -232,6 +234,9 @@ static void radioRxTask(void *parameters) { // Task loop while (1) { +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX); +#endif uint8_t serial_data[1]; uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) @@ -249,6 +254,9 @@ static void radioTxTask(void *parameters) // Task loop while (1) { uint32_t inputPort = PIOS_COM_TELEMETRY; +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX); +#endif #if defined(PIOS_INCLUDE_USB) // Determine output port (USB takes priority over telemetry port) if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) @@ -265,39 +273,6 @@ static void radioTxTask(void *parameters) } } -static void testTxTask(void *parameters) -{ - uint8_t buf[100]; - for (uint8_t i = 0; i < 100; ++i) - buf[i] = i; - - // Task loop - uint16_t packets_sent = 0; - while (1) { - PIOS_COM_SendBuffer(PIOS_COM_RADIO, buf, 100); - packets_sent++; - OPLinkStatusTXRateSet(&packets_sent); - vTaskDelay(1000); - } -} - -/** - * Radio rx task. Receive data from a com port and pass it on to the radio. - */ -static void testRxTask(void *parameters) -{ - // Task loop - uint16_t packets_recv = 0; - while (1) { - uint8_t buf[100]; - int16_t bytes_received = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, buf, 100, MAX_PORT_DELAY); - if (bytes_received > 0) { - packets_recv++; - OPLinkStatusRXRateSet(&packets_recv); - } - } -} - /** * Transmit data buffer to the com port. * \param[in] buf Data buffer to send @@ -314,7 +289,7 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) outputPort = PIOS_COM_TELEM_USB; #endif /* PIOS_INCLUDE_USB */ if(outputPort) - return PIOS_COM_SendBuffer(outputPort, buf, length); + return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); else return -1; } diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index 2c9b0062c..72127786e 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -71,12 +71,10 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 //------------------------ #define PIOS_WATCHDOG_TIMEOUT 500 #define PIOS_WDG_REGISTER BKP_DR4 -#define PIOS_WDG_COMGCS 0x0001 -#define PIOS_WDG_COMUAVTALK 0x0002 -#define PIOS_WDG_RADIORECEIVE 0x0004 -#define PIOS_WDG_SENDDATA 0x0008 -#define PIOS_WDG_TRANSCOMM 0x0008 -#define PIOS_WDG_PPMINPUT 0x0010 +#define PIOS_WDG_TELEMETRY 0x0001 +#define PIOS_WDG_RADIORX 0x0002 +#define PIOS_WDG_RADIOTX 0x0004 +#define PIOS_WDG_RFM22B 0x0008 //------------------------ // TELEMETRY diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 92bb260b6..cfc66d950 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -614,6 +614,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu PIOS_WDG_RegisterFlag(PIOS_WDG_RFM22B); #endif /* PIOS_WDG_RFM22B */ + // Initialize the ECC library. + initialize_ecc(); + // Set the state to initializing. rfm22b_dev->state = RFM22B_STATE_UNINITIALIZED; diff --git a/flight/PipXtreme/System/inc/pios_config.h b/flight/PipXtreme/System/inc/pios_config.h index 62a76aa00..21f27825d 100755 --- a/flight/PipXtreme/System/inc/pios_config.h +++ b/flight/PipXtreme/System/inc/pios_config.h @@ -59,7 +59,7 @@ #define PIOS_INCLUDE_GPIO #define PIOS_INCLUDE_EXTI #define PIOS_INCLUDE_RTC -//#define PIOS_INCLUDE_WDG +#define PIOS_INCLUDE_WDG #define PIOS_INCLUDE_BL_HELPER #define PIOS_INCLUDE_FLASH_EEPROM #define PIOS_INCLUDE_RFM22B