summaryrefslogtreecommitdiff
path: root/datalogger/datalogger.ino
blob: c04b2c9ddbda65465ec3d9073a71dd8526cc4ade (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
/*************************************************************************
  OBD-II/GPS/MEMS Data Logging Sketch
  Requires an Arduino board with 2 serial UARTs (3 if GPS required)

  Serial: serial monitor
  Serial1: Freematics OBD-II UART Adapter (V2 required for MEMS data)
  Serial2: GPS Receiver (default 115200bps)
  Change parameters in config.h

  Distributed under BSD license
  Visit http://freematics.com/products for more information
  Developed by Stanley Huang <support@freematics.com.au>

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  THE SOFTWARE.
*************************************************************************/

#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <OBD.h>
#include <TinyGPS.h>
#include "config.h"
#include "datalogger.h"

#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168P__)
#error This sketch requires at least 2 hardware serial
#endif

// states
#define STATE_SD_READY 0x1
#define STATE_OBD_READY 0x2
#define STATE_GPS_FOUND 0x4
#define STATE_GPS_READY 0x8
#define STATE_MEMS_READY 0x10
#define STATE_FILE_READY 0x20

static uint8_t lastFileSize = 0;
static uint16_t fileIndex = 0;

uint16_t MMDD = 0;
uint32_t UTC = 0;

#if USE_MPU6050
byte accCount = 0; // count of accelerometer readings
long accSum[3] = {0}; // sum of accelerometer data
int accCal[3] = {0}; // calibrated reference accelerometer data
byte deviceTemp; // device temperature (celcius degree)
#endif

#if USE_GPS
// GPS logging can only be enabled when there is additional hardware serial UART
#define GPSUART Serial2
TinyGPS gps;

typedef struct {
  uint32_t date;
  uint32_t time;
  int32_t lat;
  int32_t lng;
  int16_t alt;
  uint8_t speed;
  uint8_t sat;
  int16_t heading;
} GPS_DATA;

bool getGPSData(GPS_DATA* gd)
{
  bool parsed = false;
  while (!parsed && GPSUART.available()) {
    char c = GPSUART.read();
    parsed = gps.encode(c);
  }
  if (!parsed) return false;
  gps.get_datetime((unsigned long*)&gd->date, (unsigned long*)&gd->time, 0);
  gps.get_position((long*)&gd->lat, (long*)&gd->lng, 0);
  gd->sat = gps.satellites();
  gd->speed = gps.speed() * 1852 / 100000; /* km/h */
  gd->alt = gps.altitude();
  gd->heading = gps.course() / 100;
  return true;
}

bool initGPS(uint32_t baudrate)
{
  if (baudrate)
    GPSUART.begin(baudrate);
  else
    GPSUART.end();
  return true;
}
#endif

class COBDLogger : public COBD, public CDataLogger
{
  public:
    COBDLogger(): state(0) {}
    void setup()
    {
      state = 0;

      delay(500);
      byte ver = begin();
      Serial.print("Firmware Ver. ");
      Serial.println(ver);

#if USE_MPU6050
      Serial.print("MEMS ");
      if (memsInit()) {
        state |= STATE_MEMS_READY;
        Serial.println("OK");
      } else {
        Serial.println("NO");
      }
#endif

#if ENABLE_DATA_LOG
      Serial.print("SD ");
      uint16_t volsize = initSD();
      if (volsize) {
        Serial.print(volsize);
        Serial.println("MB");
      } else {
        Serial.println("NO");
      }
#endif

      Serial.print("OBD ");
      if (init()) {
        Serial.println("OK");
      } else {
        Serial.println("NO");
        reconnect();
      }
      state |= STATE_OBD_READY;

#if 0
      // retrieve VIN
      char buffer[128];
      if ((state & STATE_OBD_READY) && getVIN(buffer, sizeof(buffer))) {
        Serial.print("VIN:");
        Serial.println(buffer);
      }
#endif

#if USE_GPS
      Serial.print("GPS ");
      if (initGPS(GPS_SERIAL_BAUDRATE)) {
        state |= STATE_GPS_FOUND;
        Serial.println("OK");
      } else {
        Serial.println("NO");
      }
#endif

#if USE_MPU6050
      // obtain reference MEMS data
      Serial.print("Calibrating MEMS...");
      for (uint32_t t = millis(); millis() - t < 1000; ) {
        readMEMS();
      }
      calibrateMEMS();
      Serial.print(accCal[0]);
      Serial.print('/');
      Serial.print(accCal[1]);
      Serial.print('/');
      Serial.println(accCal[2]);
#endif

      delay(500);
    }
#if USE_GPS
    void logGPSData()
    {
#if LOG_GPS_NMEA_DATA
      // issue the command to get NMEA data (one line per request)
      char buf[255];
      byte n = getGPSRawData(buf, sizeof(buf));
      if (n) {
        dataTime = millis();
        // strip heading $GPS and ending \r\n
        logData(buf + 4, n - 6);
      }
#endif
#if LOG_GPS_PARSED_DATA
      // issue the command to get parsed GPS data
      GPS_DATA gd = {0};

      if (getGPSData(&gd)) {
        dataTime = millis();
        if (gd.time && gd.time != UTC) {
          byte day = gd.date / 10000;
          if (MMDD % 100 != day) {
            logData(PID_GPS_DATE, gd.date);
          }
          logData(PID_GPS_TIME, gd.time);
          logData(PID_GPS_LATITUDE, gd.lat);
          logData(PID_GPS_LONGITUDE, gd.lng);
          logData(PID_GPS_ALTITUDE, gd.alt);
          logData(PID_GPS_SPEED, gd.speed);
          logData(PID_GPS_SAT_COUNT, gd.sat);
          // save current date in MMDD format
          unsigned int DDMM = gd.date / 100;
          UTC = gd.time;
          MMDD = (DDMM % 100) * 100 + (DDMM / 100);
          // set GPS ready flag
          state |= STATE_GPS_READY;
        }
      }
#endif
    }
#endif
#if ENABLE_DATA_LOG
    uint16_t initSD()
    {
      state &= ~STATE_SD_READY;
      pinMode(SS, OUTPUT);

      Sd2Card card;
      uint32_t volumesize = 0;
      if (card.init(SPI_HALF_SPEED, SD_CS_PIN)) {
        SdVolume volume;
        if (volume.init(card)) {
          volumesize = volume.blocksPerCluster();
          volumesize >>= 1; // 512 bytes per block
          volumesize *= volume.clusterCount();
          volumesize /= 1000;
        }
      }
      if (SD.begin(SD_CS_PIN)) {
        state |= STATE_SD_READY;
        return volumesize;
      } else {
        return 0;
      }
    }
    void flushData()
    {
      // flush SD data every 1KB
      byte dataSizeKB = dataSize >> 10;
      if (dataSizeKB != lastFileSize) {
        flushFile();
        lastFileSize = dataSizeKB;
#if MAX_LOG_FILE_SIZE
        if (dataSize >= 1024L * MAX_LOG_FILE_SIZE) {
          closeFile();
          state &= ~STATE_FILE_READY;
        }
#endif
      }
    }
#endif
    void reconnect()
    {
      Serial.println("Reconnecting");
      // try to re-connect to OBD
      if (init()) return;
#if ENABLE_DATA_LOG
      closeFile();
#endif
      // turn off GPS power
#if USE_GPS
      initGPS(0);
#endif
      state &= ~(STATE_OBD_READY | STATE_GPS_READY);
      Serial.println("Standby");
      // put OBD chips into low power mode
      enterLowPowerMode();

      // calibrate MEMS for several seconds
      for (;;) {
#if USE_MPU6050
        accSum[0] = 0;
        accSum[1] = 0;
        accSum[2] = 0;
        for (accCount = 0; accCount < 10; ) {
          readMEMS();
          delay(30);
        }
        // calculate relative movement
        unsigned long motion = 0;
        for (byte i = 0; i < 3; i++) {
          long n = accSum[i] / accCount - accCal[i];
          motion += n * n;
        }
        // check movement
        if (motion > START_MOTION_THRESHOLD) {
          Serial.println(motion);
          // try OBD reading
#endif
          leaveLowPowerMode();
          if (init()) {
            // OBD is accessible
            break;
          }
          enterLowPowerMode();
#if USE_MPU6050
          // calibrate MEMS again in case the device posture changed
          calibrateMEMS();
        }
#endif
      }
#ifdef ARDUINO_ARCH_AVR
      // reset device
      void(* resetFunc) (void) = 0; //declare reset function at address 0
      resetFunc();
#else
      setup();
#endif
    }
#if USE_MPU6050
    void calibrateMEMS()
    {
      // get accelerometer calibration reference data
      if (accCount) {
        accCal[0] = accSum[0] / accCount;
        accCal[1] = accSum[1] / accCount;
        accCal[2] = accSum[2] / accCount;
      }
    }
    void readMEMS()
    {
      // load accelerometer and temperature data
      int16_t acc[3] = {0};
      int16_t temp; // device temperature (in 0.1 celcius degree)
      memsRead(acc, 0, 0, &temp);
      if (accCount >= 250) {
        accSum[0] >>= 1;
        accSum[1] >>= 1;
        accSum[2] >>= 1;
        accCount >>= 1;
      }
      accSum[0] += acc[0];
      accSum[1] += acc[1];
      accSum[2] += acc[2];
      accCount++;
      deviceTemp = temp / 10;
    }
#endif
    byte state;
};

static COBDLogger one;

void setup()
{
  Serial.begin(115200);
  one.initSender();
  one.setup();
}

void loop()
{
#if ENABLE_DATA_LOG
  if (!(one.state & STATE_FILE_READY) && (one.state & STATE_SD_READY)) {
    // create log file
    if (one.state & STATE_GPS_FOUND) {
      // GPS connected, GPS time as file name
      Serial.print("File ");
      if (one.state & STATE_GPS_READY) {
        uint32_t dateTime = (uint32_t)MMDD * 10000 + UTC / 10000;
        if (one.openFile(dateTime) != 0) {
          Serial.println(dateTime);
          MMDD = 0;
          one.state |= STATE_FILE_READY;
        } else {
          Serial.println("File error");
        }
      }
    } else {
      // no GPS connected, index number as file name
      int index = one.openFile(0);
      if (index != 0) {
        one.state |= STATE_FILE_READY;
        Serial.println(index);
      } else {
        Serial.println("File error");
      }
    }
  }
#endif

  // log some OBD-II PIDs
  if (one.state & STATE_OBD_READY) {
    byte pids[] = {0, PID_RPM, PID_SPEED, PID_THROTTLE, PID_ENGINE_LOAD};
    byte pids2[] = {PID_COOLANT_TEMP, PID_INTAKE_TEMP, PID_DISTANCE};
    int values[sizeof(pids)];
    static byte index2 = 0;
    pids[0] = pids2[index2 = (index2 + 1) % sizeof(pids2)];
    // read multiple OBD-II PIDs
    if (one.readPID(pids, sizeof(pids), values) == sizeof(pids)) {
      one.dataTime = millis();
      for (byte n = 0; n < sizeof(pids); n++) {
        one.logData((uint16_t)pids[n] | 0x100, values[n]);
      }
    }
    if (one.errors >= 10) {
      one.reconnect();
    }
  } else {
    if (!OBD_ATTEMPT_TIME || millis() < OBD_ATTEMPT_TIME * 1000) {
      if (one.init()) {
        one.state |= STATE_OBD_READY;
      }
    }
  }

  // log battery voltage (from voltmeter), data in 0.01v
  int v = one.getVoltage() * 100;
  one.dataTime = millis();
  one.logData(PID_BATTERY_VOLTAGE, v);

	// log MEMS data
#if USE_MPU6050
	if (one.state & STATE_MEMS_READY) {
		one.readMEMS();
		if (accCount > 0) {
			// log the loaded MEMS data
			one.logData(PID_ACC, accSum[0] / accCount / ACC_DATA_RATIO, accSum[1] / accCount / ACC_DATA_RATIO, accSum[2] / accCount / ACC_DATA_RATIO);
			accCount = 0;
		}
	}
#endif

  // log GPS data
#if USE_GPS
  if (one.state & STATE_GPS_FOUND) {
    one.logGPSData();
  }
#endif

#if ENABLE_DATA_LOG
  uint32_t logsize = sdfile.size();
  if (logsize > 0) {
    one.flushData();
    Serial.print(sdfile.size());
    Serial.println(" bytes");
  }
#endif
}