From 510fd89036a208d364ff765ba97f4d730e97326d Mon Sep 17 00:00:00 2001 From: PB2 Date: Thu, 27 Feb 2020 19:22:21 -0500 Subject: [PATCH] Major processing and error handling overhaul (#8) * Get rid of initial delay and turn off headers/spaces/etc * Fix return value/doc for rpm() * Update ELMduino.h * Update ELMduino.cpp * Update ELMduino.cpp * Update ELMduino.h * Update ELMduino.cpp * Update ELMduino.cpp * Get rid of excess prints * Update Arduino_software_serial_test.ino * Update Arduino_hardware_serial_test.ino * Update ESP32_test.ino * Update ELMduino.cpp * Update ELMduino.h * Update ELMduino.h * Update ELMduino.cpp * Update ELMduino.cpp * Update ELMduino.cpp * Update ELMduino.cpp * Update ELMduino.cpp * Update ELMduino.h * Bug fixes --- ELMduino.cpp | 279 ++++++++++++------ ELMduino.h | 24 +- .../Arduino_hardware_serial_test.ino | 3 - .../Arduino_software_serial_test.ino | 3 - examples/ESP32_test/ESP32_test.ino | 5 +- 5 files changed, 195 insertions(+), 119 deletions(-) diff --git a/ELMduino.cpp b/ELMduino.cpp index 6a09346..05bac01 100644 --- a/ELMduino.cpp +++ b/ELMduino.cpp @@ -24,16 +24,13 @@ bool ELM327::begin(Stream &stream) { elm_port = &stream; - while (!elm_port); - - // wait 3 sec for the ELM327 to initialize - delay(3000); + // test if serial port is connected + if (!elm_port) + return false; // try to connect - while (!initializeELM()) - delay(1000); - - delay(100); + if (!initializeELM()) + return false; return true; } @@ -78,22 +75,21 @@ bool ELM327::begin(Stream &stream) bool ELM327::initializeELM() { char *match; + connected = false; sendCommand(ECHO_OFF); + delay(100); + sendCommand(PRINTING_SPACES_OFF); delay(100); - if (sendCommand("AT SP 0") == ELM_SUCCESS) // automatic protocol + if (sendCommand("AT SP 0") == ELM_SUCCESS) // Set protocol to auto { match = strstr(payload, "OK"); if (match != NULL) connected = true; - else - connected = false; } - else - connected = false; return connected; } @@ -102,7 +98,7 @@ bool ELM327::initializeELM() /* - void ELM327::formatQueryArray(uint16_t service, uint16_t pid) + void ELM327::formatQueryArray(uint16_t service, uint32_t pid) Description: ------------ @@ -111,22 +107,39 @@ bool ELM327::initializeELM() Inputs: ------- * uint16_t service - Service number of the queried PID - * uint16_t pid - PID number of the queried PID + * uint32_t pid - PID number of the queried PID Return: ------- * void */ -void ELM327::formatQueryArray(uint16_t service, uint16_t pid) +void ELM327::formatQueryArray(uint16_t service, uint32_t pid) { query[0] = ((service >> 8) & 0xFF) + '0'; query[1] = (service & 0xFF) + '0'; - query[2] = ((pid >> 8) & 0xFF) + '0'; - query[3] = (pid & 0xFF) + '0'; - query[4] = '\n'; - query[5] = '\r'; - upper(query, 6); + // determine PID length (standard queries have 16-bit PIDs, + // but some custom queries have PIDs with 32-bit values) + if (pid & 0xFF00) + { + longQuery = true; + + query[2] = ((pid >> 24) & 0xFF) + '0'; + query[3] = ((pid >> 16) & 0xFF) + '0'; + query[4] = ((pid >> 8) & 0xFF) + '0'; + query[5] = (pid & 0xFF) + '0'; + + upper(query, 6); + } + else + { + longQuery = false; + + query[2] = ((pid >> 8) & 0xFF) + '0'; + query[3] = (pid & 0xFF) + '0'; + + upper(query, 4); + } } @@ -218,6 +231,61 @@ uint8_t ELM327::ctoi(uint8_t value) +/* + int8_t ELM327::nextIndex(char const *str, + char const *target, + uint8_t numOccur) + + Description: + ------------ + * Finds and returns the first char index of + numOccur'th instance of target in str + + Inputs: + ------- + * char const *str - string to search target + within + * char const *target - String to search for in + str + * uint8_t numOccur - Which instance of target + in str + + Return: + ------- + * int8_t - First char index of numOccur'th + instance of target in str. -1 if there is no + numOccur'th instance of target in str +*/ +int8_t ELM327::nextIndex(char const *str, + char const *target, + uint8_t numOccur=1) +{ + char const *p = str; + char const *r = str; + uint8_t count; + + for (count = 0; ; ++count) + { + p = strstr(p, target); + + if (count == (numOccur - 1)) + break; + + if (!p) + break; + + p++; + } + + if (!p) + return -1; + + return p - r; +} + + + + /* void ELM327::flushInputBuff() @@ -243,9 +311,8 @@ void ELM327::flushInputBuff() /* - bool ELM327::queryPID(uint8_t service, - uint8_t PID, - uint8_t payloadSize, + bool ELM327::queryPID(uint16_t service, + uint32_t PID, float &value) Description: @@ -262,14 +329,11 @@ void ELM327::flushInputBuff() * bool - Whether or not the query was submitted successfully */ bool ELM327::queryPID(uint16_t service, - uint16_t pid) + uint32_t pid) { if (connected) { - // determine the string needed to be passed to the OBD scanner to make the query formatQueryArray(service, pid); - - // make the query status = sendCommand(query); return true; @@ -294,12 +358,12 @@ bool ELM327::queryPID(uint16_t service, Return: ------- - * int32_t - Vehicle speed in kph + * int16_t - Vehicle speed in kph */ int32_t ELM327::kph() { if (queryPID(SERVICE_01, VEHICLE_SPEED)) - return findResponse(false); + return findResponse(); return ELM_GENERAL_ERROR; } @@ -349,12 +413,12 @@ float ELM327::mph() Return: ------- - * uint32_t - Vehicle RPM + * float - Vehicle RPM */ float ELM327::rpm() { if (queryPID(SERVICE_01, ENGINE_RPM)) - return (findResponse(true) / 4); + return (findResponse() / 4.0); return ELM_GENERAL_ERROR; } @@ -381,58 +445,78 @@ int8_t ELM327::sendCommand(const char *cmd) { uint8_t counter = 0; - // flush the input buffer + for (byte i = 0; i < PAYLOAD_LEN; i++) + payload[i] = '\0'; + + // reset input buffer and number of received bytes + recBytes = 0; flushInputBuff(); - // send the command with carriage return elm_port->print(cmd); elm_port->print('\r'); - // prime the timer + // prime the timeout timer previousTime = millis(); currentTime = previousTime; - for (byte i = 0; i < PAYLOAD_LEN; i++) - payload[i] = '\0'; - // buffer the response of the ELM327 until either the - //end marker is read or a timeout has occurred + // end marker is read or a timeout has occurred while ((counter < (PAYLOAD_LEN + 1)) && !timeout()) { if (elm_port->available()) { - payload[counter] = elm_port->read(); + char recChar = elm_port->read(); - if (payload[counter] == '>') + if (recChar == '>') break; - else - counter++; + else if ((recChar == '\r') || (recChar == '\n') || (recChar == ' ')) + continue; + + payload[counter] = recChar; + counter++; } } if (timeout()) + return ELM_TIMEOUT; + + if (nextIndex(payload, "UNABLE TO CONNECT") >= 0) + { + for (byte i = 0; i < PAYLOAD_LEN; i++) + payload[i] = '\0'; + return ELM_UNABLE_TO_CONNECT; + } - char *match; + if (nextIndex(payload, "NO DATA") >= 0) + { + for (byte i = 0; i < PAYLOAD_LEN; i++) + payload[i] = '\0'; - match = strstr(payload, "UNABLE TO CONNECT"); - if (match != NULL) + return ELM_NO_DATA; + } + + if (nextIndex(payload, "STOPPED") >= 0) { for (byte i = 0; i < PAYLOAD_LEN; i++) payload[i] = '\0'; - return ELM_UNABLE_TO_CONNECT; + return ELM_STOPPED; } - match = strstr(payload, "NO DATA"); - if (match != NULL) + if (nextIndex(payload, "ERROR") >= 0) { for (byte i = 0; i < PAYLOAD_LEN; i++) payload[i] = '\0'; - return ELM_NO_DATA; + return ELM_GENERAL_ERROR; } + // keep track of how many bytes were received in + // the ELM327's response (not counting the + // end-marker '>') if a valid response is found + recBytes = counter; + return ELM_SUCCESS; } @@ -440,7 +524,7 @@ int8_t ELM327::sendCommand(const char *cmd) /* - int8_t ELM327::findResponse(bool longResponse) + uint32_t ELM327::findResponse() Description: ------------ @@ -448,64 +532,63 @@ int8_t ELM327::sendCommand(const char *cmd) Inputs: ------- - * bool longResponse - Some responses are 2 hex digits wide and others are 4. - If the response is expected to be 2 hex digits wide, set longResponse to false, - else set longResponse to true + * void Return: ------- - * int8_t - Response status + * uint32_t - Query response value */ -int ELM327::findResponse(bool longResponse) +uint32_t ELM327::findResponse() { - byte maxIndex = 1; - int dataLoc = 0; - int response = 0; - char header[5]; - char data[4]; - - header[0] = '4'; - header[1] = query[1]; - header[2] = ' '; - header[3] = query[2]; - header[4] = query[3]; - - Serial.print("Received: "); - Serial.write((const uint8_t*)payload, PAYLOAD_LEN); - Serial.println(); - - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Waggressive-loop-optimizations" - for (byte i = 0; i < (PAYLOAD_LEN + 5); i++) + uint16_t A = 0; + uint16_t B = 0; + uint8_t firstDatum = 0; + uint8_t payBytes = 0; + char header[6] = { '\0' }; + + if (longQuery) { - if (payload[i] == header[0] && - payload[i + 1] == header[1] && - payload[i + 2] == header[2] && - payload[i + 3] == header[3] && - payload[i + 4] == header[4]) - dataLoc = i + 6; + header[0] = query[0] + 4; + header[1] = query[1]; + header[2] = query[2]; + header[3] = query[3]; + header[4] = query[4]; + header[5] = query[5]; } -#pragma GCC diagnostic pop - - if (dataLoc > 0) + else { - data[0] = ctoi(payload[dataLoc]); - data[1] = ctoi(payload[dataLoc + 1]); + header[0] = query[0] + 4; + header[1] = query[1]; + header[2] = query[2]; + header[3] = query[3]; + } - if (longResponse) - { - data[2] = ctoi(payload[dataLoc + 3]); - data[3] = ctoi(payload[dataLoc + 4]); + int8_t firstHeadIndex = nextIndex(payload, header); + int8_t secondHeadIndex = nextIndex(payload, header, 2); - maxIndex = 3; - } + if (firstHeadIndex >= 0) + { + if (longQuery) + firstDatum = firstHeadIndex + 6; + else + firstDatum = firstHeadIndex + 4; + + // Some ELM327s (such as my own) respond with two + // "responses" per query. "payBytes" represents the + // correct number of bytes returned by the ELM327 + // regardless of how many "responses" were returned + if (secondHeadIndex >= 0) + payBytes = secondHeadIndex - firstDatum; + else + payBytes = recBytes - firstDatum; - for (int i = maxIndex; i >= 0; i--) - response += data[i] * pow(16, (maxIndex - i)); + // Some PID queries return 4 hex digit values - the + // rest return 2 hex digit values + if (payBytes >= 4) + return (ctoi(payload[firstDatum]) * 4096) + (ctoi(payload[firstDatum + 1]) * 256) + (ctoi(payload[firstDatum + 2]) * 16) + ctoi(payload[firstDatum + 3]); + else + return (ctoi(payload[firstDatum]) * 16) + ctoi(payload[firstDatum + 1]); } - else - Serial.println("Header NOT found"); - return response; + return 0; } diff --git a/ELMduino.h b/ELMduino.h index 8897476..41f80b5 100644 --- a/ELMduino.h +++ b/ELMduino.h @@ -117,6 +117,9 @@ const uint8_t ENGINE_PERCENT_TORQUE_DATA = 100; // 0x64 - % const uint8_t AUX_INPUT_OUTPUT_SUPPORTED = 101; // 0x65 - bit encoded +const uint8_t SERVICE_02 = 2; + + //-------------------------------------------------------------------------------------// @@ -231,13 +234,9 @@ const char * const RESET_ALL = "AT Z"; // General //-------------------------------------------------------------------------------------// // Class constants //-------------------------------------------------------------------------------------// -const char CANCEL_OBD[] = "XXXXXXXXX\r\r\r"; const float KPH_MPH_CONVERT = 0.6213711922; const float RPM_CONVERT = 0.25; const int8_t QUERY_LEN = 6; -const int8_t HEADER_LEN = 4; -const int8_t SERVICE_LEN = 2; -const int8_t PID_LEN = 2; const int8_t PAYLOAD_LEN = 40; const int8_t ELM_SUCCESS = 0; const int8_t ELM_NO_RESPONSE = 1; @@ -245,6 +244,8 @@ const int8_t ELM_BUFFER_OVERFLOW = 2; const int8_t ELM_GARBAGE = 3; const int8_t ELM_UNABLE_TO_CONNECT = 4; const int8_t ELM_NO_DATA = 5; +const int8_t ELM_STOPPED = 6; +const int8_t ELM_TIMEOUT = 7; const int8_t ELM_GENERAL_ERROR = -1; @@ -266,8 +267,8 @@ class ELM327 bool begin(Stream& stream); bool initializeELM(); void flushInputBuff(); - int findResponse(bool longResponse); - bool queryPID(uint16_t service, uint16_t pid); + uint32_t findResponse(); + bool queryPID(uint16_t service, uint32_t pid); int8_t sendCommand(const char *cmd); bool timeout(); float rpm(); @@ -279,9 +280,8 @@ class ELM327 private: char query[QUERY_LEN]; - uint8_t hexService[SERVICE_LEN]; - uint8_t hexPid[PID_LEN]; - uint8_t responseHeader[HEADER_LEN]; + bool longQuery = false; + uint16_t recBytes; uint32_t currentTime; uint32_t previousTime; @@ -289,7 +289,9 @@ class ELM327 void upper(char string[], uint8_t buflen); - void formatQueryArray(uint16_t service, uint16_t pid); - void formatHeaderArray(); + void formatQueryArray(uint16_t service, uint32_t pid); uint8_t ctoi(uint8_t value); + int8_t nextIndex(char const *str, + char const *target, + uint8_t numOccur); }; diff --git a/examples/Arduino_hardware_serial_test/Arduino_hardware_serial_test.ino b/examples/Arduino_hardware_serial_test/Arduino_hardware_serial_test.ino index 47db45c..51f1bff 100644 --- a/examples/Arduino_hardware_serial_test/Arduino_hardware_serial_test.ino +++ b/examples/Arduino_hardware_serial_test/Arduino_hardware_serial_test.ino @@ -27,9 +27,6 @@ void setup() } Serial.println("Connected to ELM327"); - Serial.println("Ensure your serial monitor line ending is set to 'Carriage Return'"); - Serial.println("Type and send commands/queries to your ELM327 through the serial monitor"); - Serial.println(); } diff --git a/examples/Arduino_software_serial_test/Arduino_software_serial_test.ino b/examples/Arduino_software_serial_test/Arduino_software_serial_test.ino index b8dc05e..1f29136 100644 --- a/examples/Arduino_software_serial_test/Arduino_software_serial_test.ino +++ b/examples/Arduino_software_serial_test/Arduino_software_serial_test.ino @@ -29,9 +29,6 @@ void setup() } Serial.println("Connected to ELM327"); - Serial.println("Ensure your serial monitor line ending is set to 'Carriage Return'"); - Serial.println("Type and send commands/queries to your ELM327 through the serial monitor"); - Serial.println(); } diff --git a/examples/ESP32_test/ESP32_test.ino b/examples/ESP32_test/ESP32_test.ino index 50f014e..6076d2e 100644 --- a/examples/ESP32_test/ESP32_test.ino +++ b/examples/ESP32_test/ESP32_test.ino @@ -34,11 +34,8 @@ void setup() Serial.println("Couldn't connect to OBD scanner - Phase 2"); while (1); } - + Serial.println("Connected to ELM327"); - Serial.println("Ensure your serial monitor line ending is set to 'Carriage Return'"); - Serial.println("Type and send commands/queries to your ELM327 through the serial monitor"); - Serial.println(); }