handling of timestamp info for packets; better names for input and output flag values

Author: Craig Versek

PacketCommand.cpp

@@ -44,7 +44,6 @@ PacketCommand::PacketCommand(size_t maxCommands,
   else{ //do not allocate anything
     _output_buffer = NULL;
   }
-
   reset();
 }
 
@@ -66,6 +65,7 @@ PacketShared::STATUS PacketCommand::reset(){
   _input_index = 0;
   _input_len   = 0;
   _input_flags = 0x00;
+  _recv_timestamp_micros = 0;
   //reset output buffer
   _output_index = 0;
   _output_len   = 0;
@@ -347,6 +347,7 @@ PacketShared::STATUS PacketCommand::recv() {
 }
 
 PacketShared::STATUS PacketCommand::recv(bool& gotPacket) {
+  uint32_t timestamp_micros = micros();
   gotPacket = false;
   #ifdef PACKETCOMMAND_DEBUG
   Serial.println(F("In PacketCommand::recv()"));
@@ -364,13 +365,19 @@ PacketShared::STATUS PacketCommand::recv(bool& gotPacket) {
     return PacketShared::ERROR_NULL_HANDLER_FUNCTION_POINTER;
   }
   if (gotPacket){ //we have a packet
+    set_recvTimestamp(timestamp_micros);
     return PacketShared::SUCCESS;
   }
   else{  //we have no packet
     return PacketShared::NO_PACKET_RECEIVED;
   }
 }
 
+PacketShared::STATUS PacketCommand::set_recvTimestamp(uint32_t timestamp_micros){
+  _recv_timestamp_micros = timestamp_micros;
+  return PacketShared::SUCCESS;
+}
+
 /**
  *g This checks the characters in the buffer looking for a valid type_id prefix
  * string, which adheres to the following pseudocode rules:
@@ -536,8 +543,20 @@ PacketShared::STATUS PacketCommand::setupOutputCommand(PacketCommand::CommandInf
 
 // Use the '_send_callback' to send return packet
 PacketShared::STATUS PacketCommand::send(){
+  uint32_t timestamp_micros = micros();
   if (_send_callback != NULL){
-    //call the send callback
+    set_sendTimestamp(timestamp_micros);  //markdown the time write now
+    if (_output_flags & PacketShared::OPFLAG_APPEND_SEND_TIMESTAMP){
+      if ((_output_len + sizeof(uint32_t)) < PacketShared::DATA_BUFFER_SIZE){ //prevent buffer overrun
+        pack_uint32(timestamp_micros);
+      }
+      else{
+        #ifdef PACKETCOMMAND_DEBUG
+        Serial.println(F("### Error: appeding send timestamp would overrun the output buffer"));
+        #endif
+      }
+    }
+    //call the callback!
     (*_send_callback)(*this);
     return PacketShared::SUCCESS;
   }
@@ -551,7 +570,19 @@ PacketShared::STATUS PacketCommand::send(){
 
 // Use the '_send_nonblocking_callback' to send return packet
 PacketShared::STATUS PacketCommand::send_nonblocking(){
+  uint32_t timestamp_micros = micros();
   if (_send_nonblocking_callback != NULL){
+    set_sendTimestamp(timestamp_micros);  //markdown the time write now
+    if (_output_flags & PacketShared::OPFLAG_APPEND_SEND_TIMESTAMP){
+      if ((_output_len + sizeof(uint32_t)) < PacketShared::DATA_BUFFER_SIZE){ //prevent buffer overrun
+        pack_uint32(timestamp_micros);
+      }
+      else{
+        #ifdef PACKETCOMMAND_DEBUG
+        Serial.println(F("### Error: appeding send timestamp would overrun the output buffer"));
+        #endif
+      }
+    }
     //call the nonblocking send callback
     (*_send_nonblocking_callback)(*this);
     return PacketShared::SUCCESS;
@@ -564,6 +595,10 @@ PacketShared::STATUS PacketCommand::send_nonblocking(){
   }
 }
 
+PacketShared::STATUS PacketCommand::set_sendTimestamp(uint32_t timestamp_micros){
+  _send_timestamp_micros = timestamp_micros;
+  return PacketShared::SUCCESS;
+}
 
 // Use the '_reply_send_callback' to send a quick reply
 PacketShared::STATUS PacketCommand::reply_send(){
@@ -596,7 +631,6 @@ PacketShared::STATUS PacketCommand::reply_recv(){
   }
 }
 
-
 /**
  * Accessors and mutators for the input buffer
 */
@@ -673,6 +707,7 @@ PacketShared::STATUS PacketCommand::enqueueInputBuffer(PacketQueue& pq){
   //build a packet struct to hold current buffer state
   PacketShared::Packet pkt;
   pkt.length = min(_input_len, PacketShared::DATA_BUFFER_SIZE);
+  pkt.timestamp = _recv_timestamp_micros;  //this should have been recorded as close to the RX time as possible
   pkt.flags  = _input_flags;
   memcpy(pkt.data, _input_buffer, pkt.length);
   PacketShared::STATUS pqs;
@@ -704,6 +739,8 @@ PacketShared::STATUS PacketCommand::dequeueInputBuffer(PacketQueue& pq){
   }
 }
 
+
+
 void  PacketCommand::allocateOutputBuffer(size_t len){
   _outputBufferSize = len;
   _output_buffer = (byte*) calloc(_outputBufferSize, sizeof(byte));
@@ -741,6 +778,7 @@ PacketShared::STATUS PacketCommand::enqueueOutputBuffer(PacketQueue& pq){
   //build a packet struct to hold current buffer state
   PacketShared::Packet pkt;
   pkt.length = min(_output_len, PacketShared::DATA_BUFFER_SIZE);
+  pkt.timestamp = 0;
   pkt.flags  = _output_flags;
   memcpy(pkt.data, _output_buffer, pkt.length);
   PacketShared::STATUS pqs;
@@ -780,6 +818,7 @@ PacketShared::STATUS PacketCommand::requeueOutputBuffer(PacketQueue& pq){
   //build a packet struct to hold current buffer state
   PacketShared::Packet pkt;
   pkt.length = min(_output_len, PacketShared::DATA_BUFFER_SIZE);
+  pkt.timestamp = 0;
   pkt.flags  = _output_flags;
   memcpy(pkt.data, _output_buffer, pkt.length);
   PacketShared::STATUS pqs;

PacketCommand.h

@@ -34,7 +34,7 @@
 #include "PacketShared.h"
 
 // Uncomment the next line to run the library in debug mode (verbose messages)
-#define PACKETCOMMAND_DEBUG
+//#define PACKETCOMMAND_DEBUG
 
 typedef float  float32_t;
 typedef double float64_t;
@@ -82,10 +82,13 @@ class PacketCommand{
     CommandInfo getCurrentCommand();
     PacketShared::STATUS recv();                // Use the '_recv_callback' to put data into _input_buffer
     PacketShared::STATUS recv(bool& gotPacket); // Use the '_recv_callback' to put data into _input_buffer
+    PacketShared::STATUS set_recvTimestamp(uint32_t timestamp_micros);
+    uint32_t             get_recvTimestamp(){return _recv_timestamp_micros;};
     PacketShared::STATUS matchCommand();        // Read the packet header from the input buffer and locate a matching registered handler function
     PacketShared::STATUS dispatchCommand();     // Call the current Command
     PacketShared::STATUS send();                // Use the '_send_callback' to send _output_buffer
     PacketShared::STATUS send_nonblocking();    // Use the '_send_nonblocking_callback' to send _schedule the output_buffer contents to be sent, returning immediately
+    PacketShared::STATUS set_sendTimestamp(uint32_t timestamp_micros);
     PacketShared::STATUS reply_send();
     PacketShared::STATUS reply_recv();
 
@@ -102,8 +105,10 @@ class PacketCommand{
     int    getOutputBufferIndex();
     size_t getOutputLen(){return _output_len;};
     PacketShared::STATUS setOutputBufferIndex(int new_index);
-    PacketShared::STATUS markOutputAsQuery(){_output_flags|=PacketShared::PFLAG_IS_QUERY;return PacketShared::SUCCESS;}
-    bool   outputIsQuery(){return (bool)_output_flags&PacketShared::PFLAG_IS_QUERY;}
+    byte   getOutputFlags(){return _output_flags;};
+    void   flagOutputAsQuery(){_output_flags|=PacketShared::OPFLAG_IS_QUERY;};
+    void   flagOutputAppendSendTimestamp(){_output_flags|=PacketShared::OPFLAG_APPEND_SEND_TIMESTAMP;};
+    bool   outputIsQuery(){return (bool)_output_flags&PacketShared::OPFLAG_IS_QUERY;};
     PacketShared::STATUS enqueueOutputBuffer(PacketQueue& pq);
     PacketShared::STATUS dequeueOutputBuffer(PacketQueue& pq);
     PacketShared::STATUS requeueOutputBuffer(PacketQueue& pq);
@@ -162,17 +167,19 @@ class PacketCommand{
     size_t  _commandCount;
     size_t  _maxCommands;
     //track state of input buffer
-    size_t _inputBufferSize;
-    byte*  _input_buffer;        //this will be a fixed buffer location
-    size_t _input_index;
-    size_t _input_len;
-    byte   _input_flags;
+    size_t   _inputBufferSize;
+    byte*    _input_buffer;        //this will be a fixed buffer location
+    size_t   _input_index;
+    size_t   _input_len;
+    byte     _input_flags;
+    uint32_t _recv_timestamp_micros;
     //track state of output buffer
     size_t _outputBufferSize;
     byte*  _output_buffer;       //this will be a fixed buffer location
     size_t _output_index;
     size_t _output_len;
     byte   _output_flags;
+    uint32_t _send_timestamp_micros;
     //cached callbacks
     void (*_send_callback)(PacketCommand& this_pCmd);
     void (*_send_nonblocking_callback)(PacketCommand& this_pCmd);

PacketQueue.h

@@ -8,7 +8,7 @@
 #include "PacketShared.h"
 
 //uncomment for debugging
-#define PACKETQUEUE_DEBUG
+//#define PACKETQUEUE_DEBUG
 
 class PacketQueue
 {

PacketShared.h

@@ -27,20 +27,32 @@ namespace PacketShared{
   static const size_t DATA_BUFFER_SIZE = 32;
   // Packet structure
   struct Packet {
-    byte    data[DATA_BUFFER_SIZE];
-    size_t  length;
-    byte    flags;
+    byte     data[DATA_BUFFER_SIZE];
+    size_t   length;
+    uint32_t timestamp;
+    byte     flags;
+  };
+  
+  enum InputPacketFlags {
+       IPFLAG_IS_QUERY = 0x01,
+       //IPFLAG_1 = 0x02,
+       //IPFLAG_2 = 0x04,
+       //IPFLAG_3 = 0x08,
+       //IPFLAG_4 = 0x10,
+       //IPFLAG_5 = 0x20,
+       //IPFLAG_6 = 0x40,
+       //IPFLAG_7 = 0x80
   };
 
-  enum PacketFlags {
-       PFLAG_IS_QUERY = 0x01,
-       //PFLAG_1 = 0x02,
-       //PFLAG_2 = 0x04,
-       //PFLAG_3 = 0x08,
-       //PFLAG_4 = 0x10,
-       //PFLAG_5 = 0x20,
-       //PFLAG_6 = 0x40,
-       //PFLAG_7 = 0x80
+  enum OutputPacketFlags {
+       OPFLAG_IS_QUERY = 0x01,
+       OPFLAG_APPEND_SEND_TIMESTAMP = 0x02,
+       //OPFLAG_2 = 0x04,
+       //OPFLAG_3 = 0x08,
+       //OPFLAG_4 = 0x10,
+       //OPFLAG_5 = 0x20,
+       //OPFLAG_6 = 0x40,
+       //OPFLAG_7 = 0x80
   };
 };