From 57b4d8c720730676fdf84346e0bf93d92e65760a Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Thu, 31 Jan 2013 13:37:03 +0000 Subject: [PATCH 01/40] RFM22: Added support for binding to multiple remove OPLinks. --- flight/Modules/PipXtreme/pipxtrememod.c | 2 - .../Modules/RadioComBridge/RadioComBridge.c | 17 +- flight/PiOS/Common/pios_rfm22b.c | 123 +- flight/PiOS/inc/pios_rfm22b.h | 4 +- flight/PiOS/inc/pios_rfm22b_priv.h | 4 + flight/PipXtreme/System/pios_board.c | 84 +- .../plugins/config/configpipxtremewidget.cpp | 150 +- .../plugins/config/configpipxtremewidget.h | 15 +- .../src/plugins/config/pipxtreme.ui | 2237 ++++++++--------- shared/uavobjectdefinition/oplinksettings.xml | 10 +- 10 files changed, 1309 insertions(+), 1337 deletions(-) diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c index 77f61109f..269b46156 100644 --- a/flight/Modules/PipXtreme/pipxtrememod.c +++ b/flight/Modules/PipXtreme/pipxtrememod.c @@ -155,9 +155,7 @@ static void systemTask(void *parameters) // 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); diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index adda6cd60..905d08888 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -489,20 +489,20 @@ static void configureComCallback(OPLinkSettingsOutputConnectionOptions com_port, */ static void updateSettings() { - // Get the settings. OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); + // Set the bindings. + PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings); + + //bool is_coordinator = (oplinkSettings.PairID != 0); + bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); 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); @@ -534,9 +534,6 @@ static void updateSettings() 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. @@ -600,4 +597,8 @@ static void updateSettings() if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 115200); break; } + + // Reinitilize the modem. + if (is_coordinator) + PIOS_RFM22B_Reinit(pios_rfm22b_id); } diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 306e96062..bb2301515 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -564,7 +564,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->spi_id = spi_id; // Initialize our configuration parameters - rfm22b_dev->coordinator = false; rfm22b_dev->send_ppm = false; rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; @@ -586,6 +585,11 @@ 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 bindings. + for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) + rfm22b_dev->bindings[i] = 0; + rfm22b_dev->coordinator = false; + // Create the event queue rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_rfm22b_event)); @@ -630,6 +634,16 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu return(0); } +/** + * Re-initialize the modem after a configuration change. + */ +void PIOS_RFM22B_Reinit(uint32_t rfm22b_id) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_validate(rfm22b_dev)) + PIOS_RFM22B_InjectEvent(rfm22b_dev, RFM22B_EVENT_INITIALIZE, false); +} + /** * The RFM22B external interrupt routine. */ @@ -688,6 +702,20 @@ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) return 0; } +/** + * Returns true if the modem is configured as a coordinator. + * \param[in] rfm22b_id The RFM22B device index. + * \return True if the modem is configured as a coordinator. + */ +bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (PIOS_RFM22B_validate(rfm22b_dev)) + return rfm22b_dev->coordinator; + else + return false; +} + /** * Sets the radio device transmit power. * \param[in] rfm22b_id The RFM22B device index. @@ -717,40 +745,6 @@ void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_frequency, u 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)) - 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; -} - -/** - * 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); -} - /** * Set the remote com port configuration parameters. * \param[in] rfm22b_id The rfm22b device. @@ -779,6 +773,25 @@ void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigC rfm22b_dev->com_config_cb = cb; } +/** + * Set the list of modems that this modem will bind with. + * \param[in] rfm22b_id The rfm22b device. + * \param[in] bindings The array of bindings. + */ +void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindings[]) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if(!PIOS_RFM22B_validate(rfm22b_dev)) + return; + // This modem will be considered a coordinator if any bindings have been set. + rfm22b_dev->coordinator = false; + for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) + { + rfm22b_dev->bindings[i] = bindings[i]; + rfm22b_dev->coordinator |= (rfm22b_dev->bindings[i] != 0); + } +} + /** * Returns the device statistics RFM22B device. * \param[in] rfm22b_id The RFM22B device index. @@ -1029,9 +1042,6 @@ void PIOS_RFM22B_SetDatarate(uint32_t rfm22b_id, enum rfm22b_datarate datarate, 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); } // ************************************ @@ -1477,8 +1487,8 @@ static enum pios_rfm22b_event rfm22_txStart(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) + // Don't send status if we're the coordinator. + if (rfm22b_dev->coordinator) return; // Update the link quality metric. @@ -1991,6 +2001,18 @@ 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; + + // Go to the next binding, if the previous tx packet was a connection request + if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_CON_REQUEST) + { + // Increment the current binding index, and make sure that we didn't run off the end of the buffer, or past the last nonzero ID + if ((++(rfm22b_dev->cur_binding) >= OPLINKSETTINGS_BINDINGS_NUMELEM) || (rfm22b_dev->bindings[rfm22b_dev->cur_binding] == 0)) + rfm22b_dev->cur_binding = 0; + rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding]; + rfm22b_dev->tx_packet->header.destination_id = rfm22b_dev->destination_id; + } + + // Increment the reset packet counter if we're connected. 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; @@ -2007,8 +2029,9 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b 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? + bool found = false; uint8_t id_idx = 0; for ( ; id_idx < OPLINKSTATUS_PAIRIDS_NUMELEM; ++id_idx) if(rfm22b_dev->pair_stats[id_idx].pairID == id) @@ -2026,19 +2049,16 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b } // If we haven't seen it, find a slot to put it in. - if (!found) + else { 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) { - 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) { - if(rfm22b_dev->pair_stats[id_idx].rssi < min_rssi) - { - min_rssi = rfm22b_dev->pair_stats[id_idx].rssi; - min_idx = id_idx; - } + min_rssi = rfm22b_dev->pair_stats[id_idx].rssi; + min_idx = id_idx; } } rfm22b_dev->pair_stats[min_idx].pairID = id; @@ -2066,6 +2086,7 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; // Fill in the connection request + rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding]; 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)); @@ -2110,7 +2131,7 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm memcpy((uint8_t*)lcph, (uint8_t*)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); // Set the destination ID to the source of the connection request message. - PIOS_RFM22B_SetDestinationId((uint32_t)rfm22b_dev, cph->header.source_id); + rfm22b_dev->destination_id = cph->header.source_id; return RFM22B_EVENT_CONNECTION_ACCEPTED; } diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 1511c9884..5a2c96bca 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -100,14 +100,16 @@ typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsOutputConnectionOpti /* 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_Reinit(uint32_t rfb22b_id); 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 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 void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindings[]); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); +extern bool PIOS_RFM22B_IsCoordinator(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); extern bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id); diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 27883311c..e771bf6d3 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -659,6 +659,10 @@ struct pios_rfm22b_dev { // The destination ID uint32_t destination_id; + // The list of bound radios. + uint32_t bindings[OPLINKSETTINGS_BINDINGS_NUMELEM]; + uint8_t cur_binding; + // Is this device a coordinator? bool coordinator; diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 5dd59dc3d..8f2febd95 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -199,45 +199,6 @@ void PIOS_Board_Init(void) { } #endif - /* Configure PPM input */ - switch (oplinkSettings.PPM) { -#if defined(PIOS_INCLUDE_PPM) - case OPLINKSETTINGS_PPM_INPUT: - { - 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); - break; - } -#endif /* PIOS_INCLUDE_PPM */ - -#if defined(PIOS_INCLUDE_PPM_OUT) - case OPLINKSETTINGS_PPM_OUTPUT: - PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); - break; -#endif /* PIOS_INCLUDE_PPM_OUT */ - - default: - { - /* Configure the flexi serial port if PPM not selected */ - 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_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_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); - } - } - } - /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) { @@ -260,6 +221,51 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_RFM22B */ + /* Configure PPM input */ + bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); + switch (oplinkSettings.PPM) { +#if defined(PIOS_INCLUDE_PPM) + case OPLINKSETTINGS_PPM_PPM: + case OPLINKSETTINGS_PPM_PPMTELEMETRY: + { + /* PPM input is configured on the coordinator modem and output on the remote modem. */ + if (is_coordinator) + { + 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); + } +#if defined(PIOS_INCLUDE_PPM_OUT) + else + { + PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); + } +#endif /* PIOS_INCLUDE_PPM_OUT */ + break; + } +#endif /* PIOS_INCLUDE_PPM */ + + default: + { + /* Configure the flexi serial port if PPM not selected */ + 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_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_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); + } + } + } + /* Remap AFIO pin */ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 86f72df64..f6e457690 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -46,7 +46,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget } // Connect to the OPLinkSettings object updates - oplinkSettingsObj = dynamic_cast(objManager->getObject("OPLinkSettings")); + oplinkSettingsObj = dynamic_cast(objManager->getObject("OPLinkSettings")); if (oplinkSettingsObj != NULL ) { connect(oplinkSettingsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateSettings(UAVObject*))); } else { @@ -55,7 +55,6 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget autoLoadWidgets(); addApplySaveButtons(m_oplink->Apply, m_oplink->Save); - addUAVObjectToWidgetRelation("OPLinkSettings", "Coordinator", m_oplink->Coordinator); addUAVObjectToWidgetRelation("OPLinkSettings", "UAVTalk", m_oplink->UAVTalk); addUAVObjectToWidgetRelation("OPLinkSettings", "PPM", m_oplink->PPM); addUAVObjectToWidgetRelation("OPLinkSettings", "InputConnection", m_oplink->InputConnection); @@ -88,11 +87,19 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate); // Connect to the pair ID radio buttons. - 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))); + connect(m_oplink->PairSelect1, SIGNAL(toggled(bool)), this, SLOT(pairIDToggled(bool))); + connect(m_oplink->PairSelect2, SIGNAL(toggled(bool)), this, SLOT(pairIDToggled(bool))); + connect(m_oplink->PairSelect3, SIGNAL(toggled(bool)), this, SLOT(pairIDToggled(bool))); + connect(m_oplink->PairSelect4, SIGNAL(toggled(bool)), this, SLOT(pairIDToggled(bool))); + + // Connect to the binding buttons. + connect(m_oplink->BindingAdd, SIGNAL(pressed()), this, SLOT(addBinding())); + connect(m_oplink->BindingRemove, SIGNAL(pressed()), this, SLOT(removeBinding())); + m_oplink->BindingAdd->setEnabled(false); + m_oplink->BindingRemove->setEnabled(false); + + // Connect to changes to the bindings widgets. + connect(m_oplink->Bindings, SIGNAL(itemSelectionChanged()), this, SLOT(bindingsSelectionChanged())); //Add scroll bar when necessary QScrollArea *scroll = new QScrollArea; @@ -131,7 +138,6 @@ void ConfigPipXtremeWidget::applySettings() 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); } @@ -153,10 +159,8 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) oplinkSettingsObj->requestUpdate(); // Get the current pairID - OPLinkSettings *oplinkSettings = OPLinkSettings::GetInstance(getObjectManager()); + //OPLinkSettings *oplinkSettings = OPLinkSettings::GetInstance(getObjectManager()); quint32 pairID = 0; - if (oplinkSettings) - pairID = oplinkSettings->getPairID(); // Update the detected devices. UAVObjectField* pairIdField = object->getField("PairIDs"); @@ -253,9 +257,6 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) qDebug() << "PipXtremeGadgetWidget: Count not read DeviceID field."; } - // Update the PairID field - m_oplink->PairID->setText(QString::number(pairID, 16).toUpper()); - // Update the link state UAVObjectField* linkField = object->getField("LinkState"); if (linkField) { @@ -270,13 +271,16 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) */ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) { - Q_UNUSED(object); + Q_UNUSED(object); - if (!settingsUpdated) - { - settingsUpdated = true; - enableControls(true); - } + if (!settingsUpdated) + { + settingsUpdated = true; + enableControls(true); + + // Refresh the list widget. + refreshBindingsList(); + } } void ConfigPipXtremeWidget::disconnected() @@ -288,52 +292,88 @@ void ConfigPipXtremeWidget::disconnected() } } -void ConfigPipXtremeWidget::pairIDToggled(bool checked, quint8 idx) +void ConfigPipXtremeWidget::pairIDToggled(bool checked) { - if(checked) - { - OPLinkStatus *oplinkStatus = OPLinkStatus::GetInstance(getObjectManager()); - OPLinkSettings *oplinkSettings = OPLinkSettings::GetInstance(getObjectManager()); + bool enabled = (m_oplink->PairSelect1->isChecked() || m_oplink->PairSelect2->isChecked() || m_oplink->PairSelect3->isChecked() || m_oplink->PairSelect4->isChecked()); + m_oplink->BindingAdd->setEnabled(enabled); +} - if (oplinkStatus && oplinkSettings) - { - if (idx == 4) - { - oplinkSettings->setPairID(0); - } - else - { - quint32 pairID = oplinkStatus->getPairIDs(idx); - if (pairID) - oplinkSettings->setPairID(pairID); - } - } +void ConfigPipXtremeWidget::bindingsSelectionChanged() +{ + bool enabled = (m_oplink->Bindings->selectedItems().size() > 0); + m_oplink->BindingRemove->setEnabled(enabled); +} + +void ConfigPipXtremeWidget::refreshBindingsList() +{ + m_oplink->Bindings->clear(); + for (quint32 slot = 0; slot < OPLinkSettings::BINDINGS_NUMELEM; ++slot) + { + quint32 id = oplinkSettingsObj->getBindings(slot); + if (id != 0) + m_oplink->Bindings->addItem(QString::number(id, 16).toUpper()); } } -void ConfigPipXtremeWidget::pair1Toggled(bool checked) +void ConfigPipXtremeWidget::addBinding() { - pairIDToggled(checked, 0); + + // Get the pair ID out of the selection widget + quint32 pairID = 0; + bool okay; + 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); + + // Find a slot for this pair ID in the binding list. + quint32 slot = 0; + for ( ; slot < OPLinkSettings::BINDINGS_NUMELEM - 1; ++slot) + { + quint32 id = oplinkSettingsObj->getBindings(slot); + + // Is this ID already in the list? + if (id == pairID) + return; + + // Is this slot empty? + if (id == 0) + break; + } + + // Store the ID in the first open slot (or the last slot if all are full). + oplinkSettingsObj->setBindings(slot, pairID); + + // Refresh the list widget. + refreshBindingsList(); } -void ConfigPipXtremeWidget::pair2Toggled(bool checked) +void ConfigPipXtremeWidget::removeBinding() { - pairIDToggled(checked, 1); -} + // Get the selected rows from the bindings list widget. + QList selected = m_oplink->Bindings->selectedItems(); -void ConfigPipXtremeWidget::pair3Toggled(bool checked) -{ - pairIDToggled(checked, 2); -} + // Zero out the selected rows in the bindings list. + for (int i = 0; i < selected.size(); ++i) + oplinkSettingsObj->setBindings(m_oplink->Bindings->row(selected[i]), 0); -void ConfigPipXtremeWidget::pair4Toggled(bool checked) -{ - pairIDToggled(checked, 3); -} - -void ConfigPipXtremeWidget::pairBToggled(bool checked) -{ - pairIDToggled(checked, 4); + // Refresh the list widget and compact the bindings list. + m_oplink->Bindings->clear(); + quint32 outSlot = 0; + for (quint32 slot = 0; slot < OPLinkSettings::BINDINGS_NUMELEM; ++slot) + { + quint32 id = oplinkSettingsObj->getBindings(slot); + if (id != 0) { + oplinkSettingsObj->setBindings(outSlot++, id); + m_oplink->Bindings->addItem(QString::number(id, 16).toUpper()); + } + } + for ( ; outSlot < OPLinkSettings::BINDINGS_NUMELEM; ++outSlot) + oplinkSettingsObj->setBindings(outSlot, 0); } /** diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index 1243c5b65..29e67ccba 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -27,6 +27,8 @@ #ifndef CONFIGPIPXTREMEWIDGET_H #define CONFIGPIPXTREMEWIDGET_H +#include + #include "ui_pipxtreme.h" #include "configtaskwidget.h" @@ -41,6 +43,7 @@ public: public slots: void updateStatus(UAVObject *object1); void updateSettings(UAVObject *object1); + void refreshBindingsList(); private: Ui_PipXtremeWidget *m_oplink; @@ -49,7 +52,7 @@ private: UAVDataObject* oplinkStatusObj; // The OPLink ssettins UAVObject - UAVDataObject* oplinkSettingsObj; + OPLinkSettings* oplinkSettingsObj; bool settingsUpdated; @@ -58,12 +61,10 @@ private slots: void applySettings(); void saveSettings(); void disconnected(); - void pairIDToggled(bool checked, quint8 idx); - void pair1Toggled(bool checked); - void pair2Toggled(bool checked); - void pair3Toggled(bool checked); - void pair4Toggled(bool checked); - void pairBToggled(bool checked); + void pairIDToggled(bool checked); + void addBinding(); + void removeBinding(); + void bindingsSelectionChanged(); }; #endif // CONFIGTXPIDWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 2ccf5b6bf..6d7f36b8f 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -23,7 +23,7 @@ QFrame::Raised - + @@ -52,7 +52,7 @@ - + @@ -139,235 +139,15 @@ - - + + QFrame::StyledPanel QFrame::Raised - - - - - - 75 - true - - - - Pairing - - - - - - - - - - - - - false - - - - 100 - 16777215 - - - - Broadcast - - - - - - - Broadcast Address - - - - - - - - - - - - - - - 100 - 16777215 - - - - 12345678 - - - - - - - -127 - - - 0 - - - -127 - - - false - - - %v dBm - - - - - - - -100dB - - - - - - - - - - - - - - - 100 - 16777215 - - - - - - - - -127 - - - 0 - - - 0 - - - false - - - %v dBm - - - - - - - -100dB - - - - - - - - - - - - - - - 100 - 16777215 - - - - - - - - -127 - - - 0 - - - 0 - - - false - - - %v dBm - - - - - - - -100dB - - - - - - - - - - - - - - - 100 - 16777215 - - - - - - - - -127 - - - 0 - - - 0 - - - false - - - %v dBm - - - - - - - -100dB - - - - - - + @@ -418,13 +198,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } false @@ -479,13 +259,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } true @@ -530,13 +310,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } false @@ -552,426 +332,6 @@ - - - - Pair ID - - - 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 - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 90ABCDEF - - - - - - - Link State - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - 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 - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - false - - - true - - - - - - - RSSI - - - 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;*/ -} - - - true - - - - - - - AFC Corr. - - - 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;*/ -} - - - true - - - - - - - TX 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 - - - - - - - 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) - - - 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 - - - @@ -1013,13 +373,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + 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 @@ -1029,6 +389,66 @@ + + + + Link State + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + 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 + + + @@ -1070,13 +490,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + 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 @@ -1086,6 +506,48 @@ + + + + Link Quality + + + 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 + + + @@ -1121,13 +583,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + 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 @@ -1137,6 +599,51 @@ + + + + RSSI + + + 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;*/ + } + + + true + + + @@ -1178,13 +685,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + 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 @@ -1194,6 +701,45 @@ + + + + AFC Corr. + + + 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;*/ + } + + + true + + + @@ -1235,13 +781,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + 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 @@ -1251,6 +797,54 @@ + + + + TX 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 + + + @@ -1286,13 +880,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + 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 @@ -1302,6 +896,54 @@ + + + + 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 + + + @@ -1337,13 +979,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + 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 @@ -1353,6 +995,54 @@ + + + + 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 + + + @@ -1379,13 +1069,55 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + 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) + + + 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 @@ -1421,13 +1153,55 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } + + + false + + + true + + + + + + + Timeouts + + + 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 @@ -1469,13 +1243,13 @@ QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } false @@ -1485,62 +1259,358 @@ - - + + + + + + + + 0 + 0 + + + + + 75 + true + + + + Configuration + + + + - Timeouts + UAVTalk + + + + + + + PPM Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - + + - 101 + 16777215 16777215 - - - 75 - true - + + Choose the PPM function - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} + + + + + + Input Connection - - false + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - + + + + + + + 16777215 + 16777215 + + + + Choose which port to communicate over on this modem + + + + + + + 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 + 16777215 + + + + Set the telemetry port speed + + + + + + + Max RF Tx Power(mW) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 16777215 + 16777215 + + + + Set the maximum TX output power the modem will use + + + Qt::LeftToRight + + + 0 + + + + + + + Send Timeout (ms) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Set the send timeout + + true + + 255 + + + + + + + Min Packet Size + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Set the minimum packet size + + + true + + + 255 + + + + + + + Frequency Calibration + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Calibrate the modems RF carrier frequency + + + true + + + 255 + + + + + + + Min. Frequency (Hz) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Set the modems minimum RF carrier frequency + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 400000000 + + + 1000000000 + + + 100000 + + + + + + + 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 + + groupBox + groupBox_3 - - - - - 0 - 0 - - + + 75 @@ -1548,418 +1618,263 @@ - Configuration + Bindings - - - - - Coordinator - - - - - - - UAVTalk - - - - - - - PPM - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Choose the PPM function - - - - - - - Input Connection - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Choose which port to communicate over on this modem - - - - - - - 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 - 16777215 - - - - Set the telemetry port speed - - - - - - - Max RF Tx Power(mW) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Set the maximum TX output power the modem will use - - - Qt::LeftToRight - - - 0 - - - - - - - Send Timeout (ms) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Set the send timeout - - - true - - - 255 - - - - - - - Min Packet Size - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Set the minimum packet size - - - true - - - 255 - - - - - - - Frequency Calibration - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Calibrate the modems RF carrier frequency - - - true - - - 255 - - - - - - - Min. Frequency (Hz) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Set the modems minimum RF carrier frequency - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 400000000 - - - 1000000000 - - - 100000 - - - - - - - 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 - - - - - - - Courier New - 9 - + + + + + + + + - - The AES encryption key - has to be the same key on the remote modem. + + + + + + -100dB - - + + + + + + -127 - - 32 + + 0 - - true + + 0 - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + false + + + %v dBm + + + + + + + -100dB + + + + + + + + 100 + 16777215 + + + + + + + + -127 + + + 0 + + + 0 + + + false + + + %v dBm + + + + + + + -100dB + + + + + + + -127 + + + 0 + + + -127 + + + false + + + %v dBm + + + + + + + + 100 + 16777215 + + + + + + + + -100dB + + + + + + + + 100 + 16777215 + + + + 12345678 - - - - 0 - 0 - - - - Radomise the AES encryption key - + - Rand + + + + + + + + -127 + + + 0 + + + 0 + + + false + + + %v dBm + + + + + + + - - - - 75 - true - - - - Enable/Disable AES encryption - - - Qt::RightToLeft - - - Enable + + + + 100 + 16777215 + + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + - - - - Qt::Vertical + + + + + 0 + 0 + - + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + Add >> + + + + + + + Remove + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + 0 + 0 + + + - 20 - 40 + 200 + 16777215 - + @@ -1982,29 +1897,15 @@ SerialNumber DeviceID LinkState - RxAFC - Retries Errors UAVTalkErrors Resets TXRate RXRate - TelemPortConfig - TelemPortSpeed - FlexiPortConfig - FlexiPortSpeed - VCPConfig - VCPSpeed - MaxRFDatarate MaxRFTxPower SendTimeout MinPacketSize FrequencyCalibration - Frequency - ScanSpectrum - AESKey - AESKeyRandom - AESEnable graphicsView_Spectrum Apply Save diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index 5ee05bd0e..f24bba648 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -1,11 +1,10 @@ OPLink configurations options. - - - - - + + + + @@ -14,7 +13,6 @@ - From 9b6493eeca615fd6d5aee95fa3a29e06b45fb41f Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 6 Feb 2013 02:28:07 +0000 Subject: [PATCH 02/40] Set the binding on the OPLink after configuring the radio. --- flight/PipXtreme/System/pios_board.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 8f2febd95..80bc8fb61 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -218,6 +218,9 @@ void PIOS_Board_Init(void) { tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } + + /* Set the RFM22B bindings. */ + PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings); } #endif /* PIOS_INCLUDE_RFM22B */ From cf7c92903036eeb8ed602002f3a6f6f8121efef5 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 18 Feb 2013 01:49:13 +0000 Subject: [PATCH 03/40] Added the ability to store bindings and configuration paramaters for multiple (up to 8) modems from an OPLink controller modem. --- flight/Libraries/inc/packet_handler.h | 5 +- flight/Modules/PipXtreme/pipxtrememod.c | 2 +- .../Modules/RadioComBridge/RadioComBridge.c | 392 ++- .../PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 8 +- flight/PiOS/Common/pios_rfm22b.c | 75 +- flight/PiOS/Common/pios_rfm22b_com.c | 5 +- flight/PiOS/inc/pios_rfm22b.h | 8 +- flight/PiOS/inc/pios_rfm22b_priv.h | 10 +- flight/PipXtreme/System/pios_board.c | 189 +- .../plugins/config/configpipxtremewidget.cpp | 259 +- .../plugins/config/configpipxtremewidget.h | 17 +- .../src/plugins/config/pipxtreme.ui | 2900 +++++++++++------ .../src/plugins/uavobjects/uavobjectfield.cpp | 7 +- .../uavobjectwidgetutils/configtaskwidget.cpp | 66 +- .../uavobjectwidgetutils/configtaskwidget.h | 12 +- shared/uavobjectdefinition/oplinksettings.xml | 16 +- shared/uavobjectdefinition/oplinkstatus.xml | 8 +- 17 files changed, 2412 insertions(+), 1567 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 8bdfb4eb0..659cdac9e 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -91,12 +91,13 @@ typedef struct { #define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - uint8_t datarate; uint32_t frequency_hz; uint32_t min_frequency; uint32_t max_frequency; uint8_t max_tx_power; - OPLinkSettingsOutputConnectionOptions port; + OPLinkSettingsMainPortOptions main_port; + OPLinkSettingsFlexiPortOptions flexi_port; + OPLinkSettingsVCPPortOptions vcp_port; OPLinkSettingsComSpeedOptions com_speed; uint8_t ecc[RS_ECC_NPARITY]; } PHConnectionPacket, *PHConnectionPacketHandle; diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c index 269b46156..5aaed3831 100644 --- a/flight/Modules/PipXtreme/pipxtrememod.c +++ b/flight/Modules/PipXtreme/pipxtrememod.c @@ -165,6 +165,7 @@ static void systemTask(void *parameters) PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); // Update the status + oplinkStatus.HeapRemaining = xPortGetFreeHeapSize(); oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); oplinkStatus.RxGood = radio_stats.rx_good; oplinkStatus.RxCorrected = radio_stats.rx_corrected; @@ -177,7 +178,6 @@ static void systemTask(void *parameters) 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; diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index 905d08888..0e48df851 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -46,6 +46,12 @@ #include +// External functions +void PIOS_InitUartMainPort(); +void PIOS_InitUartFlexiPort(); +void PIOS_InitPPMMainPort(bool input); +void PIOS_InitPPMFlexiPort(bool input); + // **************** // Private constants @@ -82,6 +88,12 @@ typedef struct { uint32_t UAVTalkErrors; uint32_t droppedPackets; + // Should we parse UAVTalk? + bool parseUAVTalk; + + // The current configured uart speed + OPLinkSettingsComSpeedOptions comSpeed; + } RadioComBridgeData; // **************** @@ -94,7 +106,8 @@ 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 configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed); static void updateSettings(); // **************** @@ -109,30 +122,71 @@ static RadioComBridgeData *data; */ static int32_t RadioComBridgeStart(void) { - if(data) { + if(data) { - // Configure the com port configuration callback - PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); + // Configure the com port configuration callback + PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); - // Set the baudrates, etc. - updateSettings(); + // Set the baudrates, etc. + bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); + if (is_coordinator) { - // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. - 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)); + // Get the settings. + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); - // Register the watchdog timers. + // 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; + } + + // Reinitilize the modem. + PIOS_RFM22B_Reinit(pios_rfm22b_id); + } + + // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. + 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)); + + // 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); + PIOS_WDG_RegisterFlag(PIOS_WDG_TELEMETRY); + PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORX); + PIOS_WDG_RegisterFlag(PIOS_WDG_RADIOTX); #endif - return 0; - } + return 0; + } - return -1; + return -1; } /** @@ -168,6 +222,9 @@ static int32_t RadioComBridgeInitialize(void) data->comTxErrors = 0; data->comTxRetries = 0; data->UAVTalkErrors = 0; + data->parseUAVTalk = false; + data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; + PIOS_COM_RADIO = PIOS_COM_RFM22B; return 0; } @@ -235,7 +292,7 @@ static void radioRxTask(void *parameters) // Task loop while (1) { #ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX); + 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); @@ -259,17 +316,18 @@ static void radioTxTask(void *parameters) #endif #if defined(PIOS_INCLUDE_USB) // Determine output port (USB takes priority over telemetry port) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) - inputPort = PIOS_COM_TELEM_USB; + if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB_HID) + inputPort = PIOS_COM_TELEM_USB_HID; #endif /* PIOS_INCLUDE_USB */ - if(inputPort) - { + 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]); - } + if (bytes_to_process > 0) { + for (uint8_t i = 0; i < bytes_to_process; i++) + ProcessInputStream(data->inUAVTalkCon, serial_data[i]); + } + } else + vTaskDelay(5); } } @@ -285,8 +343,8 @@ 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_COM_TELEM_USB && PIOS_COM_Available(PIOS_COM_TELEM_USB)) - outputPort = PIOS_COM_TELEM_USB; + if (PIOS_COM_TELEM_USB_HID && PIOS_COM_Available(PIOS_COM_TELEM_USB_HID)) + outputPort = PIOS_COM_TELEM_USB_HID; #endif /* PIOS_INCLUDE_USB */ if(outputPort) return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); @@ -303,13 +361,14 @@ 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; - // 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 - // For some reason, if this function returns failure, it prevents saving settings. - return length; + uint32_t outputPort = PIOS_COM_RADIO; + // 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 { + // For some reason, if this function returns failure, it prevents saving settings. + return length; + } } static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) @@ -443,45 +502,60 @@ static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEve * \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) +static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed) { - // Get the settings. - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); + // Update the com baud rate + data->comSpeed = com_speed; - // 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; + // Set the output main/flexi/vcp port and speed. + bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); + if (!is_coordinator) { + + // Get the settings. + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + + switch (main_port) { + case OPLINKSETTINGS_REMOTEMAINPORT_DISABLED: + oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_DISABLED; + break; + case OPLINKSETTINGS_REMOTEMAINPORT_SERIAL: + oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_SERIAL; + break; + case OPLINKSETTINGS_REMOTEMAINPORT_PPM: + oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_PPM; + break; } - oplinkSettings.ComSpeed = com_speed; - // A non-coordinator modem should not set a remote telemetry connection. - oplinkSettings.OutputConnection = OPLINKSETTINGS_OUTPUTCONNECTION_REMOTEHID; + switch (flexi_port) { + case OPLINKSETTINGS_REMOTEFLEXIPORT_DISABLED: + oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_DISABLED; + break; + case OPLINKSETTINGS_REMOTEFLEXIPORT_SERIAL: + oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_SERIAL; + break; + case OPLINKSETTINGS_REMOTEFLEXIPORT_PPM: + oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_PPM; + break; + } - // Update the OPLinkSettings object. - OPLinkSettingsSet(&oplinkSettings); + switch (vcp_port) { + case OPLINKSETTINGS_REMOTEVCPPORT_DISABLED: + oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_DISABLED; + break; + case OPLINKSETTINGS_REMOTEVCPPORT_SERIAL: + oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_SERIAL; + break; + } - // Perform the update. - updateSettings(); + // Update the OPLinkSettings object. + OPLinkSettingsSet(&oplinkSettings); + } + + // Perform the update. + updateSettings(); } /** @@ -489,116 +563,84 @@ static void configureComCallback(OPLinkSettingsOutputConnectionOptions com_port, */ static void updateSettings() { - // Get the settings. - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); + // Get the settings. + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); - // Set the bindings. - PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings); + // Configure the main port + bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); + switch (oplinkSettings.MainPort) + { + case OPLINKSETTINGS_MAINPORT_TELEMETRY: + data->parseUAVTalk = true; + case OPLINKSETTINGS_MAINPORT_SERIAL: + /* Configure the main port for uart serial */ + PIOS_InitUartMainPort(); + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_MAIN; + break; + case OPLINKSETTINGS_MAINPORT_PPM: + PIOS_InitPPMMainPort(is_coordinator); + break; + case OPLINKSETTINGS_MAINPORT_DISABLED: + break; + } - //bool is_coordinator = (oplinkSettings.PairID != 0); - bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); - if (is_coordinator) - { - // Set the remote com configuration parameters - PIOS_RFM22B_SetRemoteComConfig(pios_rfm22b_id, oplinkSettings.OutputConnection, oplinkSettings.ComSpeed); + // Configure the flexi port + switch (oplinkSettings.FlexiPort) + { + case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: + data->parseUAVTalk = true; + case OPLINKSETTINGS_FLEXIPORT_SERIAL: + /* Configure the flexi port as uart serial */ + PIOS_InitUartFlexiPort(); + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; + break; + case OPLINKSETTINGS_FLEXIPORT_PPM: + PIOS_InitPPMFlexiPort(is_coordinator); + break; + case OPLINKSETTINGS_FLEXIPORT_DISABLED: + break; + } - // Set the frequencies. - PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency); + // Configure the USB VCP port + switch (oplinkSettings.VCPPort) + { + case OPLINKSETTINGS_VCPPORT_SERIAL: + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_USB_VCP; + break; + case OPLINKSETTINGS_VCPPORT_DISABLED: + break; + } - // 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; - } - } - - // 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 (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 (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 (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 (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 (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 (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 (is_coordinator && PIOS_COM_RADIO) PIOS_COM_ChangeBaud(PIOS_COM_RADIO, 115200); - if (PIOS_COM_TELEMETRY) PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 115200); - break; - } - - // Reinitilize the modem. - if (is_coordinator) - PIOS_RFM22B_Reinit(pios_rfm22b_id); + // Update the com baud rate. + uint32_t comBaud = 9600; + switch (data->comSpeed) { + case OPLINKSETTINGS_COMSPEED_2400: + comBaud = 2400; + break; + case OPLINKSETTINGS_COMSPEED_4800: + comBaud = 4800; + break; + case OPLINKSETTINGS_COMSPEED_9600: + comBaud = 9600; + break; + case OPLINKSETTINGS_COMSPEED_19200: + comBaud = 19200; + break; + case OPLINKSETTINGS_COMSPEED_38400: + comBaud = 38400; + break; + case OPLINKSETTINGS_COMSPEED_57600: + comBaud = 57600; + break; + case OPLINKSETTINGS_COMSPEED_115200: + comBaud = 115200; + break; + } + if (PIOS_COM_RADIO) { + PIOS_COM_ChangeBaud(PIOS_COM_RADIO, comBaud); + } + if (PIOS_COM_TELEMETRY) { + PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, comBaud); + } } diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index 05b757e9c..be0ea2f9c 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -174,17 +174,17 @@ extern uint32_t pios_i2c_flexi_adapter_id; 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_main_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; extern uint32_t pios_ppm_out_id; -#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) -#define PIOS_COM_TELEM_VCP (pios_com_telem_vcp_id) +#define PIOS_COM_TELEM_USB_HID (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_USB_VCP (pios_com_telem_vcp_id) +#define PIOS_COM_TELEM_UART_MAIN (pios_com_telem_uart_main_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) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index bb2301515..672f9332c 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -587,7 +587,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu // Initialize the bindings. for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) - rfm22b_dev->bindings[i] = 0; + rfm22b_dev->bindings[i].pairID = 0; rfm22b_dev->coordinator = false; // Create the event queue @@ -745,21 +745,6 @@ void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_frequency, u rfm22_setNominalCarrierFrequency(rfm22b_dev, (max_frequency - min_frequency) / 2); } -/** - * 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. @@ -778,17 +763,21 @@ void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigC * \param[in] rfm22b_id The rfm22b device. * \param[in] bindings The array of bindings. */ -void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindings[]) +void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], + const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if(!PIOS_RFM22B_validate(rfm22b_dev)) return; // This modem will be considered a coordinator if any bindings have been set. rfm22b_dev->coordinator = false; - for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) - { - rfm22b_dev->bindings[i] = bindings[i]; - rfm22b_dev->coordinator |= (rfm22b_dev->bindings[i] != 0); + for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { + rfm22b_dev->bindings[i].pairID = bindingPairIDs[i]; + rfm22b_dev->bindings[i].main_port = mainPortSettings[i]; + rfm22b_dev->bindings[i].flexi_port = flexiPortSettings[i]; + rfm22b_dev->bindings[i].vcp_port = vcpPortSettings[i]; + rfm22b_dev->bindings[i].com_speed = comSpeeds[i]; + rfm22b_dev->coordinator |= (rfm22b_dev->bindings[i].pairID != 0); } } @@ -1036,14 +1025,6 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_d rfm22_write(rfm22b_dev, 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; -} - // ************************************ // SPI read/write @@ -2005,11 +1986,20 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d // Go to the next binding, if the previous tx packet was a connection request if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_CON_REQUEST) { - // Increment the current binding index, and make sure that we didn't run off the end of the buffer, or past the last nonzero ID - if ((++(rfm22b_dev->cur_binding) >= OPLINKSETTINGS_BINDINGS_NUMELEM) || (rfm22b_dev->bindings[rfm22b_dev->cur_binding] == 0)) - rfm22b_dev->cur_binding = 0; - rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding]; - rfm22b_dev->tx_packet->header.destination_id = rfm22b_dev->destination_id; + PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); + // Increment the current binding index to the next non-zero binding. + for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { + if (++(rfm22b_dev->cur_binding) >= OPLINKSETTINGS_BINDINGS_NUMELEM) + rfm22b_dev->cur_binding = 0; + if (rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID != 0) + break; + } + rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; + cph->header.destination_id = rfm22b_dev->destination_id; + cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; + cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; + cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; + cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; } // Increment the reset packet counter if we're connected. @@ -2086,11 +2076,14 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; // Fill in the connection request - rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding]; + rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; 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->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; + cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; + cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; + cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; cph->frequency_hz = rfm22b_dev->frequency_hz; cph->min_frequency = rfm22b_dev->min_frequency; cph->max_frequency = rfm22b_dev->max_frequency; @@ -2107,17 +2100,17 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) // Set our connection state to connected rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + + // Call the com port configuration function + if (rfm22b_dev->com_config_cb) + rfm22b_dev->com_config_cb(cph->main_port, cph->flexi_port, cph->vcp_port, cph->com_speed); // Configure this modem from the connection request message. - rfm22_setDatarate(rfm22b_dev, cph->datarate, true); + rfm22_setDatarate(rfm22b_dev, rfm22b_dev->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); - - // 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/Common/pios_rfm22b_com.c b/flight/PiOS/Common/pios_rfm22b_com.c index e8fb4a6db..7b0b62369 100644 --- a/flight/PiOS/Common/pios_rfm22b_com.c +++ b/flight/PiOS/Common/pios_rfm22b_com.c @@ -61,9 +61,6 @@ 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) @@ -82,7 +79,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; - PIOS_RFM22B_SetDatarate(rfm22b_id, datarate, true); + rfm22b_dev->datarate = datarate; } 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 5a2c96bca..a67f5c253 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -96,18 +96,18 @@ struct rfm22b_stats { }; /* Callback function prototypes */ -typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsOutputConnectionOptions com_port, OPLinkSettingsComSpeedOptions com_speed); +typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, + OPLinkSettingsRemoteVCPPortOptions vcp_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_Reinit(uint32_t rfb22b_id); 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 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_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 void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindings[]); +extern void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], + const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern bool PIOS_RFM22B_IsCoordinator(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index e771bf6d3..1ac46b345 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -645,6 +645,14 @@ typedef struct { uint8_t lastContact; } rfm22b_pair_stats; +typedef struct { + uint32_t pairID; + OPLinkSettingsRemoteMainPortOptions main_port; + OPLinkSettingsRemoteFlexiPortOptions flexi_port; + OPLinkSettingsRemoteVCPPortOptions vcp_port; + OPLinkSettingsComSpeedOptions com_speed; +} rfm22b_binding; + struct pios_rfm22b_dev { enum pios_rfm22b_dev_magic magic; struct pios_rfm22b_cfg cfg; @@ -660,7 +668,7 @@ struct pios_rfm22b_dev { uint32_t destination_id; // The list of bound radios. - uint32_t bindings[OPLINKSETTINGS_BINDINGS_NUMELEM]; + rfm22b_binding bindings[OPLINKSETTINGS_BINDINGS_NUMELEM]; uint8_t cur_binding; // Is this device a coordinator? diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 80bc8fb61..0f598e84e 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -1,17 +1,16 @@ -/* -*- Mode: c; c-basic-offset: 2; tab-width: 2; indent-tabs-mode: t -*- */ /** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotCore OpenPilot Core - * @{ - * - * @file pios_board.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Defines board specific static initializers for hardware for the OpenPilot board. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ +****************************************************************************** +* @addtogroup OpenPilotSystem OpenPilot System +* @{ +* @addtogroup OpenPilotCore OpenPilot Core +* @{ +* +* @file pios_board.c +* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. +* @brief Defines board specific static initializers for hardware for the OpenPilot board. +* @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 @@ -33,9 +32,6 @@ #include #include -#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 @@ -45,9 +41,12 @@ #define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256 #define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256 +#define PIOS_COM_TELEM_RX_BUF_LEN 256 +#define PIOS_COM_TELEM_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_main_id = 0; uint32_t pios_com_telem_uart_flexi_id = 0; uint32_t pios_com_telemetry_id = 0; #if defined(PIOS_INCLUDE_PPM) @@ -61,6 +60,8 @@ uint32_t pios_rfm22b_id = 0; uint32_t pios_com_rfm22b_id = 0; uint32_t pios_com_radio_id = 0; #endif +uint8_t *pios_uart_rx_buffer; +uint8_t *pios_uart_tx_buffer; /** * PIOS_Board_Init() @@ -154,8 +155,8 @@ void PIOS_Board_Init(void) { 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, - rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { + rx_buffer, PIOS_COM_TELEM_USB_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_USB_TX_BUF_LEN)) { PIOS_Assert(0); } } @@ -173,31 +174,13 @@ void PIOS_Board_Init(void) { PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); 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)) { + rx_buffer, PIOS_COM_TELEM_VCP_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_VCP_TX_BUF_LEN)) { PIOS_Assert(0); } } #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)) { - 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_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); - } - } -#endif /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) @@ -214,60 +197,20 @@ void PIOS_Board_Init(void) { 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)) { + rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } /* Set the RFM22B bindings. */ - PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings); + PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings, oplinkSettings.RemoteMainPort, + oplinkSettings.RemoteFlexiPort, oplinkSettings.RemoteVCPPort, oplinkSettings.ComSpeed); } #endif /* PIOS_INCLUDE_RFM22B */ - /* Configure PPM input */ - bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); - switch (oplinkSettings.PPM) { -#if defined(PIOS_INCLUDE_PPM) - case OPLINKSETTINGS_PPM_PPM: - case OPLINKSETTINGS_PPM_PPMTELEMETRY: - { - /* PPM input is configured on the coordinator modem and output on the remote modem. */ - if (is_coordinator) - { - 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); - } -#if defined(PIOS_INCLUDE_PPM_OUT) - else - { - PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); - } -#endif /* PIOS_INCLUDE_PPM_OUT */ - break; - } -#endif /* PIOS_INCLUDE_PPM */ - - default: - { - /* Configure the flexi serial port if PPM not selected */ - 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_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_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); - } - } - } + /* Allocate the uart buffers. */ + pios_uart_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); + pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); /* Remap AFIO pin */ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); @@ -278,6 +221,80 @@ void PIOS_Board_Init(void) { PIOS_GPIO_Init(); } +void PIOS_InitUartMainPort() +{ +#ifndef PIOS_RFM22B_DEBUG_ON_TELEM + uint32_t pios_usart1_id; + if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { + PIOS_Assert(0); + } + PIOS_Assert(pios_uart_rx_buffer); + PIOS_Assert(pios_uart_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_uart_main_id, &pios_usart_com_driver, pios_usart1_id, + pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + PIOS_Assert(0); + } +#endif +} + +void PIOS_InitUartFlexiPort() +{ + uint32_t pios_usart3_id; + if (PIOS_USART_Init(&pios_usart3_id, &pios_usart_telem_flexi_cfg)) { + PIOS_Assert(0); + } + PIOS_Assert(pios_uart_rx_buffer); + PIOS_Assert(pios_uart_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_uart_flexi_id, &pios_usart_com_driver, pios_usart3_id, + pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + PIOS_Assert(0); + } +} + +void PIOS_InitPPMMainPort(bool input) +{ +#if defined(PIOS_INCLUDE_PPM) + /* PPM input is configured on the coordinator modem and output on the remote modem. */ + if (input) + { + 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); + } +#if defined(PIOS_INCLUDE_PPM_OUT) + else + { + PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); + } +#endif /* PIOS_INCLUDE_PPM_OUT */ +#endif /* PIOS_INCLUDE_PPM */ +} + +void PIOS_InitPPMFlexiPort(bool input) +{ +#if defined(PIOS_INCLUDE_PPM) + /* PPM input is configured on the coordinator modem and output on the remote modem. */ + if (input) + { + 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); + } +#if defined(PIOS_INCLUDE_PPM_OUT) + else + { + PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); + } +#endif /* PIOS_INCLUDE_PPM_OUT */ +#endif /* PIOS_INCLUDE_PPM */ +} + /** * @} */ diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index f6e457690..772f5b05c 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -27,6 +27,7 @@ #include "configpipxtremewidget.h" +#include #include #include @@ -53,20 +54,20 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget qDebug() << "Error: Object is unknown (OPLinkSettings)."; } autoLoadWidgets(); + Core::Internal::GeneralSettings *settings = pm->getObject(); + if(!settings->useExpertMode()) + m_oplink->Apply->setVisible(false); addApplySaveButtons(m_oplink->Apply, m_oplink->Save); - addUAVObjectToWidgetRelation("OPLinkSettings", "UAVTalk", m_oplink->UAVTalk); - addUAVObjectToWidgetRelation("OPLinkSettings", "PPM", m_oplink->PPM); - addUAVObjectToWidgetRelation("OPLinkSettings", "InputConnection", m_oplink->InputConnection); - addUAVObjectToWidgetRelation("OPLinkSettings", "OutputConnection", m_oplink->OutputConnection); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed); + addUAVObjectToWidgetRelation("OPLinkSettings", "MainPort", m_oplink->MainPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); 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", "MinFrequency", m_oplink->MinFrequency); addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaxFrequency); + addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); addUAVObjectToWidgetRelation("OPLinkStatus", "RxCorrected", m_oplink->Corrected); addUAVObjectToWidgetRelation("OPLinkStatus", "RxErrors", m_oplink->Errors); @@ -79,27 +80,89 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget 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", "HeapRemaining", m_oplink->FreeHeap); 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); - // Connect to the pair ID radio buttons. - connect(m_oplink->PairSelect1, SIGNAL(toggled(bool)), this, SLOT(pairIDToggled(bool))); - connect(m_oplink->PairSelect2, SIGNAL(toggled(bool)), this, SLOT(pairIDToggled(bool))); - connect(m_oplink->PairSelect3, SIGNAL(toggled(bool)), this, SLOT(pairIDToggled(bool))); - connect(m_oplink->PairSelect4, SIGNAL(toggled(bool)), this, SLOT(pairIDToggled(bool))); - - // Connect to the binding buttons. - connect(m_oplink->BindingAdd, SIGNAL(pressed()), this, SLOT(addBinding())); - connect(m_oplink->BindingRemove, SIGNAL(pressed()), this, SLOT(removeBinding())); - m_oplink->BindingAdd->setEnabled(false); - m_oplink->BindingRemove->setEnabled(false); - - // Connect to changes to the bindings widgets. - connect(m_oplink->Bindings, SIGNAL(itemSelectionChanged()), this, SLOT(bindingsSelectionChanged())); + signalMapperAddBinding = new QSignalMapper(this); + signalMapperRemBinding = new QSignalMapper(this); + connect(m_oplink->BindingAdd_1, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_1, (QWidget*)(m_oplink->BindingID_1)); + connect(m_oplink->BindingRemove_1, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_1, (QWidget*)(m_oplink->BindingID_1)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_1, "0"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_1, "0"); + connect(m_oplink->BindingAdd_2, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_2, (QWidget*)(m_oplink->BindingID_2)); + connect(m_oplink->BindingRemove_2, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_2, (QWidget*)(m_oplink->BindingID_2)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_2, "1"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_2, "1"); + connect(m_oplink->BindingAdd_3, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_3, (QWidget*)(m_oplink->BindingID_3)); + connect(m_oplink->BindingRemove_3, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_3, (QWidget*)(m_oplink->BindingID_3)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_3, "2"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_3, "2"); + connect(m_oplink->BindingAdd_4, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_4, (QWidget*)(m_oplink->BindingID_4)); + connect(m_oplink->BindingRemove_4, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_4, (QWidget*)(m_oplink->BindingID_4)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_4, "3"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_4, "3"); + connect(m_oplink->BindingAdd_5, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_5, (QWidget*)(m_oplink->BindingID_5)); + connect(m_oplink->BindingRemove_5, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_5, (QWidget*)(m_oplink->BindingID_5)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_5, "4"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_5, "4"); + connect(m_oplink->BindingAdd_6, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_6, (QWidget*)(m_oplink->BindingID_6)); + connect(m_oplink->BindingRemove_6, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_6, (QWidget*)(m_oplink->BindingID_6)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_6, "5"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_6, "5"); + connect(m_oplink->BindingAdd_7, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_7, (QWidget*)(m_oplink->BindingID_7)); + connect(m_oplink->BindingRemove_7, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_7, (QWidget*)(m_oplink->BindingID_7)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_7, "6"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_7, "6"); + connect(m_oplink->BindingAdd_8, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); + signalMapperAddBinding->setMapping(m_oplink->BindingAdd_8, (QWidget*)(m_oplink->BindingID_8)); + connect(m_oplink->BindingRemove_8, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); + signalMapperRemBinding->setMapping(m_oplink->BindingRemove_8, (QWidget*)(m_oplink->BindingID_8)); + addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_8, "7"); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_8, "7"); + connect(signalMapperAddBinding, SIGNAL(mapped(QWidget *)), this, SLOT(addBinding(QWidget *))); + connect(signalMapperRemBinding, SIGNAL(mapped(QWidget *)), this, SLOT(removeBinding(QWidget *))); //Add scroll bar when necessary QScrollArea *scroll = new QScrollArea; @@ -115,37 +178,8 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget ConfigPipXtremeWidget::~ConfigPipXtremeWidget() { - // Do nothing -} - -void ConfigPipXtremeWidget::refreshValues() -{ -} - -void ConfigPipXtremeWidget::applySettings() -{ - OPLinkSettings *oplinkSettings = OPLinkSettings::GetInstance(getObjectManager()); - OPLinkSettings::DataFields oplinkSettingsData = oplinkSettings->getData(); - - // Get the pair ID. - quint32 pairID = 0; - bool okay; - 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); - oplinkSettings->setData(oplinkSettingsData); -} - -void ConfigPipXtremeWidget::saveSettings() -{ - //applySettings(); - UAVObject *obj = OPLinkSettings::GetInstance(getObjectManager()); - saveObjectToSD(obj); + delete signalMapperAddBinding; + delete signalMapperRemBinding; } /*! @@ -158,32 +192,24 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) if (!settingsUpdated) oplinkSettingsObj->requestUpdate(); - // Get the current pairID - //OPLinkSettings *oplinkSettings = OPLinkSettings::GetInstance(getObjectManager()); - quint32 pairID = 0; - // Update the detected devices. UAVObjectField* pairIdField = object->getField("PairIDs"); if (pairIdField) { quint32 pairid1 = pairIdField->getValue(0).toUInt(); 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_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_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_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."; @@ -249,14 +275,6 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) qDebug() << "PipXtremeGadgetWidget: Count not read Description field."; } - // Update the DeviceID field - UAVObjectField* idField = object->getField("DeviceID"); - if (idField) { - m_oplink->DeviceID->setText(QString::number(idField->getValue().toUInt(), 16).toUpper()); - } else { - qDebug() << "PipXtremeGadgetWidget: Count not read DeviceID field."; - } - // Update the link state UAVObjectField* linkField = object->getField("LinkState"); if (linkField) { @@ -277,9 +295,6 @@ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) { settingsUpdated = true; enableControls(true); - - // Refresh the list widget. - refreshBindingsList(); } } @@ -292,88 +307,32 @@ void ConfigPipXtremeWidget::disconnected() } } -void ConfigPipXtremeWidget::pairIDToggled(bool checked) +void ConfigPipXtremeWidget::addBinding(QWidget *w) { - bool enabled = (m_oplink->PairSelect1->isChecked() || m_oplink->PairSelect2->isChecked() || m_oplink->PairSelect3->isChecked() || m_oplink->PairSelect4->isChecked()); - m_oplink->BindingAdd->setEnabled(enabled); + if(QLineEdit *le = qobject_cast(w)) { + + // Get the pair ID out of the selection widget + quint32 pairID = 0; + bool okay; + 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); + + // Store the ID in the first open slot (or the last slot if all are full). + le->setText(QString::number(pairID, 16).toUpper()); + } } -void ConfigPipXtremeWidget::bindingsSelectionChanged() +void ConfigPipXtremeWidget::removeBinding(QWidget *w) { - bool enabled = (m_oplink->Bindings->selectedItems().size() > 0); - m_oplink->BindingRemove->setEnabled(enabled); -} - -void ConfigPipXtremeWidget::refreshBindingsList() -{ - m_oplink->Bindings->clear(); - for (quint32 slot = 0; slot < OPLinkSettings::BINDINGS_NUMELEM; ++slot) - { - quint32 id = oplinkSettingsObj->getBindings(slot); - if (id != 0) - m_oplink->Bindings->addItem(QString::number(id, 16).toUpper()); - } -} - -void ConfigPipXtremeWidget::addBinding() -{ - - // Get the pair ID out of the selection widget - quint32 pairID = 0; - bool okay; - 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); - - // Find a slot for this pair ID in the binding list. - quint32 slot = 0; - for ( ; slot < OPLinkSettings::BINDINGS_NUMELEM - 1; ++slot) - { - quint32 id = oplinkSettingsObj->getBindings(slot); - - // Is this ID already in the list? - if (id == pairID) - return; - - // Is this slot empty? - if (id == 0) - break; - } - - // Store the ID in the first open slot (or the last slot if all are full). - oplinkSettingsObj->setBindings(slot, pairID); - - // Refresh the list widget. - refreshBindingsList(); -} - -void ConfigPipXtremeWidget::removeBinding() -{ - // Get the selected rows from the bindings list widget. - QList selected = m_oplink->Bindings->selectedItems(); - - // Zero out the selected rows in the bindings list. - for (int i = 0; i < selected.size(); ++i) - oplinkSettingsObj->setBindings(m_oplink->Bindings->row(selected[i]), 0); - - // Refresh the list widget and compact the bindings list. - m_oplink->Bindings->clear(); - quint32 outSlot = 0; - for (quint32 slot = 0; slot < OPLinkSettings::BINDINGS_NUMELEM; ++slot) - { - quint32 id = oplinkSettingsObj->getBindings(slot); - if (id != 0) { - oplinkSettingsObj->setBindings(outSlot++, id); - m_oplink->Bindings->addItem(QString::number(id, 16).toUpper()); - } - } - for ( ; outSlot < OPLinkSettings::BINDINGS_NUMELEM; ++outSlot) - oplinkSettingsObj->setBindings(outSlot, 0); + if(QLineEdit *le = qobject_cast(w)) { + le->setText(QString::number(0, 16).toUpper()); + } } /** diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index 29e67ccba..9a612bf94 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -43,28 +43,27 @@ public: public slots: void updateStatus(UAVObject *object1); void updateSettings(UAVObject *object1); - void refreshBindingsList(); private: Ui_PipXtremeWidget *m_oplink; // The OPLink status UAVObject - UAVDataObject* oplinkStatusObj; + UAVDataObject *oplinkStatusObj; // The OPLink ssettins UAVObject OPLinkSettings* oplinkSettingsObj; + // Are the settings current? bool settingsUpdated; + // Signal mappers to add arguments to signals. + QSignalMapper *signalMapperAddBinding; + QSignalMapper *signalMapperRemBinding; + private slots: - void refreshValues(); - void applySettings(); - void saveSettings(); void disconnected(); - void pairIDToggled(bool checked); - void addBinding(); - void removeBinding(); - void bindingsSelectionChanged(); + void addBinding(QWidget *w); + void removeBinding(QWidget *w); }; #endif // CONFIGTXPIDWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 6d7f36b8f..0636e5fcf 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -23,36 +23,10 @@ QFrame::Raised - - - - - - - - 0 - 0 - 0 - - - - - - - 0 - 0 - 0 - - - - - true - - - - + + - + @@ -139,7 +113,7 @@ - + QFrame::StyledPanel @@ -278,7 +252,7 @@ - + Device ID @@ -288,7 +262,7 @@ - + @@ -332,64 +306,7 @@ - - - - RX Good - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - The percentage of packets that were 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 - - - - + Link State @@ -399,7 +316,7 @@ - + @@ -449,7 +366,337 @@ - + + + + Link Quality + + + 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 + + + + + + + RSSI + + + 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;*/ + } + + + true + + + + + + + TX 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 + + + + + + + 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) + + + 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 + + + + + + + RX Good + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + The percentage of packets that were 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 Corrected @@ -459,7 +706,7 @@ - + @@ -506,49 +753,7 @@ - - - - Link Quality - - - 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 - - - - + RX Errors @@ -558,7 +763,7 @@ - + @@ -599,52 +804,7 @@ - - - - RSSI - - - 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;*/ - } - - - true - - - - + RX Missed @@ -654,7 +814,7 @@ - + @@ -701,18 +861,162 @@ - - + + - AFC Corr. + 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 + + + + + + + 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 + + + + + + + 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 + + + + + + + Free Heap + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + 101 @@ -740,6 +1044,138 @@ + + + + UAVTalk Errors + + + 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 + + + + + + + + 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 + + + + + + + Resets + + + 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 + + + + + + + Timeouts + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + @@ -797,816 +1233,10 @@ - - - - TX 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 - - - - - - - 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 - - - - - - - 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 - - - - - - - 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 - - - - - - - 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 - - - - - - - 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 - - - - - - - RX Rate (B/s) - - - 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 - - - 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 - - - - - - - Timeouts - - - 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 - - - - - - - Resets - - - 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 - - - - - - - - - - - 0 - 0 - - - - - 75 - true - - - - Configuration - - - - - - UAVTalk - - - - - - - PPM - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Choose the PPM function - - - - - - - Input Connection - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Choose which port to communicate over on this modem - - - - - - - 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 - 16777215 - - - - Set the telemetry port speed - - - - - - - Max RF Tx Power(mW) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Set the maximum TX output power the modem will use - - - Qt::LeftToRight - - - 0 - - - - - - - Send Timeout (ms) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Set the send timeout - - - true - - - 255 - - - - - - - Min Packet Size - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Set the minimum packet size - - - true - - - 255 - - - - - - - Frequency Calibration - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Calibrate the modems RF carrier frequency - - - true - - - 255 - - - - - - - Min. Frequency (Hz) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Set the modems minimum RF carrier frequency - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 400000000 - - - 1000000000 - - - 100000 - - - - - - - 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 - - - - groupBox - groupBox_3 @@ -1618,7 +1248,7 @@ - Bindings + Remote Modems @@ -1815,70 +1445,1248 @@ - - - - - 0 - 0 - + + + + + + + + 75 + true + + + + Bindings + + + + + + Add - - QFrame::StyledPanel + + Qt::AlignCenter - - QFrame::Raised - - - - - - Add >> - - - - - - - Remove - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - - - - + + + + Clear + + + Qt::AlignCenter + + + + + + + Device ID + + + Qt::AlignCenter + + + + + + + Main Port + + + Qt::AlignCenter + + + + + + + Flexi Port + + + Qt::AlignCenter + + + + + + + VCP Port + + + Qt::AlignCenter + + + + + + + COM Speed + + + Qt::AlignCenter + + + + + + + true + + + + + 0 + 0 + 757 + 244 + + + + + + + Add >> + + + + + + + Clear + + + + + + + + 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 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 12345678 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote main port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote flexi port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote USB virtual com port + + + + + + + + 16777215 + 16777215 + + + + Set the telemetry port speed + + + + + + + Add >> + + + + + + + Clear + + + + + + + + 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 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 12345678 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote main port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote flexi port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote USB virtual com port + + + + + + + + 16777215 + 16777215 + + + + Set the telemetry port speed + + + + + + + Add >> + + + + + + + Clear + + + + + + + + 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 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 12345678 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote main port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote flexi port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote USB virtual com port + + + + + + + + 16777215 + 16777215 + + + + Set the telemetry port speed + + + + + + + Add >> + + + + + + + Clear + + + + + + + + 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 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 12345678 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote main port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote flexi port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote USB virtual com port + + + + + + + + 16777215 + 16777215 + + + + Set the telemetry port speed + + + + + + + Add >> + + + + + + + Clear + + + + + + + + 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 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 12345678 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote main port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote flexi port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote USB virtual com port + + + + + + + + 16777215 + 16777215 + + + + Set the telemetry port speed + + + + + + + Add >> + + + + + + + Clear + + + + + + + + 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 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 12345678 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote main port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote flexi port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote USB virtual com port + + + + + + + + 16777215 + 16777215 + + + + Set the telemetry port speed + + + + + + + Add >> + + + + + + + Clear + + + + + + + + 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 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 12345678 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote main port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote flexi port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote USB virtual com port + + + + + + + + 16777215 + 16777215 + + + + Set the telemetry port speed + + + + + + + Add >> + + + + + + + Clear + + + + + + + + 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 + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 12345678 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote main port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote flexi port + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the remote USB virtual com port + + + + + + + + 16777215 + 16777215 + + + + Set the telemetry port speed + + + + + + + + + + + + + + + 0 + 0 + + + + + 75 + true + + + + Configuration + + + + + + Main Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the main port + + + + + + + Flexi Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the flexi port + + + + + + + VCP Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the USB virtual com port + + + + + + + Max RF Tx Power(mW) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 16777215 + 16777215 + + + + Set the maximum TX output power the modem will use + + + Qt::LeftToRight + + + 0 + + + + + + + Frequency Calibration + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + - + 0 0 - 200 + 16777215 16777215 + + Calibrate the modems RF carrier frequency + + + true + + + 255 + + + + + + + Min. Frequency (Hz) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + Set the modems minimum RF carrier frequency + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 400000000 + + + 1000000000 + + + 100000 + + + + + + + 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 + + + + + Qt::Vertical + + + + 20 + 40 + + + + @@ -1895,18 +2703,8 @@ PairID4 FirmwareVersion SerialNumber - DeviceID - LinkState - Errors - UAVTalkErrors - Resets - TXRate - RXRate MaxRFTxPower - SendTimeout - MinPacketSize FrequencyCalibration - graphicsView_Spectrum Apply Save diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp index 85b0f721f..89260ff30 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp @@ -886,10 +886,9 @@ QVariant UAVObjectField::getValue(quint32 index) { quint8 tmpenum; memcpy(&tmpenum, &data[offset + numBytesPerElement*index], numBytesPerElement); - // Q_ASSERT((tmpenum < options.length()) && (tmpenum >= 0)); // catch bad enum settings if(tmpenum >= options.length()) { qDebug() << "Invalid value for" << name; - return QVariant( QString("Bad Value") ); + tmpenum = 0; } return QVariant( options[tmpenum] ); break; @@ -1015,7 +1014,9 @@ void UAVObjectField::setValue(const QVariant& value, quint32 index) case ENUM: { qint8 tmpenum = options.indexOf( value.toString() ); - Q_ASSERT(tmpenum >= 0); // To catch any programming errors where we set invalid values + // Default to 0 on invalid values. + if(tmpenum < 0) + tmpenum = 0; memcpy(&data[offset + numBytesPerElement*index], &tmpenum, numBytesPerElement); break; } diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index b6fcabecf..08f6cde02 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -457,11 +457,11 @@ void ConfigTaskWidget::forceShadowUpdates() { foreach (shadow * sh, oTw->shadowsList) { - disconnectWidgetUpdatesToSlot((QWidget*)sh->widget,SLOT(widgetsContentsChanged())); - checkWidgetsLimits(sh->widget,oTw->field,oTw->index,sh->isLimited,getVariantFromWidget(oTw->widget,oTw->scale),sh->scale); - setWidgetFromVariant(sh->widget,getVariantFromWidget(oTw->widget,oTw->scale),sh->scale); + disconnectWidgetUpdatesToSlot((QWidget*)sh->widget, SLOT(widgetsContentsChanged())); + checkWidgetsLimits(sh->widget, oTw->field, oTw->index, sh->isLimited, getVariantFromWidget(oTw->widget, oTw->scale, oTw->getUnits()), sh->scale); + setWidgetFromVariant(sh->widget, getVariantFromWidget(oTw->widget, oTw->scale, oTw->getUnits()), sh->scale); emit widgetContentsChanged((QWidget*)sh->widget); - connectWidgetUpdatesToSlot((QWidget*)sh->widget,SLOT(widgetsContentsChanged())); + connectWidgetUpdatesToSlot((QWidget*)sh->widget, SLOT(widgetsContentsChanged())); } } @@ -480,7 +480,7 @@ void ConfigTaskWidget::widgetsContentsChanged() if(oTw->widget==(QWidget*)sender()) { scale=oTw->scale; - checkWidgetsLimits((QWidget*)sender(),oTw->field,oTw->index,oTw->isLimited,getVariantFromWidget((QWidget*)sender(),oTw->scale),oTw->scale); + checkWidgetsLimits((QWidget*)sender(), oTw->field, oTw->index, oTw->isLimited, getVariantFromWidget((QWidget*)sender(), oTw->scale, oTw->getUnits()), oTw->scale); } else { @@ -489,25 +489,25 @@ void ConfigTaskWidget::widgetsContentsChanged() if(sh->widget==(QWidget*)sender()) { scale=sh->scale; - checkWidgetsLimits((QWidget*)sender(),oTw->field,oTw->index,sh->isLimited,getVariantFromWidget((QWidget*)sender(),scale),scale); + checkWidgetsLimits((QWidget*)sender(), oTw->field, oTw->index, sh->isLimited, getVariantFromWidget((QWidget*)sender(), scale, oTw->getUnits()), scale); } } } if(oTw->widget!=(QWidget *)sender()) { - disconnectWidgetUpdatesToSlot((QWidget*)oTw->widget,SLOT(widgetsContentsChanged())); - checkWidgetsLimits(oTw->widget,oTw->field,oTw->index,oTw->isLimited,getVariantFromWidget((QWidget*)sender(),scale),oTw->scale); - setWidgetFromVariant(oTw->widget,getVariantFromWidget((QWidget*)sender(),scale),oTw->scale); + disconnectWidgetUpdatesToSlot((QWidget*)oTw->widget, SLOT(widgetsContentsChanged())); + checkWidgetsLimits(oTw->widget, oTw->field, oTw->index, oTw->isLimited, getVariantFromWidget((QWidget*)sender(), scale, oTw->getUnits()), oTw->scale); + setWidgetFromVariant(oTw->widget, getVariantFromWidget((QWidget*)sender(), scale, oTw->getUnits()), oTw->scale); emit widgetContentsChanged((QWidget*)oTw->widget); - connectWidgetUpdatesToSlot((QWidget*)oTw->widget,SLOT(widgetsContentsChanged())); + connectWidgetUpdatesToSlot((QWidget*)oTw->widget, SLOT(widgetsContentsChanged())); } foreach (shadow * sh, oTw->shadowsList) { if(sh->widget!=(QWidget*)sender()) { disconnectWidgetUpdatesToSlot((QWidget*)sh->widget,SLOT(widgetsContentsChanged())); - checkWidgetsLimits(sh->widget,oTw->field,oTw->index,sh->isLimited,getVariantFromWidget((QWidget*)sender(),scale),sh->scale); - setWidgetFromVariant(sh->widget,getVariantFromWidget((QWidget*)sender(),scale),sh->scale); + checkWidgetsLimits(sh->widget, oTw->field, oTw->index, sh->isLimited, getVariantFromWidget((QWidget*)sender(), scale, oTw->getUnits()), sh->scale); + setWidgetFromVariant(sh->widget, getVariantFromWidget((QWidget*)sender(), scale, oTw->getUnits()), sh->scale); emit widgetContentsChanged((QWidget*)sh->widget); connectWidgetUpdatesToSlot((QWidget*)sh->widget,SLOT(widgetsContentsChanged())); } @@ -1034,7 +1034,7 @@ bool ConfigTaskWidget::setFieldFromWidget(QWidget * widget,UAVObjectField * fiel { if(!widget || !field) return false; - QVariant ret=getVariantFromWidget(widget,scale); + QVariant ret = getVariantFromWidget(widget, scale, field->getUnits()); if(ret.isValid()) { field->setValue(ret,index); @@ -1051,7 +1051,7 @@ bool ConfigTaskWidget::setFieldFromWidget(QWidget * widget,UAVObjectField * fiel * @param scale scale to be used on the assignement * @return returns the value of the widget times the scale */ -QVariant ConfigTaskWidget::getVariantFromWidget(QWidget * widget,double scale) +QVariant ConfigTaskWidget::getVariantFromWidget(QWidget *widget, double scale, QString units) { if(QComboBox * cb=qobject_cast(widget)) { @@ -1075,7 +1075,13 @@ QVariant ConfigTaskWidget::getVariantFromWidget(QWidget * widget,double scale) } else if(QLineEdit * cb=qobject_cast(widget)) { - return (QString)cb->displayText(); + QString value = (QString)cb->displayText(); + if(units == "hex") { + bool ok; + return value.toUInt(&ok, 16); + } else { + return value; + } } else return QVariant(); @@ -1083,13 +1089,14 @@ QVariant ConfigTaskWidget::getVariantFromWidget(QWidget * widget,double scale) /** * Sets a widget from a variant * @param widget pointer for the widget to set - * @param scale scale to be used on the assignement * @param value value to be used on the assignement + * @param scale scale to be used on the assignement + * @param units the units for the value * @return returns true if the assignement was successfull */ -bool ConfigTaskWidget::setWidgetFromVariant(QWidget *widget, QVariant value, double scale) +bool ConfigTaskWidget::setWidgetFromVariant(QWidget *widget, QVariant value, double scale, QString units) { - if(QComboBox * cb=qobject_cast(widget)) + if(QComboBox * cb=qobject_cast(widget)) { cb->setCurrentIndex(cb->findText(value.toString())); return true; @@ -1125,15 +1132,30 @@ bool ConfigTaskWidget::setWidgetFromVariant(QWidget *widget, QVariant value, dou } else if(QLineEdit * cb=qobject_cast(widget)) { - if(scale==0) - cb->setText(value.toString()); - else + if((scale== 0) || (scale == 1)) { + if(units == "hex") { + cb->setText(QString::number(value.toUInt(), 16).toUpper()); + } else { + cb->setText(value.toString()); + } + } else cb->setText(QString::number((value.toDouble()/scale))); return true; } else return false; } +/** + * Sets a widget from a variant + * @param widget pointer for the widget to set + * @param value value to be used on the assignement + * @param scale scale to be used on the assignement + * @return returns true if the assignement was successfull + */ +bool ConfigTaskWidget::setWidgetFromVariant(QWidget *widget, QVariant value, double scale) +{ + return setWidgetFromVariant(widget, value, scale, QString("")); +} /** * Sets a widget from a UAVObject field * @param widget pointer to the widget to set @@ -1154,7 +1176,7 @@ bool ConfigTaskWidget::setWidgetFromField(QWidget * widget,UAVObjectField * fiel } QVariant var=field->getValue(index); checkWidgetsLimits(widget,field,index,hasLimits,var,scale); - bool ret=setWidgetFromVariant(widget,var,scale); + bool ret = setWidgetFromVariant(widget, var, scale, field->getUnits()); if(ret) return true; else diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h index f25292143..d3c5c21cf 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h @@ -68,6 +68,13 @@ public: double scale; bool isLimited; QList shadowsList; + QString getUnits() const { + if (field) { + return field->getUnits(); + } else { + return QString(""); + } + } }; struct temphelper @@ -181,8 +188,9 @@ private: bool dirty; bool setFieldFromWidget(QWidget *widget, UAVObjectField *field, int index, double scale); bool setWidgetFromField(QWidget *widget, UAVObjectField *field, int index, double scale, bool hasLimits); - QVariant getVariantFromWidget(QWidget *widget, double scale); - bool setWidgetFromVariant(QWidget *widget,QVariant value,double scale); + QVariant getVariantFromWidget(QWidget *widget, double scale, QString units); + bool setWidgetFromVariant(QWidget *widget, QVariant value, double scale, QString units); + bool setWidgetFromVariant(QWidget *widget, QVariant value, double scale); void connectWidgetUpdatesToSlot(QWidget *widget, const char *function); void disconnectWidgetUpdatesToSlot(QWidget *widget, const char *function); void loadWidgetLimits(QWidget *widget, UAVObjectField *field, int index, bool hasLimits, double sclale); diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index f24bba648..7c7403c87 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -1,15 +1,15 @@ OPLink configurations options. - - - - - - + + + + + + + + - - diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml index 619ab1e81..1e79630fe 100755 --- a/shared/uavobjectdefinition/oplinkstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -2,10 +2,11 @@ OPLink device status. - + - + + @@ -18,14 +19,13 @@ - - + From 1af58e510e6b7c61b861086658ea6fde55a9563d Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 18 Feb 2013 02:55:58 +0000 Subject: [PATCH 04/40] Added optional output of GCSReceiver from the OPLink to the FC. This will happen automatically if a PPM packet is receive, and PPM output is disabled. --- .../Modules/RadioComBridge/RadioComBridge.c | 4 ++- flight/PiOS/Common/pios_rfm22b.c | 36 ++++++++++++++----- flight/PiOS/inc/pios_rfm22b_priv.h | 5 --- flight/PipXtreme/System/inc/pios_config.h | 28 +-------------- 4 files changed, 31 insertions(+), 42 deletions(-) diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index 0e48df851..ef836d59c 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -203,7 +203,6 @@ static int32_t RadioComBridgeInitialize(void) return -1; // Initialize the UAVObjects that we use - GCSReceiverInitialize(); OPLinkStatusInitialize(); ObjectPersistenceInitialize(); @@ -217,6 +216,9 @@ 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); +#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) + UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); +#endif // Initialize the statistics. data->comTxErrors = 0; diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 672f9332c..2ae6e5896 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -53,6 +53,9 @@ #include #include +#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) +#include +#endif #include #include #include @@ -611,6 +614,11 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24; DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID); +#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) + // Initialize the GCSReceive object + GCSReceiverInitialize(); +#endif + // Initialize the external interrupt. PIOS_EXTI_Init(cfg->exti_cfg); @@ -1776,16 +1784,26 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) 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]; + PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet); + bool ppm_output = false; #if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT) - if (PIOS_PPM_OUTPUT) - PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]); -#endif /* PIOS_INCLUDE_PPM_OUT && PIOS_PPM_OUTPUT */ - } - rfm22b_dev->ppm_fresh = true; - break; + if (PIOS_PPM_OUTPUT) { + ppm_output = true; + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { + PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]); + } + } +#endif +#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) + if (!ppm_output) { + GCSReceiverData gcsRcvr; + for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { + gcsRcvr.Channel[i] = ppmp->channels[i]; + } + GCSReceiverSet(&gcsRcvr); + } +#endif + break; } default: break; diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 1ac46b345..8fbff558b 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -793,11 +793,6 @@ 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/PipXtreme/System/inc/pios_config.h b/flight/PipXtreme/System/inc/pios_config.h index 94893c54b..53bcc3e9d 100755 --- a/flight/PipXtreme/System/inc/pios_config.h +++ b/flight/PipXtreme/System/inc/pios_config.h @@ -40,6 +40,7 @@ #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_RFM22B #define PIOS_INCLUDE_RFM22B_COM +#define PIOS_INCLUDE_RFM22B_GCSRECEIVER #define PIOS_INCLUDE_RCVR #define PIOS_INCLUDE_TIM @@ -70,33 +71,6 @@ #define LOG_FILENAME "PIOS.LOG" #define STARTUP_LOG_ENABLED 1 -/* COM Module */ -#define GPS_BAUDRATE 19200 -#define TELEM_BAUDRATE 19200 -#define AUXUART_ENABLED 0 -#define AUXUART_BAUDRATE 19200 - -/* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 220 -#define HEAP_LIMIT_CRITICAL 40 -#define IRQSTACK_LIMIT_WARNING 100 -#define IRQSTACK_LIMIT_CRITICAL 60 -#define CPULOAD_LIMIT_WARNING 85 -#define CPULOAD_LIMIT_CRITICAL 95 - -/* Task stack sizes */ -#define PIOS_ACTUATOR_STACK_SIZE 1020 -#define PIOS_MANUAL_STACK_SIZE 724 -#define PIOS_SYSTEM_STACK_SIZE 460 -#define PIOS_STABILIZATION_STACK_SIZE 524 -#define PIOS_TELEM_STACK_SIZE 500 -#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 -//#define PIOS_QUATERNION_STABILIZATION - -// This can't be too high to stop eventdispatcher thread overflowing -#define PIOS_EVENTDISAPTCHER_QUEUE 10 - /* PIOS Initcall infrastructure */ #define PIOS_INCLUDE_INITCALL From d9baa372c7760adf1b8338d172fff5475394ddd6 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 24 Feb 2013 23:05:51 +0000 Subject: [PATCH 05/40] Added frequency hopping to the RFM22B driver. Currently it defaults to 128 channels between 430 and 440 MHz. --- flight/Libraries/inc/packet_handler.h | 26 +- .../Modules/RadioComBridge/RadioComBridge.c | 3 - flight/PiOS/Common/pios_rfm22b.c | 277 ++++++++---------- flight/PiOS/inc/pios_rfm22b.h | 1 - flight/PiOS/inc/pios_rfm22b_priv.h | 99 ++++--- .../plugins/config/configpipxtremewidget.cpp | 3 - .../src/plugins/config/pipxtreme.ui | 137 --------- shared/uavobjectdefinition/oplinksettings.xml | 3 - 8 files changed, 191 insertions(+), 358 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 659cdac9e..423f07fc6 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -42,22 +42,21 @@ // Public types 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_DATA, // data packet (packet contains user data) - PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet - PACKET_TYPE_PPM, // PPM relay values - 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 + 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, // 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 { uint32_t destination_id; - uint32_t source_id; + uint16_t seq_num; uint8_t type; uint8_t data_size; - uint16_t seq_num; } PHPacketHeader; #define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY) @@ -83,6 +82,7 @@ typedef struct { #define PH_STATUS_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; + uint32_t source_id; uint8_t link_quality; int8_t received_rssi; uint8_t ecc[RS_ECC_NPARITY]; @@ -91,14 +91,12 @@ typedef struct { #define PH_CONNECTION_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - uint32_t frequency_hz; - uint32_t min_frequency; - uint32_t max_frequency; - uint8_t max_tx_power; + uint32_t source_id; OPLinkSettingsMainPortOptions main_port; OPLinkSettingsFlexiPortOptions flexi_port; OPLinkSettingsVCPPortOptions vcp_port; OPLinkSettingsComSpeedOptions com_speed; + uint8_t max_tx_power; uint8_t ecc[RS_ECC_NPARITY]; } PHConnectionPacket, *PHConnectionPacketHandle; diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index ef836d59c..2f7845ead 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -135,9 +135,6 @@ static int32_t RadioComBridgeStart(void) OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - // Set the frequencies. - PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency); - // Set the maximum radio RF power. switch (oplinkSettings.MaxRFPower) { diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 2ae6e5896..d9ce85e57 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -66,11 +66,14 @@ #define ISR_TIMEOUT 2 // ms #define EVENT_QUEUE_SIZE 5 #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_FREQUENCY 430000000 +#define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 +#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000 +#define RFM22B_NUM_CHANNELS 128 +#define RFM22B_DEFAULT_CHANNEL 28 #define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_7 #define RFM22B_LINK_QUALITY_THRESHOLD 20 +//#define RFM22B_TEST_DROPPED_PACKETS 1 // The maximum amount of time since the last message received to consider the connection broken. #define DISCONNECT_TIMEOUT_MS 1000 // ms @@ -146,8 +149,8 @@ struct pios_rfm22b_transition { }; // Must ensure these prefilled arrays match the define sizes -static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = - {PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, +static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = { + PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, @@ -161,7 +164,8 @@ static const uint8_t FULL_PREAMBLE[FIFO_SIZE] = PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE, PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE,PREAMBLE_BYTE}; // 64 bytes static const uint8_t HEADER[(TX_PREAMBLE_NIBBLES + 1)/2 + 2] = {PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE, PREAMBLE_BYTE,PREAMBLE_BYTE, PREAMBLE_BYTE, SYNC_BYTE_1, SYNC_BYTE_2}; -static const uint8_t OUT_FF[64] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, +static const uint8_t OUT_FF[64] = { + 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, @@ -200,7 +204,8 @@ 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); +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz, uint8_t channel); +static void rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel); 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); @@ -259,7 +264,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_ACCEPTING_CONNECTION] = { .entry_fn = rfm22_acceptConnection, .next_state = { - [RFM22B_EVENT_CONNECTION_ACCEPTED] = RFM22B_STATE_SENDING_ACK, + [RFM22B_EVENT_DEFAULT] = RFM22B_STATE_SENDING_ACK, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, @@ -311,6 +316,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_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_RX_ERROR] = RFM22B_STATE_SENDING_NACK, [RFM22B_EVENT_STATUS_RECEIVED] = RFM22B_STATE_RECEIVING_STATUS, [RFM22B_EVENT_CONNECTION_REQUESTED] = RFM22B_STATE_ACCEPTING_CONNECTION, @@ -420,7 +426,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_TIMEOUT] = { .entry_fn = rfm22_timeout, .next_state = { - [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, @@ -569,6 +574,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu // Initialize our configuration parameters rfm22b_dev->send_ppm = false; rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; + rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; // Initialize the com callbacks. rfm22b_dev->com_config_cb = NULL; @@ -588,6 +594,13 @@ 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 frequency hopping step size (specified in 10khz increments). + uint32_t freq_hop_step_size = RFM22B_FREQUENCY_HOP_STEP_SIZE / 10000; + if (freq_hop_step_size > 255) { + freq_hop_step_size = 255; + } + rfm22b_dev->frequency_hop_step_size_reg = (uint8_t)freq_hop_step_size; + // Initialize the bindings. for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) rfm22b_dev->bindings[i].pairID = 0; @@ -658,7 +671,7 @@ void PIOS_RFM22B_Reinit(uint32_t rfm22b_id) bool PIOS_RFM22_EXT_Int(void) { if (!PIOS_RFM22B_validate(g_rfm22b_dev)) - return false; + return false; // Inject an interrupt event into the state machine. PIOS_RFM22B_InjectEvent(g_rfm22b_dev, RFM22B_EVENT_INT_RECEIVED, true); @@ -704,9 +717,9 @@ void PIOS_RFM22B_InjectEvent(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22 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)) - return rfm22b_dev->deviceID; - else + if (PIOS_RFM22B_validate(rfm22b_dev)) { + return rfm22b_dev->deviceID; + } else return 0; } @@ -737,22 +750,6 @@ 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); -} - /** * Set the com port configuration callback (to receive com configuration over the air) * \param[in] rfm22b_id The rfm22b device. @@ -817,7 +814,7 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) { *stats = rfm22b_dev->stats; } - /** +/** * 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. @@ -860,7 +857,7 @@ static void PIOS_RFM22B_Task(void *parameters) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)parameters; if (!PIOS_RFM22B_validate(rfm22b_dev)) - return; + return; portTickType lastEventTicks = xTaskGetTickCount(); portTickType lastStatusTicks = lastEventTicks; portTickType lastPPMTicks = lastEventTicks; @@ -908,7 +905,7 @@ 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); - // Have it been too long since we received a packet + // Has 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); @@ -1085,13 +1082,13 @@ static void rfm22_write(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_ * 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(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data) -{ - uint8_t buf[2] = {addr | 0x80, data}; - rfm22_assertCs(rfm22b_dev); - PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); - rfm22_deassertCs(rfm22b_dev); -} + static void rfm22_write_noclaim(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr, uint8_t data) + { + uint8_t buf[2] = {addr | 0x80, data}; + rfm22_assertCs(rfm22b_dev); + PIOS_SPI_TransferBlock(rfm22b_dev->spi_id, buf, NULL, sizeof(buf), NULL); + rfm22_deassertCs(rfm22b_dev); + } */ /** @@ -1169,13 +1166,8 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rf // ************************************ -static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz) +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz, uint8_t channel) { - 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; if (frequency_hz < 480000000) @@ -1198,7 +1190,8 @@ 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(rfm22b_dev, RFM22_frequency_hopping_channel_select, rfm22b_dev->frequency_hop_channel); + rfm22b_dev->frequency_hop_channel = channel; + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); // no frequency offset rfm22_write(rfm22b_dev, RFM22_frequency_offset1, 0); @@ -1211,19 +1204,13 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, 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; +static void rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel) +{ + // set the frequency hopping channel + rfm22b_dev->frequency_hop_channel = channel; rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); } -static uint32_t rfm22_freqHopSize(void) -{ // return the frequency hopping step size - return ((uint32_t)g_rfm22b_dev->frequency_hop_step_size_reg * 10000); -} -*/ - static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) { // Add the RX packet statistics @@ -1405,7 +1392,6 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) #endif // Add the error correcting code. - p->header.source_id = rfm22b_dev->deviceID; encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); rfm22b_dev->tx_packet = p; @@ -1440,7 +1426,7 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) // set the 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); + RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | RFM22B_DEFAULT_TX_POWER); // clear FIFOs rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, RFM22_opfc2_ffclrrx | RFM22_opfc2_ffclrtx); @@ -1490,6 +1476,7 @@ static void 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.source_id = rfm22b_dev->deviceID; 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; @@ -1771,6 +1758,16 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) bool 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); +#ifdef RFM22B_TEST_DROPPED_PACKETS + // Inject radnom missed ACKs + { + static uint8_t crc = 0; + static uint8_t cntr = 0; + crc = PIOS_CRC_updateByte(crc, cntr++); + if ((crc & 0x7) == 0) + ret_event = RFM22B_EVENT_RX_MODE; + } +#endif break; } case PACKET_TYPE_DUPLICATE_DATA: @@ -1784,30 +1781,31 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) break; case PACKET_TYPE_PPM: { - PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet); - bool ppm_output = false; + PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet); + bool ppm_output = false; #if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT) - if (PIOS_PPM_OUTPUT) { - ppm_output = true; - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { - PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]); - } - } + if (PIOS_PPM_OUTPUT) { + ppm_output = true; + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { + PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]); + } + } #endif #if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) - if (!ppm_output) { - GCSReceiverData gcsRcvr; - for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { - gcsRcvr.Channel[i] = ppmp->channels[i]; - } - GCSReceiverSet(&gcsRcvr); - } + if (!ppm_output) { + GCSReceiverData gcsRcvr; + for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { + gcsRcvr.Channel[i] = ppmp->channels[i]; + } + GCSReceiverSet(&gcsRcvr); + } #endif - break; + break; } default: break; } + } else ret_event = RFM22B_EVENT_RX_ERROR; @@ -1851,10 +1849,6 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) if (!rfm22_readStatus(rfm22b_dev)) return RFM22B_EVENT_FAILURE; - // 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) { @@ -1879,7 +1873,6 @@ 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 = RFM22B_EVENT_RX_MODE; if (is_ack) { @@ -1888,6 +1881,8 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) rfm22_setConnectionParameters(rfm22b_dev); + // Change the channel + rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(rfm22b_dev->rx_packet.header.seq_num, 0) & 0x7f); } else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) { @@ -1929,7 +1924,7 @@ static enum pios_rfm22b_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev 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.destination_id = rfm22b_dev->destination_id; 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; @@ -1945,7 +1940,7 @@ 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) { PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); - aph->header.destination_id = rfm22b_dev->rx_packet.header.source_id; + aph->header.destination_id = rfm22b_dev->destination_id; aph->header.type = PACKET_TYPE_NACK; aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); aph->header.seq_num = rfm22b_dev->rx_packet.header.seq_num; @@ -1977,6 +1972,12 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de break; } + // Reset the resend count + rfm22b_dev->cur_resent_count = 0; + + // Change the channel + rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(prev->header.seq_num, 0) & 0x7f); + // Should we try to start another TX? if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) { @@ -2007,10 +2008,10 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); // Increment the current binding index to the next non-zero binding. for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - if (++(rfm22b_dev->cur_binding) >= OPLINKSETTINGS_BINDINGS_NUMELEM) - rfm22b_dev->cur_binding = 0; - if (rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID != 0) - break; + if (++(rfm22b_dev->cur_binding) >= OPLINKSETTINGS_BINDINGS_NUMELEM) + rfm22b_dev->cur_binding = 0; + if (rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID != 0) + break; } rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; cph->header.destination_id = rfm22b_dev->destination_id; @@ -2018,6 +2019,12 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; + } else { + // Switch to the fallback channel if we have resent too many packets. + if (rfm22b_dev->cur_resent_count++ > 2) { + rfm22b_dev->cur_resent_count = 0; + rfm22_setFreqHopChannel(rfm22b_dev, RFM22B_DEFAULT_CHANNEL); + } } // Increment the reset packet counter if we're connected. @@ -2036,7 +2043,7 @@ static enum pios_rfm22b_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b 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; + uint32_t id = status->source_id; // Have we seen this device recently? bool found = false; @@ -2098,13 +2105,11 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf 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->source_id = rfm22b_dev->deviceID; cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; - 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->time_to_send = true; rfm22b_dev->send_connection_request = true; @@ -2126,9 +2131,6 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) // Configure this modem from the connection request message. rfm22_setDatarate(rfm22b_dev, rfm22b_dev->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) @@ -2142,9 +2144,9 @@ static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm memcpy((uint8_t*)lcph, (uint8_t*)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); // Set the destination ID to the source of the connection request message. - rfm22b_dev->destination_id = cph->header.source_id; + rfm22b_dev->destination_id = cph->source_id; - return RFM22B_EVENT_CONNECTION_ACCEPTED; + return RFM22B_EVENT_DEFAULT; } // ************************************ @@ -2177,12 +2179,12 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // Initialize the state 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; + rfm22b_dev->cur_resent_count = 0; // Initialize the packets. rfm22b_dev->rx_packet_len = 0; @@ -2193,6 +2195,16 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->data_packet.header.data_size = 0; rfm22b_dev->in_rx_mode = false; + // Initialize the devide state + rfm22b_dev->device_status = rfm22b_dev->int_status1 = rfm22b_dev->int_status2 = rfm22b_dev->ezmac_status = 0; + rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->frequency_hop_channel = 0; + rfm22b_dev->afc_correction_Hz = 0; + rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->tx_complete_ticks = 0; + rfm22b_dev->rx_complete_ticks = 0; + // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); @@ -2222,23 +2234,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); - // **************** - - rfm22b_dev->device_status = rfm22b_dev->int_status1 = rfm22b_dev->int_status2 = rfm22b_dev->ezmac_status = 0; - - rfm22b_dev->rx_buffer_wr = 0; - - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; - - rfm22b_dev->frequency_hop_channel = 0; - - 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 // read the device type @@ -2268,18 +2263,9 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) return RFM22B_EVENT_FATAL_ERROR; } - // **************** - // set the minimum and maximum carrier frequency allowed - 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 rfm22_write(rfm22b_dev, RFM22_xtal_osc_load_cap, OSC_LOAD_CAP); - // **************** - // disable Low Duty Cycle Mode rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); @@ -2293,21 +2279,18 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // GPIO port use default value rfm22_write(rfm22b_dev, RFM22_io_port_config, RFM22_io_port_default); if (rfm22b_dev->cfg.gpio_direction == GPIO0_TX_GPIO1_RX) { - 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) + // GPIO0 = TX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_txstate); + // GPIO1 = RX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_rxstate); } else { - 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) + // GPIO0 = TX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio0_config, RFM22_gpio0_config_drv3 | RFM22_gpio0_config_rxstate); + // GPIO1 = RX State (to control RF Switch) + rfm22_write(rfm22b_dev, RFM22_gpio1_config, RFM22_gpio1_config_drv3 | RFM22_gpio1_config_txstate); } - rfm22_write(rfm22b_dev, RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); // GPIO2 = Clear Channel Assessment - - // **************** - - // 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; + // GPIO2 = Clear Channel Assessment + rfm22_write(rfm22b_dev, RFM22_gpio2_config, RFM22_gpio2_config_drv3 | RFM22_gpio2_config_cca); // FIFO mode, GFSK modulation uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; @@ -2348,18 +2331,18 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // no header bytes, synchronization word length 3, 2, 1 & 0 used, packet length included in header. rfm22_write(rfm22b_dev, RFM22_header_control2, RFM22_header_cntl2_hdlen_none | - RFM22_header_cntl2_synclen_3210 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); + RFM22_header_cntl2_synclen_3210 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); #else // header control - using a 4 by header with broadcast of 0xffffffff rfm22_write(rfm22b_dev, RFM22_header_control1, - RFM22_header_cntl1_bcen_0 | - RFM22_header_cntl1_bcen_1 | - RFM22_header_cntl1_bcen_2 | - RFM22_header_cntl1_bcen_3 | - RFM22_header_cntl1_hdch_0 | - RFM22_header_cntl1_hdch_1 | - RFM22_header_cntl1_hdch_2 | - RFM22_header_cntl1_hdch_3); + RFM22_header_cntl1_bcen_0 | + RFM22_header_cntl1_bcen_1 | + RFM22_header_cntl1_bcen_2 | + RFM22_header_cntl1_bcen_3 | + RFM22_header_cntl1_hdch_0 | + RFM22_header_cntl1_hdch_1 | + RFM22_header_cntl1_hdch_2 | + RFM22_header_cntl1_hdch_3); // Check all bit of all bytes of the header rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); @@ -2373,9 +2356,9 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) 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(rfm22b_dev, RFM22_header_control2, - RFM22_header_cntl2_hdlen_3210 | - RFM22_header_cntl2_synclen_3210 | - ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); + RFM22_header_cntl2_hdlen_3210 | + RFM22_header_cntl2_synclen_3210 | + ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); #endif // sync word @@ -2405,7 +2388,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) 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); + rfm22_setNominalCarrierFrequency(rfm22b_dev, RFM22B_NOMINAL_CARRIER_FREQUENCY, RFM22B_DEFAULT_CHANNEL); rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); return RFM22B_EVENT_INITIALIZED; diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index a67f5c253..dc688d312 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -102,7 +102,6 @@ typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsRemoteMainPortOption /* 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_Reinit(uint32_t rfb22b_id); -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 PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id); extern void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb); diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 8fbff558b..782f76b65 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -1,17 +1,17 @@ /** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_RFM22B Radio Functions - * @brief PIOS interface for RFM22B Radio - * @{ - * - * @file pios_rfm22b_priv.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief RFM22B private definitions. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ +****************************************************************************** +* @addtogroup PIOS PIOS Core hardware abstraction layer +* @{ +* @addtogroup PIOS_RFM22B Radio Functions +* @brief PIOS interface for RFM22B Radio +* @{ +* +* @file pios_rfm22b_priv.h +* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. +* @brief RFM22B private definitions. +* @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 @@ -444,28 +444,28 @@ #define RFM22_adc8_control 0x4F // R/W /* -#define RFM22_analog_test_bus 0x50 // R/W -#define RFM22_digital_test_bus 0x51 // R/W -#define RFM22_tx_ramp_control 0x52 // R/W -#define RFM22_pll_tune_time 0x53 // R/W + #define RFM22_analog_test_bus 0x50 // R/W + #define RFM22_digital_test_bus 0x51 // R/W + #define RFM22_tx_ramp_control 0x52 // R/W + #define RFM22_pll_tune_time 0x53 // R/W -#define RFM22_calibration_control 0x55 // R/W + #define RFM22_calibration_control 0x55 // R/W -#define RFM22_modem_test 0x56 // R/W + #define RFM22_modem_test 0x56 // R/W -#define RFM22_chargepump_test 0x57 // R/W -#define RFM22_chargepump_current_trimming_override 0x58 // R/W + #define RFM22_chargepump_test 0x57 // R/W + #define RFM22_chargepump_current_trimming_override 0x58 // R/W -#define RFM22_divider_current_trimming 0x59 // R/W + #define RFM22_divider_current_trimming 0x59 // R/W -#define RFM22_vco_current_trimming 0x5A // R/W -#define RFM22_vco_calibration_override 0x5B // R/W + #define RFM22_vco_current_trimming 0x5A // R/W + #define RFM22_vco_calibration_override 0x5B // R/W -#define RFM22_synthersizer_test 0x5C // R/W + #define RFM22_synthersizer_test 0x5C // R/W -#define RFM22_block_enable_override1 0x5D // R/W -#define RFM22_block_enable_override2 0x5E // R/W -#define RFM22_block_enable_override3 0x5F // R/W + #define RFM22_block_enable_override1 0x5D // R/W + #define RFM22_block_enable_override2 0x5E // R/W + #define RFM22_block_enable_override3 0x5F // R/W */ #define RFM22_channel_filter_coeff_addr 0x60 // R/W #define RFM22_ch_fil_coeff_ad_inv_pre_th_mask 0xF0 // @@ -482,14 +482,14 @@ #define RFM22_xtal_osc_por_ctrl_bufovr 0x02 // Output Buffer Enable Override. #define RFM22_xtal_osc_por_ctrl_enbuf 0x01 // Output Buffer Enable. /* -#define RFM22_rc_osc_coarse_calbration_override 0x63 // R/W -#define RFM22_rc_osc_fine_calbration_override 0x64 // R/W + #define RFM22_rc_osc_coarse_calbration_override 0x63 // R/W + #define RFM22_rc_osc_fine_calbration_override 0x64 // R/W -#define RFM22_ldo_control_override 0x65 // R/W -#define RFM22_ldo_level_setting 0x66 // R/W + #define RFM22_ldo_control_override 0x65 // R/W + #define RFM22_ldo_level_setting 0x66 // R/W -#define RFM22_deltasigma_adc_tuning1 0x67 // R/W -#define RFM22_deltasigma_adc_tuning2 0x68 // R/W + #define RFM22_deltasigma_adc_tuning1 0x67 // R/W + #define RFM22_deltasigma_adc_tuning2 0x68 // R/W */ #define RFM22_agc_override1 0x69 // R/W #define RFM22_agc_ovr1_sgi 0x40 // AGC Loop, Set Gain Increase. If set to 0 then gain increasing will not be allowed. If set to 1 then gain increasing is allowed, default is 0. @@ -605,13 +605,13 @@ enum pios_rfm22b_state { }; enum pios_rfm22b_event { + RFM22B_EVENT_DEFAULT, RFM22B_EVENT_INT_RECEIVED, RFM22B_EVENT_INITIALIZE, RFM22B_EVENT_INITIALIZED, RFM22B_EVENT_REQUEST_CONNECTION, RFM22B_EVENT_WAIT_FOR_CONNECTION, RFM22B_EVENT_CONNECTION_REQUESTED, - RFM22B_EVENT_CONNECTION_ACCEPTED, RFM22B_EVENT_PACKET_ACKED, RFM22B_EVENT_PACKET_NACKED, RFM22B_EVENT_ACK_TIMEOUT, @@ -726,9 +726,6 @@ struct pios_rfm22b_dev { // RSSI in dBm int8_t rssi_dBm; - // The packet queue handle - xQueueHandle packetQueue; - // The tx data packet PHPacket data_packet; // The current tx packet @@ -770,14 +767,12 @@ struct pios_rfm22b_dev { bool send_ppm; bool send_connection_request; bool time_to_send; - uint8_t time_to_send_offset; - // The minimum frequency - uint32_t min_frequency; - // The maximum frequency - uint32_t max_frequency; - // The current nominal frequency - uint32_t frequency_hz; + // The offset between our clock and the global send clock + uint8_t time_to_send_offset; + // The number of times that the current packet has been resent. + uint8_t cur_resent_count; + // The frequency hopping step size float frequency_step_size; // current frequency hop channel @@ -787,11 +782,15 @@ struct pios_rfm22b_dev { // afc correction reading (in Hz) int8_t afc_correction_Hz; - // The maximum time (ms) that it should take to transmit / receive a packet. - uint32_t max_packet_time; + // The packet timers. portTickType packet_start_ticks; portTickType tx_complete_ticks; portTickType rx_complete_ticks; + + // The maximum time (ms) that it should take to transmit / receive a packet. + uint32_t max_packet_time; + + // The maximum time to wait for an ACK. uint8_t max_ack_delay; }; @@ -810,6 +809,6 @@ extern const struct pios_com_driver pios_rfm22b_com_driver; #endif /* PIOS_RFM22B_PRIV_H */ /** - * @} - * @} - */ + * @} + * @} + */ diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 772f5b05c..6c43557bb 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -63,9 +63,6 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort); addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); - addUAVObjectToWidgetRelation("OPLinkSettings", "FrequencyCalibration", m_oplink->FrequencyCalibration); - addUAVObjectToWidgetRelation("OPLinkSettings", "MinFrequency", m_oplink->MinFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaxFrequency); addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 0636e5fcf..f904ab51f 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -1429,19 +1429,6 @@ - - - - Qt::Vertical - - - - 20 - 40 - - - - @@ -2548,129 +2535,6 @@ - - - - Frequency Calibration - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Calibrate the modems RF carrier frequency - - - true - - - 255 - - - - - - - Min. Frequency (Hz) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Set the modems minimum RF carrier frequency - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 400000000 - - - 1000000000 - - - 100000 - - - - - - - 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 - - - @@ -2704,7 +2568,6 @@ FirmwareVersion SerialNumber MaxRFTxPower - FrequencyCalibration Apply Save diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index 7c7403c87..94000d8e4 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -10,9 +10,6 @@ - - - From 2f0498d8ad6517223e7dfd231e2a9e1056bf7607 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 25 Feb 2013 00:30:32 +0000 Subject: [PATCH 06/40] Was a little aggressive on the rfm22b device structure cleanup, and removed the PPM receiver elements. --- flight/PiOS/Common/pios_rfm22b.c | 2 ++ flight/PiOS/inc/pios_rfm22b_priv.h | 7 +++++++ 2 files changed, 9 insertions(+) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index d9ce85e57..8c32da202 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -1781,8 +1781,10 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) break; case PACKET_TYPE_PPM: { +#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet); bool ppm_output = false; +#endif #if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT) if (PIOS_PPM_OUTPUT) { ppm_output = true; diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 782f76b65..c09745487 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -792,6 +792,13 @@ struct pios_rfm22b_dev { // The maximum time to wait for an ACK. uint8_t max_ack_delay; + +#ifdef PIOS_INCLUDE_RFM22B_RCVR + // The PPM channel values + uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS]; + uint8_t ppm_supv_timer; + bool ppm_fresh; +#endif }; From 4e92bed1df22d20a3bed342dfa4bfde61fb488e4 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 25 Feb 2013 01:44:48 +0000 Subject: [PATCH 07/40] Added back missing PIOS_COM_TELEM_USB (needed by the bootloader). --- flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index be0ea2f9c..319b33dbe 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -182,6 +182,7 @@ extern uint32_t pios_com_radio_id; extern uint32_t pios_ppm_rcvr_id; extern uint32_t pios_ppm_out_id; #define PIOS_COM_TELEM_USB_HID (pios_com_telem_usb_id) +#define PIOS_COM_TELEM_USB PIOS_COM_TELEM_USB_HID #define PIOS_COM_TELEM_USB_VCP (pios_com_telem_vcp_id) #define PIOS_COM_TELEM_UART_MAIN (pios_com_telem_uart_main_id) #define PIOS_COM_TELEM_UART_FLEXI (pios_com_telem_uart_flexi_id) From 9fad194bb8727bd9fcafeb7e35acb3f41d664b3c Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 25 Feb 2013 09:20:08 -0700 Subject: [PATCH 08/40] RevoMini: Added rfm22 com configure callback to set the speed of the radio link. --- flight/RevoMini/System/pios_board.c | 52 +++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/flight/RevoMini/System/pios_board.c b/flight/RevoMini/System/pios_board.c index 6f772acdf..e5777dcc3 100644 --- a/flight/RevoMini/System/pios_board.c +++ b/flight/RevoMini/System/pios_board.c @@ -42,6 +42,12 @@ #include "hwsettings.h" #include "manualcontrolsettings.h" +#if defined(PIOS_INCLUDE_RFM22B) +// Forward declarations +static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed); +#endif + /** * Sensor configurations */ @@ -633,6 +639,10 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_rfm22b_rcvr_id; #endif + + // Set the com port configuration callback. + PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); + break; } } @@ -734,6 +744,48 @@ void PIOS_Board_Init(void) { } +#if defined(PIOS_INCLUDE_RFM22B) +/** + * Configure the radio com port based on a configuration event from the remote coordinator. + * \param[in] main_port The main com port options + * \param[in] flexi_port The flexi com port options + * \param[in] vcp_port The USB virtual com port options + * \param[in] com_speed The com port speed + */ +static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed) +{ + uint32_t comBaud = 9600; + switch (com_speed) { + case OPLINKSETTINGS_COMSPEED_2400: + comBaud = 2400; + break; + case OPLINKSETTINGS_COMSPEED_4800: + comBaud = 4800; + break; + case OPLINKSETTINGS_COMSPEED_9600: + comBaud = 9600; + break; + case OPLINKSETTINGS_COMSPEED_19200: + comBaud = 19200; + break; + case OPLINKSETTINGS_COMSPEED_38400: + comBaud = 38400; + break; + case OPLINKSETTINGS_COMSPEED_57600: + comBaud = 57600; + break; + case OPLINKSETTINGS_COMSPEED_115200: + comBaud = 115200; + break; + } + if (PIOS_COM_TELEM_RF) { + PIOS_COM_ChangeBaud(PIOS_COM_TELEM_RF, comBaud); + } +} + +#endif + /** * @} * @} From 4743404c47aa2976db55fa5141ca0931e9e9e1af Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 1 Mar 2013 07:55:00 -0700 Subject: [PATCH 09/40] RFM22B: Some more reliability improvements for frequency hopping. --- flight/PiOS/Common/pios_rfm22b.c | 51 ++++++++++++++++++------------ flight/PiOS/inc/pios_rfm22b_priv.h | 2 ++ 2 files changed, 33 insertions(+), 20 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 8c32da202..be6e7d713 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -593,6 +593,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->stats.tx_seq = 0; + rfm22b_dev->stats.rx_seq = 0; // initialize the frequency hopping step size (specified in 10khz increments). uint32_t freq_hop_step_size = RFM22B_FREQUENCY_HOP_STEP_SIZE / 10000; @@ -901,16 +903,16 @@ static void PIOS_RFM22B_Task(void *parameters) } portTickType curTicks = xTaskGetTickCount(); + uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : timeDifferenceMs(rfm22b_dev->rx_complete_ticks, curTicks); // 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))) + 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); // Has 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)) + } else if (last_rec_ms > DISCONNECT_TIMEOUT_MS) { rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); - else - { + } else { // Are we waiting for an ACK? if (rfm22b_dev->prev_tx_packet) @@ -934,7 +936,7 @@ static void PIOS_RFM22B_Task(void *parameters) } // Queue up a status packet if it's time. - if (timeDifferenceMs(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) + if ((timeDifferenceMs(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) || (last_rec_ms > rfm22b_dev->max_packet_time * 4)) { rfm22_sendStatus(rfm22b_dev); lastStatusTicks = curTicks; @@ -977,7 +979,7 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_d { // 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; + rfm22b_dev->max_ack_delay = (uint16_t)((float)(sizeof(PHPacketHeader) * 8 * 1000) / (float)(datarate_bps) + 0.5) * 6 + random; } else rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS; @@ -1462,16 +1464,14 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) { - // Don't send status if we're the coordinator. - if (rfm22b_dev->coordinator) - return; - // Update the link quality metric. rfm22_calculateLinkQuality(rfm22b_dev); // Queue the status message if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) rfm22b_dev->status_packet.header.destination_id = rfm22b_dev->destination_id; + else if (rfm22b_dev->coordinator) + return; else rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; @@ -2022,10 +2022,24 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; } else { - // Switch to the fallback channel if we have resent too many packets. - if (rfm22b_dev->cur_resent_count++ > 2) { + // First resend on the current channel, then resend on the next channel in case the receive has changed channels, then fallback. + rfm22b_dev->cur_resent_count++; + switch (rfm22b_dev->cur_resent_count) { + case 1: + case 2: + rfm22b_dev->prev_frequency_hop_channel = rfm22b_dev->frequency_hop_channel; + case 5: + rfm22_setFreqHopChannel(rfm22b_dev, rfm22b_dev->prev_frequency_hop_channel); + break; + case 3: + case 4: + case 6: + rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(rfm22b_dev->tx_packet->header.seq_num, 0) & 0x7f); + break; + default: rfm22b_dev->cur_resent_count = 0; rfm22_setFreqHopChannel(rfm22b_dev, RFM22B_DEFAULT_CHANNEL); + break; } } @@ -2192,8 +2206,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) 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_dev->in_rx_mode = false; @@ -2202,6 +2214,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->rx_buffer_wr = 0; rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; rfm22b_dev->frequency_hop_channel = 0; + rfm22b_dev->prev_frequency_hop_channel = 0; rfm22b_dev->afc_correction_Hz = 0; rfm22b_dev->packet_start_ticks = 0; rfm22b_dev->tx_complete_ticks = 0; @@ -2210,18 +2223,16 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); - // wait 26ms - PIOS_DELAY_WaitmS(26); - for (int i = 50; i > 0; i--) { - // wait 1ms - PIOS_DELAY_WaitmS(1); - // read the status registers 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; + + // wait 1ms + PIOS_DELAY_WaitmS(1); + } // **************** diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index c09745487..3e453614b 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -777,6 +777,8 @@ struct pios_rfm22b_dev { float frequency_step_size; // current frequency hop channel uint8_t frequency_hop_channel; + // previous frequency hop channel + uint8_t prev_frequency_hop_channel; // the frequency hop step size uint8_t frequency_hop_step_size_reg; // afc correction reading (in Hz) From 31a3a666569d2d07bee500e49bfc255ef2c09a53 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 3 Mar 2013 21:08:17 -0700 Subject: [PATCH 10/40] Mostly working periodic (as opposed to per message) channel hopping. This version should have less risk of getting hopelessly out of sync, but currently drops some packets. The dropped packets are likely around the channel hops. --- flight/Libraries/inc/packet_handler.h | 3 + .../PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 1 + flight/PiOS/Common/pios_rfm22b.c | 104 +++++++++++++++--- flight/PiOS/inc/pios_rfm22b_priv.h | 3 + 4 files changed, 94 insertions(+), 17 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 423f07fc6..7f52ccbbc 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -69,6 +69,9 @@ typedef struct { #define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + uint8_t packet_recv_time; +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP uint8_t ecc[RS_ECC_NPARITY]; } PHAckNackPacket, *PHAckNackPacketHandle; diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index 319b33dbe..3679c95e2 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -137,6 +137,7 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 //------------------------- #define PIOS_MASTER_CLOCK 72000000 #define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) +#define PIOS_RFM22B_PERIODIC_CHANNEL_HOP //------------------------- // Interrupt Priorities diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index be6e7d713..84878d7dc 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -205,10 +205,15 @@ 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, uint8_t channel); -static void rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel); +static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel); 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); +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP +static uint16_t rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks); +static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev); +static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev); +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP static void rfm22_clearLEDs(); // SPI read/write functions @@ -955,6 +960,10 @@ static void PIOS_RFM22B_Task(void *parameters) #endif if (rfm22b_dev->time_to_send) rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + else if(rfm22b_dev->state == RFM22B_STATE_RX_MODE) + rfm22_process_event(rfm22b_dev, RFM22B_EVENT_RX_MODE); +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP } } @@ -1206,11 +1215,14 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency0, fc & 0xff); } -static void rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel) +static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel) { // set the frequency hopping channel + if (rfm22b_dev->frequency_hop_channel == channel) + return false; rfm22b_dev->frequency_hop_channel = channel; rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); + return true; } static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev) @@ -1255,14 +1267,19 @@ 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? +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + if (rfm22b_dev->in_rx_mode && !rfm22_changeChannel(rfm22b_dev)) + return RFM22B_EVENT_NUM_EVENTS; +#else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP if (rfm22b_dev->in_rx_mode) - return RFM22B_EVENT_NUM_EVENTS; + return RFM22B_EVENT_NUM_EVENTS; +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP rfm22b_dev->packet_start_ticks = 0; #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D2_LED_ON; D3_LED_TOGGLE; -#endif +#endif // PIOS_RFM22B_DEBUG_ON_TELEM // disable interrupts rfm22_write(rfm22b_dev, RFM22_interrupt_enable1, 0x00); @@ -1393,6 +1410,13 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) D3_LED_TOGGLE; #endif +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + // Change the channel if necessary. + if (((p->header.type != PACKET_TYPE_ACK) && (p->header.type != PACKET_TYPE_ACK_RTS)) || + (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) + rfm22_changeChannel(rfm22b_dev); +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP + // Add the error correcting code. encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); @@ -1871,6 +1895,7 @@ 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) { + portTickType curTicks = xTaskGetTickCount(); rfm22b_dev->stats.tx_byte_count += PH_PACKET_SIZE(rfm22b_dev->tx_packet); // Is this an ACK? @@ -1884,7 +1909,14 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) rfm22_setConnectionParameters(rfm22b_dev); // Change the channel +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + // On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message. + if (!rfm22b_dev->coordinator && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST)) { + rfm22b_dev->time_delta = 0xffff - (curTicks & 0xffff); + } +#else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(rfm22b_dev->rx_packet.header.seq_num, 0) & 0x7f); +#endif // !PIOS_RFM22B_PERIODIC_CHANNEL_HOP } else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) { @@ -1893,7 +1925,6 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) 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 + 0x4; else if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_ACK_RTS) @@ -1930,6 +1961,9 @@ static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) 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; +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + aph->packet_recv_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->rx_complete_ticks); +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP rfm22b_dev->tx_packet = (PHPacketHandle)aph; rfm22b_dev->time_to_send = true; return RFM22B_EVENT_TX_START; @@ -1964,8 +1998,7 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de rfm22b_dev->prev_tx_packet = NULL; // Was this a connection request? - switch (prev->header.type) - { + switch (prev->header.type) { case PACKET_TYPE_CON_REQUEST: rfm22_setConnectionParameters(rfm22b_dev); break; @@ -1974,21 +2007,33 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de break; } +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + // On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message. + if (prev->header.type == PACKET_TYPE_CON_REQUEST) { + rfm22b_dev->time_delta = 0xffff - (rfm22b_dev->rx_complete_ticks & 0xffff); + } else if (!rfm22b_dev->coordinator) { + PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->rx_packet)); + uint16_t local_tx_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->tx_complete_ticks); + uint16_t remote_rx_time = aph->packet_recv_time; + // Adjust the time delta based on the difference between our estimated time offset and the coordinator offset. + // This is not working yet + if (0) + rfm22b_dev->time_delta += remote_rx_time - local_tx_time; + } +#else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP + // Change the channel + rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(prev->header.seq_num, 0) & 0x7f); +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP + // Reset the resend count rfm22b_dev->cur_resent_count = 0; - // Change the channel - rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(prev->header.seq_num, 0) & 0x7f); - // Should we try to start another TX? - if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_ACK) - { + 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 - { + } else { rfm22b_dev->time_to_send_offset = curTicks + 0x4; return RFM22B_EVENT_RX_MODE; } @@ -2022,12 +2067,13 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; } else { +#ifndef PIOS_RFM22B_PERIODIC_CHANNEL_HOP // First resend on the current channel, then resend on the next channel in case the receive has changed channels, then fallback. rfm22b_dev->cur_resent_count++; switch (rfm22b_dev->cur_resent_count) { case 1: - case 2: rfm22b_dev->prev_frequency_hop_channel = rfm22b_dev->frequency_hop_channel; + case 2: case 5: rfm22_setFreqHopChannel(rfm22b_dev, rfm22b_dev->prev_frequency_hop_channel); break; @@ -2037,10 +2083,10 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(rfm22b_dev->tx_packet->header.seq_num, 0) & 0x7f); break; default: - rfm22b_dev->cur_resent_count = 0; rfm22_setFreqHopChannel(rfm22b_dev, RFM22B_DEFAULT_CHANNEL); break; } +#endif //PIOS_RFM22B_PERIODIC_CHANNEL_HOP } // Increment the reset packet counter if we're connected. @@ -2149,6 +2195,30 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power); } +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP +static uint16_t rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks) +{ + return (ticks & 0xffff) + rfm22b_dev->time_delta; +} + +static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) +{ + uint16_t time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); + // We change channels every 128 ms. + time = time >> 7; + // The channel is calculated using the 16 bit CRC as the pseudo random number generator, and there are 128 channels. + return (uint8_t)(time & 0x7f); + //return (uint8_t)(PIOS_CRC16_updateByte((time && 0xffff), 0) & 0x7f); +} + +static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) +{ + if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) + return false; + return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev)); +} +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP + static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) { // Set our connection state to connected diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 3e453614b..6862ab423 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -788,6 +788,9 @@ struct pios_rfm22b_dev { portTickType packet_start_ticks; portTickType tx_complete_ticks; portTickType rx_complete_ticks; +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + uint16_t time_delta; +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP // The maximum time (ms) that it should take to transmit / receive a packet. uint32_t max_packet_time; From bf5ef5bd00edbc9bdede75dd71b0ed03a3ad5cae Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 9 Mar 2013 01:06:42 +0000 Subject: [PATCH 11/40] RFM22B/RM: Added GPIO debugging to revomini to test rfm22b. Fixed (apparently) the reset issues with the rfm22b frequency hopping. Periodic frequency hopping is working pretty well now. --- flight/Libraries/inc/packet_handler.h | 2 +- flight/PiOS/Boards/STM32F4xx_RevoMini.h | 24 ++- flight/PiOS/Common/pios_rfm22b.c | 155 ++++++++++-------- flight/PiOS/inc/pios_rfm22b_priv.h | 4 +- flight/RevoMini/System/inc/pios_config.h | 3 + flight/board_hw_defs/revomini/board_hw_defs.c | 50 ++++++ 6 files changed, 164 insertions(+), 74 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 7f52ccbbc..1d2b04e3d 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -70,7 +70,7 @@ typedef struct { typedef struct { PHPacketHeader header; #ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP - uint8_t packet_recv_time; + portTickType packet_recv_time; #endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP uint8_t ecc[RS_ECC_NPARITY]; } PHAckNackPacket, *PHAckNackPacketHandle; diff --git a/flight/PiOS/Boards/STM32F4xx_RevoMini.h b/flight/PiOS/Boards/STM32F4xx_RevoMini.h index 49c0d1c96..65eef1676 100644 --- a/flight/PiOS/Boards/STM32F4xx_RevoMini.h +++ b/flight/PiOS/Boards/STM32F4xx_RevoMini.h @@ -84,6 +84,28 @@ TIM8 | | | | //------------------------ #define PIOS_LED_HEARTBEAT 0 #define PIOS_LED_ALARM 1 +#ifdef PIOS_RFM22B_DEBUG_ON_SERVO +#define PIOS_LED_D1 2 +#define PIOS_LED_D2 3 +#define PIOS_LED_D3 4 +#define PIOS_LED_D4 5 + +#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 //------------------------ // PIOS_SPI @@ -197,7 +219,7 @@ extern uint32_t pios_packet_handler; // Default APB2 Prescaler = 2 // #define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK - +#define PIOS_RFM22B_PERIODIC_CHANNEL_HOP //------------------------- // Interrupt Priorities diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 84878d7dc..2dae15a55 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -208,9 +208,11 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel); static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev); +static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev); #ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP -static uint16_t rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks); +static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks); +static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev); static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev); #endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP @@ -431,7 +433,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_TIMEOUT] = { .entry_fn = rfm22_timeout, .next_state = { - [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, + [RFM22B_EVENT_TX_START] = RFM22B_STATE_TX_START, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, @@ -854,7 +856,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.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD); + return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD)); } /** @@ -907,6 +909,11 @@ static void PIOS_RFM22B_Task(void *parameters) } } + // Change channels if necessary. + if ((rfm22b_dev->state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->state == RFM22B_STATE_WAIT_PREAMBLE)) { + rfm22_changeChannel(rfm22b_dev); + } + portTickType curTicks = xTaskGetTickCount(); uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : timeDifferenceMs(rfm22b_dev->rx_complete_ticks, curTicks); // Have we been sending this packet too long? @@ -916,7 +923,6 @@ static void PIOS_RFM22B_Task(void *parameters) // Has it been too long since we received a packet } else if (last_rec_ms > DISCONNECT_TIMEOUT_MS) { rfm22_process_event(rfm22b_dev, RFM22B_EVENT_ERROR); - } else { // Are we waiting for an ACK? @@ -952,18 +958,20 @@ 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) & 0x6) == 0); -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM - if (rfm22b_dev->time_to_send) +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) + if (rfm22b_dev->time_to_send) { D4_LED_ON; - else + } else { D4_LED_OFF; + } + if (rfm22_inChannelBuffer(rfm22b_dev)) { + D3_LED_ON; + } else { + D3_LED_OFF; + } #endif if (rfm22b_dev->time_to_send) rfm22_process_event(rfm22b_dev, RFM22B_EVENT_TX_START); -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP - else if(rfm22b_dev->state == RFM22B_STATE_RX_MODE) - rfm22_process_event(rfm22b_dev, RFM22B_EVENT_RX_MODE); -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP } } @@ -984,11 +992,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) + if (rfm22_isConnected(rfm22b_dev)) { // 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) * 6 + random; + rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5) * 4 + 4 + random; } else rfm22b_dev->max_ack_delay = CONNECT_ATTEMPT_PERIOD_MS; @@ -1267,18 +1275,11 @@ 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? -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP - if (rfm22b_dev->in_rx_mode && !rfm22_changeChannel(rfm22b_dev)) - return RFM22B_EVENT_NUM_EVENTS; -#else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP if (rfm22b_dev->in_rx_mode) return RFM22B_EVENT_NUM_EVENTS; -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP - rfm22b_dev->packet_start_ticks = 0; -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) D2_LED_ON; - D3_LED_TOGGLE; #endif // PIOS_RFM22B_DEBUG_ON_TELEM // disable interrupts @@ -1338,49 +1339,56 @@ static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev) return false; } +static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) +{ + return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED); +} + 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. +#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + if (!rfm22b_dev->time_to_send || (rfm22_inChannelBuffer(rfm22b_dev) && rfm22_isConnected(rfm22b_dev))) + return RFM22B_EVENT_RX_MODE; +#else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP if (!rfm22b_dev->time_to_send) return RFM22B_EVENT_RX_MODE; +#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP // 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 - { + else { // Don't send a packet if we're waiting for an ACK if (rfm22b_dev->prev_tx_packet) return RFM22B_EVENT_RX_MODE; - if (!p && rfm22b_dev->send_connection_request) - { + // Send a connection request? + 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) - { + // Send a PPM packet? + if (!p && rfm22b_dev->send_ppm) { p = (PHPacketHandle)&(rfm22b_dev->ppm_packet); rfm22b_dev->send_ppm = false; } #endif - if (!p && rfm22b_dev->send_status) - { + // Send status? + if (!p && rfm22b_dev->send_status) { p = (PHPacketHandle)&(rfm22b_dev->status_packet); rfm22b_dev->send_status = false; } - if (!p) - { - // Try to get some data to send + // Try to get some data to send + if (!p) { bool need_yield = false; p = &rfm22b_dev->data_packet; p->header.type = PACKET_TYPE_DATA; @@ -1389,7 +1397,7 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) 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) + if (!rfm22_isConnected(rfm22b_dev)) p->header.data_size = 0; if (p->header.data_size == 0) p = NULL; @@ -1404,10 +1412,9 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) // We're transitioning out of Rx mode. rfm22b_dev->in_rx_mode = false; -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) D1_LED_ON; D2_LED_OFF; - D3_LED_TOGGLE; #endif #ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP @@ -1492,7 +1499,7 @@ static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) rfm22_calculateLinkQuality(rfm22b_dev); // Queue the status message - if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) + if (rfm22_isConnected(rfm22b_dev)) rfm22b_dev->status_packet.header.destination_id = rfm22b_dev->destination_id; else if (rfm22b_dev->coordinator) return; @@ -1512,12 +1519,14 @@ 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) + if (!rfm22_isConnected(rfm22b_dev)) { return; + } // Just return if the PPM receiver is not configured. - if (PIOS_PPM_RECEIVER == 0) + if (PIOS_PPM_RECEIVER == 0) { return; + } // See if we have any valid channels. bool valid_input_detected = false; @@ -1600,10 +1609,6 @@ 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; } @@ -1664,8 +1669,7 @@ static bool rfm22_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHand if (!ack_nack_packet && (good_packet || corrected_packet)) { uint16_t seq_num = p->header.seq_num; - if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) - { + if (rfm22_isConnected(rfm22b_dev)) { static bool first_time = true; uint16_t missed_packets = 0; if (first_time) @@ -1839,9 +1843,8 @@ 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 +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) D2_LED_OFF; - D3_LED_TOGGLE; #endif } @@ -1901,8 +1904,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 = RFM22B_EVENT_RX_MODE; - if (is_ack) - { + 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) @@ -1910,16 +1913,16 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // Change the channel #ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP - // On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message. - if (!rfm22b_dev->coordinator && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST)) { - rfm22b_dev->time_delta = 0xffff - (curTicks & 0xffff); + // On the remote side, we initialize the time delta when we finish sending the ACK for the connection request message. + if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { + rfm22b_dev->time_delta = portMAX_DELAY - curTicks; } #else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(rfm22b_dev->rx_packet.header.seq_num, 0) & 0x7f); #endif // !PIOS_RFM22B_PERIODIC_CHANNEL_HOP - } - else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) - { + + } 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(); @@ -1934,9 +1937,8 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // Start a new transaction rfm22b_dev->packet_start_ticks = 0; -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) D1_LED_OFF; - D3_LED_TOGGLE; #endif } @@ -2010,15 +2012,14 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de #ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP // On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message. if (prev->header.type == PACKET_TYPE_CON_REQUEST) { - rfm22b_dev->time_delta = 0xffff - (rfm22b_dev->rx_complete_ticks & 0xffff); + rfm22b_dev->time_delta = portMAX_DELAY - rfm22b_dev->rx_complete_ticks; } else if (!rfm22b_dev->coordinator) { PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->rx_packet)); - uint16_t local_tx_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->tx_complete_ticks); - uint16_t remote_rx_time = aph->packet_recv_time; + portTickType local_tx_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->tx_complete_ticks); + portTickType remote_rx_time = aph->packet_recv_time; // Adjust the time delta based on the difference between our estimated time offset and the coordinator offset. // This is not working yet - if (0) - rfm22b_dev->time_delta += remote_rx_time - local_tx_time; + rfm22b_dev->time_delta += remote_rx_time - local_tx_time; } #else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP // Change the channel @@ -2045,6 +2046,7 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de */ 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; @@ -2090,8 +2092,9 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d } // Increment the reset packet counter if we're connected. - if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) + if (rfm22_isConnected(rfm22b_dev)) { rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET); + } rfm22b_dev->time_to_send = true; return RFM22B_EVENT_TX_START; } @@ -2196,14 +2199,21 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) } #ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP -static uint16_t rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks) +static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks) { - return (ticks & 0xffff) + rfm22b_dev->time_delta; + return ticks + rfm22b_dev->time_delta; +} + +static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev) +{ + portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); + uint8_t window = (uint8_t)(time & 0x7e); + return ((window == 0x7e) || (window == 0)); } static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) { - uint16_t time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); + portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); // We change channels every 128 ms. time = time >> 7; // The channel is calculated using the 16 bit CRC as the pseudo random number generator, and there are 128 channels. @@ -2213,9 +2223,10 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) { - if (rfm22b_dev->stats.link_state != OPLINKSTATUS_LINKSTATE_CONNECTED) - return false; - return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev)); + if (rfm22_isConnected(rfm22b_dev)) { + return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev)); + } + return false; } #endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP @@ -2284,7 +2295,9 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->rx_buffer_wr = 0; rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; rfm22b_dev->frequency_hop_channel = 0; +#ifndef PIOS_RFM22B_PERIODIC_CHANNEL_HOP rfm22b_dev->prev_frequency_hop_channel = 0; +#endif // !PIOS_RFM22B_PERIODIC_CHANNEL_HOP rfm22b_dev->afc_correction_Hz = 0; rfm22b_dev->packet_start_ticks = 0; rfm22b_dev->tx_complete_ticks = 0; @@ -2481,7 +2494,7 @@ static void rfm22_clearLEDs() { LINK_LED_OFF; RX_LED_OFF; TX_LED_OFF; -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) D1_LED_OFF; D2_LED_OFF; D3_LED_OFF; @@ -2501,7 +2514,7 @@ static enum pios_rfm22b_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->rx_buffer_wr = 0; TX_LED_OFF; RX_LED_OFF; -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM +#if defined(PIOS_RFM22B_DEBUG_ON_TELEM) || defined(PIOS_RFM22B_DEBUG_ON_RCVR) D1_LED_OFF; D2_LED_OFF; D3_LED_OFF; diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 6862ab423..473157231 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -777,8 +777,10 @@ struct pios_rfm22b_dev { float frequency_step_size; // current frequency hop channel uint8_t frequency_hop_channel; +#ifndef PIOS_RFM22B_PERIODIC_CHANNEL_HOP // previous frequency hop channel uint8_t prev_frequency_hop_channel; +#endif // !PIOS_RFM22B_PERIODIC_CHANNEL_HOP // the frequency hop step size uint8_t frequency_hop_step_size_reg; // afc correction reading (in Hz) @@ -789,7 +791,7 @@ struct pios_rfm22b_dev { portTickType tx_complete_ticks; portTickType rx_complete_ticks; #ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP - uint16_t time_delta; + portTickType time_delta; #endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP // The maximum time (ms) that it should take to transmit / receive a packet. diff --git a/flight/RevoMini/System/inc/pios_config.h b/flight/RevoMini/System/inc/pios_config.h index 6ea05446c..e615c12d2 100644 --- a/flight/RevoMini/System/inc/pios_config.h +++ b/flight/RevoMini/System/inc/pios_config.h @@ -120,6 +120,9 @@ #define REVOLUTION +/* Turn on debugging signals on the receiver input port */ +//#define PIOS_RFM22B_DEBUG_ON_RCVR + #endif /* PIOS_CONFIG_H */ /** * @} diff --git a/flight/board_hw_defs/revomini/board_hw_defs.c b/flight/board_hw_defs/revomini/board_hw_defs.c index 7375d367a..95ee3d35b 100644 --- a/flight/board_hw_defs/revomini/board_hw_defs.c +++ b/flight/board_hw_defs/revomini/board_hw_defs.c @@ -77,6 +77,56 @@ static const struct pios_led pios_leds_v2[] = { }, }, }, +#ifdef PIOS_RFM22B_DEBUG_ON_SERVO + [PIOS_LED_D1] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_13, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D2] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_14, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D3] = { + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, + [PIOS_LED_D4] = { + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + }, +#endif }; static const struct pios_led_cfg pios_led_v2_cfg = { From 031b0073b1e62450c70b07b182f354077f729fa8 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 9 Mar 2013 02:27:58 +0000 Subject: [PATCH 12/40] RFM22B: Possible fix for the failure to connect at times. --- flight/PiOS/Common/pios_rfm22b.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 2dae15a55..9730957a7 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -392,6 +392,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_REQUEST_CONNECTION] = RFM22B_STATE_INITIATING_CONNECTION, [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_FAILURE] = RFM22B_STATE_TX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, @@ -1908,8 +1909,12 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // 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) + if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { rfm22_setConnectionParameters(rfm22b_dev); + } else if (!rfm22_isConnected(rfm22b_dev)) { + // Request a connection if we're not yet connected. + ret_event = RFM22B_EVENT_REQUEST_CONNECTION; + } // Change the channel #ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP From 27c87e4b89e799e99068c9eeb713d933e9933a4c Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 9 Mar 2013 16:11:29 +0000 Subject: [PATCH 13/40] RFM22B: Fixed OPLink receiver output on RM. --- flight/PiOS/Common/pios_rfm22b.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 9730957a7..2a8b9c459 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -1810,10 +1810,17 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) break; case PACKET_TYPE_PPM: { -#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) +#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR) PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet); +#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) bool ppm_output = false; #endif +#endif +#if defined(PIOS_INCLUDE_RFM22B_RCVR) + for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { + rfm22b_dev->ppm_channel[i] = ppmp->channels[i]; + } +#endif #if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT) if (PIOS_PPM_OUTPUT) { ppm_output = true; From 7db7ab0e451469b5f8af4137c8075275a3597318 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 9 Mar 2013 16:44:26 +0000 Subject: [PATCH 14/40] Switched to non-blocking I/O on radio port, which should prevent watchdog restarts when the radio link backs up. --- flight/Modules/RadioComBridge/RadioComBridge.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index 2f7845ead..f1686737f 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -363,7 +363,7 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) uint32_t outputPort = PIOS_COM_RADIO; // Don't send any data unless the radio port is available. if(outputPort && PIOS_COM_Available(outputPort)) { - return PIOS_COM_SendBuffer(outputPort, buf, length); + return PIOS_COM_SendBufferNonBlocking(outputPort, buf, length); } else { // For some reason, if this function returns failure, it prevents saving settings. return length; From 08021799ee172a1bbb316d845a7582769c99f5d0 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 10 Mar 2013 16:08:39 +0100 Subject: [PATCH 15/40] RFM22B: Some style cleanup, and turned off status messages from coordinator. --- .../Modules/RadioComBridge/RadioComBridge.c | 3 + flight/PiOS/Common/pios_rfm22b.c | 55 +++++++++++++++++-- 2 files changed, 52 insertions(+), 6 deletions(-) diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index f1686737f..c1b1b213a 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -162,6 +162,9 @@ static int32_t RadioComBridgeStart(void) case OPLINKSETTINGS_MAXRFPOWER_100: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); break; + default: + // do nothing + break; } // Reinitilize the modem. diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 2a8b9c459..1978cd908 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -612,8 +612,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->frequency_hop_step_size_reg = (uint8_t)freq_hop_step_size; // Initialize the bindings. - for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) + for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { rfm22b_dev->bindings[i].pairID = 0; + } rfm22b_dev->coordinator = false; // Create the event queue @@ -729,8 +730,9 @@ 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)) { return rfm22b_dev->deviceID; - } else + } else { return 0; + } } /** @@ -741,10 +743,11 @@ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) bool PIOS_RFM22B_IsCoordinator(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->coordinator; - else + } else { return false; + } } /** @@ -755,8 +758,9 @@ bool PIOS_RFM22B_IsCoordinator(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; } @@ -768,8 +772,9 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) 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)) + if(!PIOS_RFM22B_validate(rfm22b_dev)) { return; + } rfm22b_dev->com_config_cb = cb; } @@ -860,6 +865,40 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) return (rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD)); } +/** + * Send a PPM packet with the given channel values. + * \param[in] rfm22b_id The rfm22b device. + * \param[in] channels The channel values. + * \param[in] nchannels The number of channels. + * Returns true if there is a valid connection to paired radio, false otherwise. + */ +void PIOS_RFM22B_SendPPM(uint32_t rfm22b_id, const uint16_t *channels, uint8_t nchannels) +{ +#ifdef PIOS_PPM_RECEIVER + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) { + return; + } + + // Only send PPM if we're connected + if (!rfm22_isConnected(rfm22b_dev)) { + return; + } + + // See if we have any valid channels. + uint8_t nchan = (nchannels <= PIOS_PPM_NUM_INPUTS) ? nchannels : PIOS_PPM_NUM_INPUTS; + for (uint8_t i = 0; i < nchan; ++i) { + rfm22b_dev->ppm_packet.channels[i] = channels[i]; + } + + // Send the PPM packet. + 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)); + rfm22b_dev->send_ppm = true; +#endif +} + /** * The task that controls the radio state machine. */ @@ -1496,6 +1535,10 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) { + // The coordinator doesn't send status. + if (rfm22b_dev->coordinator) + return; + // Update the link quality metric. rfm22_calculateLinkQuality(rfm22b_dev); From 9986b58f9bc3f12b714b7ca0b6e15e063a3dcfab Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 10 Mar 2013 19:38:35 +0100 Subject: [PATCH 16/40] RFM22B/RM: Tweaked some PPM parameters to make PPM more stable on OPLink/RM. Conflicts: flight/PiOS/Common/pios_rfm22b.c --- flight/PiOS/Boards/STM32F4xx_RevoMini.h | 2 +- flight/PiOS/Common/pios_rfm22b.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/PiOS/Boards/STM32F4xx_RevoMini.h b/flight/PiOS/Boards/STM32F4xx_RevoMini.h index 65eef1676..86efff55d 100644 --- a/flight/PiOS/Boards/STM32F4xx_RevoMini.h +++ b/flight/PiOS/Boards/STM32F4xx_RevoMini.h @@ -236,7 +236,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 +#define PIOS_RFM22B_RCVR_TIMEOUT_MS 200 //------------------------- // Receiver PPM input diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 1978cd908..60bd62918 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -91,7 +91,7 @@ #define MAX_RADIOSTATS_MISS_COUNT 3 // The time between PPM updates -#define PPM_UPDATE_PERIOD_MS 40 +#define PPM_UPDATE_PERIOD_MS 20 // this is too adjust the RF module so that it is on frequency #define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default From b2da9606445e0d9e3fecd01ee8036820fe60ddf4 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 15 Mar 2013 02:23:33 +0100 Subject: [PATCH 17/40] Fixed configuration of PPM intput on main port. --- flight/PipXtreme/System/pios_board.c | 11 +++----- .../board_hw_defs/pipxtreme/board_hw_defs.c | 26 ++++++++++++++++++- 2 files changed, 28 insertions(+), 9 deletions(-) diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 0f598e84e..8db6b8703 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -260,17 +260,12 @@ void PIOS_InitPPMMainPort(bool input) if (input) { uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_main_cfg); if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) PIOS_Assert(0); } -#if defined(PIOS_INCLUDE_PPM_OUT) - else - { - PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); - } -#endif /* PIOS_INCLUDE_PPM_OUT */ + // For some reason, PPM output on the main port doesn't work. #endif /* PIOS_INCLUDE_PPM */ } @@ -281,7 +276,7 @@ void PIOS_InitPPMFlexiPort(bool input) if (input) { uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) PIOS_Assert(0); diff --git a/flight/board_hw_defs/pipxtreme/board_hw_defs.c b/flight/board_hw_defs/pipxtreme/board_hw_defs.c index 26df458b9..2b87f1404 100644 --- a/flight/board_hw_defs/pipxtreme/board_hw_defs.c +++ b/flight/board_hw_defs/pipxtreme/board_hw_defs.c @@ -410,6 +410,19 @@ static const struct pios_tim_channel pios_tim_ppm_flexi_port = { .remap = GPIO_PartialRemap2_TIM2, }; +static const struct pios_tim_channel pios_tim_ppm_main_port = { + .timer = TIM1, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, +}; + #endif /* PIOS_INCLUDE_TIM */ #if defined(PIOS_INCLUDE_USART) @@ -541,7 +554,7 @@ void PIOS_RTC_IRQ_Handler (void) #if defined(PIOS_INCLUDE_PPM) #include -const struct pios_ppm_cfg pios_ppm_cfg = { +const struct pios_ppm_cfg pios_ppm_flexi_cfg = { .tim_ic_init = { .TIM_ICPolarity = TIM_ICPolarity_Rising, .TIM_ICSelection = TIM_ICSelection_DirectTI, @@ -552,6 +565,17 @@ const struct pios_ppm_cfg pios_ppm_cfg = { .num_channels = 1, }; +const struct pios_ppm_cfg pios_ppm_main_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_ppm_main_port, + .num_channels = 1, +}; + #endif /* PIOS_INCLUDE_PPM */ /* From 15134137952dde833db74f55b74b5745e4403614 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 18 Mar 2013 04:28:13 +0100 Subject: [PATCH 18/40] Added 56k data rate mode to rfm22 driver. --- flight/PiOS/Common/pios_rfm22b.c | 46 ++++++++++++---------------- flight/PiOS/Common/pios_rfm22b_com.c | 2 +- flight/PiOS/inc/pios_rfm22b.h | 9 +++--- 3 files changed, 26 insertions(+), 31 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 60bd62918..b0c999d6d 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -457,35 +457,32 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S }; // xtal 10 ppm, 434MHz -#define LOOKUP_SIZE 14 -static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 64000, 128000, 192000, 256000}; -static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; -static const uint32_t freq_deviation[] = { 4000, 4000, 4000, 4000, 4000, 4800, 8000, 9600, 12000, 16000, 32000, 64000, 96000, 128000}; -static const uint32_t rx_bandwidth[] = { 17500, 17500, 17500, 17500, 17500, 19400, 32200, 38600, 51200, 64100, 137900, 269300, 420200, 518800}; -static const int8_t est_rx_sens_dBm[] = { -118, -118, -117, -116, -115, -115, -112, -112, -110, -109, -106, -103, -101, -100}; // estimated receiver sensitivity for BER = 1E-3 +#define LOOKUP_SIZE 15 +static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 56000, 64000, 128000, 192000, 256000}; +static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; -static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x07, 0x83, 0x8A, 0x8C}; // rfm22_if_filter_bandwidth +static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x95, 0x07, 0x83, 0x8A, 0x8C}; // rfm22_if_filter_bandwidth -static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44}; // rfm22_afc_loop_gearshift_override -static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02}; // rfm22_afc_timing_control +static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44}; // rfm22_afc_loop_gearshift_override +static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02}; // rfm22_afc_timing_control -static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03}; // rfm22_clk_recovery_gearshift_override -static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x3F, 0x5E, 0x3F, 0x2F}; // rfm22_clk_recovery_oversampling_ratio -static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x02, 0x02, 0x02, 0x03, 0x02, 0x02, 0x01, 0x02, 0x02}; // rfm22_clk_recovery_offset2 -static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0x0C, 0x5D, 0x0C, 0xBB}; // rfm22_clk_recovery_offset1 -static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0x4A, 0x86, 0x4A, 0x0D}; // rfm22_clk_recovery_offset0 -static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07}; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x74, 0xFF, 0xFF}; // rfm22_clk_recovery_timing_loop_gain0 +static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03}; // rfm22_clk_recovery_gearshift_override +static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x6B, 0x3F, 0x5E, 0x3F, 0x2F}; // rfm22_clk_recovery_oversampling_ratio +static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x02, 0x02, 0x02, 0x03, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02}; // rfm22_clk_recovery_offset2 +static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0x31, 0x0c, 0x5D, 0x0C, 0xBB}; // rfm22_clk_recovery_offset1 +static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0xD6, 0x4A, 0x86, 0x4A, 0x0D}; // rfm22_clk_recovery_offset0 +static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x02, 0x07, 0x05, 0x07, 0x07}; // rfm22_clk_recovery_timing_loop_gain1 +static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xB0, 0xFF, 0x74, 0xFF, 0xFF}; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x31, 0x50, 0x50, 0x50}; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x3C, 0x31, 0x50, 0x50, 0x50}; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz -static const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x10, 0x20, 0x31, 0x41}; // rfm22_tx_data_rate1 -static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0x62, 0xC5, 0x27, 0x89}; // rfm22_tx_data_rate0 +static const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41}; // rfm22_tx_data_rate1 +static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0x56, 0x62, 0xC5, 0x27, 0x89}; // rfm22_tx_data_rate0 -static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x0D, 0x0D, 0x0D, 0x0D, 0x0D}; // rfm22_modulation_mode_control1 -static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23}; // rfm22_modulation_mode_control2 +static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D}; // rfm22_modulation_mode_control1 +static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x80, 0x23, 0x23, 0x23, 0x23}; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x33, 0x66, 0x9A, 0xCD}; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x50, 0x33, 0x66, 0x9A, 0xCD}; // rfm22_frequency_deviation // ************************************ // Scan Spectrum settings @@ -497,9 +494,6 @@ static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x // 10-nibble tx preamble length #define SS_LOOKUP_SIZE 2 -// xtal 1 ppm, 434MHz -static const uint32_t ss_rx_bandwidth[] = { 2600, 10600}; - static const uint8_t ss_reg_1C[] = { 0x51, 0x32}; // rfm22_if_filter_bandwidth static const uint8_t ss_reg_1D[] = { 0x00, 0x00}; // rfm22_afc_loop_gearshift_override @@ -2263,7 +2257,7 @@ static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev) { portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); uint8_t window = (uint8_t)(time & 0x7e); - return ((window == 0x7e) || (window == 0)); + return ((window == 0x7f) || (window == 0)); } static uint8_t rfm22_calcChannel(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 7b0b62369..744a43b68 100644 --- a/flight/PiOS/Common/pios_rfm22b_com.c +++ b/flight/PiOS/Common/pios_rfm22b_com.c @@ -74,7 +74,7 @@ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) else if (baud <= 19200) datarate = RFM22_datarate_32000; else if (baud <= 38400) - datarate = RFM22_datarate_64000; + datarate = RFM22_datarate_56000; else if (baud <= 57600) datarate = RFM22_datarate_128000; else if (baud <= 115200) diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index dc688d312..18512a420 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -67,10 +67,11 @@ enum rfm22b_datarate { RFM22_datarate_19200 = 7, RFM22_datarate_24000 = 8, RFM22_datarate_32000 = 9, - RFM22_datarate_64000 = 10, - RFM22_datarate_128000 = 11, - RFM22_datarate_192000 = 12, - RFM22_datarate_256000 = 13, + RFM22_datarate_56000 = 10, + RFM22_datarate_64000 = 11, + RFM22_datarate_128000 = 12, + RFM22_datarate_192000 = 13, + RFM22_datarate_256000 = 14, }; struct rfm22b_stats { From 538d854627b3e22808a8e48020da0b060c49af87 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 31 Mar 2013 16:37:14 +0100 Subject: [PATCH 19/40] Changed test for GCSRECEIVER in OPLink to PIOS_INCLUDE_GCSRCVR to match new define in pios_config. --- flight/PiOS/Common/pios_rfm22b.c | 10 +++++----- flight/targets/PipXtreme/System/inc/pios_config.h | 2 +- .../src/internals/projections/lks94projection.h | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index b152741f2..0b361b25b 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -52,7 +52,7 @@ #include #include -#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) +#if defined(PIOS_INCLUDE_GCSRCVR) #include #endif #include @@ -631,7 +631,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24; DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID); -#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) +#if defined(PIOS_INCLUDE_GCSRCVR) // Initialize the GCSReceive object GCSReceiverInitialize(); #endif @@ -1846,9 +1846,9 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) break; case PACKET_TYPE_PPM: { -#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR) +#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR) PHPpmPacketHandle ppmp = (PHPpmPacketHandle)&(rfm22b_dev->rx_packet); -#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) +#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) bool ppm_output = false; #endif #endif @@ -1865,7 +1865,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) } } #endif -#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) +#if defined(PIOS_INCLUDE_GCSRCVR) if (!ppm_output) { GCSReceiverData gcsRcvr; for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { diff --git a/flight/targets/PipXtreme/System/inc/pios_config.h b/flight/targets/PipXtreme/System/inc/pios_config.h index b7e7551fa..1da5edbe0 100755 --- a/flight/targets/PipXtreme/System/inc/pios_config.h +++ b/flight/targets/PipXtreme/System/inc/pios_config.h @@ -90,7 +90,7 @@ /* #define PIOS_INCLUDE_PPM_FLEXI */ /* #define PIOS_INCLUDE_DSM */ /* #define PIOS_INCLUDE_SBUS */ -/* #define PIOS_INCLUDE_GCSRCVR */ +#define PIOS_INCLUDE_GCSRCVR /* PIOS abstract receiver interface */ #define PIOS_INCLUDE_RCVR diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h index d3e09b868..1956cb613 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/lks94projection.h @@ -28,7 +28,7 @@ #define LKS94PROJECTION_H #include #include "cmath" -#include "../pureprojection.h" +#include namespace projections { From 61712be03da76fc82948e96b6bc5f2c3476ba99a Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 3 Apr 2013 02:05:51 +0100 Subject: [PATCH 20/40] Added 57600 kbps air datarate mode, and added setting of registers 69 and 58. --- flight/PiOS/Common/pios_rfm22b.c | 58 ++++++++++++++-------------- flight/PiOS/Common/pios_rfm22b_com.c | 2 +- flight/PiOS/inc/pios_rfm22b.h | 2 +- 3 files changed, 32 insertions(+), 30 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 0b361b25b..b8bbce9b5 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -457,31 +457,33 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S // xtal 10 ppm, 434MHz #define LOOKUP_SIZE 15 -static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 56000, 64000, 128000, 192000, 256000}; +static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 57600, 64000, 128000, 192000, 256000}; static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}; -static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x95, 0x07, 0x83, 0x8A, 0x8C}; // rfm22_if_filter_bandwidth +static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C}; // rfm22_if_filter_bandwidth static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44}; // rfm22_afc_loop_gearshift_override static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02}; // rfm22_afc_timing_control static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03}; // rfm22_clk_recovery_gearshift_override -static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x6B, 0x3F, 0x5E, 0x3F, 0x2F}; // rfm22_clk_recovery_oversampling_ratio +static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F}; // rfm22_clk_recovery_oversampling_ratio static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x02, 0x02, 0x02, 0x03, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02}; // rfm22_clk_recovery_offset2 -static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0x31, 0x0c, 0x5D, 0x0C, 0xBB}; // rfm22_clk_recovery_offset1 -static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0xD6, 0x4A, 0x86, 0x4A, 0x0D}; // rfm22_clk_recovery_offset0 -static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x02, 0x07, 0x05, 0x07, 0x07}; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xB0, 0xFF, 0x74, 0xFF, 0xFF}; // rfm22_clk_recovery_timing_loop_gain0 +static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB}; // rfm22_clk_recovery_offset1 +static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D}; // rfm22_clk_recovery_offset0 +static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07}; // rfm22_clk_recovery_timing_loop_gain1 +static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF}; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x3C, 0x31, 0x50, 0x50, 0x50}; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50}; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80}; // rfm22_cpcuu +static const uint8_t reg_69[] = { 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20}; // rfm22_agc_override1 static const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41}; // rfm22_tx_data_rate1 -static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0x56, 0x62, 0xC5, 0x27, 0x89}; // rfm22_tx_data_rate0 +static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89}; // rfm22_tx_data_rate0 static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D}; // rfm22_modulation_mode_control1 -static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x80, 0x23, 0x23, 0x23, 0x23}; // rfm22_modulation_mode_control2 +static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23}; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x50, 0x33, 0x66, 0x9A, 0xCD}; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD}; // rfm22_frequency_deviation // ************************************ // Scan Spectrum settings @@ -1056,6 +1058,8 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_d rfm22_write(rfm22b_dev, 0x24, reg_24[datarate]); // rfm22_clk_recovery_timing_loop_gain0 rfm22_write(rfm22b_dev, 0x25, reg_25[datarate]); + // rfm22_agc_override1 + rfm22_write(rfm22b_dev, RFM22_agc_override1, reg_69[datarate]); // rfm22_afc_limiter rfm22_write(rfm22b_dev, 0x2A, reg_2A[datarate]); @@ -1078,6 +1082,9 @@ static void rfm22_setDatarate(struct pios_rfm22b_dev * rfm22b_dev, enum rfm22b_d // rfm22_frequency_deviation rfm22_write(rfm22b_dev, 0x72, reg_72[datarate]); + // rfm22_cpcuu + rfm22_write(rfm22b_dev, 0x58, reg_58[datarate]); + rfm22_write(rfm22b_dev, RFM22_ook_counter_value1, 0x00); rfm22_write(rfm22b_dev, RFM22_ook_counter_value2, 0x00); } @@ -1223,20 +1230,17 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, // holds the hbsel (1 or 2) uint8_t hbsel; if (frequency_hz < 480000000) - hbsel = 1; + hbsel = 0; else - hbsel = 2; - uint8_t fb = (uint8_t)(frequency_hz / (10000000 * hbsel)); + hbsel = 1; + float freq_mhz = (float)(frequency_hz) / 1000000.0; + float xtal_freq_khz = 30000; + float sfreq = freq_mhz / (10.0 * (xtal_freq_khz / 30000.0) * (1 + hbsel)); + uint32_t fb = (uint32_t)sfreq - 24 + (64 + 32 * hbsel); + uint32_t fc = (uint32_t)((sfreq - (uint32_t)sfreq) * 64000.0); + uint8_t fch = (fc >> 8) & 0xff; + uint8_t fcl = fc & 0xff; - uint32_t fc = (uint32_t)(frequency_hz - (10000000 * hbsel * fb)); - - fc = (fc * 64u) / (10000ul * hbsel); - fb -= 24; - - if (hbsel > 1) - fb |= RFM22_fbs_hbsel; - - fb |= RFM22_fbs_sbse; // is this the RX LO polarity? // frequency hopping channel (0-255) rfm22b_dev->frequency_step_size = 156.25f * hbsel; @@ -1251,9 +1255,9 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, rfm22_write(rfm22b_dev, RFM22_frequency_offset2, 0); // set the carrier frequency - 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); + rfm22_write(rfm22b_dev, RFM22_frequency_band_select, fb & 0xff); + rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency1, fch); + rfm22_write(rfm22b_dev, RFM22_nominal_carrier_frequency0, fcl); } static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel) @@ -2511,8 +2515,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3); rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4); - rfm22_write(rfm22b_dev, RFM22_agc_override1, RFM22_agc_ovr1_agcen); - // set frequency hopping channel step size (multiples of 10kHz) rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, rfm22b_dev->frequency_hop_step_size_reg); diff --git a/flight/PiOS/Common/pios_rfm22b_com.c b/flight/PiOS/Common/pios_rfm22b_com.c index 72bd89da9..89364777c 100644 --- a/flight/PiOS/Common/pios_rfm22b_com.c +++ b/flight/PiOS/Common/pios_rfm22b_com.c @@ -75,7 +75,7 @@ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) else if (baud <= 19200) datarate = RFM22_datarate_32000; else if (baud <= 38400) - datarate = RFM22_datarate_56000; + datarate = RFM22_datarate_57600; else if (baud <= 57600) datarate = RFM22_datarate_128000; else if (baud <= 115200) diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 18512a420..4c1f43822 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -67,7 +67,7 @@ enum rfm22b_datarate { RFM22_datarate_19200 = 7, RFM22_datarate_24000 = 8, RFM22_datarate_32000 = 9, - RFM22_datarate_56000 = 10, + RFM22_datarate_57600 = 10, RFM22_datarate_64000 = 11, RFM22_datarate_128000 = 12, RFM22_datarate_192000 = 13, From c330db3a28e206ac995e95d40a32cb2ac85c3ea2 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 6 Apr 2013 01:32:15 +0100 Subject: [PATCH 21/40] Added setting of min/max frequencies on OPLink coordinator. --- flight/Libraries/inc/packet_handler.h | 3 + .../Modules/RadioComBridge/RadioComBridge.c | 25 ++-- flight/PiOS/Common/pios_rfm22b.c | 113 ++++++++++-------- flight/PiOS/inc/pios_rfm22b.h | 6 +- flight/PiOS/inc/pios_rfm22b_priv.h | 5 + flight/targets/RevoMini/System/pios_board.c | 9 +- .../plugins/config/configpipxtremewidget.cpp | 4 + .../src/plugins/config/pipxtreme.ui | 78 +++++++++++- shared/uavobjectdefinition/hwsettings.xml | 2 + shared/uavobjectdefinition/oplinksettings.xml | 4 + 10 files changed, 184 insertions(+), 65 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 1d2b04e3d..a7afc7d42 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -95,6 +95,9 @@ typedef struct { typedef struct { PHPacketHeader header; uint32_t source_id; + uint32_t min_frequency; + uint32_t max_frequency; + uint32_t channel_spacing; OPLinkSettingsMainPortOptions main_port; OPLinkSettingsFlexiPortOptions flexi_port; OPLinkSettingsVCPPortOptions vcp_port; diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index c1b1b213a..c3263276d 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -107,7 +107,8 @@ 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(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed); + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); static void updateSettings(); // **************** @@ -127,14 +128,14 @@ static int32_t RadioComBridgeStart(void) // Configure the com port configuration callback PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); + // Get the settings. + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + // Set the baudrates, etc. bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); if (is_coordinator) { - // Get the settings. - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - // Set the maximum radio RF power. switch (oplinkSettings.MaxRFPower) { @@ -167,10 +168,16 @@ static int32_t RadioComBridgeStart(void) break; } + // Set the frequency range. + PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing); + // Reinitilize the modem. PIOS_RFM22B_Reinit(pios_rfm22b_id); } + // Set the initial frequency. + PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency); + // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. 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)); @@ -505,7 +512,8 @@ static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEve * \param[in] com_speed The com port speed */ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed) + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) { // Update the com baud rate @@ -552,6 +560,9 @@ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, break; } + // Set the frequency range. + PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing); + // Update the OPLinkSettings object. OPLinkSettingsSet(&oplinkSettings); } @@ -561,7 +572,7 @@ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, } /** - * Update the oplink settings, called on startup. + * Update the oplink settings. */ static void updateSettings() { diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index b8bbce9b5..acc0c6897 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -65,13 +65,12 @@ #define ISR_TIMEOUT 2 // ms #define EVENT_QUEUE_SIZE 5 #define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 -#define RFM22B_DEFAULT_FREQUENCY 430000000 -#define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 -#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000 -#define RFM22B_NUM_CHANNELS 128 -#define RFM22B_DEFAULT_CHANNEL 28 #define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_7 #define RFM22B_LINK_QUALITY_THRESHOLD 20 +#define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 +#define RFM22B_MAXIMUM_FREQUENCY 440000000 +#define RFM22B_DEFAULT_FREQUENCY 433000000 +#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000 //#define RFM22B_TEST_DROPPED_PACKETS 1 // The maximum amount of time since the last message received to consider the connection broken. @@ -203,7 +202,7 @@ 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, uint8_t channel); +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size); static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel); static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev); @@ -485,31 +484,6 @@ static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD}; // rfm22_frequency_deviation -// ************************************ -// Scan Spectrum settings -// GFSK modulation -// no manchester encoding -// data whitening -// FIFO mode -// 5-nibble rx preamble length detection -// 10-nibble tx preamble length -#define SS_LOOKUP_SIZE 2 - -static const uint8_t ss_reg_1C[] = { 0x51, 0x32}; // rfm22_if_filter_bandwidth -static const uint8_t ss_reg_1D[] = { 0x00, 0x00}; // rfm22_afc_loop_gearshift_override - -static const uint8_t ss_reg_20[] = { 0xE8, 0x38}; // rfm22_clk_recovery_oversampling_ratio -static const uint8_t ss_reg_21[] = { 0x60, 0x02}; // rfm22_clk_recovery_offset2 -static const uint8_t ss_reg_22[] = { 0x20, 0x4D}; // rfm22_clk_recovery_offset1 -static const uint8_t ss_reg_23[] = { 0xC5, 0xD3}; // rfm22_clk_recovery_offset0 -static const uint8_t ss_reg_24[] = { 0x00, 0x07}; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t ss_reg_25[] = { 0x0F, 0xFF}; // rfm22_clk_recovery_timing_loop_gain0 - -static const uint8_t ss_reg_2A[] = { 0xFF, 0xFF}; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz - -static const uint8_t ss_reg_70[] = { 0x24, 0x2D}; // rfm22_modulation_mode_control1 -static const uint8_t ss_reg_71[] = { 0x2B, 0x23}; // rfm22_modulation_mode_control2 - static inline uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { @@ -599,12 +573,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->stats.tx_seq = 0; rfm22b_dev->stats.rx_seq = 0; - // initialize the frequency hopping step size (specified in 10khz increments). - uint32_t freq_hop_step_size = RFM22B_FREQUENCY_HOP_STEP_SIZE / 10000; - if (freq_hop_step_size > 255) { - freq_hop_step_size = 255; - } - rfm22b_dev->frequency_hop_step_size_reg = (uint8_t)freq_hop_step_size; + // Initialize the frequencies. + PIOS_RFM22B_SetFrequencyRange(*rfm22b_id, RFM22B_NOMINAL_CARRIER_FREQUENCY, RFM22B_NOMINAL_CARRIER_FREQUENCY, RFM22B_FREQUENCY_HOP_STEP_SIZE); + PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY); // Initialize the bindings. for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { @@ -759,6 +730,38 @@ 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 initial frequency + * \param[in] rfm22b_id The RFM22B device index. + * \param[in] min_freq The minimum frequency + * \param[in] max_freq The maximum frequency + * \param[in] step_size The channel step size + */ +void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32_t max_freq, uint32_t step_size) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) { + return; + } + rfm22b_dev->con_packet.min_frequency = min_freq; + rfm22b_dev->con_packet.max_frequency = max_freq; + rfm22b_dev->con_packet.channel_spacing = step_size; +} + +/** + * Sets the initial radio frequency range + * \param[in] rfm22b_id The RFM22B device index. + * \param[in] init_freq The initial frequency + */ +void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + if (!PIOS_RFM22B_validate(rfm22b_dev)) { + return; + } + rfm22b_dev->init_frequency = init_freq; +} + /** * Set the com port configuration callback (to receive com configuration over the air) * \param[in] rfm22b_id The rfm22b device. @@ -1225,8 +1228,10 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rf // ************************************ -static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t frequency_hz, uint8_t channel) +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size) { + uint32_t frequency_hz = min_frequency; + // holds the hbsel (1 or 2) uint8_t hbsel; if (frequency_hz < 480000000) @@ -1241,17 +1246,25 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t fch = (fc >> 8) & 0xff; uint8_t fcl = fc & 0xff; + // Calculate the number of frequency hopping channels. + rfm22b_dev->num_channels = (step_size == 0) ? 1 : (uint16_t)((max_frequency - min_frequency) / step_size); + + // initialize the frequency hopping step size (specified in 10khz increments). + uint32_t freq_hop_step_size = step_size / 10000; + if (freq_hop_step_size > 255) { + freq_hop_step_size = 255; + } + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, (uint8_t)freq_hop_step_size); // frequency hopping channel (0-255) rfm22b_dev->frequency_step_size = 156.25f * hbsel; // frequency hopping channel (0-255) - rfm22b_dev->frequency_hop_channel = channel; - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); + rfm22b_dev->frequency_hop_channel = 0; + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, 0); // no frequency offset rfm22_write(rfm22b_dev, RFM22_frequency_offset1, 0); - // no frequency offset rfm22_write(rfm22b_dev, RFM22_frequency_offset2, 0); // set the carrier frequency @@ -2243,9 +2256,11 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) // Call the com port configuration function if (rfm22b_dev->com_config_cb) - rfm22b_dev->com_config_cb(cph->main_port, cph->flexi_port, cph->vcp_port, cph->com_speed); + rfm22b_dev->com_config_cb(cph->main_port, cph->flexi_port, cph->vcp_port, cph->com_speed, + cph->min_frequency, cph->max_frequency, cph->channel_spacing); // Configure this modem from the connection request message. + rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->min_frequency, cph->max_frequency, cph->channel_spacing); rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true); PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power); } @@ -2267,10 +2282,11 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) { portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); // We change channels every 128 ms. - time = time >> 7; - // The channel is calculated using the 16 bit CRC as the pseudo random number generator, and there are 128 channels. - return (uint8_t)(time & 0x7f); - //return (uint8_t)(PIOS_CRC16_updateByte((time && 0xffff), 0) & 0x7f); + uint16_t n = (time >> 7) & 0xffff; + // The channel is calculated using the 16 bit CRC as the pseudo random number generator. + n = PIOS_CRC16_updateByte(n, 0); + float num_channels = rfm22b_dev->num_channels; + return (uint8_t)(num_channels * (float)n / (float)0xffff); } static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) @@ -2515,9 +2531,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_write(rfm22b_dev, RFM22_sync_word1, SYNC_BYTE_3); rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4); - // set frequency hopping channel step size (multiples of 10kHz) - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, rfm22b_dev->frequency_hop_step_size_reg); - // set the 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); @@ -2534,7 +2547,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) 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_NOMINAL_CARRIER_FREQUENCY, RFM22B_DEFAULT_CHANNEL); + rfm22_setNominalCarrierFrequency(rfm22b_dev, RFM22B_DEFAULT_FREQUENCY, RFM22B_DEFAULT_FREQUENCY, RFM22B_FREQUENCY_HOP_STEP_SIZE); rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); return RFM22B_EVENT_INITIALIZED; diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index 4c1f43822..88f7bff17 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -34,6 +34,7 @@ #include #include +/* Constant definitions */ enum gpio_direction {GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX}; /* Global Types */ @@ -98,12 +99,15 @@ struct rfm22b_stats { /* Callback function prototypes */ typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed); + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); /* 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_Reinit(uint32_t rfb22b_id); extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr); +extern void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32_t max_freq, uint32_t step_size); +extern void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq); extern void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id); extern void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb); extern void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 473157231..489bf4d29 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -773,6 +773,11 @@ struct pios_rfm22b_dev { // The number of times that the current packet has been resent. uint8_t cur_resent_count; + // The initial frequency + uint32_t init_frequency; + // The number of frequency hopping channels. + uint16_t num_channels; + // The frequency hopping step size float frequency_step_size; // current frequency hop channel diff --git a/flight/targets/RevoMini/System/pios_board.c b/flight/targets/RevoMini/System/pios_board.c index 1df723223..3f5f66962 100644 --- a/flight/targets/RevoMini/System/pios_board.c +++ b/flight/targets/RevoMini/System/pios_board.c @@ -43,7 +43,8 @@ #if defined(PIOS_INCLUDE_RFM22B) // Forward declarations static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed); + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing); #endif /** @@ -767,7 +768,8 @@ void PIOS_Board_Init(void) { * \param[in] com_speed The com port speed */ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed) + OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed, + uint32_t min_frequency, uint32_t max_frequency, uint32_t channel_spacing) { uint32_t comBaud = 9600; switch (com_speed) { @@ -796,6 +798,9 @@ static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, if (PIOS_COM_TELEM_RF) { PIOS_COM_ChangeBaud(PIOS_COM_TELEM_RF, comBaud); } + + // Set the frequency range. + PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, min_frequency, max_frequency, channel_spacing); } #endif diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 6c43557bb..a0e4c43de 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -63,6 +63,10 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort); addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); + addUAVObjectToWidgetRelation("OPLinkSettings", "MinFrequency", m_oplink->MinimumFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaximumFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "InitFrequency", m_oplink->InitFrequency); + addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSpacing", m_oplink->StepSize); addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index f904ab51f..d83f29dc2 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -2460,6 +2460,23 @@ + + + + Min Freq + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + @@ -2483,6 +2500,23 @@ + + + + Max Freq + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + @@ -2506,17 +2540,34 @@ - - + + - Max RF Tx Power(mW) + Initial Freq Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - + + + + + + + + + + + Max Power + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + @@ -2525,7 +2576,7 @@ - Set the maximum TX output power the modem will use + Set the maximum TX output power the modem will use (mW) Qt::LeftToRight @@ -2535,6 +2586,23 @@ + + + + Step Size + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 761252f96..5ee22c43e 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -17,6 +17,8 @@ + + diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index 94000d8e4..62d2b1edd 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -10,6 +10,10 @@ + + + + From fef2c16f5a93afb66cb4bf12c209c5591bb81de7 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 6 Apr 2013 02:25:59 +0100 Subject: [PATCH 22/40] RFM22B: Changed the connection process. Now the coordinator waits to receive a status packet from a modem that it is bound to before it sends the connection request. --- flight/PiOS/Common/pios_rfm22b.c | 68 +++++++++--------------------- flight/PiOS/inc/pios_rfm22b_priv.h | 5 --- 2 files changed, 20 insertions(+), 53 deletions(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index acc0c6897..1378336ce 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -188,7 +188,6 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d 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_txStart(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev); @@ -239,18 +238,7 @@ 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_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_INITIALIZED] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_ERROR] = RFM22B_STATE_ERROR, [RFM22B_EVENT_INITIALIZE] = RFM22B_STATE_INITIALIZING, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, @@ -263,7 +251,7 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [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_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR }, }, [RFM22B_STATE_ACCEPTING_CONNECTION] = { @@ -390,7 +378,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_REQUEST_CONNECTION] = RFM22B_STATE_INITIATING_CONNECTION, + [RFM22B_EVENT_REQUEST_CONNECTION] = RFM22B_STATE_REQUESTING_CONNECTION, [RFM22B_EVENT_RX_MODE] = RFM22B_STATE_RX_MODE, [RFM22B_EVENT_FAILURE] = RFM22B_STATE_TX_FAILURE, [RFM22B_EVENT_TIMEOUT] = RFM22B_STATE_TIMEOUT, @@ -413,7 +401,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_sendAck, .next_state = { [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, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, @@ -423,7 +410,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S .entry_fn = rfm22_sendNack, .next_state = { [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, [RFM22B_EVENT_FATAL_ERROR] = RFM22B_STATE_FATAL_ERROR, @@ -441,7 +427,6 @@ const static struct pios_rfm22b_transition rfm22b_transitions[RFM22B_STATE_NUM_S [RFM22B_STATE_ERROR] = { .entry_fn = rfm22_error, .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, @@ -1970,10 +1955,21 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) // 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 (!rfm22_isConnected(rfm22b_dev)) { - // Request a connection if we're not yet connected. - ret_event = RFM22B_EVENT_REQUEST_CONNECTION; + + } else if (!rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_STATUS)) { + + // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to. + PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet); + uint32_t source_id = status->source_id; + for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { + if (rfm22b_dev->bindings[i].pairID == source_id) { + rfm22b_dev->cur_binding = i; + ret_event = RFM22B_EVENT_REQUEST_CONNECTION; + break; + } + } } // Change the channel @@ -2116,25 +2112,8 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; rfm22b_dev->prev_tx_packet = NULL; - // Go to the next binding, if the previous tx packet was a connection request - if (rfm22b_dev->tx_packet->header.type == PACKET_TYPE_CON_REQUEST) - { - PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); - // Increment the current binding index to the next non-zero binding. - for (uint8_t i = 0; OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - if (++(rfm22b_dev->cur_binding) >= OPLINKSETTINGS_BINDINGS_NUMELEM) - rfm22b_dev->cur_binding = 0; - if (rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID != 0) - break; - } - rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; - cph->header.destination_id = rfm22b_dev->destination_id; - cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; - cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; - cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; - cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; - } else { #ifndef PIOS_RFM22B_PERIODIC_CHANNEL_HOP + if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_CON_REQUEST) { // First resend on the current channel, then resend on the next channel in case the receive has changed channels, then fallback. rfm22b_dev->cur_resent_count++; switch (rfm22b_dev->cur_resent_count) { @@ -2153,8 +2132,8 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d rfm22_setFreqHopChannel(rfm22b_dev, RFM22B_DEFAULT_CHANNEL); break; } -#endif //PIOS_RFM22B_PERIODIC_CHANNEL_HOP } +#endif //PIOS_RFM22B_PERIODIC_CHANNEL_HOP // Increment the reset packet counter if we're connected. if (rfm22_isConnected(rfm22b_dev)) { @@ -2215,14 +2194,6 @@ 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) { PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); @@ -2243,6 +2214,7 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf cph->max_tx_power = rfm22b_dev->tx_power; rfm22b_dev->time_to_send = true; rfm22b_dev->send_connection_request = true; + rfm22b_dev->prev_tx_packet = NULL; return RFM22B_EVENT_TX_START; } diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 489bf4d29..d8041142e 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -578,12 +578,8 @@ enum pios_rfm22b_dev_magic { 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_RX_MODE, RFM22B_STATE_WAIT_PREAMBLE, RFM22B_STATE_WAIT_SYNC, @@ -610,7 +606,6 @@ enum pios_rfm22b_event { RFM22B_EVENT_INITIALIZE, RFM22B_EVENT_INITIALIZED, RFM22B_EVENT_REQUEST_CONNECTION, - RFM22B_EVENT_WAIT_FOR_CONNECTION, RFM22B_EVENT_CONNECTION_REQUESTED, RFM22B_EVENT_PACKET_ACKED, RFM22B_EVENT_PACKET_NACKED, From 4564718708080b21352b3f325a09d7860b63aee5 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 6 Apr 2013 04:28:25 +0100 Subject: [PATCH 23/40] Added setting of max RF power and initial frequency on Revo. --- flight/Modules/PipXtreme/pipxtrememod.c | 1 - flight/PiOS/Common/pios_rfm22b.c | 8 +++-- flight/targets/RevoMini/System/pios_board.c | 10 ++++++ .../src/plugins/config/configrevohwwidget.cpp | 13 ++++++- .../src/plugins/config/configrevohwwidget.ui | 36 +++++++++++++++++-- 5 files changed, 61 insertions(+), 7 deletions(-) diff --git a/flight/Modules/PipXtreme/pipxtrememod.c b/flight/Modules/PipXtreme/pipxtrememod.c index 5aaed3831..729333a75 100644 --- a/flight/Modules/PipXtreme/pipxtrememod.c +++ b/flight/Modules/PipXtreme/pipxtrememod.c @@ -47,7 +47,6 @@ // Private constants #define SYSTEM_UPDATE_PERIOD_MS 1000 -#define LED_BLINK_RATE_HZ 5 #if defined(PIOS_SYSTEM_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 1378336ce..beddb1e08 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -559,8 +559,8 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->stats.rx_seq = 0; // Initialize the frequencies. - PIOS_RFM22B_SetFrequencyRange(*rfm22b_id, RFM22B_NOMINAL_CARRIER_FREQUENCY, RFM22B_NOMINAL_CARRIER_FREQUENCY, RFM22B_FREQUENCY_HOP_STEP_SIZE); PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY); + PIOS_RFM22B_SetFrequencyRange(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY, RFM22B_DEFAULT_FREQUENCY, RFM22B_FREQUENCY_HOP_STEP_SIZE); // Initialize the bindings. for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { @@ -1361,6 +1361,10 @@ static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev) if (rfm22b_dev->prev_tx_packet || rfm22b_dev->send_ppm || rfm22b_dev->send_status) return true; + // Are we not connected yet? + if (!rfm22_isConnected(rfm22b_dev)) + return true; + // Is there some data ready to sent? PHPacketHandle dp = &rfm22b_dev->data_packet; if (dp->header.data_size > 0) @@ -2519,7 +2523,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) 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, RFM22B_DEFAULT_FREQUENCY, RFM22B_FREQUENCY_HOP_STEP_SIZE); + rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE); rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); return RFM22B_EVENT_INITIALIZED; diff --git a/flight/targets/RevoMini/System/pios_board.c b/flight/targets/RevoMini/System/pios_board.c index 3f5f66962..98c9067f9 100644 --- a/flight/targets/RevoMini/System/pios_board.c +++ b/flight/targets/RevoMini/System/pios_board.c @@ -631,6 +631,10 @@ void PIOS_Board_Init(void) { #if defined(PIOS_INCLUDE_RFM22B) uint8_t hwsettings_radioport; HwSettingsRadioPortGet(&hwsettings_radioport); + uint8_t hwsettings_maxrfpower; + HwSettingsMaxRFPowerGet(&hwsettings_maxrfpower); + uint32_t hwsettings_deffreq; + HwSettingsDefaultFrequencyGet(&hwsettings_deffreq); switch (hwsettings_radioport) { case HWSETTINGS_RADIOPORT_DISABLED: break; @@ -642,6 +646,12 @@ 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); } + + // Set the modem parameters and reinitilize the modem. + PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, hwsettings_deffreq); + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, hwsettings_maxrfpower); + PIOS_RFM22B_Reinit(pios_rfm22b_id); + #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); diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp index 6564819a7..bd96ca5ee 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp @@ -64,7 +64,9 @@ ConfigRevoHWWidget::ConfigRevoHWWidget(QWidget *parent) : ConfigTaskWidget(paren addUAVObjectToWidgetRelation("HwSettings","GPSSpeed",m_ui->cbMainGPSSpeed); addUAVObjectToWidgetRelation("HwSettings","ComUsbBridgeSpeed",m_ui->cbMainComSpeed); - addUAVObjectToWidgetRelation("HwSettings","RadioPort",m_ui->cbModem); + addUAVObjectToWidgetRelation("HwSettings", "RadioPort", m_ui->cbModem); + addUAVObjectToWidgetRelation("HwSettings", "MaxRFPower", m_ui->cbTxPower); + addUAVObjectToWidgetRelation("HwSettings", "DefaultFrequency", m_ui->leInitFreq); connect(m_ui->cchwHelp,SIGNAL(clicked()),this,SLOT(openHelp())); @@ -288,6 +290,15 @@ void ConfigRevoHWWidget::modemPortChanged(int index) if(m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } + m_ui->lblTxPower->setVisible(true); + m_ui->cbTxPower->setVisible(true); + m_ui->lblInitFreq->setVisible(true); + m_ui->leInitFreq->setVisible(true); + } else { + m_ui->lblTxPower->setVisible(false); + m_ui->cbTxPower->setVisible(false); + m_ui->lblInitFreq->setVisible(false); + m_ui->leInitFreq->setVisible(false); } } diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui index 199d2e817..fb81114bd 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui @@ -169,9 +169,6 @@ - - - @@ -375,6 +372,39 @@ + + + + + + + Max Tx Power (mW) + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + + + + + Initial Frequency (Hz) + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + + + + + From d36cf828bc47038d5e98661ec4a36319091e71ca Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 6 Apr 2013 16:27:02 +0100 Subject: [PATCH 24/40] RFM22B: Don't send a connection request from a non-coordinator. --- flight/PiOS/Common/pios_rfm22b.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index beddb1e08..187396d22 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -1962,7 +1962,7 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) rfm22_setConnectionParameters(rfm22b_dev); - } else if (!rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_STATUS)) { + } else if (rfm22b_dev->coordinator && !rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_STATUS)) { // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to. PHStatusPacketHandle status = (PHStatusPacketHandle)&(rfm22b_dev->rx_packet); From ee96890b9631faf3b3dc6844ade76163892cdc31 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 6 Apr 2013 16:59:03 +0100 Subject: [PATCH 25/40] Initialize the Radio configuration widgets in the Revo HW configuration page correctly. --- ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp index bd96ca5ee..b4416eb93 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp @@ -103,6 +103,7 @@ void ConfigRevoHWWidget::refreshWidgetsValues(UAVObject *obj) usbVCPPortChanged(0); mainPortChanged(0); flexiPortChanged(0); + modemPortChanged(0); } void ConfigRevoHWWidget::updateObjectsFromWidgets() From 95c150213a27e57a34140e49302ec137ea2c6db0 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 6 Apr 2013 17:56:57 +0100 Subject: [PATCH 26/40] Don't send GCSReceiver when using OPLink receiver (on Revo). --- flight/PiOS/Common/pios_rfm22b.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 187396d22..e43997e09 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -1859,6 +1859,7 @@ static enum pios_rfm22b_event rfm22_rxData(struct pios_rfm22b_dev *rfm22b_dev) #endif #endif #if defined(PIOS_INCLUDE_RFM22B_RCVR) + ppm_output = true; for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { rfm22b_dev->ppm_channel[i] = ppmp->channels[i]; } From 90b0987e43ad23ad944ce67572eef897b7cd35d6 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 7 Apr 2013 02:16:46 +0100 Subject: [PATCH 27/40] Fixed setting max Tx power on Revo. --- flight/Libraries/inc/packet_handler.h | 1 - flight/PiOS/Common/pios_rfm22b.c | 10 ++----- flight/PiOS/inc/pios_rfm22b_priv.h | 4 --- flight/targets/RevoMini/System/pios_board.c | 31 ++++++++++++++++++++- 4 files changed, 32 insertions(+), 14 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index a7afc7d42..be92aa6a6 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -102,7 +102,6 @@ typedef struct { OPLinkSettingsFlexiPortOptions flexi_port; OPLinkSettingsVCPPortOptions vcp_port; OPLinkSettingsComSpeedOptions com_speed; - uint8_t max_tx_power; uint8_t ecc[RS_ECC_NPARITY]; } PHConnectionPacket, *PHConnectionPacketHandle; diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index e43997e09..df7b92294 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -65,7 +65,7 @@ #define ISR_TIMEOUT 2 // ms #define EVENT_QUEUE_SIZE 5 #define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 -#define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_7 +#define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 #define RFM22B_LINK_QUALITY_THRESHOLD 20 #define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 #define RFM22B_MAXIMUM_FREQUENCY 440000000 @@ -1496,10 +1496,6 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) rfm22_write(rfm22b_dev, RFM22_modulation_mode_control2, fd_bit | RFM22_mmc2_dtmod_fifo | RFM22_mmc2_modtyp_gfsk); - // set the 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_DEFAULT_TX_POWER); - // clear FIFOs 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); @@ -2216,7 +2212,6 @@ static enum pios_rfm22b_event rfm22_requestConnection(struct pios_rfm22b_dev *rf cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; - cph->max_tx_power = rfm22b_dev->tx_power; rfm22b_dev->time_to_send = true; rfm22b_dev->send_connection_request = true; rfm22b_dev->prev_tx_packet = NULL; @@ -2239,7 +2234,6 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) // Configure this modem from the connection request message. rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->min_frequency, cph->max_frequency, cph->channel_spacing); rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true); - PIOS_RFM22B_SetTxPower((uint32_t)rfm22b_dev, cph->max_tx_power); } #ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP @@ -2509,7 +2503,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_write(rfm22b_dev, RFM22_sync_word0, SYNC_BYTE_4); // set the 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); + rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); // TX FIFO Almost Full Threshold (0 - 63) rfm22_write(rfm22b_dev, RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index d8041142e..75ffcd04a 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -504,10 +504,6 @@ #define RFM22_tx_power 0x6D // R/W #define RFM22_tx_pwr_lna_sw 0x08 // LNA Switch Controller. If set, lna_sw control from the digital will go high during TX modes, and low during other times. If reset, the digital control signal is low at all times. -#define RFM22_tx_pwr_papeaklvl_0 0x10 // " " -#define RFM22_tx_pwr_papeaklvl_1 0x20 // PA Peak Detect Level (direct from register). 00 = 6.5, 01 = 7, 10 = 7.5, 11 = 8, 00 = default -#define RFM22_tx_pwr_papeaken 0x40 // PA Peak Detector Enable. -#define RFM22_tx_pwr_papeakval 0x80 // PA Peak Detector Value Read Register. Reading a 1 in this register when the papeaken=1 then the PA drain voltage is too high and the match network needs adjusting for optimal efficiency. #define RFM22_tx_data_rate1 0x6E // R/W #define RFM22_tx_data_rate0 0x6F // R/W diff --git a/flight/targets/RevoMini/System/pios_board.c b/flight/targets/RevoMini/System/pios_board.c index 98c9067f9..b29004192 100644 --- a/flight/targets/RevoMini/System/pios_board.c +++ b/flight/targets/RevoMini/System/pios_board.c @@ -649,7 +649,36 @@ void PIOS_Board_Init(void) { // Set the modem parameters and reinitilize the modem. PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, hwsettings_deffreq); - PIOS_RFM22B_SetTxPower(pios_rfm22b_id, hwsettings_maxrfpower); + switch (hwsettings_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; + default: + // do nothing + break; + } PIOS_RFM22B_Reinit(pios_rfm22b_id); #ifdef PIOS_INCLUDE_RFM22B_COM From e27be3f594a30b7d8067306825b8763230e8d1f3 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 7 Apr 2013 15:59:31 +0100 Subject: [PATCH 28/40] Removed code for per message channel hop. --- flight/Libraries/inc/packet_handler.h | 2 - .../PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 1 - flight/PiOS/Boards/STM32F4xx_RevoMini.h | 1 - flight/PiOS/Common/pios_rfm22b.c | 55 +------------------ flight/PiOS/inc/pios_rfm22b_priv.h | 6 -- 5 files changed, 2 insertions(+), 63 deletions(-) diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index be92aa6a6..3f654584b 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -69,9 +69,7 @@ typedef struct { #define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP portTickType packet_recv_time; -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP uint8_t ecc[RS_ECC_NPARITY]; } PHAckNackPacket, *PHAckNackPacketHandle; diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index 3810dc05d..d4b1e79f4 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -137,7 +137,6 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1 //------------------------- #define PIOS_MASTER_CLOCK 72000000 #define PIOS_PERIPHERAL_CLOCK (PIOS_MASTER_CLOCK / 2) -#define PIOS_RFM22B_PERIODIC_CHANNEL_HOP //------------------------- // Interrupt Priorities diff --git a/flight/PiOS/Boards/STM32F4xx_RevoMini.h b/flight/PiOS/Boards/STM32F4xx_RevoMini.h index 1b15e1a3f..31f880fac 100644 --- a/flight/PiOS/Boards/STM32F4xx_RevoMini.h +++ b/flight/PiOS/Boards/STM32F4xx_RevoMini.h @@ -212,7 +212,6 @@ extern uint32_t pios_packet_handler; // Default APB2 Prescaler = 2 // #define PIOS_PERIPHERAL_APB2_CLOCK PIOS_SYSCLK -#define PIOS_RFM22B_PERIODIC_CHANNEL_HOP //------------------------- // Interrupt Priorities diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index df7b92294..28b46dc35 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -207,12 +207,10 @@ static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_ready_to_send(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev); -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks); static bool rfm22_inChannelBuffer(struct pios_rfm22b_dev *rfm22b_dev); static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev); -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP static void rfm22_clearLEDs(); // SPI read/write functions @@ -1388,13 +1386,8 @@ 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. -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP if (!rfm22b_dev->time_to_send || (rfm22_inChannelBuffer(rfm22b_dev) && rfm22_isConnected(rfm22b_dev))) return RFM22B_EVENT_RX_MODE; -#else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP - if (!rfm22b_dev->time_to_send) - return RFM22B_EVENT_RX_MODE; -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP // See if there's a packet ready to send. if (rfm22b_dev->tx_packet) @@ -1456,12 +1449,10 @@ static enum pios_rfm22b_event rfm22_txStart(struct pios_rfm22b_dev *rfm22b_dev) D2_LED_OFF; #endif -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP // Change the channel if necessary. if (((p->header.type != PACKET_TYPE_ACK) && (p->header.type != PACKET_TYPE_ACK_RTS)) || (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) rfm22_changeChannel(rfm22b_dev); -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP // Add the error correcting code. encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); @@ -1974,14 +1965,10 @@ static enum pios_rfm22b_event rfm22_txData(struct pios_rfm22b_dev *rfm22b_dev) } // Change the channel -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP // On the remote side, we initialize the time delta when we finish sending the ACK for the connection request message. if (rfm22b_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { rfm22b_dev->time_delta = portMAX_DELAY - curTicks; } -#else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP - rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(rfm22b_dev->rx_packet.header.seq_num, 0) & 0x7f); -#endif // !PIOS_RFM22B_PERIODIC_CHANNEL_HOP } else if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_NACK) { @@ -2025,9 +2012,7 @@ static enum pios_rfm22b_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) 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; -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP aph->packet_recv_time = rfm22_coordinatorTime(rfm22b_dev, rfm22b_dev->rx_complete_ticks); -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP rfm22b_dev->tx_packet = (PHPacketHandle)aph; rfm22b_dev->time_to_send = true; return RFM22B_EVENT_TX_START; @@ -2071,7 +2056,6 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de break; } -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP // On the coordinator side, we initialize the time delta when we receive the ACK for the connection request message. if (prev->header.type == PACKET_TYPE_CON_REQUEST) { rfm22b_dev->time_delta = portMAX_DELAY - rfm22b_dev->rx_complete_ticks; @@ -2083,10 +2067,6 @@ static enum pios_rfm22b_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_de // This is not working yet rfm22b_dev->time_delta += remote_rx_time - local_tx_time; } -#else // PIOS_RFM22B_PERIODIC_CHANNEL_HOP - // Change the channel - rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(prev->header.seq_num, 0) & 0x7f); -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP // Reset the resend count rfm22b_dev->cur_resent_count = 0; @@ -2113,29 +2093,6 @@ static enum pios_rfm22b_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_d rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; rfm22b_dev->prev_tx_packet = NULL; -#ifndef PIOS_RFM22B_PERIODIC_CHANNEL_HOP - if (rfm22b_dev->tx_packet->header.type != PACKET_TYPE_CON_REQUEST) { - // First resend on the current channel, then resend on the next channel in case the receive has changed channels, then fallback. - rfm22b_dev->cur_resent_count++; - switch (rfm22b_dev->cur_resent_count) { - case 1: - rfm22b_dev->prev_frequency_hop_channel = rfm22b_dev->frequency_hop_channel; - case 2: - case 5: - rfm22_setFreqHopChannel(rfm22b_dev, rfm22b_dev->prev_frequency_hop_channel); - break; - case 3: - case 4: - case 6: - rfm22_setFreqHopChannel(rfm22b_dev, PIOS_CRC16_updateByte(rfm22b_dev->tx_packet->header.seq_num, 0) & 0x7f); - break; - default: - rfm22_setFreqHopChannel(rfm22b_dev, RFM22B_DEFAULT_CHANNEL); - break; - } - } -#endif //PIOS_RFM22B_PERIODIC_CHANNEL_HOP - // Increment the reset packet counter if we're connected. if (rfm22_isConnected(rfm22b_dev)) { rfm22b_add_rx_status(rfm22b_dev, RFM22B_RESENT_TX_PACKET); @@ -2236,7 +2193,6 @@ static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true); } -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks) { return ticks + rfm22b_dev->time_delta; @@ -2267,7 +2223,6 @@ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) } return false; } -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP static enum pios_rfm22b_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) { @@ -2301,8 +2256,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_clearLEDs(); // Initialize the detected device statistics. - for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) - { + 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; @@ -2334,9 +2288,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->rx_buffer_wr = 0; rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; rfm22b_dev->frequency_hop_channel = 0; -#ifndef PIOS_RFM22B_PERIODIC_CHANNEL_HOP - rfm22b_dev->prev_frequency_hop_channel = 0; -#endif // !PIOS_RFM22B_PERIODIC_CHANNEL_HOP rfm22b_dev->afc_correction_Hz = 0; rfm22b_dev->packet_start_ticks = 0; rfm22b_dev->tx_complete_ticks = 0; @@ -2345,8 +2296,7 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); - for (int i = 50; i > 0; i--) - { + for (int i = 50; i > 0; i--) { // read the status registers rfm22b_dev->int_status1 = rfm22_read(rfm22b_dev, RFM22_interrupt_status1); rfm22b_dev->int_status2 = rfm22_read(rfm22b_dev, RFM22_interrupt_status2); @@ -2354,7 +2304,6 @@ static enum pios_rfm22b_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) // wait 1ms PIOS_DELAY_WaitmS(1); - } // **************** diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 75ffcd04a..831338987 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -773,10 +773,6 @@ struct pios_rfm22b_dev { float frequency_step_size; // current frequency hop channel uint8_t frequency_hop_channel; -#ifndef PIOS_RFM22B_PERIODIC_CHANNEL_HOP - // previous frequency hop channel - uint8_t prev_frequency_hop_channel; -#endif // !PIOS_RFM22B_PERIODIC_CHANNEL_HOP // the frequency hop step size uint8_t frequency_hop_step_size_reg; // afc correction reading (in Hz) @@ -786,9 +782,7 @@ struct pios_rfm22b_dev { portTickType packet_start_ticks; portTickType tx_complete_ticks; portTickType rx_complete_ticks; -#ifdef PIOS_RFM22B_PERIODIC_CHANNEL_HOP portTickType time_delta; -#endif // PIOS_RFM22B_PERIODIC_CHANNEL_HOP // The maximum time (ms) that it should take to transmit / receive a packet. uint32_t max_packet_time; From 081c016dfabc1bb5c05fe2f5e8bf9cae0b0e4226 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 7 Apr 2013 16:04:24 +0100 Subject: [PATCH 29/40] Added braces around if body. --- ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp index 89260ff30..fa60d9154 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjectfield.cpp @@ -1015,8 +1015,9 @@ void UAVObjectField::setValue(const QVariant& value, quint32 index) { qint8 tmpenum = options.indexOf( value.toString() ); // Default to 0 on invalid values. - if(tmpenum < 0) + if(tmpenum < 0) { tmpenum = 0; + } memcpy(&data[offset + numBytesPerElement*index], &tmpenum, numBytesPerElement); break; } From 9591791d9331a73fdd2f95378a4b487977539908 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sun, 7 Apr 2013 18:28:25 +0200 Subject: [PATCH 30/40] Update KNOWN_ISSUES.txt --- KNOWN_ISSUES.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/KNOWN_ISSUES.txt b/KNOWN_ISSUES.txt index a35b55945..c9ac135f1 100644 --- a/KNOWN_ISSUES.txt +++ b/KNOWN_ISSUES.txt @@ -1,7 +1,7 @@ Here is a list of some known unresolved issues. If an issue has JIRA ID [OP-XXX], you may track it using the following URL: http://bugs.openpilot.org/browse/OP-XXX -+ Missing Translations, use English. ++ Only French translation is updated, use English for other locales or help with translations. + Radio Wizard confused by a reversed throttle, fix it on your transmitter before starting wizard. + Radio Wizard Throttle display does not show full range properly. + [Windows 8] USB Driver is broken. @@ -17,6 +17,4 @@ following URL: http://bugs.openpilot.org/browse/OP-XXX + [OP-725] GCS camera stab config error message disappears too fast (but config error is cleared as it should) + [OP-767] GCS does not send AttitudeActual packets over serial port when GPS is connected and system is armed + [OP-768] GCS does not show UAV position on the map (master or next CC branches, but works in Revo branches) -+ [OP-682] GCS crashes on firmware page. Noted on Windows and OSX platforms, difficult to reproduce. - Workaround: use Vehicle setup wizard to update the firmware. + [OP-769] Can't enter "12,45" on German system. Workaround: change GCS language (in fact, locale) to German. From be0a386cf89e11d79fbe2e4823b02b95018bdc8c Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sun, 7 Apr 2013 19:48:33 +0300 Subject: [PATCH 31/40] OP-821: ignore Eclipse data that git would like to track (fixed by Nigel) --- .gitignore | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/.gitignore b/.gitignore index 06eca8f32..142a5efbf 100644 --- a/.gitignore +++ b/.gitignore @@ -50,5 +50,16 @@ openpilotgcs-build-desktop # Ignore build output from Eclipse android builds /androidgcs/bin/ /androidgcs/gen/ + +# Ignore Eclipse Projects and Metadata /.cproject /.project +/.metadata + +# Ignore Eclipse temp folder, git plugin based? +RemoteSystemsTempFiles + +# Ignore patch-related files +*.rej +*.orig +*.diff~ From 8f516107766607bf1b9581d4e678ff1d334ccce7 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Thu, 11 Apr 2013 14:40:52 +0100 Subject: [PATCH 32/40] Fixed some formatting issues. --- .../src/plugins/uavobjectwidgetutils/configtaskwidget.cpp | 7 +++++-- .../src/plugins/uavobjectwidgetutils/configtaskwidget.h | 3 +-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index 08f6cde02..bf7a7dcc3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -1132,19 +1132,21 @@ bool ConfigTaskWidget::setWidgetFromVariant(QWidget *widget, QVariant value, dou } else if(QLineEdit * cb=qobject_cast(widget)) { - if((scale== 0) || (scale == 1)) { + if ((scale== 0) || (scale == 1)) { if(units == "hex") { cb->setText(QString::number(value.toUInt(), 16).toUpper()); } else { cb->setText(value.toString()); } - } else + } else { cb->setText(QString::number((value.toDouble()/scale))); + } return true; } else return false; } + /** * Sets a widget from a variant * @param widget pointer for the widget to set @@ -1156,6 +1158,7 @@ bool ConfigTaskWidget::setWidgetFromVariant(QWidget *widget, QVariant value, dou { return setWidgetFromVariant(widget, value, scale, QString("")); } + /** * Sets a widget from a UAVObject field * @param widget pointer to the widget to set diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h index d3c5c21cf..42db71297 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.h @@ -71,9 +71,8 @@ public: QString getUnits() const { if (field) { return field->getUnits(); - } else { - return QString(""); } + return QString(""); } }; From bc4d25242f36c38dc2c637840e97cdd63a817346 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Fri, 12 Apr 2013 10:46:25 +0200 Subject: [PATCH 33/40] makefile: use the same coding style for all make variables --- flight/targets/CopterControl/Makefile | 2 +- flight/targets/OSD/Makefile | 2 +- flight/targets/PipXtreme/Makefile | 2 +- flight/targets/RevoMini/Makefile | 2 +- flight/targets/Revolution/Makefile | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/flight/targets/CopterControl/Makefile b/flight/targets/CopterControl/Makefile index 1db667a81..f5a2b64b3 100644 --- a/flight/targets/CopterControl/Makefile +++ b/flight/targets/CopterControl/Makefile @@ -58,7 +58,7 @@ endif # Use file-extension c for "c-only"-files ifndef TESTAPP ## Application Core - SRC += ${OPMODULEDIR}/System/systemmod.c + SRC += $(OPMODULEDIR)}/System/systemmod.c SRC += $(OPSYSTEM)/coptercontrol.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/pios_usb_board_data.c diff --git a/flight/targets/OSD/Makefile b/flight/targets/OSD/Makefile index 9ff2df65d..2d0deafcd 100644 --- a/flight/targets/OSD/Makefile +++ b/flight/targets/OSD/Makefile @@ -44,7 +44,7 @@ CFLAGS += -ffast-math # Use file-extension c for "c-only"-files ifndef TESTAPP ## Application Core - SRC += ${OPMODULEDIR}/System/systemmod.c + SRC += $(OPMODULEDIR)/System/systemmod.c SRC += $(OPSYSTEM)/osd.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/pios_usb_board_data.c diff --git a/flight/targets/PipXtreme/Makefile b/flight/targets/PipXtreme/Makefile index 8eeb6195b..e0de6fe89 100644 --- a/flight/targets/PipXtreme/Makefile +++ b/flight/targets/PipXtreme/Makefile @@ -36,7 +36,7 @@ OPTMODULES = # Use file-extension c for "c-only"-files ifndef TESTAPP ## Application Core - SRC += ${OPMODULEDIR}/PipXtreme/pipxtrememod.c + SRC += $(OPMODULEDIR)/PipXtreme/pipxtrememod.c SRC += $(OPSYSTEM)/pipxtreme.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/pios_usb_board_data.c diff --git a/flight/targets/RevoMini/Makefile b/flight/targets/RevoMini/Makefile index f432a535a..47f631789 100644 --- a/flight/targets/RevoMini/Makefile +++ b/flight/targets/RevoMini/Makefile @@ -62,7 +62,7 @@ CFLAGS += -ffast-math # Use file-extension c for "c-only"-files ifndef TESTAPP ## Application Core - SRC += ${OPMODULEDIR}/System/systemmod.c + SRC += $(OPMODULEDIR)/System/systemmod.c SRC += $(OPSYSTEM)/revolution.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/pios_usb_board_data.c diff --git a/flight/targets/Revolution/Makefile b/flight/targets/Revolution/Makefile index 60ae27377..42b17ea00 100644 --- a/flight/targets/Revolution/Makefile +++ b/flight/targets/Revolution/Makefile @@ -62,7 +62,7 @@ CFLAGS += -ffast-math # Use file-extension c for "c-only"-files ifndef TESTAPP ## Application Core - SRC += ${OPMODULEDIR}/System/systemmod.c + SRC += $(OPMODULEDIR)/System/systemmod.c SRC += $(OPSYSTEM)/revolution.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/pios_usb_board_data.c From a12470f7a7316fff9e061c23514ae3777c8b6e74 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Wed, 10 Apr 2013 10:01:03 +0300 Subject: [PATCH 34/40] makefile: if all_fw or all_flight are requested, gcs will depend on opfw_resource --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index d18a66497..957f7b748 100644 --- a/Makefile +++ b/Makefile @@ -755,8 +755,8 @@ $(OPFW_RESOURCE): $(FW_TARGETS) $(V1) $(MKDIR) -p $(dir $@) $(V1) $(ECHO) $(QUOTE)$(OPFW_CONTENTS)$(QUOTE) > $@ -# If opfw_resource or all are requested, GCS should depend on the resource -ifneq ($(strip $(filter opfw_resource all,$(MAKECMDGOALS))),) +# If opfw_resource or all firmware are requested, GCS should depend on the resource +ifneq ($(strip $(filter opfw_resource all all_fw all_flight,$(MAKECMDGOALS))),) $(eval openpilotgcs: | opfw_resource) endif From 5040a13efbe13440e1977a28bfe4179345cf47bb Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Fri, 12 Apr 2013 10:53:00 +0200 Subject: [PATCH 35/40] gcs: set default UDP port to 9000 as used by simposix firmware --- .../openpilotgcs/default_configurations/Developer.xml | 2 +- .../openpilotgcs/default_configurations/OpenPilotGCS.xml | 9 +++++++++ .../default_configurations/OpenPilotGCS_wide.xml | 9 +++++++++ 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml index 651a2f025..d04c74168 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/Developer.xml @@ -16,7 +16,7 @@ - 1 + 9000 0 1 diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml index 7e6179226..5050e9f23 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS.xml @@ -13,6 +13,15 @@ default false + + + + 9000 + 0 + + 1 + + 0 diff --git a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml index ab7449588..bd99b9685 100644 --- a/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml +++ b/ground/openpilotgcs/share/openpilotgcs/default_configurations/OpenPilotGCS_wide.xml @@ -13,6 +13,15 @@ default false + + + + 9000 + 0 + + 1 + + 0 From 8ab580ae82a96f15741ba6df0de44f37b20b5e6b Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Fri, 12 Apr 2013 12:50:12 +0300 Subject: [PATCH 36/40] Remove stray '}' in a makefile --- flight/targets/CopterControl/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/targets/CopterControl/Makefile b/flight/targets/CopterControl/Makefile index f5a2b64b3..c2c69871d 100644 --- a/flight/targets/CopterControl/Makefile +++ b/flight/targets/CopterControl/Makefile @@ -58,7 +58,7 @@ endif # Use file-extension c for "c-only"-files ifndef TESTAPP ## Application Core - SRC += $(OPMODULEDIR)}/System/systemmod.c + SRC += $(OPMODULEDIR)/System/systemmod.c SRC += $(OPSYSTEM)/coptercontrol.c SRC += $(OPSYSTEM)/pios_board.c SRC += $(OPSYSTEM)/pios_usb_board_data.c From e15c40b35db97ed19401f334c7b64b0ff09f882a Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Fri, 12 Apr 2013 17:03:32 +0300 Subject: [PATCH 37/40] makefile: deprecate CLEAN_BUILD option (use clean_package or package instead) --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 957f7b748..6d6ad4c38 100644 --- a/Makefile +++ b/Makefile @@ -70,7 +70,7 @@ SANITIZE_GCC_VARS += CFLAGS CPATH C_INCLUDE_PATH CPLUS_INCLUDE_PATH OBJC_INCLUDE $(foreach var, $(SANITIZE_GCC_VARS), $(eval $(call SANITIZE_VAR,$(var),disallowed))) # These specific variables used to be valid but now they make no sense -SANITIZE_DEPRECATED_VARS := USE_BOOTLOADER +SANITIZE_DEPRECATED_VARS := USE_BOOTLOADER CLEAN_BUILD $(foreach var, $(SANITIZE_DEPRECATED_VARS), $(eval $(call SANITIZE_VAR,$(var),deprecated))) # Make sure this isn't being run as root (no whoami on Windows, but that is ok here) From 9cc0abe2628dacf44830c8c3dfb75bbce62f99ca Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Fri, 12 Apr 2013 17:55:23 +0300 Subject: [PATCH 38/40] makefile: use correct opfw_resource file name as dependancy When 'opfw_resource' target is used as a dependency, it seems that make can't determine when the resource file is remade, and sometimes may start building GCS before the resource really exists. This is an attempt to fix that issue by referencing the real file instead of phony target. --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 6d6ad4c38..3c0cb974f 100644 --- a/Makefile +++ b/Makefile @@ -757,7 +757,7 @@ $(OPFW_RESOURCE): $(FW_TARGETS) # If opfw_resource or all firmware are requested, GCS should depend on the resource ifneq ($(strip $(filter opfw_resource all all_fw all_flight,$(MAKECMDGOALS))),) - $(eval openpilotgcs: | opfw_resource) + $(eval openpilotgcs: $(OPFW_RESOURCE)) endif # Packaging targets: package, clean_package @@ -780,7 +780,7 @@ ifneq ($(strip $(filter package clean_package,$(MAKECMDGOALS))),) # Packaged GCS should depend on opfw_resource ifneq ($(strip $(filter package clean_package,$(MAKECMDGOALS))),) - $(eval openpilotgcs: | opfw_resource) + $(eval openpilotgcs: $(OPFW_RESOURCE)) endif # Clean the build directory if clean_package is requested From 273d6cb1d21d764c4555f1982306af9b229d4950 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Fri, 12 Apr 2013 20:10:20 +0300 Subject: [PATCH 39/40] OP-892: fix broken op_overo path in .gitmodules The submodule itself was deleted in commit b621b057b60e426ff191f1a983406b9134b514a8 This commit updates the reference, so it can be reenabled if desired. --- .gitmodules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index cb32dd44f..2ffdf87f5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "overo"] path = overo - url = git@github.com:peabody124/op_overo.git + url = gitolite@git.openpilot.org:op_overo.git From afbb11f43e0507f647998fb4177a4619d52700a6 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sun, 14 Apr 2013 01:36:07 +0200 Subject: [PATCH 40/40] Fix 'variable not used' warning. Still needs to be reworked for timing --- flight/Modules/ManualControl/manualcontrol.c | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 4a4481566..de3ca578a 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -697,11 +697,8 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home) { static portTickType lastSysTime; - portTickType thisSysTime; - float dT; - - thisSysTime = xTaskGetTickCount(); - dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; + portTickType thisSysTime = xTaskGetTickCount(); + /* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */ lastSysTime = thisSysTime; if (home && changed) { @@ -738,16 +735,15 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool pathDesired.EndingVelocity=0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); + /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. } else { - -/*Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); -*/ + */ } }