Get cru document format working

Author: alex-taffe

docs/drive_format.md

@@ -4,37 +4,39 @@ All drives are stored in a custom binary format to save space. The header will a
 ## Version 1:
 ## Packet format
 ```
-|------Header------|------n drive frames------|------final drive frames (all bits set to 1)------|------Footer------|
-      20 bytes          n frames * 67 bytes                         67 bytes  							1 byte
+|------Header------|------Polyline------|------drive frames------|------Footer------|
+      28 bytes 	  int8_t*Polyline Size  n frames * 14 bytes         1 byte
 ```
 
 ### Header:
 ```
-|------Version------|------VIN------|------Fuel tank level------|
-      uint16_t          int8_t * 17             uint8_t
+|------Version------|------VIN------|------Fuel tank level------|------Polyline Size------|------Drive Frame Size------|
+      uint16_t          int8_t * 17             uint8_t		      uint32_t			        uint32_t
 ```
 - Version: 16 bit unsigned integer to allow clients to correctly parse the data
-- VIN: A 17 character ascii string representing the vehicle VIN
+- VIN: A 17 character ascii string representing the vehicle VIN. **Not** null terminated
 - Fuel tank level: 8 bit unsigned integer representing the vehicle fuel tank level at the start of the drive, a percentage between 0 and 100 (inclusive)
+- Polyline Size: The length of the polyline string. Each element of the polyline string is int8_t
+- Drive Frame Size: The size of the drive frame buffer. Each frame is divided into 14 bytes, so n_frames = drive frame size / 14
+
 
 ### Drive frame:
-Each frame represents the vehicle state at a particular point in time. The final drive frame will have all bits set to 1 and is considered a stop frame and is therefore invalid.
+Each frame represents the vehicle state at a particular point in time.
 ```
-|-----GPS Time-----|-----GPS Speed-----|-----Latitutde-----|-----Longitude-----|-----Heading-----|-----Altitude-----|-----AccelX-----|-----AccelY-----|-----AccelZ-----|-----Vehicle Speed-----|
-      uint64_t		  float (4 byte)      float (4 byte)       float (4 byte)       uint16_t       float (4 byte)    float (4 byte)    float (4 byte)   float (4 byte)        uint8_t
+|-----GPS Time-----|-----GPS Speed-----|-----Heading-----|-----Altitude-----|-----Vehicle Speed-----|
+      uint64_t		    uint8_t              int16_t          int16_t              uint8_t
 ```
 - GPS Time: a 64 bit unsigned integer representing the unix time of the data frame
 - GPS Speed: a 64 bit float representing the GPS speed in km/h
-- GPS latitude: a 64 bit float representing the GPS latitude
-- GPS longtitude: a 64 bit float representing the GPS longitude
 - GPS heading: a 16 bit unsigned integer representing the GPS heading
 - GPS altitude: a 64 bit float representing the GPS heading
-- AccelX: a 64 bit float representing the acceleration of the device in the x axis
-- AccelY: a 64 bit float representing the acceleration of the device in the y axis
-- AccelZ: a 64 bit float representing the acceleration of the device in the z axis
 - Vehicle Speed: an 8 bit unsigned integer representing the vehicle speed as reported by OBD
 
 
+### Polyline data:
+To save space, all longitude and latitude data is compressed with the polyline encoding format as a base64 string. Every drive frame will have a corresponding lat/long.
+The polyline string is **not** null terminated and the size must be determined with the polyline size information.
+
 ## Footer:
 ```
 |------Fuel tank level------|

include/Carduino+Drive.h

@@ -0,0 +1,36 @@
+#ifndef _CARDUINO_DEVICE_H
+#define _CARDUINO_DEVICE_H
+#include <FreematicsPlus.h>
+#include "polylineencoder.h"
+
+class Carduino_Drive {
+
+public:
+    /**
+     * Create an instance of a drive
+     */
+    Carduino_Drive(char vin[17], uint8_t startFuelLevel, time_t startTime);
+
+    /**
+     * Destructor
+     */
+    ~Carduino_Drive();
+
+    void addFrame(time_t time, uint8_t gpsSpeed, int32_t latitude,
+                  int32_t longitude,
+                  int16_t heading, int16_t altitude,
+                  uint8_t vehicleSpeed);
+
+
+    void getDriveData(int8_t **data, size_t *size, uint8_t finalFuelTankLevel);
+    time_t startTime;
+
+private:
+    gepaf::PolylineEncoder<> encoder;
+    int bufferCapacity;
+    int bufferSize;
+    int8_t *buffer;
+    char vin[17];
+    uint8_t startFuelLevel;
+};
+#endif

include/Carduino+OBD.h

@@ -3,6 +3,8 @@
 #include <FreematicsPlus.h>
 #include "Carduino+Accelerometer.h"
 
+#define OBD_RECV_BUF_SIZE 80
+
 class Carduino_OBD {
 
 public:
@@ -19,9 +21,14 @@ class Carduino_OBD {
 
 
     bool isConnected();
+
+    char vinBuffer[OBD_RECV_BUF_SIZE + 1];
+
+    uint8_t getVehicleSpeed();
 private:
     Carduino_Accelerometer *accelerometerUnit;
     bool connected;
+    uint8_t vehicleSpeed;
 };
 
 

include/Carduino+SD.h

@@ -45,7 +45,7 @@ class CStorage {
     virtual uint16_t samples() {
         return m_samples;
     }
-    virtual void dispatch(const char *buf, byte len) {
+    virtual void dispatch(const char *buf, size_t len) {
         // output data via serial
         Serial.write((uint8_t *)buf, len);
         Serial.write(' ');
@@ -89,7 +89,7 @@ class CStorageRAM: public CStorage {
     char *buffer() {
         return m_cache;
     }
-    void dispatch(const char *buf, byte len) {
+    void dispatch(const char *buf, size_t len) {
         // reserve some space for checksum
         int remain = m_cacheSize - m_cacheBytes - len - 3;
         if (remain < 0) {
@@ -129,7 +129,7 @@ class FileLogger : public CStorage {
     FileLogger() {
         m_delimiter = ',';
     }
-    virtual void dispatch(const char *buf, byte len) {
+    virtual void dispatch(const char *buf, size_t len) {
         if(!open) return;
 
         if (m_file.write((uint8_t *)buf, len) != len) {

src/Carduino+Drive.cpp

@@ -0,0 +1,110 @@
+#include "Carduino+Drive.h"
+
+const int HEADER_SIZE = 28;
+const int DRIVE_FRAME_SIZE = 14;
+const int FOOTER_SIZE = 1;
+const uint16_t VERSION = 1;
+
+Carduino_Drive::Carduino_Drive(char vin[], uint8_t startFuelLevel,
+                               time_t startTime) {
+    this->bufferCapacity = DRIVE_FRAME_SIZE * 200; //Start with 200 drive frames
+    this->bufferSize = 0;
+
+    this->buffer = (int8_t *)(malloc(this->bufferCapacity));
+
+    memcpy(this->vin, vin, 17);
+
+    this->startFuelLevel = startFuelLevel;
+    this->startTime = startTime;
+}
+
+Carduino_Drive::~Carduino_Drive() {
+    free(this->buffer);
+}
+
+void Carduino_Drive::addFrame(time_t time, uint8_t gpsSpeed, int32_t latitude,
+                              int32_t longitude,
+                              int16_t heading, int16_t altitude,
+                              uint8_t vehicleSpeed) {
+    this->encoder.addPoint(latitude, longitude);
+    /*
+    |-----GPS Time-----|-----GPS Speed-----|-----Heading-----|-----Altitude-----|-----Vehicle Speed-----|
+      uint64_t        uint8_t              int16_t          int16_t              uint8_t
+     */
+
+    if (this->bufferCapacity == this->bufferSize) {
+        // Allocate another 200 drive frames
+        int newCapacity = this->bufferCapacity + DRIVE_FRAME_SIZE * 200;
+        this->buffer = (int8_t *)(realloc(this->buffer, newCapacity));
+        this->bufferCapacity = newCapacity;
+    }
+
+    this->buffer[this->bufferSize + 0] = (uint8_t)(time & 0xff);
+    this->buffer[this->bufferSize + 1] = (uint8_t)(time >> 8) & 0xff;
+    this->buffer[this->bufferSize + 2] = (uint8_t)(time >> 16) & 0xff;
+    this->buffer[this->bufferSize + 3] = (uint8_t)(time >> 24) & 0xff;
+    this->buffer[this->bufferSize + 4] = (uint8_t)(time >> 32) & 0xff;
+    this->buffer[this->bufferSize + 5] = (uint8_t)(time >> 40) & 0xff;
+    this->buffer[this->bufferSize + 6] = (uint8_t)(time >> 48) & 0xff;
+    this->buffer[this->bufferSize + 7] = (uint8_t)(time >> 56) & 0xff;
+
+    this->buffer[this->bufferSize + 8] = gpsSpeed;
+
+    this->buffer[this->bufferSize + 9] = (uint8_t)(heading & 0xff);
+    this->buffer[this->bufferSize + 10] = (uint8_t)(heading >> 8);
+
+    this->buffer[this->bufferSize + 11] = (uint8_t)(altitude & 0xff);
+    this->buffer[this->bufferSize + 12] = (uint8_t)(altitude >> 8);
+
+    this->buffer[this->bufferSize + 13] = vehicleSpeed;
+
+    this->bufferSize += DRIVE_FRAME_SIZE;
+
+
+    Serial.print("Buffer size: ");
+    Serial.print(bufferSize);
+    Serial.println();
+
+}
+
+
+void Carduino_Drive::getDriveData(int8_t **data, size_t *size,
+                                  uint8_t finalFuelTankLevel) {
+
+    std::string polyline = this->encoder.encode();
+    uint32_t polyLineCount = polyline.length();
+
+    size_t dataSize = HEADER_SIZE + polyLineCount + this->bufferSize + FOOTER_SIZE;
+
+    *data = (int8_t *)(malloc(dataSize));
+    *size = dataSize;
+
+
+    Serial.print("data size: ");
+    Serial.print(dataSize);
+    Serial.println();
+
+    data[0][0] = (uint8_t)(VERSION & 0xff);
+    data[0][1] = (uint8_t)(VERSION >> 8);
+
+    memcpy((data[0] + 2), this->vin, 17);
+
+    data[0][19] = this->startFuelLevel;
+
+    data[0][20] = (uint8_t)(polyLineCount & 0xff);
+    data[0][21] = (uint8_t)(polyLineCount >> 8) & 0xff;
+    data[0][22] = (uint8_t)(polyLineCount >> 16) & 0xff;
+    data[0][23] = (uint8_t)(polyLineCount >> 24);
+
+    data[0][24] = (uint8_t)(this->bufferSize & 0xff);
+    data[0][25] = (uint8_t)(this->bufferSize >> 8) & 0xff;
+    data[0][26] = (uint8_t)(this->bufferSize >> 16) & 0xff;
+    data[0][27] = (uint8_t)(this->bufferSize >> 24);
+
+
+    memcpy((data[0] + 28), polyline.c_str(), polyLineCount);
+    memcpy((data[0] + 28 + polyLineCount), this->buffer, this->bufferSize);
+
+
+    data[0][28 + this->bufferSize + polyLineCount] = finalFuelTankLevel;
+}

src/Carduino+DriveManager.cpp

@@ -7,7 +7,7 @@
 
 #include <FreematicsPlus.h>
 #include "Carduino+DriveManager.h"
-#include "polylineencoder.h"
+#include "Carduino+Drive.h"
 #define OBD_RECV_BUF_SIZE 80
 
 
@@ -56,32 +56,14 @@ Carduino_DriveManager::Carduino_DriveManager(Carduino_GPS *gpsUnit,
 
 unsigned long lastSaveTime = 0;
 uint32_t lastGPSSavetime = 0;
-
-gepaf::PolylineEncoder<> encoder;
 SDLogger logger;
 
 bool hasSaved = false;
 bool hasConnected = false;
 
-void Carduino_DriveManager::runLoop(void) {
-
-    char timeBuf[32];
-    sprintf(timeBuf, "%02u:%02u:%02u",
-            this->gpsUnit->gd->time / 1000000, (this->gpsUnit->gd->time % 1000000) / 10000,
-            (this->gpsUnit->gd->time % 10000) / 100);
-
-    char dateBuf[32];
-    sprintf(dateBuf, "%02u/%02u/%02u", (this->gpsUnit->gd->date % 1000000) / 10000,
-            (this->gpsUnit->gd->date % 10000) / 100, this->gpsUnit->gd->date % 100);
-
-    struct std::tm tm;
-    std::stringstream ss;
-    ss << timeBuf << " " << dateBuf << " +0";
-    strptime(ss.str().c_str(), "%H:%M:%S %d/%m/%y %z", &tm);
-    std::time_t time = timegm(&tm);
+Carduino_Drive *currentDrive = NULL;
 
-
-    return;
+void Carduino_DriveManager::runLoop(void) {
 
     //millis() rolls over after 50 days so take the abs
     if(abs(millis() - lastSaveTime) > 7000 && this->obdUnit->isConnected()) {
@@ -93,27 +75,79 @@ void Carduino_DriveManager::runLoop(void) {
             Serial.println(" No GPS, returning");
             return;
         }
+
+        if (this->gpsUnit->gd->lat == 0 || this->gpsUnit->gd->lng == 0) {
+            Serial.println(" GPS coordinates 0, returning");
+            return;
+        }
+
+        char timeBuf[32];
+        sprintf(timeBuf, "%02u:%02u:%02u",
+                this->gpsUnit->gd->time / 1000000, (this->gpsUnit->gd->time % 1000000) / 10000,
+                (this->gpsUnit->gd->time % 10000) / 100);
+
+        char dateBuf[32];
+        sprintf(dateBuf, "%02u/%02u/%02u", (this->gpsUnit->gd->date % 1000000) / 10000,
+                (this->gpsUnit->gd->date % 10000) / 100, this->gpsUnit->gd->date % 100);
+
+        struct std::tm tm;
+        std::stringstream ss;
+        ss << timeBuf << " " << dateBuf << " +0";
+        strptime(ss.str().c_str(), "%H:%M:%S %d/%m/%y %z", &tm);
+        std::time_t time = timegm(&tm);
+
+        if(currentDrive == NULL) {
+            //TODO: Get the actual fuel level
+            currentDrive = new Carduino_Drive(this->obdUnit->vinBuffer, 100,
+                                              time);
+        }
+
         hasSaved = false;
         lastGPSSavetime = this->gpsUnit->gd->time;
 
-        encoder.addPoint(this->gpsUnit->gd->lat, this->gpsUnit->gd->lng);
+        /*
+            void addFrame(uint32_t time, uint8_t gpsSpeed, int32_t latitude,
+                  int32_t longitude,
+                  int16_t heading, int16_t altitude,
+                  uint8_t vehicleSpeed);
+         */
+
+        //TODO: get actual vehicle speed
+        currentDrive->addFrame(time, this->gpsUnit->gd->speed * 1852 / 1000,
+                               this->gpsUnit->gd->lat, this->gpsUnit->gd->lng, this->gpsUnit->gd->heading,
+                               this->gpsUnit->gd->alt, this->obdUnit->getVehicleSpeed());
 
         lastSaveTime = millis();
     }
 
     if(!this->obdUnit->isConnected() && !hasSaved && hasConnected) {
         Serial.println("Attempting save");
         logger.init();
-        int fileid = logger.begin("name");
 
-        std::string res = encoder.encode();
-        encoder.clear();
+        char fileName[15];
+        sprintf(fileName, "%10lu.cdu", currentDrive->startTime);
+
+        int fileid = logger.begin(fileName);
 
-        logger.dispatch(res.c_str(), res.length());
+        int8_t *buffer;
+        size_t bufferSize;
+        //TODO: Get actual fuel level
+        currentDrive->getDriveData(&buffer, &bufferSize, 50);
+
+        Serial.print("Final Buffer size: ");
+        Serial.print(bufferSize);
+        Serial.println();
+
+        logger.dispatch((const char *)buffer, bufferSize);
         logger.flush();
         logger.end();
+
+        free(buffer);
+
         hasConnected = false;
         hasSaved = true;
+
+        currentDrive = NULL;
     }
 
 }