rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Now using read and write threads for both radio and com ports on PipX. Also cleaned up UAVTalk message handling.

Browse Source
This commit is contained in:
Brian Webb 2012-05-01 19:14:58 -07:00
parent 84a19ebeab
commit 217aad8c37
6 changed files with 365 additions and 163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
flight/Libraries/inc/packet_handler.h
View File

@ -78,7 +78,7 @@ typedef struct {
} PHPpmPacket, *PHPpmPacketHandle;
typedef struct {
uint8_t txWinSize;
uint8_t winSize;
uint16_t maxConnections;
} PacketHandlerConfig;
@ -97,11 +97,13 @@ void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f);
void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f);
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
PHPacketHandle PHGetRXPacket(PHInstHandle h);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
PHPacketHandle PHGetTXPacket(PHInstHandle h);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHBroadcastStatus(PHInstHandle h, uint32_t id, int8_t rssi);
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, uint16_t len);
uint8_t PHFillStatusPacket(PHInstHandle h, uint32_t id, int8_t rssi);
int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len);
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error);
#endif // __PACKET_HANDLER_H__

140
flight/Libraries/packet_handler.c
View File

@ -40,8 +40,10 @@ typedef struct {
PHPacket *tx_packets;
uint8_t tx_win_start;
uint8_t tx_win_end;
PHPacket *rx_packets;
uint8_t rx_win_start;
uint8_t rx_win_end;
uint16_t tx_seq_id;
PHPacket rx_packet;
PHOutputStream stream;
xSemaphoreHandle lock;
PHOutputStream output_stream;
@ -72,13 +74,18 @@ PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
data->cfg = *cfg;
data->tx_seq_id = 0;
// Allocate the tx packet window
data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.txWinSize);
// Allocate the packet windows
data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize);
// Initialize the window
// Initialize the windows
data->tx_win_start = data->tx_win_end = 0;
for (uint8_t i = 0; i < data->cfg.txWinSize; ++i)
data->rx_win_start = data->rx_win_end = 0;
for (uint8_t i = 0; i < data->cfg.winSize; ++i)
{
data->tx_packets[i].header.type = PACKET_TYPE_NONE;
data->rx_packets[i].header.type = PACKET_TYPE_NONE;
}
// Create the lock
data->lock = xSemaphoreCreateRecursiveMutex();
@ -165,7 +172,7 @@ PHPacketHandle PHGetTXPacket(PHInstHandle h)
PHPacketHandle p = data->tx_packets + data->tx_win_end;
// Is the window full?
uint8_t next_end = (data->tx_win_end + 1) % data->cfg.txWinSize;
uint8_t next_end = (data->tx_win_end + 1) % data->cfg.winSize;
if(next_end == data->tx_win_start)
return NULL;
data->tx_win_end = next_end;
@ -177,17 +184,6 @@ PHPacketHandle PHGetTXPacket(PHInstHandle h)
return p;
}
/**
* Get a pointer to the the receive buffer.
* \param[in] h The packet handler instance data pointer.
* \return PHPacketHandle A pointer to the packet buffer.
*/
PHPacketHandle PHGetRXPacket(PHInstHandle h)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
return &(data->rx_packet);
}
/**
* Release a packet from the transmit packet buffer window.
* \param[in] h The packet handler instance data pointer.
@ -207,7 +203,59 @@ void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p)
// If this packet is at the start of the window, increment the start index.
while ((data->tx_win_start != data->tx_win_end) &&
(data->tx_packets[data->tx_win_start].header.type == PACKET_TYPE_NONE))
data->tx_win_start = (data->tx_win_start + 1) % data->cfg.txWinSize;
data->tx_win_start = (data->tx_win_start + 1) % data->cfg.winSize;
// Release lock
xSemaphoreGiveRecursive(data->lock);
}
/**
* Get a packet out of the receive buffer.
* \param[in] h The packet handler instance data pointer.
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
PHPacketHandle PHGetRXPacket(PHInstHandle h)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
PHPacketHandle p = data->rx_packets + data->rx_win_end;
// Is the window full?
uint8_t next_end = (data->rx_win_end + 1) % data->cfg.winSize;
if(next_end == data->rx_win_start)
return NULL;
data->rx_win_end = next_end;
// Release lock
xSemaphoreGiveRecursive(data->lock);
// Return a pointer to the packet at the end of the TX window.
return p;
}
/**
* Release a packet from the receive packet buffer window.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return Nothing
*/
void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
// Change the packet type so we know this packet is unused.
p->header.type = PACKET_TYPE_NONE;
// If this packet is at the start of the window, increment the start index.
while ((data->rx_win_start != data->rx_win_end) &&
(data->rx_packets[data->rx_win_start].header.type == PACKET_TYPE_NONE))
data->rx_win_start = (data->rx_win_start + 1) % data->cfg.winSize;
// Release lock
xSemaphoreGiveRecursive(data->lock);
@ -242,24 +290,23 @@ uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p)
}
/**
* Process a packet that has been received.
* Verify that a buffer contains a valid packet.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \param[in] received_len The length of data received.
* \return 1 Success
* \return 0 Failure
* \return < 0 Failure
* \return > 0 Number of bytes consumed.
*/
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Verify the packet length.
// Note: The last two bytes should be the RSSI and AFC.
uint16_t len = PHPacketSizeECC(p);
if (received_len != (len + 2))
if (received_len < (len + 2))
{
DEBUG_PRINTF(1, "Packet length error\n\r");
return 0;
DEBUG_PRINTF(1, "Packet length error %d %d\n\r", received_len, len + 2);
return -1;
}
// Attempt to correct any errors in the packet.
@ -268,7 +315,26 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
// Check that there were no unfixed errors.
bool rx_error = check_syndrome() != 0;
if(rx_error)
{
DEBUG_PRINTF(1, "Error in packet\n\r");
return -2;
}
return len + 2;
}
/**
* Process a packet that has been received.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \param[in] received_len The length of data received.
* \return 0 Failure
* \return 1 Success
*/
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
uint16_t len = PHPacketSizeECC(p);
// Add the RSSI and AFC to the packet.
p->header.rssi = *(((int8_t*)p) + len);
@ -310,11 +376,11 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
{
// Find the packet ID in the TX buffer, and free it.
unsigned int i = 0;
for (unsigned int i = 0; i < data->cfg.txWinSize; ++i)
for (unsigned int i = 0; i < data->cfg.winSize; ++i)
if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
PHReleaseTXPacket(h, data->tx_packets + i);
#ifdef DEBUG_LEVEL
if (i == data->cfg.txWinSize)
if (i == data->cfg.winSize)
DEBUG_PRINTF(1, "Error finding acked packet to release\n\r");
#endif
}
@ -324,11 +390,11 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
{
// Resend the packet.
unsigned int i = 0;
for ( ; i < data->cfg.txWinSize; ++i)
for ( ; i < data->cfg.winSize; ++i)
if (data->tx_packets[i].header.tx_seq == p->header.rx_seq)
PHLTransmitPacket(data, data->tx_packets + i);
#ifdef DEBUG_LEVEL
if (i == data->cfg.txWinSize)
if (i == data->cfg.winSize)
DEBUG_PRINTF(1, "Error finding acked packet to NACK\n\r");
DEBUG_PRINTF(1, "Resending after NACK\n\r");
#endif
@ -353,12 +419,16 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len)
if(data->data_handler)
data->data_handler(p->data, p->header.data_size);
// Release the packet.
PHReleaseTXPacket(h, p);
break;
default:
break;
}
return 1;
}
@ -373,16 +443,8 @@ uint8_t PHBroadcastStatus(PHInstHandle h, uint32_t id, int8_t rssi)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Create the status message
PHPacketHeader header;
header.destination_id = 0xffffffff;
header.source_id = id;
header.rssi = rssi;
header.type = PACKET_TYPE_STATUS;
header.data_size = 0;
// Send the packet.
return PHLTransmitPacket(data, (PHPacketHandle)&header);
return PHLTransmitPacket(data, (PHPacketHandle)&h);
}
/**

370
flight/Modules/RadioComBridge/RadioComBridge.c
View File

@ -44,24 +44,11 @@
//#undef PIOS_INCLUDE_USB
// ****************
// Private functions
static void radio2ComBridgeTask(void *parameters);
static void com2RadioBridgeTask(void *parameters);
static void radioStatusTask(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);
static void SendGCSReceiver(void);
static void SendPipXStatus(void);
static void StatusHandler(PHPacketHandle p);
static void PPMHandler(uint16_t *channels);
static void updateSettings();
// ****************
// Private constants
#define TEMP_BUFFER_SIZE 25
#define STACK_SIZE_BYTES 300
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
@ -75,6 +62,10 @@ static void updateSettings();
#define MAX_LOST_CONTACT_TIME 10
#define PACKET_QUEUE_SIZE 5
#define EV_PACKET_RECEIVED 0x10
// ****************
// Private types
@ -86,14 +77,12 @@ typedef struct {
typedef struct {
// The task handles.
xTaskHandle radio2ComBridgeTaskHandle;
xTaskHandle com2RadioBridgeTaskHandle;
xTaskHandle comUAVTalkTaskHandle;
xTaskHandle radioReceiveTaskHandle;
xTaskHandle sendPacketTaskHandle;
xTaskHandle sendDataTaskHandle;
xTaskHandle radioStatusTaskHandle;
// The com buffers.
uint8_t *radio2com_buf;
uint8_t *com2radio_buf;
// The com ports
uint32_t com_port;
uint32_t radio_port;
@ -102,6 +91,10 @@ typedef struct {
UAVTalkConnection inUAVTalkCon;
UAVTalkConnection outUAVTalkCon;
// Queue handles.
xQueueHandle sendPacketQueue;
xQueueHandle objEventQueue;
// Error statistics.
uint32_t comTxErrors;
uint32_t comTxRetries;
@ -131,6 +124,35 @@ typedef struct {
} 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 radioStatusTask(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);
/*
static void SendGCSReceiver(void);
static void SendPipXStatus(void);
*/
static void StatusHandler(PHPacketHandle p);
static void PPMHandler(uint16_t *channels);
static void updateSettings();
static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length);
static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms);
// ****************
// Private variables
@ -145,12 +167,16 @@ static int32_t RadioComBridgeStart(void)
{
if(data) {
// Start the tasks
xTaskCreate(radio2ComBridgeTask, (signed char *)"Radio2ComBridge", STACK_SIZE_BYTES/2, NULL, TASK_PRIORITY, &(data->radio2ComBridgeTaskHandle));
xTaskCreate(com2RadioBridgeTask, (signed char *)"Com2RadioBridge", STACK_SIZE_BYTES/2, NULL, TASK_PRIORITY, &(data->com2RadioBridgeTaskHandle));
xTaskCreate(comUAVTalkTask, (signed char *)"ComUAVTalk", STACK_SIZE_BYTES/2, NULL, TASK_PRIORITY + 2, &(data->comUAVTalkTaskHandle));
xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES/2, NULL, TASK_PRIORITY, &(data->radioReceiveTaskHandle));
xTaskCreate(sendPacketTask, (signed char *)"SendPacketTask", STACK_SIZE_BYTES/2, NULL, TASK_PRIORITY, &(data->sendPacketTaskHandle));
xTaskCreate(sendDataTask, (signed char *)"SendDataTask", STACK_SIZE_BYTES/2, NULL, TASK_PRIORITY, &(data->sendDataTaskHandle));
xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES/2, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle));
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIOCOM);
PIOS_WDG_RegisterFlag(PIOS_WDG_COMRADIO);
PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK);
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE);
PIOS_WDG_RegisterFlag(PIOS_WDG_SENDPACKET);
PIOS_WDG_RegisterFlag(PIOS_WDG_SENDDATA);
#endif
return 0;
}
@ -181,16 +207,14 @@ static int32_t RadioComBridgeInitialize(void)
data->com_port = PIOS_COM_BRIDGE_COM;
data->radio_port = PIOS_COM_BRIDGE_RADIO;
// Allocate the com buffers.
data->radio2com_buf = pvPortMalloc(PIOS_PH_MAX_PACKET);
PIOS_Assert(data->radio2com_buf);
data->com2radio_buf = pvPortMalloc(BRIDGE_BUF_LEN);
PIOS_Assert(data->com2radio_buf);
// 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 destination ID
data->destination_id = 0xffffffff;
@ -225,35 +249,178 @@ static int32_t RadioComBridgeInitialize(void)
data->pairStats[i].lastContact = 0;
}
// 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);
updateSettings();
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(data->com_port, TEMP_BUFFER_SIZE);
while (1) {
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
PIOS_WDG_UpdateFlag(PIOS_WDG_COMUAVTALK);
#endif /* PIOS_INCLUDE_WDG */
// Read the next byte
uint8_t rx_byte;
if(!BufferedRead(f, &rx_byte, 100))
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)
{
DEBUG_PRINTF(2, "Packet dropped!\n\r");
return;
}
// 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);
if (state == UAVTALK_STATE_COMPLETE) {
// Queue the packet for transmission.
xQueueSend(data->sendPacketQueue, &p, portMAX_DELAY);
p = NULL;
} else if(state == UAVTALK_STATE_ERROR) {
DEBUG_PRINTF(1, "UAVTalk FAILED!\n\r");
// Release the packet and start over again.
PHReleaseTXPacket(pios_packet_handler, p);
p = NULL;
}
}
}
/**
* The radio to com bridge task.
*/
static void radio2ComBridgeTask(void *parameters)
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_RADIOCOM);
PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE);
#endif /* PIOS_INCLUDE_WDG */
// Receive data from the radio port
rx_bytes = PIOS_COM_ReceiveBuffer(data->radio_port, data->radio2com_buf, PIOS_PH_MAX_PACKET, 200);
// Get a RX packet from the packet handler if required.
if (p == NULL)
p = PHGetRXPacket(pios_packet_handler);
// Receive the packet.
if (rx_bytes > 0)
PHReceivePacket(pios_packet_handler, (PHPacketHandle)data->radio2com_buf, rx_bytes);
// Receive data from the radio port
rx_bytes = PIOS_COM_ReceiveBuffer(data->radio_port, (uint8_t*)p, PIOS_PH_MAX_PACKET, 200);
// 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
PHReceivePacket(pios_packet_handler, p, false);
}
}
/**
* 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, portMAX_DELAY) == pdTRUE) {
// Send the packet.
PHTransmitPacket(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.
PIOS_WDG_UpdateFlag(PIOS_WDG_SENDDATA);
#endif /* PIOS_INCLUDE_WDG */
// Wait for a packet on the queue.
if (xQueueReceive(data->objEventQueue, &ev, portMAX_DELAY) == pdTRUE) {
if (ev.event == EV_UPDATED)
{
// 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, REQ_TIMEOUT_MS);
++retries;
}
}
else if(ev.event == EV_PACKET_RECEIVED)
{
// Receive the packet.
PHReceivePacket(pios_packet_handler, (PHPacketHandle)ev.obj, false);
}
}
}
}
#ifdef NEVER
/**
* The com to radio bridge task.
*/
@ -340,12 +507,18 @@ static void com2RadioBridgeTask(void * parameters)
}
}
}
#endif
/**
* The stats update task.
*/
static void radioStatusTask(void *parameters)
{
PHPacketHeader status_packet;
status_packet.destination_id = 0xffffffff;
status_packet.type = PACKET_TYPE_STATUS;
status_packet.data_size = 0;
while (1) {
PipXStatusData pipxStatus;
@ -367,15 +540,16 @@ static void radioStatusTask(void *parameters)
// Update the object
PipXStatusSet(&pipxStatus);
// Schedule the status for transmssion
data->send_pipxstatus = true;
// Broadcast the status.
{
static uint16_t cntr = 0;
if(cntr++ == RADIOSTATS_UPDATE_PERIOD_MS / STATS_UPDATE_PERIOD_MS)
{
PHBroadcastStatus(pios_packet_handler, pipxStatus.DeviceID, pipxStatus.RSSI);
// Queue the status message
status_packet.source_id = pipxStatus.DeviceID;
status_packet.rssi = pipxStatus.RSSI;
PHPacketHandle sph = (PHPacketHandle)&status_packet;
xQueueSend(data->sendPacketQueue, &sph, portMAX_DELAY);
cntr = 0;
}
}
@ -429,81 +603,10 @@ static void receiveData(uint8_t *buf, uint8_t len)
outputPort = PIOS_COM_TELEM_USB;
#endif /* PIOS_INCLUDE_USB */
// Send a local UAVTalk message if one is waiting to be sent.
if (data->send_gcsreceiver && data->uavtalk_idle)
SendGCSReceiver();
if (data->send_pipxstatus && data->uavtalk_idle)
SendPipXStatus();
uint8_t sent_bytes = 0;
for (uint8_t i = 0; i < len; ++i)
{
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->outUAVTalkCon, buf[i]);
/* if(state == UAVTALK_STATE_ERROR) */
/* DEBUG_PRINTF(2, "OUT Error\n\r\r"); */
if((state == UAVTALK_STATE_COMPLETE) || (state == UAVTALK_STATE_SYNC))
{
// Send the buffer up to this point
uint8_t send_bytes = i - sent_bytes;
if (PIOS_COM_SendBufferNonBlocking(outputPort, buf + sent_bytes, send_bytes) != send_bytes)
// Error on transmit
data->comTxErrors++;
sent_bytes = i;
// The connection is now idle.
data->uavtalk_idle = true;
// Send a local UAVTalk message if one is waiting to be sent.
if (data->send_gcsreceiver && data->uavtalk_idle)
SendGCSReceiver();
if (data->send_pipxstatus && data->uavtalk_idle)
SendPipXStatus();
}
}
// Send the received data to the com port
uint8_t send_bytes = len - sent_bytes;
if (send_bytes > 0)
{
if (PIOS_COM_SendBufferNonBlocking(outputPort, buf + sent_bytes, send_bytes) != send_bytes)
// Error on transmit
data->comTxErrors++;
// The connection is not idle.
data->uavtalk_idle = false;
}
}
/**
* Transmit the GCSReceiver object using UAVTalk
* \param[in] channels The ppm channels
*/
static void SendGCSReceiver(void)
{
// Send update (with retries)
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->outUAVTalkCon, GCSReceiverHandle(), 0, 0, REQ_TIMEOUT_MS);
++retries;
}
if(success >= 0)
data->send_gcsreceiver = false;
}
/**
* Transmit the GCSReceiver object using UAVTalk
* \param[in] channels The ppm channels
*/
static void SendPipXStatus(void)
{
// Transmit the PipXStatus
UAVTalkSendObject(data->outUAVTalkCon, PipXStatusHandle(), 0, 0, REQ_TIMEOUT_MS);
data->send_pipxstatus = false;
// Increment the last contact for each device
for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
++data->pairStats[i].lastContact;
if (PIOS_COM_SendBufferNonBlocking(outputPort, buf, len) != len)
// Error on transmit
data->comTxErrors++;
}
/**
@ -578,6 +681,41 @@ static void PPMHandler(uint16_t *channels)
data->send_gcsreceiver = true;
}
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 updateSettings()
{
if (data->com_port) {

6
flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h
View File

@ -71,8 +71,10 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
//------------------------
#define PIOS_WATCHDOG_TIMEOUT 500
#define PIOS_WDG_REGISTER BKP_DR4
#define PIOS_WDG_RADIOCOM 0x0001
#define PIOS_WDG_COMRADIO 0x0002
#define PIOS_WDG_COMUAVTALK 0x0001
#define PIOS_WDG_RADIORECEIVE 0x0002
#define PIOS_WDG_SENDPACKET 0x0004
#define PIOS_WDG_SENDDATA 0x0008
//------------------------
// TELEMETRY

1
flight/PiOS/inc/pios_rfm22b.h
View File

@ -41,7 +41,6 @@ struct pios_rfm22b_cfg {
uint8_t maxTxPower;
uint32_t sendTimeout;
uint8_t minPacketSize;
uint8_t txWinSize;
uint8_t maxConnections;
};

3
flight/board_hw_defs/pipxtreme/board_hw_defs.c
View File

@ -496,7 +496,6 @@ const struct pios_rfm22b_cfg pios_rfm22b_cfg = {
.maxTxPower = RFM22_tx_pwr_txpow_7, // +20dBm .. 100mW
.sendTimeout = 25, /* ms */
.minPacketSize = 100,
.txWinSize = 4,
.maxConnections = 1,
};
@ -507,7 +506,7 @@ const struct pios_rfm22b_cfg pios_rfm22b_cfg = {
// Initialize the packet handler
PacketHandlerConfig pios_ph_cfg = {
.txWinSize = PIOS_PH_TX_WIN_SIZE,
.winSize = PIOS_PH_TX_WIN_SIZE,
.maxConnections = PIOS_PH_MAX_CONNECTIONS,
};