From 3137d5288bd16aad77a82cdd9bdf3223d8699aee Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 23 Jul 2012 19:39:17 -0500 Subject: [PATCH] Add a message type flag (0x80) to UAVTalk which inserts a timestamp after the previous header and the decoding to read this out. --- flight/UAVTalk/inc/uavtalk.h | 3 +- flight/UAVTalk/inc/uavtalk_priv.h | 7 +++- flight/UAVTalk/uavtalk.c | 68 ++++++++++++++++++++++++++++++- 3 files changed, 74 insertions(+), 4 deletions(-) diff --git a/flight/UAVTalk/inc/uavtalk.h b/flight/UAVTalk/inc/uavtalk.h index a45fdb0db..9a2ab6d32 100644 --- a/flight/UAVTalk/inc/uavtalk.h +++ b/flight/UAVTalk/inc/uavtalk.h @@ -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); diff --git a/flight/UAVTalk/inc/uavtalk_priv.h b/flight/UAVTalk/inc/uavtalk_priv.h index 992bf91af..f23adabe4 100644 --- a/flight/UAVTalk/inc/uavtalk_priv.h +++ b/flight/UAVTalk/inc/uavtalk_priv.h @@ -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) \ diff --git a/flight/UAVTalk/uavtalk.c b/flight/UAVTalk/uavtalk.c index bf0d7e100..a24eec9f3 100644 --- a/flight/UAVTalk/uavtalk.c +++ b/flight/UAVTalk/uavtalk.c @@ -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)