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

OP-932 Changes the UAVTalkRelayInputStream function to UAVTalkRelayPacket to allow for not relaying a packet on error, etc. Also adds a function to get the object ID out of the current packet (UAVTalkGetPacketObjId). These functions are used by the OPLink.

Browse Source
This commit is contained in:
Brian Webb 2013-06-15 07:31:29 -07:00
parent 08efc8d152
commit 12c8ef2e3a
2 changed files with 116 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -59,10 +59,12 @@ int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId);
int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len); int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len);
UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte); UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte); UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte);
UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle);
int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle);
void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats); void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats);
void UAVTalkResetStats(UAVTalkConnection connection); void UAVTalkResetStats(UAVTalkConnection connection);
void UAVTalkGetLastTimestamp(UAVTalkConnection connection, uint16_t *timestamp); void UAVTalkGetLastTimestamp(UAVTalkConnection connection, uint16_t *timestamp);
uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connection);
#endif // UAVTALK_H #endif // UAVTALK_H
/** /**

160
flight/uavtalk/uavtalk.c
View File

@ -287,7 +287,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle
/** /**
* Process an byte from the telemetry stream. * Process an byte from the telemetry stream.
* \param[in] connection UAVTalkConnection to be used * \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] rxbyte Received byte * \param[in] rxbyte Received byte
* \return UAVTalkRxState * \return UAVTalkRxState
*/ */
@ -534,83 +534,117 @@ UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uin
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte);
if (state == UAVTALK_STATE_COMPLETE) { if (state == UAVTALK_STATE_COMPLETE) {
UAVTalkConnectionData *connection; UAVTalkReceiveObject(connectionHandle);
CHECKCONHANDLE(connectionHandle, connection, return -1);
UAVTalkInputProcessor *iproc = &connection->iproc;
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
xSemaphoreGiveRecursive(connection->lock);
} }
return state; return state;
} }
/** /**
* Process an byte from the telemetry stream, sending the packet out the output stream when it's complete * Send a parsed packet received on one connection handle out on a different connection handle.
* This allows the interlieving of packets on an output UAVTalk stream, and is used by the OPLink device to * The packet must be in a complete state, meaning it is completed parsing.
* relay packets from an input com port to a different output com port without sending one packet in the middle * The packet is re-assembled from the component parts into a complete message and sent.
* of another packet. Because this uses both the receive buffer and transmit buffer, it should only be used * This can be used to relay packets from one UAVTalk connection to another.
* for relaying a packet, not for the standard sending and receiving of packets.
* \param[in] connection UAVTalkConnection to be used * \param[in] connection UAVTalkConnection to be used
* \param[in] rxbyte Received byte * \param[in] rxbyte Received byte
* \return UAVTalkRxState * \return 0 Success
* \return -1 Failure
*/ */
UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle)
{ {
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); UAVTalkConnectionData *inConnection;
if (state == UAVTALK_STATE_COMPLETE) { CHECKCONHANDLE(inConnectionHandle, inConnection, return -1);
UAVTalkConnectionData *connection; UAVTalkInputProcessor *inIproc = &inConnection->iproc;
CHECKCONHANDLE(connectionHandle, connection, return -1);
UAVTalkInputProcessor *iproc = &connection->iproc;
if (!connection->outStream) { // The input packet must be completely parsed.
if (inIproc->state != UAVTALK_STATE_COMPLETE) {
return -1; return -1;
} }
UAVTalkConnectionData *outConnection;
CHECKCONHANDLE(outConnectionHandle, outConnection, return -1);
if (!outConnection->outStream) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY);
// Setup type and object id fields // Setup type and object id fields
connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
connection->txBuffer[1] = iproc->type; outConnection->txBuffer[1] = inIproc->type;
// data length inserted here below // data length inserted here below
connection->txBuffer[4] = (uint8_t)(iproc->objId & 0xFF); int32_t dataOffset = 8;
connection->txBuffer[5] = (uint8_t)((iproc->objId >> 8) & 0xFF); if (inIproc->objId != 0) {
connection->txBuffer[6] = (uint8_t)((iproc->objId >> 16) & 0xFF); outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF);
connection->txBuffer[7] = (uint8_t)((iproc->objId >> 24) & 0xFF); outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF);
outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF);
outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF);
// Setup instance ID if one is required // Setup instance ID if one is required
int32_t dataOffset = 8; if (inIproc->instanceLength > 0) {
if (iproc->instanceLength > 0) { outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF);
connection->txBuffer[8] = (uint8_t)(iproc->instId & 0xFF); outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF);
connection->txBuffer[9] = (uint8_t)((iproc->instId >> 8) & 0xFF);
dataOffset = 10; dataOffset = 10;
} }
} else {
dataOffset = 4;
}
// Add timestamp when the transaction type is appropriate // Add timestamp when the transaction type is appropriate
if (iproc->type & UAVTALK_TIMESTAMPED) { if (inIproc->type & UAVTALK_TIMESTAMPED) {
portTickType time = xTaskGetTickCount(); portTickType time = xTaskGetTickCount();
connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); outConnection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF);
connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); outConnection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF);
dataOffset += 2; dataOffset += 2;
} }
// Copy data (if any) // Copy data (if any)
memcpy(&connection->txBuffer[dataOffset], connection->rxBuffer, iproc->length); memcpy(&outConnection->txBuffer[dataOffset], inConnection->rxBuffer, inIproc->length);
// Store the packet length // Store the packet length
connection->txBuffer[2] = (uint8_t)((dataOffset + iproc->length) & 0xFF); outConnection->txBuffer[2] = (uint8_t)((dataOffset + inIproc->length) & 0xFF);
connection->txBuffer[3] = (uint8_t)(((dataOffset + iproc->length) >> 8) & 0xFF); outConnection->txBuffer[3] = (uint8_t)(((dataOffset + inIproc->length) >> 8) & 0xFF);
// Copy the checksum // Copy the checksum
connection->txBuffer[dataOffset + iproc->length] = iproc->cs; outConnection->txBuffer[dataOffset + inIproc->length] = inIproc->cs;
// Send the buffer. // Send the buffer.
if (UAVTalkSendBuf(connectionHandle, connection->txBuffer, iproc->rxPacketLength) < 0) { int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, inIproc->rxPacketLength);
return UAVTALK_STATE_ERROR;
} // Update stats
outConnection->stats.txBytes += rc;
// Release lock
xSemaphoreGiveRecursive(outConnection->lock);
// Done
if (rc != inIproc->rxPacketLength) {
return -1;
} }
return state; return 0;
}
/**
* Complete receiving a UAVTalk packet. This will cause the packet to be unpacked, acked, etc.
* \param[in] connectionHandle UAVTalkConnection to be used
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1);
UAVTalkInputProcessor *iproc = &connection->iproc;
if (iproc->state != UAVTALK_STATE_COMPLETE) {
return -1;
}
receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
return 0;
} }
/** /**
@ -662,37 +696,17 @@ int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId)
} }
/** /**
* Send a buffer containing a UAVTalk message through the telemetry link. * Get the object ID of the current packet.
* This function locks the connection prior to sending. * \param[in] connectionHandle UAVTalkConnection to be used
* \param[in] connection UAVTalkConnection to be used * \param[in] objId Object ID to send a NACK for
* \param[in] buf The data buffer containing the UAVTalk message * \return The object ID, or 0 on error.
* \param[in] len The number of bytes to send from the data buffer
* \return 0 Success
* \return -1 Failure
*/ */
int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len) uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle)
{ {
UAVTalkConnectionData *connection; UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle, connection, return -1); CHECKCONHANDLE(connectionHandle, connection, return 0);
return connection->iproc.objId;
// Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// Output the buffer
int32_t rc = (*connection->outStream)(buf, len);
// Update stats
connection->stats.txBytes += len;
// Release lock
xSemaphoreGiveRecursive(connection->lock);
// Done
if (rc != len) {
return -1;
}
return 0;
} }
/** /**
@ -716,6 +730,9 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
UAVObjHandle obj; UAVObjHandle obj;
int32_t ret = 0; int32_t ret = 0;
// Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// Get the handle to the Object. Will be zero // Get the handle to the Object. Will be zero
// if object does not exist. // if object does not exist.
obj = UAVObjGetByID(objId); obj = UAVObjGetByID(objId);
@ -734,6 +751,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
ret = -1; ret = -1;
} }
break; break;
case UAVTALK_TYPE_OBJ_ACK: case UAVTALK_TYPE_OBJ_ACK:
case UAVTALK_TYPE_OBJ_ACK_TS: case UAVTALK_TYPE_OBJ_ACK_TS:
// All instances, not allowed for OBJ_ACK messages // All instances, not allowed for OBJ_ACK messages
@ -772,6 +790,10 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
default: default:
ret = -1; ret = -1;
} }
// Unlock
xSemaphoreGiveRecursive(connection->lock);
// Done // Done
return ret; return ret;
} }