/** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup Radio Input / Output Module * @brief Read and Write packets from/to a radio device. * @{ * * @file radio.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Bridges selected Com Port to the COM VCP emulated serial port * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include #include #include #include #include #include #include #include #include // **************** // Private constants #define STACK_SIZE_BYTES 150 #define TASK_PRIORITY (tskIDLE_PRIORITY + 2) #define PACKET_QUEUE_SIZE PIOS_PH_WIN_SIZE #define MAX_PORT_DELAY 200 #define STATS_UPDATE_PERIOD_MS 500 #define RADIOSTATS_UPDATE_PERIOD_MS 250 #define MAX_LOST_CONTACT_TIME 4 #ifndef LINK_LED_ON #define LINK_LED_ON #define LINK_LED_OFF #endif // **************** // Private types typedef struct { uint32_t pairID; uint16_t retries; uint16_t errors; uint16_t uavtalk_errors; uint16_t resets; uint16_t dropped; int8_t rssi; uint8_t lastContact; } PairStats; typedef struct { // The task handles. xTaskHandle radioReceiveTaskHandle; xTaskHandle radioStatusTaskHandle; xTaskHandle sendPacketTaskHandle; // Queue handles. xQueueHandle radioPacketQueue; // Error statistics. uint32_t radioTxErrors; uint32_t radioRxErrors; uint32_t packetErrors; uint16_t txBytes; uint16_t rxBytes; // External error statistics uint32_t droppedPackets; uint32_t comTxRetries; uint32_t UAVTalkErrors; // The destination ID uint32_t destination_id; // Track other radios that are in range. PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM]; // The RSSI of the last packet received. int8_t RSSI; } RadioData; // **************** // Private functions static void radioReceiveTask(void *parameters); static void radioStatusTask(void *parameters); static void sendPacketTask(void *parameters); static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc); static int32_t transmitPacket(PHPacketHandle packet); static void PPMHandler(uint16_t *channels); // **************** // Private variables static RadioData *data = 0; // **************** // Global variables uint32_t pios_rfm22b_id = 0; uint32_t pios_com_rfm22b_id = 0; uint32_t pios_packet_handler = 0; extern struct pios_rfm22b_cfg pios_rfm22b_cfg; /** * Start the module * \return -1 if initialisation failed * \return 0 on success */ static int32_t RadioStart(void) { if (!data) return -1; // Start the tasks. xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioReceiveTaskHandle)); xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle)); xTaskCreate(sendPacketTask, (signed char *)"SendPacket", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->sendPacketTaskHandle)); // Install the monitors TaskMonitorAdd(TASKINFO_RUNNING_MODEMRX, data->radioReceiveTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_MODEMTX, data->sendPacketTaskHandle); TaskMonitorAdd(TASKINFO_RUNNING_MODEMSTAT, data->radioStatusTaskHandle); // Register the watchdog timers. #ifdef PIOS_WDG_RADIORECEIVE PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE); #endif /* PIOS_WDG_RADIORECEIVE */ return 0; } /** * Initialise the module * \return -1 if initialisation failed * \return 0 on success */ static int32_t RadioInitialize(void) { // See if this module is enabled. #ifndef RADIO_BUILTIN HwSettingsInitialize(); uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; HwSettingsOptionalModulesGet(optionalModules); if (optionalModules[HWSETTINGS_OPTIONALMODULES_RADIO] != HWSETTINGS_OPTIONALMODULES_ENABLED) return -1; #endif // Initalize out UAVOs PipXSettingsInitialize(); PipXStatusInitialize(); PipXSettingsData pipxSettings; PipXSettingsGet(&pipxSettings); /* Retrieve hardware settings. */ pios_rfm22b_cfg.frequencyHz = pipxSettings.Frequency; pios_rfm22b_cfg.RFXtalCap = pipxSettings.FrequencyCalibration; switch (pipxSettings.RFSpeed) { case PIPXSETTINGS_RFSPEED_2400: pios_rfm22b_cfg.maxRFBandwidth = 2000; break; case PIPXSETTINGS_RFSPEED_4800: pios_rfm22b_cfg.maxRFBandwidth = 4000; break; case PIPXSETTINGS_RFSPEED_9600: pios_rfm22b_cfg.maxRFBandwidth = 9600; break; case PIPXSETTINGS_RFSPEED_19200: pios_rfm22b_cfg.maxRFBandwidth = 19200; break; case PIPXSETTINGS_RFSPEED_38400: pios_rfm22b_cfg.maxRFBandwidth = 32000; break; case PIPXSETTINGS_RFSPEED_57600: pios_rfm22b_cfg.maxRFBandwidth = 64000; break; case PIPXSETTINGS_RFSPEED_115200: pios_rfm22b_cfg.maxRFBandwidth = 128000; break; } switch (pipxSettings.MaxRFPower) { case PIPXSETTINGS_MAXRFPOWER_125: pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_0; break; case PIPXSETTINGS_MAXRFPOWER_16: pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_1; break; case PIPXSETTINGS_MAXRFPOWER_316: pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_2; break; case PIPXSETTINGS_MAXRFPOWER_63: pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_3; break; case PIPXSETTINGS_MAXRFPOWER_126: pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_4; break; case PIPXSETTINGS_MAXRFPOWER_25: pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_5; break; case PIPXSETTINGS_MAXRFPOWER_50: pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_6; break; case PIPXSETTINGS_MAXRFPOWER_100: pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_7; break; } /* Initalize the RFM22B radio COM device. */ { if (PIOS_RFM22B_Init(&pios_rfm22b_id, &pios_rfm22b_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN); uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN); PIOS_Assert(rx_buffer); PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_rfm22b_id, &pios_rfm22b_com_driver, pios_rfm22b_id, rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN, tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } } // Initialize the packet handler PacketHandlerConfig pios_ph_cfg = { .default_destination_id = 0xffffffff, // Broadcast .source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id), .win_size = PIOS_PH_WIN_SIZE, .max_connections = PIOS_PH_MAX_CONNECTIONS, }; pios_packet_handler = PHInitialize(&pios_ph_cfg); // allocate and initialize the static data storage only if module is enabled data = (RadioData *)pvPortMalloc(sizeof(RadioData)); if (!data) return -1; // Initialize the statistics. data->radioTxErrors = 0; data->radioRxErrors = 0; data->packetErrors = 0; data->droppedPackets = 0; data->comTxRetries = 0; data->UAVTalkErrors = 0; data->RSSI = -127; // Initialize the detected device statistics. for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i) { data->pairStats[i].pairID = 0; data->pairStats[i].rssi = -127; data->pairStats[i].retries = 0; data->pairStats[i].errors = 0; data->pairStats[i].uavtalk_errors = 0; data->pairStats[i].resets = 0; data->pairStats[i].dropped = 0; data->pairStats[i].lastContact = 0; } // The first slot is reserved for our current pairID PipXSettingsPairIDGet(&(data->pairStats[0].pairID)); data->destination_id = data->pairStats[0].pairID ? data->pairStats[0].pairID : 0xffffffff; // Create the packet queue. data->radioPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle)); // Register the callbacks with the packet handler PHRegisterStatusHandler(pios_packet_handler, StatusHandler); PHRegisterOutputStream(pios_packet_handler, transmitPacket); PHRegisterPPMHandler(pios_packet_handler, PPMHandler); return 0; } MODULE_INITCALL(RadioInitialize, RadioStart) /** * The task that receives packets from the radio. */ static void radioReceiveTask(void *parameters) { PHPacketHandle p = NULL; /* Handle radio -> usart/usb direction */ while (1) { uint32_t rx_bytes; #ifdef PIOS_WDG_RADIORECEIVE // Update the watchdog timer. PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE); #endif /* PIOS_INCLUDE_WDG */ PIOS_RFM22_processPendingISR(5); // Get a RX packet from the packet handler if required. if (p == NULL) p = PHGetRXPacket(pios_packet_handler); if(p == NULL) { // Wait a bit for a packet to come available. vTaskDelay(5); continue; } // Receive data from the radio port rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, (uint8_t*)p, PIOS_PH_MAX_PACKET, MAX_PORT_DELAY); if(rx_bytes == 0) continue; data->rxBytes += rx_bytes; // Verify that the packet is valid and pass it on. bool rx_error = PHVerifyPacket(pios_packet_handler, p, rx_bytes) < 0; if(rx_error) data->packetErrors++; PHReceivePacket(pios_packet_handler, p, rx_error); p = NULL; } } /** * Send packets to the radio. */ static void sendPacketTask(void *parameters) { PHPacketHandle p; // Loop forever while (1) { #ifdef PIOS_INCLUDE_WDG // Update the watchdog timer. //PIOS_WDG_UpdateFlag(PIOS_WDG_SENDPACKET); #endif /* PIOS_INCLUDE_WDG */ // Wait for a packet on the queue. if (xQueueReceive(data->radioPacketQueue, &p, MAX_PORT_DELAY) == pdTRUE) { PIOS_COM_SendBuffer(PIOS_COM_RADIO, (uint8_t*)p, PH_PACKET_SIZE(p)); PHReleaseTXPacket(pios_packet_handler, p); } } } /** * Transmit a packet to the radio port. * \param[in] buf Data buffer to send * \param[in] length Length of buffer * \return -1 on failure * \return number of bytes transmitted on success */ static int32_t transmitPacket(PHPacketHandle p) { uint16_t len = PH_PACKET_SIZE(p); data->txBytes += len; if (xQueueSend(data->radioPacketQueue, &p, portMAX_DELAY) != pdTRUE) return -1; return len; } /** * Receive a status packet * \param[in] status The status structure */ static void StatusHandler(PHStatusPacketHandle status, int8_t rssi, int8_t afc) { uint32_t id = status->header.source_id; bool found = false; // Have we seen this device recently? uint8_t id_idx = 0; for ( ; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx) if(data->pairStats[id_idx].pairID == id) { found = true; break; } // If we have seen it, update the RSSI and reset the last contact couter if(found) { data->pairStats[id_idx].rssi = rssi; data->pairStats[id_idx].retries = status->retries; data->pairStats[id_idx].errors = status->errors; data->pairStats[id_idx].uavtalk_errors = status->uavtalk_errors; data->pairStats[id_idx].resets = status->resets; data->pairStats[id_idx].dropped = status->dropped; data->pairStats[id_idx].lastContact = 0; } // If we haven't seen it, find a slot to put it in. if (!found) { uint32_t pairID; PipXSettingsPairIDGet(&pairID); uint8_t min_idx = 0; if(id != pairID) { int8_t min_rssi = data->pairStats[0].rssi; for (id_idx = 1; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx) { if(data->pairStats[id_idx].rssi < min_rssi) { min_rssi = data->pairStats[id_idx].rssi; min_idx = id_idx; } } } data->pairStats[min_idx].pairID = id; data->pairStats[min_idx].rssi = rssi; data->pairStats[min_idx].retries = status->retries; data->pairStats[min_idx].errors = status->errors; data->pairStats[min_idx].uavtalk_errors = status->uavtalk_errors; data->pairStats[min_idx].resets = status->resets; data->pairStats[min_idx].dropped = status->dropped; data->pairStats[min_idx].lastContact = 0; } } /** * The stats update task. */ static void radioStatusTask(void *parameters) { static portTickType lastSysTime; PHStatusPacket status_packet; while (1) { lastSysTime = xTaskGetTickCount(); PipXStatusData pipxStatus; uint32_t pairID; // Get object data PipXStatusGet(&pipxStatus); PipXSettingsPairIDGet(&pairID); // Update the status pipxStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); pipxStatus.Retries = data->comTxRetries; pipxStatus.Errors = data->packetErrors; pipxStatus.UAVTalkErrors = data->UAVTalkErrors; pipxStatus.Dropped = data->droppedPackets; pipxStatus.Resets = PIOS_RFM22B_Resets(pios_rfm22b_id); pipxStatus.TXRate = (uint16_t)((float)(data->txBytes * 1000) / STATS_UPDATE_PERIOD_MS); data->txBytes = 0; pipxStatus.RXRate = (uint16_t)((float)(data->rxBytes * 1000) / STATS_UPDATE_PERIOD_MS); data->rxBytes = 0; pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_DISCONNECTED; pipxStatus.RSSI = data->RSSI; LINK_LED_OFF; // Update the potential pairing contacts for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i) { pipxStatus.PairIDs[i] = data->pairStats[i].pairID; pipxStatus.PairSignalStrengths[i] = data->pairStats[i].rssi; data->pairStats[i].lastContact++; // Remove this device if it's stale. if(data->pairStats[i].lastContact > MAX_LOST_CONTACT_TIME) { data->pairStats[i].pairID = 0; data->pairStats[i].rssi = -127; data->pairStats[i].retries = 0; data->pairStats[i].errors = 0; data->pairStats[i].uavtalk_errors = 0; data->pairStats[i].resets = 0; data->pairStats[i].dropped = 0; data->pairStats[i].lastContact = 0; } // Add the paired devices statistics to ours. if(pairID && (data->pairStats[i].pairID == pairID) && (data->pairStats[i].rssi > -127)) { pipxStatus.Retries += data->pairStats[i].retries; pipxStatus.Errors += data->pairStats[i].errors; pipxStatus.UAVTalkErrors += data->pairStats[i].uavtalk_errors; pipxStatus.Dropped += data->pairStats[i].dropped; pipxStatus.Resets += data->pairStats[i].resets; pipxStatus.Dropped += data->pairStats[i].dropped; pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_CONNECTED; LINK_LED_ON; } } // Update the object PipXStatusSet(&pipxStatus); // Broadcast the status. { static uint16_t cntr = 0; if(cntr++ == RADIOSTATS_UPDATE_PERIOD_MS / STATS_UPDATE_PERIOD_MS) { // Queue the status message status_packet.header.destination_id = 0xffffffff; status_packet.header.type = PACKET_TYPE_STATUS; status_packet.header.data_size = PH_STATUS_DATA_SIZE(&status_packet); status_packet.header.source_id = pipxStatus.DeviceID; status_packet.retries = data->comTxRetries; status_packet.errors = data->packetErrors; status_packet.uavtalk_errors = data->UAVTalkErrors; status_packet.dropped = data->droppedPackets; status_packet.resets = PIOS_RFM22B_Resets(pios_rfm22b_id); PHPacketHandle sph = (PHPacketHandle)&status_packet; PHTransmitPacket(PIOS_PACKET_HANDLER, sph); cntr = 0; } } portTickType timeSinceUpdate; do { PIOS_RFM22_processPendingISR(5); timeSinceUpdate = xTaskGetTickCount() - lastSysTime; } while(timeSinceUpdate < STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } /** * Receive a ppm packet * \param[in] channels The ppm channels */ static void PPMHandler(uint16_t *channels) { GCSReceiverData rcvr; // Copy the receiver channels into the GCSReceiver object. for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i) rcvr.Channel[i] = channels[i]; // Set the GCSReceiverData object. GCSReceiverSet(&rcvr); }