Permalink
Cannot retrieve contributors at this time
Name already in use
A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
Freematics/firmware_v4/datalogger/datalogger.ino
Go to fileThis commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
576 lines (546 sloc)
14.4 KB
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/************************************************************************* | |
* OBD-II/MEMS/GPS Data Logging Sketch for Freematics ONE | |
* Distributed under BSD license | |
* Visit http://freematics.com/products/freematics-one 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 <SPI.h> | |
#include <SD.h> | |
#include <avr/wdt.h> | |
#include <avr/sleep.h> | |
#include <FreematicsONE.h> | |
#include "config.h" | |
#include "datalogger.h" | |
// states | |
#define STATE_SD_READY 0x1 | |
#define STATE_OBD_READY 0x2 | |
#define STATE_GPS_FOUND 0x4 | |
#define STATE_CELL_GPS_FOUND 0x8 | |
#define STATE_GPS_READY 0x10 | |
#define STATE_MEMS_READY 0x20 | |
#define STATE_FILE_READY 0x40 | |
void cmdline(bool nonblocking); | |
char VIN[18] = {0}; | |
uint16_t MMDD = 0; | |
uint32_t UTC = 0; | |
byte pids[]= {PID_RPM, PID_SPEED, PID_THROTTLE, PID_ENGINE_LOAD}; | |
int values[4] = {0}; | |
float voltage = 0; | |
bool sleeping = false; | |
#if USE_MEMS | |
MPU9250_ACC mems; | |
float acc[3] = {0}; | |
float accBias[3] = {0}; | |
int16_t temp = 0; | |
GPS_DATA gd = {0}; | |
void calibrateMEMS() | |
{ | |
// MEMS data collected while sleeping | |
accBias[0] = 0; | |
accBias[1] = 0; | |
accBias[2] = 0; | |
int n; | |
for (n = 0; n < 100; n++) { | |
float acc[3] = {0}; | |
mems.read(acc); | |
accBias[0] += acc[0]; | |
accBias[1] += acc[1]; | |
accBias[2] += acc[2]; | |
delay(10); | |
} | |
accBias[0] /= n; | |
accBias[1] /= n; | |
accBias[2] /= n; | |
} | |
#endif | |
SIGNAL(WDT_vect) { | |
wdt_disable(); | |
wdt_reset(); | |
WDTCSR &= ~_BV(WDIE); | |
} | |
void sleep(uint8_t wdt_period) { | |
wdt_enable(wdt_period); | |
wdt_reset(); | |
WDTCSR |= _BV(WDIE); | |
set_sleep_mode(SLEEP_MODE_PWR_DOWN); | |
sleep_mode(); | |
wdt_disable(); | |
WDTCSR &= ~_BV(WDIE); | |
} | |
void resetMCU() | |
{ | |
void(* resetFunc) (void) = 0; //declare reset function @ address 0 | |
resetFunc(); | |
} | |
class ONE : public COBDSPI, public CDataLogger | |
{ | |
public: | |
ONE():state(0) {} | |
void setup() | |
{ | |
Serial.print("OBD:"); | |
if (init()) { | |
Serial.println("OK"); | |
state |= STATE_OBD_READY; | |
} else { | |
Serial.println("NO"); | |
} | |
#if USE_GPS | |
Serial.print("GPS:"); | |
if (gpsInit(GPS_SERIAL_BAUDRATE)) { | |
state |= STATE_GPS_FOUND; | |
Serial.println("OK"); | |
} else { | |
Serial.println("NO"); | |
} | |
#endif | |
#if USE_CELL_GPS | |
if (!(state & STATE_GPS_FOUND)) { | |
Serial.print("CELL GPS:"); | |
if (cellInit()) { | |
Serial.println("OK"); | |
state |= STATE_CELL_GPS_FOUND; | |
} else { | |
Serial.println("NO"); | |
} | |
} | |
#endif | |
#if ENABLE_DATA_LOG | |
if (!(state & STATE_SD_READY)) { | |
Serial.print("SD:"); | |
pinMode(SD_CS_PIN, OUTPUT); | |
if (SD.begin(SD_CS_PIN)) { | |
Serial.println("OK"); | |
state |= STATE_SD_READY; | |
} else { | |
Serial.println("NO"); | |
} | |
} | |
#endif | |
// retrieve VIN | |
if (state & STATE_OBD_READY) { | |
char buffer[128]; | |
if (getVIN(buffer, sizeof(buffer))) { | |
Serial.print("VIN:"); | |
Serial.println(buffer); | |
strncpy(VIN, buffer, sizeof(VIN) - 1); | |
} | |
} | |
calibrateMEMS(); | |
} | |
void logLocationData(GPS_DATA* gd) | |
{ | |
if (gd->time && gd->time != UTC) { | |
byte day = gd->date / 10000; | |
dataTime = millis(); | |
if (MMDD % 100 != day) { | |
log(PID_GPS_DATE, gd->date); | |
} | |
log(PID_GPS_TIME, gd->time); | |
log(PID_GPS_LATITUDE, gd->lat); | |
log(PID_GPS_LONGITUDE, gd->lng); | |
log(PID_GPS_ALTITUDE, gd->alt); | |
log(PID_GPS_SPEED, gd->speed); | |
log(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; | |
} | |
} | |
#if USE_GPS | |
void processGPS() | |
{ | |
// issue the command to get parsed GPS data | |
if (gpsGetData(&gd)) { | |
logLocationData(&gd); | |
} | |
} | |
#endif | |
#if USE_CELL_GPS | |
void processCellGPS() | |
{ | |
if (cellGetGPSInfo(gd)) { | |
logLocationData(&gd); | |
} | |
} | |
#endif | |
#if ENABLE_DATA_LOG | |
void flushData(uint32_t fileSize) | |
{ | |
// flush SD data every 1KB | |
static uint8_t lastFileSize = 0; | |
byte dataSizeKB = fileSize >> 10; | |
if (dataSizeKB != lastFileSize) { | |
flushFile(); | |
lastFileSize = dataSizeKB; | |
#if MAX_DATA_FILE_SIZE | |
if (fileSize >= 1024L * MAX_DATA_FILE_SIZE) { | |
closeFile(); | |
state &= ~STATE_FILE_READY; | |
} | |
#endif | |
} | |
} | |
#endif | |
void standby() | |
{ | |
Serial.println("Standby"); | |
lowPowerMode(); | |
sleeping = true; | |
#if USE_MEMS | |
if (state & STATE_MEMS_READY) { | |
byte count = 0; | |
calibrateMEMS(); | |
while (sleeping) { | |
cmdline(false); | |
delay(100); | |
// calculate relative movement | |
float motion = 0; | |
mems.read(acc, 0, 0, &temp); | |
for (byte i = 0; i < 3; i++) { | |
float m = (acc[i] - accBias[i]); | |
motion += m * m; | |
} | |
//Serial.println(motion); | |
// check movement | |
if (motion > WAKEUP_MOTION_THRESHOLD * WAKEUP_MOTION_THRESHOLD) { | |
break; | |
} | |
// read voltage at an interval | |
if (!count++) { | |
voltage = getVoltage(); | |
} | |
} | |
} | |
#else | |
while (sleeping) { | |
cmdline(false); | |
sleep(WDTO_2S); | |
voltage = getVoltage(); | |
Serial.println(v); | |
if (voltage >= JUMPSTART_VOLTAGE) break; | |
} | |
#endif | |
Serial.println("Wakeup"); | |
} | |
void reconnect() | |
{ | |
Serial.println("Reconnect"); | |
if (init()) return; | |
// try to re-connect to OBD | |
#if ENABLE_DATA_LOG | |
closeFile(); | |
#endif | |
// turn off GPS power | |
#if USE_GPS | |
gpsInit(0); | |
#endif | |
state &= ~(STATE_OBD_READY | STATE_GPS_READY | STATE_FILE_READY); | |
end(); | |
standby(); | |
delay(100); | |
resetMCU(); | |
} | |
// library idle callback from sleep() | |
void idleTasks() | |
{ | |
mems.read(acc, 0, 0, &temp); | |
cmdline(true); | |
} | |
#if USE_CELL_GPS | |
bool cellSendCommand(const char* cmd, char* buf, int bufsize, const char* expected = "\r\nOK", unsigned int timeout = 1000) | |
{ | |
if (cmd) xbWrite(cmd); | |
memset(buf, 0, bufsize); | |
byte ret = xbReceive(buf, bufsize, timeout, &expected, 1); | |
if (ret) { | |
return true; | |
} else { | |
return false; | |
} | |
} | |
bool cellInit() | |
{ | |
xbBegin(115200); | |
for (byte n = 0; n < 2; n++) { | |
char buf[128]; | |
if (!cellSendCommand("AT\r", buf, sizeof(buf))) cellSendCommand(0, buf, sizeof(buf), "START", 5000); | |
if (cellSendCommand("ATE0\r", buf, sizeof(buf)) && cellSendCommand("ATI\r", buf, sizeof(buf), "SIM5360")) { | |
cellSendCommand("AT+CVAUXV=61\r", buf, sizeof(buf)); | |
cellSendCommand("AT+CVAUXS=1\r", buf, sizeof(buf)); | |
if (cellSendCommand("AT+CGPS?\r", buf, sizeof(buf), "+CGPS: 1")) return true; | |
delay(2000); | |
if (cellSendCommand("AT+CGPS=1,1\r", buf, sizeof(buf))) return true; | |
} | |
xbTogglePower(); | |
} | |
return false; | |
} | |
long parseDegree(const char* s) | |
{ | |
char *p; | |
unsigned long left = atol(s); | |
unsigned long tenk_minutes = (left % 100UL) * 100000UL; | |
if ((p = strchr(s, '.'))) | |
{ | |
unsigned long mult = 10000; | |
while (isdigit(*++p)) | |
{ | |
tenk_minutes += mult * (*p - '0'); | |
mult /= 10; | |
} | |
} | |
return (left / 100) * 1000000 + tenk_minutes / 6; | |
} | |
bool cellGetGPSInfo(GPS_DATA& gd) | |
{ | |
char *p; | |
char buf[160]; | |
if (cellSendCommand("AT+CGPSINFO\r", buf, sizeof(buf), "+CGPSINFO:")) do { | |
if (!(p = strchr(buf, ':'))) break; | |
if (*(++p) == ',') break; | |
gd.lat = parseDegree(p); | |
if (!(p = strchr(p, ','))) break; | |
if (*(++p) == 'S') gd.lat = -gd.lat; | |
if (!(p = strchr(p, ','))) break; | |
gd.lng = parseDegree(++p); | |
if (!(p = strchr(p, ','))) break; | |
if (*(++p) == 'W') gd.lng = -gd.lng; | |
if (!(p = strchr(p, ','))) break; | |
gd.date = atol(++p); | |
if (!(p = strchr(p, ','))) break; | |
gd.time = atol(++p); | |
if (!(p = strchr(p, ','))) break; | |
gd.alt = atoi(++p); | |
if (!(p = strchr(p, ','))) break; | |
gd.speed = atof(++p) * 100; | |
if (!(p = strchr(p, ','))) break; | |
gd.heading = atoi(++p); | |
Serial.print("UTC:"); | |
Serial.print(gd.date); | |
Serial.print(' '); | |
Serial.print(gd.time); | |
Serial.print(" LAT:"); | |
Serial.print(gd.lat); | |
Serial.print(" LNG:"); | |
Serial.println(gd.lng); | |
return true; | |
} while (0); | |
return false; | |
} | |
#endif | |
byte state; | |
}; | |
ONE one; | |
void setup() | |
{ | |
// Disable the ADC | |
ADCSRA = ADCSRA & B01111111; | |
// Disable the analog comparator | |
ACSR = B10000000; | |
// Disable digital input buffers on all analog input pins | |
DIDR0 = DIDR0 | B00111111; | |
// change to 9600bps when using BLE | |
Serial.begin(115200); | |
byte ver = one.begin(); | |
Serial.print("FW:R"); | |
Serial.println(ver); | |
#if USE_MEMS | |
Serial.print("MEMS:"); | |
if (mems.begin()) { | |
one.state |= STATE_MEMS_READY; | |
Serial.println("OK"); | |
} else { | |
Serial.println("NO"); | |
} | |
#endif | |
one.setup(); | |
#if !USE_GPS | |
if (one.ioConfig(1, IO_PIN_OUTPUT)) { | |
one.ioConfig(2, IO_PIN_OUTPUT); | |
one.ioWrite(1, 0); | |
one.ioWrite(2, 0); | |
Serial.println("IO:OK"); | |
} | |
#endif | |
} | |
void cmdline(bool nonblocking) | |
{ | |
#if ENABLE_CLI | |
static char cmd[16] = {0}; | |
static byte cmdlen = 0; | |
static bool echo = true; | |
static char c = 0; | |
if (c != '\r') { | |
while (Serial.available() && c != '\r') { | |
c = Serial.read(); | |
if (c >= 32 && cmdlen < sizeof(cmd) - 1) { | |
cmd[cmdlen++] = c; | |
} | |
} | |
if (c != '\r') return; | |
if (echo) Serial.println(cmd); | |
} | |
if (!strcmp(cmd, "VIN")) { | |
Serial.println(VIN[0] ? VIN : "N/A"); | |
} else if (!strcmp(cmd, "BATT")) { | |
Serial.println(voltage, 1); | |
} else if (!strcmp(cmd, "ON?")) { | |
Serial.println(sleeping ? 0 : 1); | |
} else if (!strcmp(cmd, "ON")) { | |
sleeping = false; | |
Serial.println("OK"); | |
} else if (!strcmp(cmd, "OFF")) { | |
sleeping = true; | |
Serial.println("OK"); | |
} else if (!strcmp(cmd, "UPTIME")) { | |
Serial.println(millis()); | |
} else if (!memcmp(cmd, "01", 2)) { | |
byte pid = hex2uint8(cmd + 2); | |
for (byte i = 0; i < sizeof(pids) / sizeof(pids[0]); i++) { | |
if (pids[i] == pid) { | |
Serial.println(values[i]); | |
pid = 0; | |
break; | |
} | |
} | |
if (pid) { | |
if (nonblocking) return; | |
int value; | |
if (one.readPID(pid, value)) { | |
Serial.println(value); | |
} else { | |
Serial.println("N/A"); | |
} | |
} | |
} else if (!strcmp(cmd, "TEMP")) { | |
Serial.println(temp / 10); | |
} else if (!strcmp(cmd, "ACC")) { | |
Serial.print(acc[0], 2); | |
Serial.print('/'); | |
Serial.print(acc[1], 2); | |
Serial.print('/'); | |
Serial.println(acc[2], 2); | |
} else if (!strcmp(cmd, "GF")) { | |
Serial.println(sqrt(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2])); | |
} else if (!strcmp(cmd, "FS")) { | |
Serial.println(sdfile.size()); | |
} else if (!strcmp(cmd, "LAT")) { | |
Serial.println((float)gd.lat / 1000000, 6); | |
} else if (!strcmp(cmd, "LNG")) { | |
Serial.println((float)gd.lng / 1000000, 6); | |
} else if (!strcmp(cmd, "ALT")) { | |
Serial.println(gd.alt); | |
} else if (!strcmp(cmd, "SAT")) { | |
Serial.println(gd.sat); | |
} else if (!strcmp(cmd, "SPD")) { | |
Serial.println((float)gd.speed * 1852 / 100000, 1); | |
} else if (!strcmp(cmd, "CRS")) { | |
Serial.println(gd.heading / 100); | |
} else if (!strcmp(cmd, "ATE0")) { | |
echo = false; | |
Serial.println("OK"); | |
} else if (!strcmp(cmd, "ATE1")) { | |
echo = true; | |
Serial.println("OK"); | |
} else { | |
Serial.print("ERROR:"); | |
Serial.println(cmd); | |
} | |
cmdlen = 0; | |
memset(cmd, 0, sizeof(cmd)); | |
c = 0; | |
#endif | |
} | |
void loop() | |
{ | |
#if ENABLE_DATA_LOG | |
if (!(one.state & STATE_FILE_READY) && (one.state & STATE_SD_READY)) { | |
#if USE_GPS | |
if (one.state & STATE_GPS_FOUND) { | |
// GPS connected | |
one.processGPS(); | |
if (one.state & STATE_GPS_READY) { | |
uint32_t dateTime = (uint32_t)MMDD * 10000 + UTC / 10000; | |
if (one.openFile(dateTime)) { | |
MMDD = 0; | |
one.state |= STATE_FILE_READY; | |
} | |
} | |
} | |
else | |
#endif | |
{ | |
// no GPS connected | |
if (one.openFile(0)) { | |
one.state |= STATE_FILE_READY; | |
} | |
} | |
delay(1000); | |
return; | |
} | |
#endif | |
cmdline(false); | |
if (sleeping) one.standby(); | |
if (one.state & STATE_OBD_READY) { | |
one.dataTime = millis(); | |
for (byte i = 0; i < sizeof(pids) / sizeof(pids[0]); i++) { | |
if (one.readPID(pids[i], values[i])) { | |
one.log((uint16_t)pids[i] | 0x100, values[i]); | |
} | |
#if USE_MEMS | |
one.log(PID_ACC, (int16_t)((acc[0] - accBias[0]) * 100), (int16_t)((acc[1] - accBias[1]) * 100), (int16_t)((acc[2] - accBias[2]) * 100)); | |
#endif | |
} | |
if (one.errors >= 3) { | |
one.reset(); | |
one.reconnect(); | |
} | |
} else { | |
if (!OBD_ATTEMPT_TIME || millis() < OBD_ATTEMPT_TIME * 1000L) { | |
if (one.init()) { | |
one.state |= STATE_OBD_READY; | |
} | |
} | |
} | |
// log battery voltage (from voltmeter), data in 0.01v | |
voltage = one.getVoltage(); | |
one.log(PID_BATTERY_VOLTAGE, (int)(voltage * 100)); | |
#if USE_GPS | |
if (one.state & STATE_GPS_FOUND) { | |
one.processGPS(); | |
} | |
#endif | |
#if USE_CELL_GPS | |
if (one.state & STATE_CELL_GPS_FOUND) { | |
one.processCellGPS(); | |
} | |
#endif | |
#if !ENABLE_CLI | |
uint32_t t = millis(); | |
Serial.print(one.dataCount); | |
Serial.print(" samples "); | |
Serial.print((float)one.dataCount * 1000 / t, 1); | |
Serial.print(" sps "); | |
#if ENABLE_DATA_LOG | |
if (one.state & STATE_FILE_READY) { | |
uint32_t fileSize = sdfile.size(); | |
one.flushData(fileSize); | |
Serial.print(fileSize); | |
Serial.print(" bytes"); | |
} | |
#endif | |
Serial.println(); | |
#endif | |
} |