/** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup RadioComBridgeModule Com Port to Radio Bridge Module * @brief Bridge Com and Radio ports * @{ * * @file RadioComBridge.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Bridges selected Com Port to the COM VCP emulated serial port * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ // **************** #include #include #include #include #include #include #include #include #include #include #if defined(PIOS_INCLUDE_FLASH_EEPROM) #include #endif #include // **************** // Private constants #define TEMP_BUFFER_SIZE 25 #define STACK_SIZE_BYTES 150 #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) #define BRIDGE_BUF_LEN 512 #define MAX_RETRIES 2 #define RETRY_TIMEOUT_MS 20 #define STATS_UPDATE_PERIOD_MS 500 #define RADIOSTATS_UPDATE_PERIOD_MS 250 #define MAX_LOST_CONTACT_TIME 4 #define PACKET_QUEUE_SIZE 10 #define MAX_PORT_DELAY 200 #define EV_PACKET_RECEIVED 0x20 #define EV_SEND_ACK 0x40 #define EV_SEND_NACK 0x80 // **************** // 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 comUAVTalkTaskHandle; xTaskHandle radioReceiveTaskHandle; xTaskHandle sendPacketTaskHandle; xTaskHandle sendDataTaskHandle; xTaskHandle radioStatusTaskHandle; xTaskHandle transparentCommTaskHandle; xTaskHandle ppmInputTaskHandle; // The UAVTalk connection on the com side. UAVTalkConnection inUAVTalkCon; UAVTalkConnection outUAVTalkCon; // Queue handles. xQueueHandle sendPacketQueue; xQueueHandle objEventQueue; // Error statistics. uint32_t comTxErrors; uint32_t comTxRetries; uint32_t comRxErrors; uint32_t radioTxErrors; uint32_t radioTxRetries; uint32_t radioRxErrors; uint32_t UAVTalkErrors; uint32_t packetErrors; uint32_t droppedPackets; uint16_t txBytes; uint16_t rxBytes; // The destination ID uint32_t destination_id; // The packet timeout. portTickType send_timeout; uint16_t min_packet_size; // Track other radios that are in range. PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM]; // The RSSI of the last packet received. int8_t RSSI; } RadioComBridgeData; typedef struct { uint32_t com_port; uint8_t *buffer; uint16_t length; uint16_t index; uint16_t data_length; } ReadBuffer, *BufferedReadHandle; // **************** // Private functions static void comUAVTalkTask(void *parameters); static void radioReceiveTask(void *parameters); static void sendPacketTask(void *parameters); static void sendDataTask(void *parameters); static void transparentCommTask(void * parameters); static void radioStatusTask(void *parameters); static void ppmInputTask(void *parameters); static int32_t transmitData(uint8_t * data, int32_t length); static int32_t transmitPacket(PHPacketHandle packet); static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc); static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc); static void PPMHandler(uint16_t *channels); static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length); static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms); static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port); static void updateSettings(); // **************** // Private variables static RadioComBridgeData *data; /** * Start the module * \return -1 if initialisation failed * \return 0 on success */ static int32_t RadioComBridgeStart(void) { if(data) { // Start the tasks xTaskCreate(comUAVTalkTask, (signed char *)"ComUAVTalk", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->comUAVTalkTaskHandle)); if(PIOS_COM_TRANS_COM) xTaskCreate(transparentCommTask, (signed char *)"transparentComm", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->transparentCommTaskHandle)); xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->radioReceiveTaskHandle)); xTaskCreate(sendPacketTask, (signed char *)"SendPacketTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->sendPacketTaskHandle)); xTaskCreate(sendDataTask, (signed char *)"SendDataTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->sendDataTaskHandle)); xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle)); if(PIOS_PPM_RECEIVER) xTaskCreate(ppmInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->ppmInputTaskHandle)); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK); if(PIOS_COM_TRANS_COM) PIOS_WDG_RegisterFlag(PIOS_WDG_TRANSCOMM); PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE); //PIOS_WDG_RegisterFlag(PIOS_WDG_SENDPACKET); //PIOS_WDG_RegisterFlag(PIOS_WDG_SENDDATA); if(PIOS_PPM_RECEIVER) PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT); #endif return 0; } return -1; } /** * Initialise the module * \return -1 if initialisation failed * \return 0 on success */ static int32_t RadioComBridgeInitialize(void) { // allocate and initialize the static data storage only if module is enabled data = (RadioComBridgeData *)pvPortMalloc(sizeof(RadioComBridgeData)); if (!data) return -1; // Initialize the UAVObjects that we use GCSReceiverInitialize(); PipXStatusInitialize(); ObjectPersistenceInitialize(); updateSettings(); // Initialise UAVTalk data->inUAVTalkCon = UAVTalkInitialize(0); data->outUAVTalkCon = UAVTalkInitialize(&transmitData); // Initialize the queues. data->sendPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle)); data->objEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); // Initialize the statistics. data->radioTxErrors = 0; data->radioTxRetries = 0; data->radioRxErrors = 0; data->comTxErrors = 0; data->comTxRetries = 0; data->comRxErrors = 0; data->UAVTalkErrors = 0; data->packetErrors = 0; data->RSSI = -127; // Register the callbacks with the packet handler PHRegisterOutputStream(pios_packet_handler, transmitPacket); PHRegisterDataHandler(pios_packet_handler, receiveData); PHRegisterStatusHandler(pios_packet_handler, StatusHandler); PHRegisterPPMHandler(pios_packet_handler, PPMHandler); // Initialize the packet send timeout data->send_timeout = 25; // ms data->min_packet_size = 50; // 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)); // Configure our UAVObjects for updates. UAVObjConnectQueue(UAVObjGetByName("PipXStatus"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByName("GCSReceiver"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByName("ObjectPersistence"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); return 0; } MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart) /** * Reads UAVTalk messages froma com port and creates packets out of them. */ static void comUAVTalkTask(void *parameters) { PHPacketHandle p = NULL; // Create the buffered reader. BufferedReadHandle f = BufferedReadInit(PIOS_COM_UAVTALK, TEMP_BUFFER_SIZE); while (1) { #ifdef PIOS_INCLUDE_WDG // Update the watchdog timer. PIOS_WDG_UpdateFlag(PIOS_WDG_COMUAVTALK); #endif /* PIOS_INCLUDE_WDG */ // Receive from USB HID if available, otherwise UAVTalk com if it's available. #if defined(PIOS_INCLUDE_USB) // Determine input port (USB takes priority over telemetry port) if (PIOS_USB_CheckAvailable(0)) BufferedReadSetCom(f, PIOS_COM_USB_HID); else #endif /* PIOS_INCLUDE_USB */ { if (PIOS_COM_UAVTALK) BufferedReadSetCom(f, PIOS_COM_UAVTALK); else { vTaskDelay(5); continue; } } // Read the next byte uint8_t rx_byte; if(!BufferedRead(f, &rx_byte, MAX_PORT_DELAY)) continue; // Get a TX packet from the packet handler if required. if (p == NULL) { // Wait until we receive a sync. UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte); if (state != UAVTALK_STATE_TYPE) continue; // Get a packet when we see the sync p = PHGetTXPacket(pios_packet_handler); // No packets available? if (p == NULL) { data->droppedPackets++; continue; } // Initialize the packet. p->header.destination_id = data->destination_id; p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id); //p->header.type = PACKET_TYPE_ACKED_DATA; p->header.type = PACKET_TYPE_DATA; p->data[0] = rx_byte; p->header.data_size = 1; continue; } // Insert this byte. p->data[p->header.data_size++] = rx_byte; // Keep reading until we receive a completed packet. UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte); UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(data->inUAVTalkCon); UAVTalkInputProcessor *iproc = &(connection->iproc); if (state == UAVTALK_STATE_COMPLETE) { // Is this a local UAVObject? // We only generate GcsReceiver ojects, we don't consume them. if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) { // We treat the ObjectPersistence object differently if(iproc->objId == OBJECTPERSISTENCE_OBJID) { // Unpack object, if the instance does not exist it will be created! UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); // Get the ObjectPersistence object. ObjectPersistenceData obj_per; ObjectPersistenceGet(&obj_per); // Is this concerning or setting object? if (obj_per.ObjectID == PIPXSETTINGS_OBJID) { // Queue up the ACK. UAVObjEvent ev; ev.obj = iproc->obj; ev.instId = iproc->instId; ev.event = EV_SEND_ACK; xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY); // Is this a save, load, or delete? bool success = true; switch (obj_per.Operation) { case OBJECTPERSISTENCE_OPERATION_LOAD: { #if defined(PIOS_INCLUDE_FLASH_EEPROM) // Load the settings. PipXSettingsData pipxSettings; if (PIOS_EEPROM_Load((uint8_t*)&pipxSettings, sizeof(PipXSettingsData)) == 0) PipXSettingsSet(&pipxSettings); else success = false; #endif break; } case OBJECTPERSISTENCE_OPERATION_SAVE: { #if defined(PIOS_INCLUDE_FLASH_EEPROM) // Save the settings. PipXSettingsData pipxSettings; PipXSettingsGet(&pipxSettings); int32_t ret = PIOS_EEPROM_Save((uint8_t*)&pipxSettings, sizeof(PipXSettingsData)); if (ret != 0) success = false; #endif break; } default: break; } if (success == true) { obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; ObjectPersistenceSet(&obj_per); } // Release the packet, since we don't need it. PHReleaseTXPacket(pios_packet_handler, p); } else { // Otherwise, queue the packet for transmission. xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY); } } else { UAVObjEvent ev; ev.obj = iproc->obj; ev.instId = 0; switch (iproc->type) { case UAVTALK_TYPE_OBJ: // Unpack object, if the instance does not exist it will be created! UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); break; case UAVTALK_TYPE_OBJ_REQ: // Queue up an object send request. ev.event = EV_UPDATE_REQ; xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY); break; case UAVTALK_TYPE_OBJ_ACK: if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) { // Queue up an ACK ev.event = EV_SEND_ACK; xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY); } break; } // Release the packet, since we don't need it. PHReleaseTXPacket(pios_packet_handler, p); } } else { // Queue the packet for transmission. xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY); } p = NULL; } else if(state == UAVTALK_STATE_ERROR) { DEBUG_PRINTF(1, "UAVTalk FAILED!\n\r"); data->UAVTalkErrors++; // Send a NACK if required. if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) { // Queue up a NACK UAVObjEvent ev; ev.obj = iproc->obj; ev.event = EV_SEND_NACK; xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY); // Release the packet and start over again. PHReleaseTXPacket(pios_packet_handler, p); } else { // Transmit the packet anyway... xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY); } p = NULL; } } } /** * The radio to com bridge task. */ static void radioReceiveTask(void *parameters) { PHPacketHandle p = NULL; /* Handle radio -> usart/usb direction */ while (1) { uint32_t rx_bytes; #ifdef PIOS_INCLUDE_WDG // Update the watchdog timer. PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE); #endif /* PIOS_INCLUDE_WDG */ // Get a RX packet from the packet handler if required. if (p == NULL) p = PHGetRXPacket(pios_packet_handler); if(p == NULL) { DEBUG_PRINTF(2, "RX Packet Unavailable.!\n\r"); // 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. if(PHVerifyPacket(pios_packet_handler, p, rx_bytes) > 0) { UAVObjEvent ev; ev.obj = (UAVObjHandle)p; ev.event = EV_PACKET_RECEIVED; xQueueSend(data->objEventQueue, &ev, portMAX_DELAY); } else { data->packetErrors++; PHReceivePacket(pios_packet_handler, p, true); } 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->sendPacketQueue, &p, MAX_PORT_DELAY) == pdTRUE) { // Send the packet. if(!PHTransmitPacket(pios_packet_handler, p)) PHReleaseTXPacket(pios_packet_handler, p); } } } /** * Send packets to the radio. */ static void sendDataTask(void *parameters) { UAVObjEvent ev; // Loop forever while (1) { #ifdef PIOS_INCLUDE_WDG // Update the watchdog timer. // NOTE: this is temporarily turned off becase PIOS_Com_SendBuffer appears to block for an uncontrollable time, // and SendBufferNonBlocking doesn't seem to be working in this case. //PIOS_WDG_UpdateFlag(PIOS_WDG_SENDDATA); #endif /* PIOS_INCLUDE_WDG */ // Wait for a packet on the queue. if (xQueueReceive(data->objEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { // Send update (with retries) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; if (!success) ++retries; } data->comTxRetries += retries; } else if(ev.event == EV_SEND_ACK) { // Send the ACK uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId) == 0; if (!success) ++retries; } data->comTxRetries += retries; } else if(ev.event == EV_SEND_NACK) { // Send the NACK uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj)) == 0; if (!success) ++retries; } data->comTxRetries += retries; } else if(ev.event == EV_PACKET_RECEIVED) { // Receive the packet. PHReceivePacket(pios_packet_handler, (PHPacketHandle)ev.obj, false); } } } } /** * The com to radio bridge task. */ static void transparentCommTask(void * parameters) { portTickType packet_start_time = 0; uint32_t timeout = MAX_PORT_DELAY; PHPacketHandle p = NULL; /* Handle usart/usb -> radio direction */ while (1) { #ifdef PIOS_INCLUDE_WDG // Update the watchdog timer. PIOS_WDG_UpdateFlag(PIOS_WDG_TRANSCOMM); #endif /* PIOS_INCLUDE_WDG */ // Get a TX packet from the packet handler if required. if (p == NULL) { p = PHGetTXPacket(pios_packet_handler); // No packets available? if (p == NULL) { data->droppedPackets++; // Wait a bit for a packet to come available. vTaskDelay(5); continue; } // Initialize the packet. p->header.destination_id = data->destination_id; p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id); //p->header.type = PACKET_TYPE_ACKED_DATA; p->header.type = PACKET_TYPE_DATA; p->header.data_size = 0; } // Receive data from the com port uint32_t cur_rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_TRANS_COM, p->data + p->header.data_size, PH_MAX_DATA - p->header.data_size, timeout); // Do we have an data to send? p->header.data_size += cur_rx_bytes; if (p->header.data_size > 0) { // Check how long since last update portTickType cur_sys_time = xTaskGetTickCount(); // Is this the start of a packet? if(packet_start_time == 0) packet_start_time = cur_sys_time; // Just send the packet on wraparound bool send_packet = (cur_sys_time < packet_start_time); if (!send_packet) { portTickType dT = (cur_sys_time - packet_start_time) / portTICK_RATE_MS; if (dT > data->send_timeout) send_packet = true; else timeout = data->send_timeout - dT; } // Also send the packet if the size is over the minimum. send_packet |= (p->header.data_size > data->min_packet_size); // Should we send this packet? if (send_packet) { // Queue the packet for transmission. xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY); // Reset the timeout timeout = MAX_PORT_DELAY; p = NULL; packet_start_time = 0; } } } } /** * The stats update task. */ static void radioStatusTask(void *parameters) { PHStatusPacket status_packet; while (1) { 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; xQueueSend(data->sendPacketQueue, &sph, MAX_PORT_DELAY); cntr = 0; } } // Delay until the next update period. vTaskDelay(STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } /** * The PPM input task. */ static void ppmInputTask(void *parameters) { PHPpmPacket ppm_packet; PHPacketHandle pph = (PHPacketHandle)&ppm_packet; while (1) { #ifdef PIOS_INCLUDE_WDG // Update the watchdog timer. PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT); #endif /* PIOS_INCLUDE_WDG */ // Send the PPM packet for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i) ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i); // Send the packet. ppm_packet.header.destination_id = data->destination_id; ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id); ppm_packet.header.type = PACKET_TYPE_PPM; ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet); xQueueSend(data->sendPacketQueue, &pph, MAX_PORT_DELAY); // Delay until the next update period. vTaskDelay(PIOS_PPM_PACKET_UPDATE_PERIOD_MS / portTICK_RATE_MS); } } /** * Transmit data buffer to the com port. * \param[in] 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 transmitData(uint8_t *buf, int32_t length) { uint32_t outputPort = PIOS_COM_UAVTALK; #if defined(PIOS_INCLUDE_USB) // Determine output port (USB takes priority over telemetry port) if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) outputPort = PIOS_COM_USB_HID; #endif /* PIOS_INCLUDE_USB */ if(outputPort) return PIOS_COM_SendBuffer(outputPort, buf, length); else return -1; } /** * 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) { data->txBytes += PH_PACKET_SIZE(p); return PIOS_COM_SendBuffer(PIOS_COM_RADIO, (uint8_t*)p, PH_PACKET_SIZE(p)); } /** * Receive a packet * \param[in] buf The received data buffer * \param[in] length Length of buffer */ static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc) { data->RSSI = rssi; // Packet data should go to transparent com if it's configured, // USB HID if it's connected, otherwise, UAVTalk com if it's configured. uint32_t outputPort = PIOS_COM_TRANS_COM; if (!outputPort) { outputPort = PIOS_COM_UAVTALK; #if defined(PIOS_INCLUDE_USB) // Determine output port (USB takes priority over telemetry port) if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) outputPort = PIOS_COM_USB_HID; #endif /* PIOS_INCLUDE_USB */ } if (!outputPort) return; // Send the received data to the com port if (PIOS_COM_SendBuffer(outputPort, buf, len) != len) // Error on transmit data->comTxErrors++; } /** * Receive a 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; } } /** * 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); } static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length) { BufferedReadHandle h = (BufferedReadHandle)pvPortMalloc(sizeof(ReadBuffer)); if (!h) return NULL; h->com_port = com_port; h->buffer = (uint8_t*)pvPortMalloc(buffer_length); h->length = buffer_length; h->index = 0; h->data_length = 0; if (h->buffer == NULL) return NULL; return h; } static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms) { // Read some data if required. if(h->index == h->data_length) { uint32_t rx_bytes = PIOS_COM_ReceiveBuffer(h->com_port, h->buffer, h->length, timeout_ms); if (rx_bytes == 0) return false; h->index = 0; h->data_length = rx_bytes; } // Return the next byte. *value = h->buffer[h->index++]; return true; } static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port) { h->com_port = com_port; } /** * Update the telemetry settings, called on startup. * FIXME: This should be in the TelemetrySettings object. But objects * have too much overhead yet. Also the telemetry has no any specific * settings, etc. Thus the HwSettings object which contains the * telemetry port speed is used for now. */ static void updateSettings() { // Get the settings. PipXSettingsData pipxSettings; PipXSettingsGet(&pipxSettings); // Initialize the destination ID data->destination_id = pipxSettings.PairID ? pipxSettings.PairID : 0xffffffff; DEBUG_PRINTF(2, "PairID: %x\n\r", data->destination_id); if (PIOS_COM_TELEMETRY) { switch (pipxSettings.TelemetrySpeed) { case PIPXSETTINGS_TELEMETRYSPEED_2400: PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 2400); break; case PIPXSETTINGS_TELEMETRYSPEED_4800: PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 4800); break; case PIPXSETTINGS_TELEMETRYSPEED_9600: PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 9600); break; case PIPXSETTINGS_TELEMETRYSPEED_19200: PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 19200); break; case PIPXSETTINGS_TELEMETRYSPEED_38400: PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 38400); break; case PIPXSETTINGS_TELEMETRYSPEED_57600: PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 57600); break; case PIPXSETTINGS_TELEMETRYSPEED_115200: PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, 115200); break; } } if (PIOS_COM_FLEXI) { switch (pipxSettings.FlexiSpeed) { case PIPXSETTINGS_TELEMETRYSPEED_2400: PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 2400); break; case PIPXSETTINGS_TELEMETRYSPEED_4800: PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 4800); break; case PIPXSETTINGS_TELEMETRYSPEED_9600: PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 9600); break; case PIPXSETTINGS_TELEMETRYSPEED_19200: PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 19200); break; case PIPXSETTINGS_TELEMETRYSPEED_38400: PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 38400); break; case PIPXSETTINGS_TELEMETRYSPEED_57600: PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 57600); break; case PIPXSETTINGS_TELEMETRYSPEED_115200: PIOS_COM_ChangeBaud(PIOS_COM_FLEXI, 115200); break; } } if (PIOS_COM_VCP) { switch (pipxSettings.VCPSpeed) { case PIPXSETTINGS_TELEMETRYSPEED_2400: PIOS_COM_ChangeBaud(PIOS_COM_VCP, 2400); break; case PIPXSETTINGS_TELEMETRYSPEED_4800: PIOS_COM_ChangeBaud(PIOS_COM_VCP, 4800); break; case PIPXSETTINGS_TELEMETRYSPEED_9600: PIOS_COM_ChangeBaud(PIOS_COM_VCP, 9600); break; case PIPXSETTINGS_TELEMETRYSPEED_19200: PIOS_COM_ChangeBaud(PIOS_COM_VCP, 19200); break; case PIPXSETTINGS_TELEMETRYSPEED_38400: PIOS_COM_ChangeBaud(PIOS_COM_VCP, 38400); break; case PIPXSETTINGS_TELEMETRYSPEED_57600: PIOS_COM_ChangeBaud(PIOS_COM_VCP, 57600); break; case PIPXSETTINGS_TELEMETRYSPEED_115200: PIOS_COM_ChangeBaud(PIOS_COM_VCP, 115200); break; } } }