Skip to content
Permalink
master
Switch branches/tags

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?
Go to file
 
 
Cannot retrieve contributors at this time
/*************************************************************************
* 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
}