738 lines
16 KiB
C++
738 lines
16 KiB
C++
|
/*************************************************************************
|
||
|
|
* Arduino Library for Freematics ONE
|
||
|
|
* Distributed under BSD license
|
||
|
|
* Visit http://freematics.com/products/freematics-one for more information
|
||
|
|
* (C)2012-2018 Stanley Huang <stanley@freematics.com.au>
|
||
|
|
*************************************************************************/
|
||
|
|
|
||
|
|
#include <Arduino.h>
|
||
|
|
#include <SPI.h>
|
||
|
|
#include "FreematicsONE.h"
|
||
|
|
|
||
|
|
//#define XBEE_DEBUG
|
||
|
|
//#define DEBUG Serial
|
||
|
|
|
||
|
|
static const uint8_t targets[][4] = {
|
||
|
|
{0x24, 0x4f, 0x42, 0x44},
|
||
|
|
{0x24, 0x47, 0x50, 0x53},
|
||
|
|
{0x24, 0x47, 0x53, 0x4D}
|
||
|
|
};
|
||
|
|
|
||
|
|
static SPISettings settings = SPISettings(250000, MSBFIRST, SPI_MODE0);
|
||
|
|
|
||
|
|
uint16_t hex2uint16(const char *p)
|
||
|
|
{
|
||
|
|
char c = *p;
|
||
|
|
uint16_t i = 0;
|
||
|
|
for (uint8_t n = 0; c && n < 4; c = *(++p)) {
|
||
|
|
if (c >= 'A' && c <= 'F') {
|
||
|
|
c -= 7;
|
||
|
|
} else if (c>='a' && c<='f') {
|
||
|
|
c -= 39;
|
||
|
|
} else if (c == ' ' && n == 2) {
|
||
|
|
continue;
|
||
|
|
} else if (c < '0' || c > '9') {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
i = (i << 4) | (c & 0xF);
|
||
|
|
n++;
|
||
|
|
}
|
||
|
|
return i;
|
||
|
|
}
|
||
|
|
|
||
|
|
byte hex2uint8(const char *p)
|
||
|
|
{
|
||
|
|
byte c1 = *p;
|
||
|
|
byte c2 = *(p + 1);
|
||
|
|
if (c1 >= 'A' && c1 <= 'F')
|
||
|
|
c1 -= 7;
|
||
|
|
else if (c1 >='a' && c1 <= 'f')
|
||
|
|
c1 -= 39;
|
||
|
|
else if (c1 < '0' || c1 > '9')
|
||
|
|
return 0;
|
||
|
|
|
||
|
|
if (c2 >= 'A' && c2 <= 'F')
|
||
|
|
c2 -= 7;
|
||
|
|
else if (c2 >= 'a' && c2 <= 'f')
|
||
|
|
c2 -= 39;
|
||
|
|
else if (c2 < '0' || c2 > '9')
|
||
|
|
return 0;
|
||
|
|
|
||
|
|
return c1 << 4 | (c2 & 0xf);
|
||
|
|
}
|
||
|
|
|
||
|
|
int dumpLine(char* buffer, int len)
|
||
|
|
{
|
||
|
|
int bytesToDump = len >> 1;
|
||
|
|
for (int i = 0; i < len; i++) {
|
||
|
|
// find out first line end and discard the first line
|
||
|
|
if (buffer[i] == '\r' || buffer[i] == '\n') {
|
||
|
|
// go through all following \r or \n if any
|
||
|
|
while (++i < len && (buffer[i] == '\r' || buffer[i] == '\n'));
|
||
|
|
bytesToDump = i;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
memmove(buffer, buffer + bytesToDump, len - bytesToDump);
|
||
|
|
return bytesToDump;
|
||
|
|
}
|
||
|
|
|
||
|
|
byte COBDSPI::readDTC(uint16_t codes[], byte maxCodes)
|
||
|
|
{
|
||
|
|
/*
|
||
|
|
Response example:
|
||
|
|
0: 43 04 01 08 01 09
|
||
|
|
1: 01 11 01 15 00 00 00
|
||
|
|
*/
|
||
|
|
byte codesRead = 0;
|
||
|
|
for (byte n = 0; n < 6; n++) {
|
||
|
|
char buffer[128];
|
||
|
|
sprintf_P(buffer, n == 0 ? PSTR("03\r") : PSTR("03%02X\r"), n);
|
||
|
|
write(buffer);
|
||
|
|
if (receive(buffer, sizeof(buffer)) > 0) {
|
||
|
|
if (!strstr_P(buffer, PSTR("NO DATA"))) {
|
||
|
|
char *p = strstr_P(buffer, PSTR("43"));
|
||
|
|
if (p) {
|
||
|
|
while (codesRead < maxCodes && *p) {
|
||
|
|
p += 6;
|
||
|
|
if (*p == '\r') {
|
||
|
|
p = strchr(p, ':');
|
||
|
|
if (!p) break;
|
||
|
|
p += 2;
|
||
|
|
}
|
||
|
|
uint16_t code = hex2uint16(p);
|
||
|
|
if (code == 0) break;
|
||
|
|
codes[codesRead++] = code;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return codesRead;
|
||
|
|
}
|
||
|
|
|
||
|
|
void COBDSPI::clearDTC()
|
||
|
|
{
|
||
|
|
char buffer[32];
|
||
|
|
write("04\r");
|
||
|
|
receive(buffer, sizeof(buffer));
|
||
|
|
}
|
||
|
|
|
||
|
|
void COBDSPI::lowPowerMode()
|
||
|
|
{
|
||
|
|
char buf[16];
|
||
|
|
sendCommand("ATLP\r", buf, sizeof(buf));
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::testPID(byte pid)
|
||
|
|
{
|
||
|
|
char buffer[32];
|
||
|
|
sprintf_P(buffer, PSTR("%02X%02X\r"), dataMode, pid);
|
||
|
|
write(buffer);
|
||
|
|
idleTasks();
|
||
|
|
if (receive(buffer, sizeof(buffer)) <= 0 || checkErrorMessage(buffer)) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::readPID(byte pid, int& result)
|
||
|
|
{
|
||
|
|
char buffer[32];
|
||
|
|
// send a single query command
|
||
|
|
sprintf_P(buffer, PSTR("%02X%02X\r"), dataMode, pid);
|
||
|
|
write(buffer);
|
||
|
|
// receive and parse the response
|
||
|
|
idleTasks();
|
||
|
|
char* data = 0;
|
||
|
|
if (receive(buffer, sizeof(buffer)) > 0 && !checkErrorMessage(buffer)) {
|
||
|
|
char *p = buffer;
|
||
|
|
while ((p = strstr(p, "41 "))) {
|
||
|
|
p += 3;
|
||
|
|
byte curpid = hex2uint8(p);
|
||
|
|
if (curpid == pid) {
|
||
|
|
errors = 0;
|
||
|
|
p += 2;
|
||
|
|
if (*p == ' ') {
|
||
|
|
data = p + 1;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (!data) {
|
||
|
|
errors++;
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
result = normalizeData(pid, data);
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
int COBDSPI::normalizeData(byte pid, char* data)
|
||
|
|
{
|
||
|
|
int result;
|
||
|
|
switch (pid) {
|
||
|
|
case PID_RPM:
|
||
|
|
case PID_EVAP_SYS_VAPOR_PRESSURE:
|
||
|
|
result = getLargeValue(data) >> 2;
|
||
|
|
break;
|
||
|
|
case PID_FUEL_PRESSURE:
|
||
|
|
result = getSmallValue(data) * 3;
|
||
|
|
break;
|
||
|
|
case PID_COOLANT_TEMP:
|
||
|
|
case PID_INTAKE_TEMP:
|
||
|
|
case PID_AMBIENT_TEMP:
|
||
|
|
case PID_ENGINE_OIL_TEMP:
|
||
|
|
result = getTemperatureValue(data);
|
||
|
|
break;
|
||
|
|
case PID_THROTTLE:
|
||
|
|
case PID_COMMANDED_EGR:
|
||
|
|
case PID_COMMANDED_EVAPORATIVE_PURGE:
|
||
|
|
case PID_FUEL_LEVEL:
|
||
|
|
case PID_RELATIVE_THROTTLE_POS:
|
||
|
|
case PID_ABSOLUTE_THROTTLE_POS_B:
|
||
|
|
case PID_ABSOLUTE_THROTTLE_POS_C:
|
||
|
|
case PID_ACC_PEDAL_POS_D:
|
||
|
|
case PID_ACC_PEDAL_POS_E:
|
||
|
|
case PID_ACC_PEDAL_POS_F:
|
||
|
|
case PID_COMMANDED_THROTTLE_ACTUATOR:
|
||
|
|
case PID_ENGINE_LOAD:
|
||
|
|
case PID_ABSOLUTE_ENGINE_LOAD:
|
||
|
|
case PID_ETHANOL_FUEL:
|
||
|
|
case PID_HYBRID_BATTERY_PERCENTAGE:
|
||
|
|
result = getPercentageValue(data);
|
||
|
|
break;
|
||
|
|
case PID_MAF_FLOW:
|
||
|
|
result = getLargeValue(data) / 100;
|
||
|
|
break;
|
||
|
|
case PID_TIMING_ADVANCE:
|
||
|
|
result = (int)(getSmallValue(data) / 2) - 64;
|
||
|
|
break;
|
||
|
|
case PID_DISTANCE: // km
|
||
|
|
case PID_DISTANCE_WITH_MIL: // km
|
||
|
|
case PID_TIME_WITH_MIL: // minute
|
||
|
|
case PID_TIME_SINCE_CODES_CLEARED: // minute
|
||
|
|
case PID_RUNTIME: // second
|
||
|
|
case PID_FUEL_RAIL_PRESSURE: // kPa
|
||
|
|
case PID_ENGINE_REF_TORQUE: // Nm
|
||
|
|
result = getLargeValue(data);
|
||
|
|
break;
|
||
|
|
case PID_CONTROL_MODULE_VOLTAGE: // V
|
||
|
|
result = getLargeValue(data) / 1000;
|
||
|
|
break;
|
||
|
|
case PID_ENGINE_FUEL_RATE: // L/h
|
||
|
|
result = getLargeValue(data) / 20;
|
||
|
|
break;
|
||
|
|
case PID_ENGINE_TORQUE_DEMANDED: // %
|
||
|
|
case PID_ENGINE_TORQUE_PERCENTAGE: // %
|
||
|
|
result = (int)getSmallValue(data) - 125;
|
||
|
|
break;
|
||
|
|
case PID_SHORT_TERM_FUEL_TRIM_1:
|
||
|
|
case PID_LONG_TERM_FUEL_TRIM_1:
|
||
|
|
case PID_SHORT_TERM_FUEL_TRIM_2:
|
||
|
|
case PID_LONG_TERM_FUEL_TRIM_2:
|
||
|
|
case PID_EGR_ERROR:
|
||
|
|
result = ((int)getSmallValue(data) - 128) * 100 / 128;
|
||
|
|
break;
|
||
|
|
case PID_FUEL_INJECTION_TIMING:
|
||
|
|
result = ((int32_t)getLargeValue(data) - 26880) / 128;
|
||
|
|
break;
|
||
|
|
case PID_CATALYST_TEMP_B1S1:
|
||
|
|
case PID_CATALYST_TEMP_B2S1:
|
||
|
|
case PID_CATALYST_TEMP_B1S2:
|
||
|
|
case PID_CATALYST_TEMP_B2S2:
|
||
|
|
result = getLargeValue(data) / 10 - 40;
|
||
|
|
break;
|
||
|
|
case PID_AIR_FUEL_EQUIV_RATIO: // 0~200
|
||
|
|
result = (long)getLargeValue(data) * 200 / 65536;
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
result = getSmallValue(data);
|
||
|
|
}
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
float COBDSPI::getVoltage()
|
||
|
|
{
|
||
|
|
char buf[32];
|
||
|
|
if (sendCommand("ATRV\r", buf, sizeof(buf)) > 0) {
|
||
|
|
return atof(buf + 4);
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::getVIN(char* buffer, int bufsize)
|
||
|
|
{
|
||
|
|
for (byte n = 0; n < 5; n++) {
|
||
|
|
if (sendCommand("0902\r", buffer, bufsize)) {
|
||
|
|
int len = hex2uint16(buffer + sizeof(targets[0]));
|
||
|
|
char *p = strstr_P(buffer + 4, PSTR("0: 49 02 01"));
|
||
|
|
if (p) {
|
||
|
|
char *q = buffer;
|
||
|
|
p += 11; // skip the header
|
||
|
|
do {
|
||
|
|
while (*(++p) == ' ');
|
||
|
|
for (;;) {
|
||
|
|
*(q++) = hex2uint8(p);
|
||
|
|
while (*p && *p != ' ') p++;
|
||
|
|
while (*p == ' ') p++;
|
||
|
|
if (!*p || *p == '\r') break;
|
||
|
|
}
|
||
|
|
p = strchr(p, ':');
|
||
|
|
} while(p);
|
||
|
|
*q = 0;
|
||
|
|
if (q - buffer == len - 3) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
delay(200);
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::isValidPID(byte pid)
|
||
|
|
{
|
||
|
|
if (pid >= 0x7f)
|
||
|
|
return true;
|
||
|
|
pid--;
|
||
|
|
byte i = pid >> 3;
|
||
|
|
byte b = 0x80 >> (pid & 0x7);
|
||
|
|
return pidmap[i] & b;
|
||
|
|
}
|
||
|
|
|
||
|
|
void COBDSPI::reset()
|
||
|
|
{
|
||
|
|
char buf[32];
|
||
|
|
sendCommand("ATR\r", buf, sizeof(buf));
|
||
|
|
delay(2000);
|
||
|
|
errors = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void COBDSPI::end()
|
||
|
|
{
|
||
|
|
char buf[32];
|
||
|
|
sendCommand("ATPC\r", buf, sizeof(buf));
|
||
|
|
SPI.end();
|
||
|
|
}
|
||
|
|
|
||
|
|
byte COBDSPI::checkErrorMessage(const char* buffer)
|
||
|
|
{
|
||
|
|
const char *errmsg[] = {"UNABLE", "ERROR", "TIMEOUT", "NO DATA"};
|
||
|
|
for (byte i = 0; i < sizeof(errmsg) / sizeof(errmsg[0]); i++) {
|
||
|
|
if (strstr(buffer, errmsg[i])) return i + 1;
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::init(OBD_PROTOCOLS protocol)
|
||
|
|
{
|
||
|
|
const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATH0\r"};
|
||
|
|
char buffer[64];
|
||
|
|
m_state = OBD_DISCONNECTED;
|
||
|
|
for (byte i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
|
||
|
|
if (!sendCommand(initcmd[i], buffer, sizeof(buffer), OBD_TIMEOUT_SHORT)) {
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (protocol != PROTO_AUTO) {
|
||
|
|
sprintf_P(buffer, PSTR("ATSP%u\r"), protocol);
|
||
|
|
sendCommand(buffer, buffer, sizeof(buffer), OBD_TIMEOUT_SHORT);
|
||
|
|
}
|
||
|
|
bool success = sendCommand("010D\r", buffer, sizeof(buffer), OBD_TIMEOUT_SHORT) && !checkErrorMessage(buffer);
|
||
|
|
// load pid map
|
||
|
|
memset(pidmap, 0xff, sizeof(pidmap));
|
||
|
|
for (byte i = 0; i < 4; i++) {
|
||
|
|
byte pid = i * 0x20;
|
||
|
|
sprintf_P(buffer, PSTR("%02X%02X\r"), dataMode, pid);
|
||
|
|
write(buffer);
|
||
|
|
delay(10);
|
||
|
|
if (!receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) || checkErrorMessage(buffer)) break;
|
||
|
|
for (char *p = buffer; (p = strstr_P(p, PSTR("41 "))); ) {
|
||
|
|
p += 3;
|
||
|
|
if (hex2uint8(p) == pid) {
|
||
|
|
p += 2;
|
||
|
|
for (byte n = 0; n < 4 && *(p + n * 3) == ' '; n++) {
|
||
|
|
pidmap[i * 4 + n] = hex2uint8(p + n * 3 + 1);
|
||
|
|
}
|
||
|
|
success = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (success) {
|
||
|
|
m_state = OBD_CONNECTED;
|
||
|
|
errors = 0;
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef DEBUG
|
||
|
|
void COBDSPI::debugOutput(const char *s)
|
||
|
|
{
|
||
|
|
DEBUG.print('[');
|
||
|
|
DEBUG.print(millis());
|
||
|
|
DEBUG.print(']');
|
||
|
|
DEBUG.println(s);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
byte COBDSPI::begin()
|
||
|
|
{
|
||
|
|
m_target = TARGET_OBD;
|
||
|
|
pinMode(SPI_PIN_READY, INPUT);
|
||
|
|
pinMode(SPI_PIN_CS, OUTPUT);
|
||
|
|
digitalWrite(SPI_PIN_CS, HIGH);
|
||
|
|
SPI.begin();
|
||
|
|
//SPI.setClockDivider(SPI_CLOCK_DIV64);
|
||
|
|
return getVersion();
|
||
|
|
}
|
||
|
|
|
||
|
|
byte COBDSPI::getVersion()
|
||
|
|
{
|
||
|
|
byte version = 0;
|
||
|
|
for (byte n = 0; n < 30; n++) {
|
||
|
|
char buffer[32];
|
||
|
|
if (sendCommand("ATI\r", buffer, sizeof(buffer), 1000)) {
|
||
|
|
char *p = strstr_P(buffer, PSTR("OBDUART"));
|
||
|
|
if (p) {
|
||
|
|
p += 9;
|
||
|
|
version = (*p - '0') * 10 + (*(p + 2) - '0');
|
||
|
|
if (version) break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return version;
|
||
|
|
}
|
||
|
|
|
||
|
|
int COBDSPI::receive(char* buffer, int bufsize, unsigned int timeout)
|
||
|
|
{
|
||
|
|
int n = 0;
|
||
|
|
bool eos = false;
|
||
|
|
bool matched = false;
|
||
|
|
uint32_t t = millis();
|
||
|
|
do {
|
||
|
|
while (digitalRead(SPI_PIN_READY) == HIGH) {
|
||
|
|
if (millis() - t > timeout) return -1;
|
||
|
|
}
|
||
|
|
SPI.beginTransaction(settings);
|
||
|
|
digitalWrite(SPI_PIN_CS, LOW);
|
||
|
|
while (digitalRead(SPI_PIN_READY) == LOW && millis() - t < timeout) {
|
||
|
|
char c = SPI.transfer(' ');
|
||
|
|
if (eos) continue;
|
||
|
|
if (!matched) {
|
||
|
|
if (c == '$') {
|
||
|
|
buffer[0] = c;
|
||
|
|
n = 1;
|
||
|
|
matched = true;
|
||
|
|
} else if (c == 0x9) {
|
||
|
|
eos = true;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
if (n > 6 && c == '.' && buffer[n - 1] == '.' && buffer[n - 2] == '.') {
|
||
|
|
// SEARCHING...
|
||
|
|
n = 4;
|
||
|
|
timeout += OBD_TIMEOUT_LONG;
|
||
|
|
} else if (c == 0x9) {
|
||
|
|
eos = true;
|
||
|
|
break;
|
||
|
|
} else if (c != 0 && c != 0xff && n < bufsize - 1) {
|
||
|
|
buffer[n++] = c;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
delay(1);
|
||
|
|
digitalWrite(SPI_PIN_CS, HIGH);
|
||
|
|
SPI.endTransaction();
|
||
|
|
} while (!eos && millis() - t < timeout);
|
||
|
|
#ifdef DEBUG
|
||
|
|
if (!eos && millis() - t >= timeout) {
|
||
|
|
// timed out
|
||
|
|
debugOutput("RECV TIMEOUT");
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
if (n >= 2 && buffer[n - 1] == '>' && buffer[n - 2] == '\r') n -= 2;
|
||
|
|
buffer[n] = 0;
|
||
|
|
#ifdef DEBUG
|
||
|
|
if (m_target == TARGET_OBD) {
|
||
|
|
debugOutput(buffer);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
// wait for READY pin to restore high level so SPI bus is released
|
||
|
|
while (digitalRead(SPI_PIN_READY) == LOW);
|
||
|
|
return n;
|
||
|
|
}
|
||
|
|
|
||
|
|
void COBDSPI::write(const char* s)
|
||
|
|
{
|
||
|
|
#ifdef DEBUG
|
||
|
|
debugOutput(s);
|
||
|
|
#endif
|
||
|
|
//delay(1);
|
||
|
|
SPI.beginTransaction(settings);
|
||
|
|
digitalWrite(SPI_PIN_CS, LOW);
|
||
|
|
delay(1);
|
||
|
|
if (*s != '$') {
|
||
|
|
for (byte i = 0; i < sizeof(targets[0]); i++) {
|
||
|
|
SPI.transfer(targets[m_target][i]);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
byte c = 0;
|
||
|
|
for (; *s ;s++) {
|
||
|
|
c = *s;
|
||
|
|
SPI.transfer((byte)c);
|
||
|
|
}
|
||
|
|
if (c != '\r') SPI.transfer('\r');
|
||
|
|
// send terminating byte
|
||
|
|
SPI.transfer(0x1B);
|
||
|
|
delay(1);
|
||
|
|
digitalWrite(SPI_PIN_CS, HIGH);
|
||
|
|
SPI.endTransaction();
|
||
|
|
}
|
||
|
|
|
||
|
|
byte COBDSPI::sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout)
|
||
|
|
{
|
||
|
|
uint32_t t = millis();
|
||
|
|
int n = 0;
|
||
|
|
for (byte i = 0; i < 30 && millis() - t < timeout; i++) {
|
||
|
|
write(cmd);
|
||
|
|
n = receive(buf, bufsize, timeout);
|
||
|
|
if (n == -1) {
|
||
|
|
t = millis();
|
||
|
|
Serial.print('_');
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
if (n == 0 || (buf[1] != 'O' && !memcmp_P(buf + 5, PSTR("NO DATA"), 7))) {
|
||
|
|
// data not ready
|
||
|
|
delay(50);
|
||
|
|
i = 0;
|
||
|
|
} else {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return n;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::ioConfig(byte pin, IO_PIN_MODE mode)
|
||
|
|
{
|
||
|
|
// mode: 0: digital input, 1: digial output
|
||
|
|
char buf[32];
|
||
|
|
sprintf_P(buf, PSTR("ATMXCFG %u,%u\r"), pin, mode);
|
||
|
|
return sendCommand(buf, buf, sizeof(buf), 100) && strstr_P(buf, PSTR("OK"));
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::ioWrite(byte pin, byte level)
|
||
|
|
{
|
||
|
|
char buf[32];
|
||
|
|
sprintf_P(buf, PSTR("ATMXSET %u,%u\r"), pin, level);
|
||
|
|
return sendCommand(buf, buf, sizeof(buf), 100) && strstr_P(buf, PSTR("OK"));
|
||
|
|
}
|
||
|
|
|
||
|
|
byte COBDSPI::ioRead()
|
||
|
|
{
|
||
|
|
char buf[32];
|
||
|
|
sprintf_P(buf, PSTR("ATMXGET\r"));
|
||
|
|
sendCommand(buf, buf, sizeof(buf), 50);
|
||
|
|
char *p = strstr_P(buf, PSTR("MX:"));
|
||
|
|
if (!p || *(p + 4) != ',') return -1;
|
||
|
|
return (*(p + 3) - '0') | (*(p + 5) - '0') << 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::gpsInit(unsigned long baudrate)
|
||
|
|
{
|
||
|
|
bool success = false;
|
||
|
|
char buf[64];
|
||
|
|
if (baudrate) {
|
||
|
|
if (sendCommand("ATGPSON\r", buf, sizeof(buf))) {
|
||
|
|
sprintf_P(buf, PSTR("ATBR2%lu\r"), baudrate);
|
||
|
|
sendCommand(buf, buf, sizeof(buf));
|
||
|
|
uint32_t t = millis();
|
||
|
|
delay(100);
|
||
|
|
do {
|
||
|
|
if (gpsGetRawData(buf, sizeof(buf)) && strstr_P(buf, PSTR("S$G"))) {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
} while (millis() - t < GPS_INIT_TIMEOUT);
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
success = true;
|
||
|
|
}
|
||
|
|
//turn off GPS power
|
||
|
|
sendCommand("ATGPSOFF\r", buf, sizeof(buf));
|
||
|
|
return success;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::gpsGetData(GPS_DATA* gdata)
|
||
|
|
{
|
||
|
|
char buf[128];
|
||
|
|
if (sendCommand("ATGPS\r", buf, sizeof(buf), GPS_READ_TIMEOUT) == 0) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
if (memcmp_P(buf, PSTR("$GPS,"), 5)) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
byte index = 0;
|
||
|
|
bool valid = true;
|
||
|
|
char *s = buf + 5;
|
||
|
|
for (char* p = s; *p && valid; p++) {
|
||
|
|
char c = *p;
|
||
|
|
if (c == ',' || c == '>' || c <= 0x0d) {
|
||
|
|
long value = atol(s);
|
||
|
|
switch (index) {
|
||
|
|
case 0:
|
||
|
|
if (value == 0)
|
||
|
|
valid = false;
|
||
|
|
else
|
||
|
|
gdata->date = (uint32_t)value;
|
||
|
|
break;
|
||
|
|
case 1:
|
||
|
|
gdata->time = (uint32_t)value;
|
||
|
|
break;
|
||
|
|
case 2:
|
||
|
|
gdata->lat = value;
|
||
|
|
break;
|
||
|
|
case 3:
|
||
|
|
gdata->lng = value;
|
||
|
|
break;
|
||
|
|
case 4:
|
||
|
|
gdata->alt = value / 100;
|
||
|
|
break;
|
||
|
|
case 5:
|
||
|
|
gdata->speed = value * 100000 / 1852;
|
||
|
|
break;
|
||
|
|
case 6:
|
||
|
|
gdata->heading = value;
|
||
|
|
break;
|
||
|
|
case 7:
|
||
|
|
gdata->sat = value;
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
valid = false;
|
||
|
|
}
|
||
|
|
index++;
|
||
|
|
if (c != ',') break;
|
||
|
|
s = p + 1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return valid && index > 7;
|
||
|
|
}
|
||
|
|
|
||
|
|
int COBDSPI::gpsGetRawData(char* buf, int bufsize)
|
||
|
|
{
|
||
|
|
int n = sendCommand("ATGRR\r", buf, bufsize, GPS_READ_TIMEOUT);
|
||
|
|
if (n > 2) {
|
||
|
|
n -= 2;
|
||
|
|
buf[n] = 0;
|
||
|
|
}
|
||
|
|
return n;
|
||
|
|
}
|
||
|
|
|
||
|
|
void COBDSPI::gpsSendCommand(const char* cmd)
|
||
|
|
{
|
||
|
|
m_target = TARGET_GPS;
|
||
|
|
write(cmd);
|
||
|
|
m_target = TARGET_OBD;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool COBDSPI::xbBegin(unsigned long baudrate)
|
||
|
|
{
|
||
|
|
char buf[16];
|
||
|
|
sprintf_P(buf, PSTR("ATBR1%lu\r"), baudrate);
|
||
|
|
if (sendCommand(buf, buf, sizeof(buf))) {
|
||
|
|
xbPurge();
|
||
|
|
return true;
|
||
|
|
} else {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void COBDSPI::xbWrite(const char* cmd)
|
||
|
|
{
|
||
|
|
#ifdef XBEE_DEBUG
|
||
|
|
Serial.print("[SEND]");
|
||
|
|
Serial.println(cmd);
|
||
|
|
Serial.println("[/SEND]");
|
||
|
|
#endif
|
||
|
|
m_target = TARGET_BEE;
|
||
|
|
write(cmd);
|
||
|
|
m_target = TARGET_OBD;
|
||
|
|
}
|
||
|
|
|
||
|
|
int COBDSPI::xbRead(char* buffer, int bufsize)
|
||
|
|
{
|
||
|
|
write("ATGRD\r");
|
||
|
|
delay(10);
|
||
|
|
int bytes = receive(buffer, bufsize, 500);
|
||
|
|
if (bytes < 4 || memcmp(buffer, targets[TARGET_BEE], 4)) {
|
||
|
|
bytes = -1;
|
||
|
|
} else if (bytes >= 11 && !memcmp_P(buffer + 4, PSTR("NO DATA"), 7)) {
|
||
|
|
buffer[0] = 0;
|
||
|
|
bytes = 0;
|
||
|
|
}
|
||
|
|
return bytes;
|
||
|
|
}
|
||
|
|
|
||
|
|
int COBDSPI::xbReceive(char* buffer, int bufsize, unsigned int timeout, const char** expected, byte expectedCount)
|
||
|
|
{
|
||
|
|
int bytesRecv = 0;
|
||
|
|
uint32_t t = millis();
|
||
|
|
buffer[0] = 0;
|
||
|
|
do {
|
||
|
|
while (bytesRecv >= bufsize / 2) {
|
||
|
|
bytesRecv -= dumpLine(buffer, bytesRecv);
|
||
|
|
}
|
||
|
|
int n = xbRead(buffer + bytesRecv, bufsize - bytesRecv);
|
||
|
|
if (n > 0) {
|
||
|
|
buffer[bytesRecv + n] = 0;
|
||
|
|
// skip 4-byte header
|
||
|
|
char *p = buffer + bytesRecv + 4;
|
||
|
|
n -= 4;
|
||
|
|
if (bytesRecv == 0 && *p == '\n') {
|
||
|
|
// strip first \n
|
||
|
|
p++;
|
||
|
|
n--;
|
||
|
|
}
|
||
|
|
memmove(buffer + bytesRecv, p, n);
|
||
|
|
bytesRecv += n;
|
||
|
|
buffer[bytesRecv] = 0;
|
||
|
|
#ifdef XBEE_DEBUG
|
||
|
|
if (n > 0) {
|
||
|
|
Serial.print("[RECV]");
|
||
|
|
Serial.print(buffer + bytesRecv - n);
|
||
|
|
Serial.println("[/RECV]");
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
if (expectedCount == 0 && bytesRecv > 0) {
|
||
|
|
return bytesRecv;
|
||
|
|
}
|
||
|
|
for (byte i = 0; i < expectedCount; i++) {
|
||
|
|
// match expected string(s)
|
||
|
|
if (expected[i] && strstr(buffer, expected[i])) return i + 1;
|
||
|
|
}
|
||
|
|
delay(50);
|
||
|
|
} if (n < 0) {
|
||
|
|
break;
|
||
|
|
} else {
|
||
|
|
if (millis() - t + 200 < timeout)
|
||
|
|
delay(200);
|
||
|
|
else
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
} while (millis() - t < timeout);
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void COBDSPI::xbPurge()
|
||
|
|
{
|
||
|
|
char buf[32];
|
||
|
|
sendCommand("ATCLRGSM\r", buf, sizeof(buf));
|
||
|
|
}
|
||
|
|
|
||
|
|
void COBDSPI::xbTogglePower()
|
||
|
|
{
|
||
|
|
char buf[32];
|
||
|
|
sendCommand("ATGSMPWR\r", buf, sizeof(buf));
|
||
|
|
}
|