rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-21 13:28:58 +01:00
Code Issues Releases Activity

OP-1776 - Optimize RX exchanging buffers instead of working a byte at time

Browse Source
This commit is contained in:
Alessio Morale 2015-03-10 20:35:40 +01:00
parent 83ce7c7b9a
commit 5657f4757b
3 changed files with 200 additions and 195 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/modules/Telemetry/telemetry.c
View File

@ -432,14 +432,12 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters)
if (inputPort) {
// Block until data are available
uint8_t serial_data[1];
uint8_t serial_data[16];
uint16_t bytes_to_process;
bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
UAVTalkProcessInputStream(uavTalkCon, serial_data[i]);
}
UAVTalkProcessInputStream(uavTalkCon, serial_data, bytes_to_process);
}
} else {
vTaskDelay(5);
@ -457,14 +455,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
while (1) {
if (radioPort) {
// Block until data are available
uint8_t serial_data[1];
uint8_t serial_data[16];
uint16_t bytes_to_process;
bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]);
}
UAVTalkProcessInputStream(radioUavTalkCon, serial_data, bytes_to_process);
}
} else {
vTaskDelay(5);

4
flight/uavtalk/inc/uavtalk.h
View File

@ -57,8 +57,8 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t count);
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t *count, uint8_t *position);
int32_t UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle);
int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle);
void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats, bool reset);

379
flight/uavtalk/uavtalk.c
View File

@ -350,10 +350,12 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
/**
* Process an byte from the telemetry stream.
* \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] rxbyte Received byte
* \param[in] rxbuffer Received buffer
* \param[in/out] count Number of bytes to read inside buffer
* \param[in/out] position Next item to be read inside rxbuffer
* \return UAVTalkRxState
*/
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t rxbyte)
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t *count, uint8_t *position)
{
UAVTalkConnectionData *connection;
@ -361,231 +363,238 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
UAVTalkInputProcessor *iproc = &connection->iproc;
++connection->stats.rxBytes;
if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
iproc->state = UAVTALK_STATE_SYNC;
}
// stop processing as soon as a complete packet is received
while (*count > 0 && iproc->state != UAVTALK_STATE_COMPLETE) {
uint8_t rxbyte = rxbuffer[(*position)];
(*position)++;
(*count)--;
if (iproc->rxPacketLength < 0xffff) {
// update packet byte count
iproc->rxPacketLength++;
}
// Receive state machine
switch (iproc->state) {
case UAVTALK_STATE_SYNC:
if (rxbyte != UAVTALK_SYNC_VAL) {
connection->stats.rxSyncErrors++;
break;
if (iproc->rxPacketLength < 0xffff) {
// update packet byte count
iproc->rxPacketLength++;
}
++connection->stats.rxBytes;
// Receive state machine
switch (iproc->state) {
case UAVTALK_STATE_SYNC:
// Initialize and update the CRC
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->rxPacketLength = 1;
iproc->rxCount = 0;
iproc->type = 0;
iproc->state = UAVTALK_STATE_TYPE;
break;
case UAVTALK_STATE_TYPE:
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->type = rxbyte;
iproc->packet_size = 0;
iproc->state = UAVTALK_STATE_SIZE;
break;
case UAVTALK_STATE_SIZE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if (iproc->rxCount == 0) {
iproc->packet_size += rxbyte;
iproc->rxCount++;
break;
}
iproc->packet_size += rxbyte << 8;
iproc->rxCount = 0;
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
// incorrect packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
break;
case UAVTALK_STATE_OBJID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 4) {
break;
}
iproc->rxCount = 0;
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
break;
case UAVTALK_STATE_INSTID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
UAVObjHandle obj = UAVObjGetByID(iproc->objId);
// Determine data length
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
iproc->length = 0;
iproc->timestampLength = 0;
} else {
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
if (obj) {
iproc->length = UAVObjGetNumBytes(obj);
} else {
iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
if (rxbyte != UAVTALK_SYNC_VAL) {
connection->stats.rxSyncErrors++;
break;
}
}
// Check length
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
// packet error - exceeded payload max length
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
// Initialize and update the CRC
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->rxPacketLength = 1;
iproc->rxCount = 0;
iproc->type = 0;
iproc->state = UAVTALK_STATE_TYPE;
break;
}
// Check the lengths match
if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
case UAVTALK_STATE_TYPE:
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) {
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->type = rxbyte;
iproc->packet_size = 0;
iproc->state = UAVTALK_STATE_SIZE;
break;
}
// Determine next state
if (iproc->type & UAVTALK_TIMESTAMPED) {
// If there is a timestamp get it
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
} else {
case UAVTALK_STATE_SIZE:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if (iproc->rxCount == 0) {
iproc->packet_size += rxbyte;
iproc->rxCount++;
break;
}
iproc->packet_size += rxbyte << 8;
iproc->rxCount = 0;
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) {
// incorrect packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
break;
case UAVTALK_STATE_OBJID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->objId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 4) {
break;
}
iproc->rxCount = 0;
iproc->instId = 0;
iproc->state = UAVTALK_STATE_INSTID;
break;
case UAVTALK_STATE_INSTID:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->instId += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
UAVObjHandle obj = UAVObjGetByID(iproc->objId);
// Determine data length
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
iproc->length = 0;
iproc->timestampLength = 0;
} else {
iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
if (obj) {
iproc->length = UAVObjGetNumBytes(obj);
} else {
iproc->length = iproc->packet_size - iproc->rxPacketLength - iproc->timestampLength;
}
}
// Check length
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
// packet error - exceeded payload max length
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Check the lengths match
if ((iproc->rxPacketLength + iproc->timestampLength + iproc->length) != iproc->packet_size) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Determine next state
if (iproc->type & UAVTALK_TIMESTAMPED) {
// If there is a timestamp get it
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
} else {
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
}
break;
case UAVTALK_STATE_TIMESTAMP:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
}
break;
case UAVTALK_STATE_TIMESTAMP:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
iproc->timestamp += rxbyte << (8 * (iproc->rxCount++));
if (iproc->rxCount < 2) {
break;
}
iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0) {
iproc->state = UAVTALK_STATE_DATA;
} else {
iproc->state = UAVTALK_STATE_CS;
}
break;
case UAVTALK_STATE_DATA:
case UAVTALK_STATE_DATA:
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
// update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
connection->rxBuffer[iproc->rxCount++] = rxbyte;
if (iproc->rxCount < iproc->length) {
break;
}
iproc->rxCount = 0;
connection->rxBuffer[iproc->rxCount++] = rxbyte;
if (iproc->rxCount < iproc->length) {
iproc->state = UAVTALK_STATE_CS;
break;
}
iproc->rxCount = 0;
iproc->state = UAVTALK_STATE_CS;
break;
case UAVTALK_STATE_CS:
case UAVTALK_STATE_CS:
// Check the CRC byte
if (rxbyte != iproc->cs) {
// packet error - faulty CRC
UAVT_DEBUGLOG_PRINTF("BAD CRC");
connection->stats.rxCrcErrors++;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
connection->stats.rxObjects++;
connection->stats.rxObjectBytes += iproc->length;
iproc->state = UAVTALK_STATE_COMPLETE;
break;
default:
// Check the CRC byte
if (rxbyte != iproc->cs) {
// packet error - faulty CRC
UAVT_DEBUGLOG_PRINTF("BAD CRC");
connection->stats.rxCrcErrors++;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) {
// packet error - mismatched packet size
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_ERROR;
break;
}
connection->stats.rxObjects++;
connection->stats.rxObjectBytes += iproc->length;
iproc->state = UAVTALK_STATE_COMPLETE;
break;
default:
iproc->state = UAVTALK_STATE_ERROR;
break;
}
// Done
return iproc->state;
}
/**
* Process an byte from the telemetry stream.
* Process an buffer from the telemetry stream.
* \param[in] connection UAVTalkConnection to be used
* \param[in] rxbyte Received byte
* \param[in] rxbuffer Received buffer
* \param[in] count bytes inside rxbuffer
* \return UAVTalkRxState
*/
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte)
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t *rxbuffer, uint8_t count)
{
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte);
uint8_t position = 0;
UAVTalkRxState state = UAVTALK_STATE_ERROR;
if (state == UAVTALK_STATE_COMPLETE) {
UAVTalkReceiveObject(connectionHandle);
while (count > 0) {
state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbuffer, &count, &position);
if (state == UAVTALK_STATE_COMPLETE) {
UAVTalkReceiveObject(connectionHandle);
}
}
return state;
}