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

Add a message type flag (0x80) to UAVTalk which inserts a timestamp after the

Browse Source 
previous header and the decoding to read this out.
This commit is contained in:
James Cotton 2012-07-23 19:39:17 -05:00
parent 26f07c21f6
commit 3137d5288b
3 changed files with 74 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/UAVTalk/inc/uavtalk.h
View File

@ -45,13 +45,14 @@ typedef struct {
typedef void* UAVTalkConnection;
typedef enum {UAVTALK_STATE_ERROR=0, UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS, UAVTALK_STATE_COMPLETE} UAVTalkRxState;
typedef enum {UAVTALK_STATE_ERROR=0, UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_TIMESTAMP, UAVTALK_STATE_DATA, UAVTALK_STATE_CS, UAVTALK_STATE_COMPLETE} UAVTalkRxState;
// Public functions
UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream);
int32_t UAVTalkSetOutputStream(UAVTalkConnection connection, UAVTalkOutputStream outputStream);
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);
int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId);
int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId);

7
flight/UAVTalk/inc/uavtalk_priv.h
View File

@ -46,13 +46,14 @@ typedef struct {
uint16_t size;
uint32_t objId;
uint16_t instId;
uint16_t timestamp;
} uavtalk_max_header;
#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header)
typedef uint8_t uavtalk_checksum;
#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum)
#define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1)
#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH
#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH
#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH
typedef struct {
@ -64,6 +65,7 @@ typedef struct {
uint32_t length;
uint8_t instanceLength;
uint8_t cs;
uint16_t timestamp;
int32_t rxCount;
UAVTalkRxState state;
uint16_t rxPacketLength;
@ -90,11 +92,14 @@ typedef struct {
#define UAVTALK_SYNC_VAL 0x3C
#define UAVTALK_TYPE_MASK 0xF8
#define UAVTALK_TYPE_VER 0x20
#define UAVTALK_TIMESTAMPED 0x80
#define UAVTALK_TYPE_OBJ (UAVTALK_TYPE_VER | 0x00)
#define UAVTALK_TYPE_OBJ_REQ (UAVTALK_TYPE_VER | 0x01)
#define UAVTALK_TYPE_OBJ_ACK (UAVTALK_TYPE_VER | 0x02)
#define UAVTALK_TYPE_ACK (UAVTALK_TYPE_VER | 0x03)
#define UAVTALK_TYPE_NACK (UAVTALK_TYPE_VER | 0x04)
#define UAVTALK_TYPE_OBJ_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ)
#define UAVTALK_TYPE_OBJ_ACK_TS (UAVTALK_TIMESTAMPED | UAVTALK_TYPE_OBJ_ACK)
//macros
#define CHECKCONHANDLE(handle,variable,failcommand) \

68
flight/UAVTalk/uavtalk.c
View File

@ -189,6 +189,31 @@ int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj,
}
}
/**
* Send the specified object through the telemetry link with a timestamp.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object to send
* \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] acked Selects if an ack is required (1:ack required, 0: ack not required)
* \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return -1);
// Send object
if (acked == 1)
{
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK_TS, timeoutMs);
}
else
{
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_TS, timeoutMs);
}
}
/**
* Execute the requested transaction on an object.
* \param[in] connection UAVTalkConnection to be used
@ -388,6 +413,12 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
{
iproc->state = UAVTALK_STATE_INSTID;
}
// Check if this is a single instance and has a timestamp in it
else if ((iproc->obj != 0) && (iproc->type & UAVTALK_TIMESTAMPED))
{
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
}
else
{
// If there is a payload get it, otherwise receive checksum
@ -412,14 +443,38 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
iproc->rxCount = 0;
// If there is a timestamp, get it
if ((iproc->length > 0) && (iproc->type & UAVTALK_TIMESTAMPED))
{
iproc->timestamp = 0;
iproc->state = UAVTALK_STATE_TIMESTAMP;
}
// If there is a payload get it, otherwise receive checksum
if (iproc->length > 0)
else 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:
// update the CRC
@ -717,6 +772,15 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle
connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
dataOffset = 10;
}
// Add timestamp when the transaction type is appropriate
if (type & UAVTALK_TIMESTAMPED)
{
portTickType time = xTaskGetTickCount();
connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF);
connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF);
dataOffset += 2;
}
// Determine data length
if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK)