mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-11 19:24:10 +01:00
1087 lines
31 KiB
C
1087 lines
31 KiB
C
/**
|
|
******************************************************************************
|
|
* @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 <openpilot.h>
|
|
#include <radiocombridge.h>
|
|
#include <packet_handler.h>
|
|
#include <gcsreceiver.h>
|
|
#include <pipxstatus.h>
|
|
#include <objectpersistence.h>
|
|
#include <pipxsettings.h>
|
|
#include <uavtalk_priv.h>
|
|
#include <pios_rfm22b.h>
|
|
#include <ecc.h>
|
|
#if defined(PIOS_INCLUDE_FLASH_EEPROM)
|
|
#include <pios_eeprom.h>
|
|
#endif
|
|
|
|
#include <stdbool.h>
|
|
|
|
// ****************
|
|
// 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;
|
|
}
|
|
}
|
|
}
|