publish final code
Dependencies: FXOS8700CQ Pubnub_mbed2_sync2 SoftSerial TinyGPSplus WNCInterface2 mbed-rtos mbed
main.cpp@0:1092494506a3, 2017-02-21 (annotated)
- Committer:
- cswiger
- Date:
- Tue Feb 21 22:28:26 2017 +0000
- Revision:
- 0:1092494506a3
publish final code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cswiger | 0:1092494506a3 | 1 | /* ===================================================================== |
cswiger | 0:1092494506a3 | 2 | Copyright © 2016, Avnet (R) |
cswiger | 0:1092494506a3 | 3 | |
cswiger | 0:1092494506a3 | 4 | Contributors: |
cswiger | 0:1092494506a3 | 5 | * James M Flynn, www.em.avnet.com |
cswiger | 0:1092494506a3 | 6 | * Chuck Swiger |
cswiger | 0:1092494506a3 | 7 | |
cswiger | 0:1092494506a3 | 8 | Licensed under the Apache License, Version 2.0 (the "License"); |
cswiger | 0:1092494506a3 | 9 | you may not use this file except in compliance with the License. |
cswiger | 0:1092494506a3 | 10 | You may obtain a copy of the License at |
cswiger | 0:1092494506a3 | 11 | |
cswiger | 0:1092494506a3 | 12 | http://www.apache.org/licenses/LICENSE-2.0 |
cswiger | 0:1092494506a3 | 13 | |
cswiger | 0:1092494506a3 | 14 | Unless required by applicable law or agreed to in writing, |
cswiger | 0:1092494506a3 | 15 | software distributed under the License is distributed on an |
cswiger | 0:1092494506a3 | 16 | "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, |
cswiger | 0:1092494506a3 | 17 | either express or implied. See the License for the specific |
cswiger | 0:1092494506a3 | 18 | language governing permissions and limitations under the License. |
cswiger | 0:1092494506a3 | 19 | |
cswiger | 0:1092494506a3 | 20 | @file WNC_Pubnub_obd2b |
cswiger | 0:1092494506a3 | 21 | @version 1.0b |
cswiger | 0:1092494506a3 | 22 | @date Dec 2016 |
cswiger | 0:1092494506a3 | 23 | |
cswiger | 0:1092494506a3 | 24 | - This version has the AccelX,Y,Z sensor and code for GPS |
cswiger | 0:1092494506a3 | 25 | ======================================================================== */ |
cswiger | 0:1092494506a3 | 26 | |
cswiger | 0:1092494506a3 | 27 | #include "mbed.h" |
cswiger | 0:1092494506a3 | 28 | #include "WNCInterface.h" |
cswiger | 0:1092494506a3 | 29 | #include "main.h" // obd2 includes |
cswiger | 0:1092494506a3 | 30 | //#include "MODSERIAL.h" // already exists in WNCInterface |
cswiger | 0:1092494506a3 | 31 | #include "SoftSerial.h" |
cswiger | 0:1092494506a3 | 32 | #include "pubnub_sync.h" |
cswiger | 0:1092494506a3 | 33 | #include "sensors.h" |
cswiger | 0:1092494506a3 | 34 | #include "hardware.h" |
cswiger | 0:1092494506a3 | 35 | #include "TinyGPSplus.h" |
cswiger | 0:1092494506a3 | 36 | |
cswiger | 0:1092494506a3 | 37 | #define DEBUG |
cswiger | 0:1092494506a3 | 38 | #define MBED_PLATFORM |
cswiger | 0:1092494506a3 | 39 | |
cswiger | 0:1092494506a3 | 40 | #define CRLF "\n\r" |
cswiger | 0:1092494506a3 | 41 | |
cswiger | 0:1092494506a3 | 42 | I2C i2c(PTC11, PTC10); //SDA, SCL -- define the I2C pins being used |
cswiger | 0:1092494506a3 | 43 | |
cswiger | 0:1092494506a3 | 44 | WNCInterface wnc; |
cswiger | 0:1092494506a3 | 45 | MODSERIAL pc(USBTX,USBRX,256,256); |
cswiger | 0:1092494506a3 | 46 | MODSERIAL obd2(PTC15,PTC14,256,256); // (TX,RX,txbuffer,rxbuffer) |
cswiger | 0:1092494506a3 | 47 | // This is the Bluetooth J199 connector - pin3 PTC14 RX, pin4 PTC15 TX |
cswiger | 0:1092494506a3 | 48 | SoftSerial gpss2(PTC7,PTC5); // Moved from uart0 to softserial - this worked! |
cswiger | 0:1092494506a3 | 49 | |
cswiger | 0:1092494506a3 | 50 | pubnub_t *pbp = pubnub_alloc(); |
cswiger | 0:1092494506a3 | 51 | |
cswiger | 0:1092494506a3 | 52 | TinyGPSPlus gps; |
cswiger | 0:1092494506a3 | 53 | |
cswiger | 0:1092494506a3 | 54 | DigitalOut rled(PTB19); // RED panel lamp |
cswiger | 0:1092494506a3 | 55 | DigitalOut gled(PTB18); // GREEN |
cswiger | 0:1092494506a3 | 56 | |
cswiger | 0:1092494506a3 | 57 | |
cswiger | 0:1092494506a3 | 58 | long GPSBAUD = 9600; |
cswiger | 0:1092494506a3 | 59 | long OBDBAUD = 9600; |
cswiger | 0:1092494506a3 | 60 | long PCBAUD = 115200; |
cswiger | 0:1092494506a3 | 61 | |
cswiger | 0:1092494506a3 | 62 | K64F_Sensors_t SENSOR_DATA = |
cswiger | 0:1092494506a3 | 63 | { |
cswiger | 0:1092494506a3 | 64 | .Temperature = "0", |
cswiger | 0:1092494506a3 | 65 | .Humidity = "0", |
cswiger | 0:1092494506a3 | 66 | .AccelX = "0", |
cswiger | 0:1092494506a3 | 67 | .AccelY = "0", |
cswiger | 0:1092494506a3 | 68 | .AccelZ = "0", |
cswiger | 0:1092494506a3 | 69 | }; |
cswiger | 0:1092494506a3 | 70 | |
cswiger | 0:1092494506a3 | 71 | void feedGPS() { |
cswiger | 0:1092494506a3 | 72 | while (gpss2.readable()) { |
cswiger | 0:1092494506a3 | 73 | gps.encode(gpss2.getc()); |
cswiger | 0:1092494506a3 | 74 | } |
cswiger | 0:1092494506a3 | 75 | } |
cswiger | 0:1092494506a3 | 76 | |
cswiger | 0:1092494506a3 | 77 | |
cswiger | 0:1092494506a3 | 78 | char* itoa(int val, int base){ |
cswiger | 0:1092494506a3 | 79 | static char buf[32] = {0}; |
cswiger | 0:1092494506a3 | 80 | int i = 30; |
cswiger | 0:1092494506a3 | 81 | for(; val && i ; --i, val /= base) |
cswiger | 0:1092494506a3 | 82 | buf[i] = "0123456789abcdef"[val % base]; |
cswiger | 0:1092494506a3 | 83 | return &buf[i+1]; |
cswiger | 0:1092494506a3 | 84 | } |
cswiger | 0:1092494506a3 | 85 | |
cswiger | 0:1092494506a3 | 86 | |
cswiger | 0:1092494506a3 | 87 | //Periodic timer |
cswiger | 0:1092494506a3 | 88 | Ticker OneMsTicker; |
cswiger | 0:1092494506a3 | 89 | volatile bool bTimerExpiredFlag = false; |
cswiger | 0:1092494506a3 | 90 | int OneMsTicks = 0; |
cswiger | 0:1092494506a3 | 91 | int iTimer1Interval_ms = 1000; |
cswiger | 0:1092494506a3 | 92 | |
cswiger | 0:1092494506a3 | 93 | void OneMsFunction() |
cswiger | 0:1092494506a3 | 94 | { |
cswiger | 0:1092494506a3 | 95 | OneMsTicks++; |
cswiger | 0:1092494506a3 | 96 | if ((OneMsTicks % iTimer1Interval_ms) == 0) |
cswiger | 0:1092494506a3 | 97 | { |
cswiger | 0:1092494506a3 | 98 | bTimerExpiredFlag = true; |
cswiger | 0:1092494506a3 | 99 | } |
cswiger | 0:1092494506a3 | 100 | } //OneMsFunction() |
cswiger | 0:1092494506a3 | 101 | |
cswiger | 0:1092494506a3 | 102 | |
cswiger | 0:1092494506a3 | 103 | void GetDTC(void) |
cswiger | 0:1092494506a3 | 104 | { |
cswiger | 0:1092494506a3 | 105 | char jsonreply[140] = {'\0'}; |
cswiger | 0:1092494506a3 | 106 | dtc_read(); // populate the DTC struct if any |
cswiger | 0:1092494506a3 | 107 | if(has_dtc) { |
cswiger | 0:1092494506a3 | 108 | strcpy(jsonreply,"{\""); |
cswiger | 0:1092494506a3 | 109 | for (int i=0; i<MAX_DTC_READ; i++) { |
cswiger | 0:1092494506a3 | 110 | strcat(jsonreply,itoa((i+1),10)); |
cswiger | 0:1092494506a3 | 111 | strcat(jsonreply,"\":\""); |
cswiger | 0:1092494506a3 | 112 | if(i!=MAX_DTC_READ-1) { // last one? |
cswiger | 0:1092494506a3 | 113 | strcat(jsonreply,DTC[i].code); |
cswiger | 0:1092494506a3 | 114 | strcat(jsonreply,"\",\""); |
cswiger | 0:1092494506a3 | 115 | } else { |
cswiger | 0:1092494506a3 | 116 | strcat(jsonreply,DTC[i].code); |
cswiger | 0:1092494506a3 | 117 | strcat(jsonreply,"\"}"); |
cswiger | 0:1092494506a3 | 118 | } |
cswiger | 0:1092494506a3 | 119 | } |
cswiger | 0:1092494506a3 | 120 | } else { |
cswiger | 0:1092494506a3 | 121 | strcpy(jsonreply,"{\"1\":\"none\"}"); |
cswiger | 0:1092494506a3 | 122 | } |
cswiger | 0:1092494506a3 | 123 | pubnub_res rslt = pubnub_publish(pbp, "carpal2", jsonreply); |
cswiger | 0:1092494506a3 | 124 | if (rslt != PNR_STARTED) |
cswiger | 0:1092494506a3 | 125 | pc.printf("Failed to start publishing, rslt=%d"CRLF, rslt); |
cswiger | 0:1092494506a3 | 126 | else { |
cswiger | 0:1092494506a3 | 127 | rslt = pubnub_await(pbp); |
cswiger | 0:1092494506a3 | 128 | if (rslt != PNR_OK) |
cswiger | 0:1092494506a3 | 129 | pc.printf("Failed to finished publishing, rslt=%d"CRLF, rslt); |
cswiger | 0:1092494506a3 | 130 | else { |
cswiger | 0:1092494506a3 | 131 | pc.printf("Published! Response from Pubnub: %s"CRLF, pubnub_last_publish_result(pbp)); |
cswiger | 0:1092494506a3 | 132 | } |
cswiger | 0:1092494506a3 | 133 | } |
cswiger | 0:1092494506a3 | 134 | |
cswiger | 0:1092494506a3 | 135 | } // GetDTC |
cswiger | 0:1092494506a3 | 136 | |
cswiger | 0:1092494506a3 | 137 | bool parse_JSON(char const* json_string) |
cswiger | 0:1092494506a3 | 138 | { |
cswiger | 0:1092494506a3 | 139 | char const* beginquote; |
cswiger | 0:1092494506a3 | 140 | char token[] = "get:"; |
cswiger | 0:1092494506a3 | 141 | beginquote = strstr(json_string, token ); |
cswiger | 0:1092494506a3 | 142 | if ((beginquote != 0)) |
cswiger | 0:1092494506a3 | 143 | { |
cswiger | 0:1092494506a3 | 144 | char getreq = beginquote[strlen(token)]; // this only gets a single letter at the position after 'get:' |
cswiger | 0:1092494506a3 | 145 | PRINTF(GRN "get Found : %c" DEF "\r\n", getreq); |
cswiger | 0:1092494506a3 | 146 | switch(getreq) |
cswiger | 0:1092494506a3 | 147 | { |
cswiger | 0:1092494506a3 | 148 | case 'd': |
cswiger | 0:1092494506a3 | 149 | { // dtc |
cswiger | 0:1092494506a3 | 150 | GetDTC(); |
cswiger | 0:1092494506a3 | 151 | break; |
cswiger | 0:1092494506a3 | 152 | } |
cswiger | 0:1092494506a3 | 153 | default: |
cswiger | 0:1092494506a3 | 154 | { |
cswiger | 0:1092494506a3 | 155 | break; |
cswiger | 0:1092494506a3 | 156 | } |
cswiger | 0:1092494506a3 | 157 | } //switch(getreq) |
cswiger | 0:1092494506a3 | 158 | return true; |
cswiger | 0:1092494506a3 | 159 | } |
cswiger | 0:1092494506a3 | 160 | else |
cswiger | 0:1092494506a3 | 161 | { |
cswiger | 0:1092494506a3 | 162 | return false; |
cswiger | 0:1092494506a3 | 163 | } |
cswiger | 0:1092494506a3 | 164 | } //parse_JSON |
cswiger | 0:1092494506a3 | 165 | |
cswiger | 0:1092494506a3 | 166 | |
cswiger | 0:1092494506a3 | 167 | int main() |
cswiger | 0:1092494506a3 | 168 | { |
cswiger | 0:1092494506a3 | 169 | char data[256]; |
cswiger | 0:1092494506a3 | 170 | int ret; |
cswiger | 0:1092494506a3 | 171 | |
cswiger | 0:1092494506a3 | 172 | long vehicle_speed = 0; |
cswiger | 0:1092494506a3 | 173 | long engine_rpm = 0; |
cswiger | 0:1092494506a3 | 174 | long fuel_status = 0; |
cswiger | 0:1092494506a3 | 175 | long engine_load = 0; |
cswiger | 0:1092494506a3 | 176 | long coolant_temp = 0; |
cswiger | 0:1092494506a3 | 177 | long stft_b1 = 0; |
cswiger | 0:1092494506a3 | 178 | long ltft_b1 = 0; |
cswiger | 0:1092494506a3 | 179 | long stft_b2 = 0; |
cswiger | 0:1092494506a3 | 180 | long ltft_b2 = 0; |
cswiger | 0:1092494506a3 | 181 | long timing_adv = 0; |
cswiger | 0:1092494506a3 | 182 | long int_air_temp = 0; |
cswiger | 0:1092494506a3 | 183 | long maf_air_flow = 0; |
cswiger | 0:1092494506a3 | 184 | long throttle_pos = 0; |
cswiger | 0:1092494506a3 | 185 | long b1s2_o2_v = 0; |
cswiger | 0:1092494506a3 | 186 | |
cswiger | 0:1092494506a3 | 187 | |
cswiger | 0:1092494506a3 | 188 | rled = 1; // red panel lamp while initializing |
cswiger | 0:1092494506a3 | 189 | gled = 0; |
cswiger | 0:1092494506a3 | 190 | |
cswiger | 0:1092494506a3 | 191 | // Configure PC BAUD |
cswiger | 0:1092494506a3 | 192 | pc.baud(PCBAUD); |
cswiger | 0:1092494506a3 | 193 | gpss2.baud(GPSBAUD); |
cswiger | 0:1092494506a3 | 194 | |
cswiger | 0:1092494506a3 | 195 | |
cswiger | 0:1092494506a3 | 196 | //debug |
cswiger | 0:1092494506a3 | 197 | //wait(10); // wait for terminal connected - not needed for normal use |
cswiger | 0:1092494506a3 | 198 | pc.printf(WHT "STARTING WNCInterface PubNub Test" CRLF); |
cswiger | 0:1092494506a3 | 199 | |
cswiger | 0:1092494506a3 | 200 | //Create a 1ms timer tick function: |
cswiger | 0:1092494506a3 | 201 | iTimer1Interval_ms = SENSOR_UPDATE_INTERVAL_MS; |
cswiger | 0:1092494506a3 | 202 | OneMsTicker.attach(OneMsFunction, 0.001f) ; |
cswiger | 0:1092494506a3 | 203 | |
cswiger | 0:1092494506a3 | 204 | //Initialize the I2C sensors that are present |
cswiger | 0:1092494506a3 | 205 | sensors_init(); |
cswiger | 0:1092494506a3 | 206 | read_sensors(); |
cswiger | 0:1092494506a3 | 207 | |
cswiger | 0:1092494506a3 | 208 | // init and connect the WNCInterface cell modem |
cswiger | 0:1092494506a3 | 209 | pc.printf(DEF "init() returned 0x%04X" CRLF, wnc.init(NULL,&pc)); |
cswiger | 0:1092494506a3 | 210 | ret = wnc.connect(); // this does NOT really connect, connect happens in wnc.init() above |
cswiger | 0:1092494506a3 | 211 | // it just returns 0 for ok and -1 for not connected |
cswiger | 0:1092494506a3 | 212 | if (ret) NVIC_SystemReset(); // so if not connected, reset! |
cswiger | 0:1092494506a3 | 213 | wnc.doDebug(0); // if you want a LOT of AT commands logged (1) |
cswiger | 0:1092494506a3 | 214 | |
cswiger | 0:1092494506a3 | 215 | pc.printf("IP Address: %s " CRLF, wnc.getIPAddress()); |
cswiger | 0:1092494506a3 | 216 | pc.printf("-------------------------------------" CRLF); |
cswiger | 0:1092494506a3 | 217 | pc.printf("starting PubNub sync..." CRLF); |
cswiger | 0:1092494506a3 | 218 | |
cswiger | 0:1092494506a3 | 219 | // create the Pubnub interface |
cswiger | 0:1092494506a3 | 220 | pubnub_init(pbp, "pub-c-a7e5745b-0ad1-42ac-8cce-229057b406ed", "sub-c-4df1fc96-825f-11e5-8495-02ee2ddab7fe"); |
cswiger | 0:1092494506a3 | 221 | |
cswiger | 0:1092494506a3 | 222 | // Setup STN1110 UART |
cswiger | 0:1092494506a3 | 223 | pc.printf("Configuring OBD UART\r\n"); |
cswiger | 0:1092494506a3 | 224 | Init(); |
cswiger | 0:1092494506a3 | 225 | pc.printf("Configure done..\r\n"); |
cswiger | 0:1092494506a3 | 226 | |
cswiger | 0:1092494506a3 | 227 | while(1) { |
cswiger | 0:1092494506a3 | 228 | feedGPS(); |
cswiger | 0:1092494506a3 | 229 | if (bTimerExpiredFlag) |
cswiger | 0:1092494506a3 | 230 | { |
cswiger | 0:1092494506a3 | 231 | bTimerExpiredFlag = false; |
cswiger | 0:1092494506a3 | 232 | ret = wnc.connect(); // check if still connected - it just returns 0 for ok and -1 for not connected |
cswiger | 0:1092494506a3 | 233 | if (ret) NVIC_SystemReset(); // so if not connected, reset! |
cswiger | 0:1092494506a3 | 234 | pc.printf("Refresh...\n\r"); |
cswiger | 0:1092494506a3 | 235 | Refresh(); |
cswiger | 0:1092494506a3 | 236 | pc.printf("Ignition on: %d\n\r",isIgnitionOn); |
cswiger | 0:1092494506a3 | 237 | pc.printf("Engine on: %d\n\r",isEngineOn); |
cswiger | 0:1092494506a3 | 238 | if(isIgnitionOn) { |
cswiger | 0:1092494506a3 | 239 | rled = 1; |
cswiger | 0:1092494506a3 | 240 | pc.printf("Get coolant temp...\n\r"); |
cswiger | 0:1092494506a3 | 241 | get_pid(COOLANT_TEMP,&coolant_temp); |
cswiger | 0:1092494506a3 | 242 | pc.printf("Get rpm...\n\r"); |
cswiger | 0:1092494506a3 | 243 | get_pid(ENGINE_RPM,&engine_rpm); |
cswiger | 0:1092494506a3 | 244 | get_pid(LOAD_VALUE,&engine_load); |
cswiger | 0:1092494506a3 | 245 | get_pid(VEHICLE_SPEED,&vehicle_speed); |
cswiger | 0:1092494506a3 | 246 | pc.printf("Coolant: %d\n\rRPM: %d\n\r",coolant_temp,engine_rpm); |
cswiger | 0:1092494506a3 | 247 | // short keys to keep air co$t$ down |
cswiger | 0:1092494506a3 | 248 | // oc or os ol - obd2 coolant temp, rpm, speed, load |
cswiger | 0:1092494506a3 | 249 | // la lo - gps latitude, longitude |
cswiger | 0:1092494506a3 | 250 | // ax ay - accelarometer X,Y |
cswiger | 0:1092494506a3 | 251 | pc.printf("Read sensors\n\r"); |
cswiger | 0:1092494506a3 | 252 | read_sensors(); |
cswiger | 0:1092494506a3 | 253 | pc.printf("GPS chars processed: %d\n\r",gps.charsProcessed()); |
cswiger | 0:1092494506a3 | 254 | if(gps.location.isValid()) { |
cswiger | 0:1092494506a3 | 255 | snprintf(data,256,"{\"la\":%f,\"lo\":%f,\"ax\":%s,\"ay\":%s,\"oc\":%d,\"or\":%d,\"ol\":%d,\"os\":%d}",gps.location.lat(),gps.location.lng(),SENSOR_DATA.AccelX,SENSOR_DATA.AccelY,coolant_temp,engine_rpm,engine_load,vehicle_speed); |
cswiger | 0:1092494506a3 | 256 | } else { |
cswiger | 0:1092494506a3 | 257 | snprintf(data,256,"{\"ax\":%s,\"ay\":%s,\"oc\":%d,\"or\":%d,\"ol\":%d,\"os\":%d}",SENSOR_DATA.AccelX,SENSOR_DATA.AccelY,coolant_temp,engine_rpm,engine_load,vehicle_speed); |
cswiger | 0:1092494506a3 | 258 | } |
cswiger | 0:1092494506a3 | 259 | ret = wnc.connect(); // check if still connected - it just returns 0 for ok and -1 for not connected |
cswiger | 0:1092494506a3 | 260 | if (ret) NVIC_SystemReset(); // so if not connected, reset! |
cswiger | 0:1092494506a3 | 261 | // publish json and wait for result |
cswiger | 0:1092494506a3 | 262 | pubnub_res rslt = pubnub_publish(pbp, "carpal2", data); |
cswiger | 0:1092494506a3 | 263 | if (rslt != PNR_STARTED) { |
cswiger | 0:1092494506a3 | 264 | pc.printf("Failed to start publishing, rslt=%d"CRLF, rslt); |
cswiger | 0:1092494506a3 | 265 | rled = 1; |
cswiger | 0:1092494506a3 | 266 | gled = 0; |
cswiger | 0:1092494506a3 | 267 | } else { |
cswiger | 0:1092494506a3 | 268 | rslt = pubnub_await(pbp); |
cswiger | 0:1092494506a3 | 269 | if (rslt != PNR_OK) { |
cswiger | 0:1092494506a3 | 270 | pc.printf("Failed to finished publishing, rslt=%d"CRLF, rslt); |
cswiger | 0:1092494506a3 | 271 | rled = 1; |
cswiger | 0:1092494506a3 | 272 | gled = 0; |
cswiger | 0:1092494506a3 | 273 | } else { |
cswiger | 0:1092494506a3 | 274 | pc.printf("Published! Response from Pubnub: %s"CRLF, pubnub_last_publish_result(pbp)); |
cswiger | 0:1092494506a3 | 275 | rled = 0; |
cswiger | 0:1092494506a3 | 276 | gled = 1; |
cswiger | 0:1092494506a3 | 277 | } |
cswiger | 0:1092494506a3 | 278 | } |
cswiger | 0:1092494506a3 | 279 | ret = wnc.connect(); // check if still connected - it just returns 0 for ok and -1 for not connected |
cswiger | 0:1092494506a3 | 280 | if (ret) NVIC_SystemReset(); // so if not connected, reset! |
cswiger | 0:1092494506a3 | 281 | rslt = pubnub_subscribe(pbp, "carpal2", 0); |
cswiger | 0:1092494506a3 | 282 | if (rslt != PNR_STARTED) { |
cswiger | 0:1092494506a3 | 283 | pc.printf("Failed to start subscribing, rslt=%d"CRLF, rslt); |
cswiger | 0:1092494506a3 | 284 | rled = 1; |
cswiger | 0:1092494506a3 | 285 | gled = 0; |
cswiger | 0:1092494506a3 | 286 | } else { |
cswiger | 0:1092494506a3 | 287 | rslt = pubnub_await(pbp); |
cswiger | 0:1092494506a3 | 288 | if (rslt != PNR_OK) { |
cswiger | 0:1092494506a3 | 289 | pc.printf("Failed to finished subscribing, rslt=%d"CRLF, rslt);\ |
cswiger | 0:1092494506a3 | 290 | rled = 1; |
cswiger | 0:1092494506a3 | 291 | gled = 0; |
cswiger | 0:1092494506a3 | 292 | } else { |
cswiger | 0:1092494506a3 | 293 | pc.printf("Subscribed! Received messages follow:"CRLF); |
cswiger | 0:1092494506a3 | 294 | while (char const *msg = pubnub_get(pbp)) { |
cswiger | 0:1092494506a3 | 295 | pc.printf("subscribe got: %s"CRLF, msg); |
cswiger | 0:1092494506a3 | 296 | parse_JSON(msg); |
cswiger | 0:1092494506a3 | 297 | } |
cswiger | 0:1092494506a3 | 298 | } |
cswiger | 0:1092494506a3 | 299 | rled = 0; |
cswiger | 0:1092494506a3 | 300 | gled = 1; |
cswiger | 0:1092494506a3 | 301 | } // subscribe result |
cswiger | 0:1092494506a3 | 302 | } else { |
cswiger | 0:1092494506a3 | 303 | rled = 1; |
cswiger | 0:1092494506a3 | 304 | gled = 0; |
cswiger | 0:1092494506a3 | 305 | pc.printf("Ignition off... attempting Init()\n\r"); |
cswiger | 0:1092494506a3 | 306 | Init(); |
cswiger | 0:1092494506a3 | 307 | } // end isIgnitionOn |
cswiger | 0:1092494506a3 | 308 | } // end timer expired |
cswiger | 0:1092494506a3 | 309 | } // end infinite loop |
cswiger | 0:1092494506a3 | 310 | } |
cswiger | 0:1092494506a3 | 311 | |
cswiger | 0:1092494506a3 | 312 | char pid_reslen[] = |
cswiger | 0:1092494506a3 | 313 | { |
cswiger | 0:1092494506a3 | 314 | // pid 0x00 to 0x1F |
cswiger | 0:1092494506a3 | 315 | 4,4,2,2,1,1,1,1,1,1,1,1,2,1,1,1, |
cswiger | 0:1092494506a3 | 316 | 2,1,1,1,2,2,2,2,2,2,2,2,1,1,1,4, |
cswiger | 0:1092494506a3 | 317 | |
cswiger | 0:1092494506a3 | 318 | // pid 0x20 to 0x3F |
cswiger | 0:1092494506a3 | 319 | 4,2,2,2,4,4,4,4,4,4,4,4,1,1,1,1, |
cswiger | 0:1092494506a3 | 320 | 1,2,2,1,4,4,4,4,4,4,4,4,2,2,2,2, |
cswiger | 0:1092494506a3 | 321 | |
cswiger | 0:1092494506a3 | 322 | // pid 0x40 to 0x4E |
cswiger | 0:1092494506a3 | 323 | 4,8,2,2,2,1,1,1,1,1,1,1,1,2,2 |
cswiger | 0:1092494506a3 | 324 | }; |
cswiger | 0:1092494506a3 | 325 | |
cswiger | 0:1092494506a3 | 326 | |
cswiger | 0:1092494506a3 | 327 | |
cswiger | 0:1092494506a3 | 328 | /*********************** |
cswiger | 0:1092494506a3 | 329 | Init |
cswiger | 0:1092494506a3 | 330 | ***********************/ |
cswiger | 0:1092494506a3 | 331 | char Init() |
cswiger | 0:1092494506a3 | 332 | { |
cswiger | 0:1092494506a3 | 333 | obd2.baud(OBDBAUD); |
cswiger | 0:1092494506a3 | 334 | char str[STRLEN]; |
cswiger | 0:1092494506a3 | 335 | // reset |
cswiger | 0:1092494506a3 | 336 | stn1110_command(str, "ATWS\r"); |
cswiger | 0:1092494506a3 | 337 | // turn echo off |
cswiger | 0:1092494506a3 | 338 | stn1110_command(str, "ATE0\r"); |
cswiger | 0:1092494506a3 | 339 | |
cswiger | 0:1092494506a3 | 340 | // send 01 00 until we are connected |
cswiger | 0:1092494506a3 | 341 | do |
cswiger | 0:1092494506a3 | 342 | { |
cswiger | 0:1092494506a3 | 343 | stn1110_command(str, "0100\r"); |
cswiger | 0:1092494506a3 | 344 | wait(1); // might want to change this to return non-zero for engine off or something, otherwise it's stuck here |
cswiger | 0:1092494506a3 | 345 | } |
cswiger | 0:1092494506a3 | 346 | while(stn1110_check_response("0100", str)!=0); |
cswiger | 0:1092494506a3 | 347 | |
cswiger | 0:1092494506a3 | 348 | // ask protocol |
cswiger | 0:1092494506a3 | 349 | stn1110_command(str, "ATDPN\r"); |
cswiger | 0:1092494506a3 | 350 | |
cswiger | 0:1092494506a3 | 351 | check_supported_pids(); |
cswiger | 0:1092494506a3 | 352 | |
cswiger | 0:1092494506a3 | 353 | return 0; |
cswiger | 0:1092494506a3 | 354 | } |
cswiger | 0:1092494506a3 | 355 | |
cswiger | 0:1092494506a3 | 356 | char stn1110_read(char *str, char size) |
cswiger | 0:1092494506a3 | 357 | { |
cswiger | 0:1092494506a3 | 358 | int b; |
cswiger | 0:1092494506a3 | 359 | char i=0; |
cswiger | 0:1092494506a3 | 360 | |
cswiger | 0:1092494506a3 | 361 | // wait for something on com port |
cswiger | 0:1092494506a3 | 362 | while((b=obd2.getcNb())!=PROMPT && i<size) // getcNb() does NOT block, |
cswiger | 0:1092494506a3 | 363 | { |
cswiger | 0:1092494506a3 | 364 | if(b>=' ') |
cswiger | 0:1092494506a3 | 365 | str[i++]=b; |
cswiger | 0:1092494506a3 | 366 | } |
cswiger | 0:1092494506a3 | 367 | |
cswiger | 0:1092494506a3 | 368 | if(i!=size) // we got a prompt |
cswiger | 0:1092494506a3 | 369 | { |
cswiger | 0:1092494506a3 | 370 | str[i]=NUL; // replace CR by NUL |
cswiger | 0:1092494506a3 | 371 | return PROMPT; |
cswiger | 0:1092494506a3 | 372 | } |
cswiger | 0:1092494506a3 | 373 | else |
cswiger | 0:1092494506a3 | 374 | return DATA; |
cswiger | 0:1092494506a3 | 375 | } |
cswiger | 0:1092494506a3 | 376 | |
cswiger | 0:1092494506a3 | 377 | void stn1110_write(char *str) |
cswiger | 0:1092494506a3 | 378 | { |
cswiger | 0:1092494506a3 | 379 | while(*str!=NUL) |
cswiger | 0:1092494506a3 | 380 | obd2.putc(*str++); |
cswiger | 0:1092494506a3 | 381 | } |
cswiger | 0:1092494506a3 | 382 | |
cswiger | 0:1092494506a3 | 383 | char stn1110_check_response(const char *cmd, char *str) |
cswiger | 0:1092494506a3 | 384 | { |
cswiger | 0:1092494506a3 | 385 | // cmd is something like "010D" |
cswiger | 0:1092494506a3 | 386 | // str should be "41 0D blabla" |
cswiger | 0:1092494506a3 | 387 | if(cmd[0]+4 != str[0] |
cswiger | 0:1092494506a3 | 388 | || cmd[1]!=str[1] |
cswiger | 0:1092494506a3 | 389 | || cmd[2]!=str[3] |
cswiger | 0:1092494506a3 | 390 | || cmd[3]!=str[4]) |
cswiger | 0:1092494506a3 | 391 | return 1; |
cswiger | 0:1092494506a3 | 392 | |
cswiger | 0:1092494506a3 | 393 | return 0; // no error |
cswiger | 0:1092494506a3 | 394 | } |
cswiger | 0:1092494506a3 | 395 | |
cswiger | 0:1092494506a3 | 396 | char stn1110_compact_response(char *buf, char *str) |
cswiger | 0:1092494506a3 | 397 | { |
cswiger | 0:1092494506a3 | 398 | char i=0; |
cswiger | 0:1092494506a3 | 399 | |
cswiger | 0:1092494506a3 | 400 | str+=6; |
cswiger | 0:1092494506a3 | 401 | while(*str!=NUL) |
cswiger | 0:1092494506a3 | 402 | buf[i++]=strtoul(str, &str, 16); // 16 = hex |
cswiger | 0:1092494506a3 | 403 | |
cswiger | 0:1092494506a3 | 404 | return i; |
cswiger | 0:1092494506a3 | 405 | } |
cswiger | 0:1092494506a3 | 406 | |
cswiger | 0:1092494506a3 | 407 | char stn1110_command(char *str, char *cmd) |
cswiger | 0:1092494506a3 | 408 | { |
cswiger | 0:1092494506a3 | 409 | strcpy(str, cmd); |
cswiger | 0:1092494506a3 | 410 | stn1110_write(str); |
cswiger | 0:1092494506a3 | 411 | return stn1110_read(str, STRLEN); |
cswiger | 0:1092494506a3 | 412 | } |
cswiger | 0:1092494506a3 | 413 | |
cswiger | 0:1092494506a3 | 414 | void check_supported_pids(void) |
cswiger | 0:1092494506a3 | 415 | { |
cswiger | 0:1092494506a3 | 416 | |
cswiger | 0:1092494506a3 | 417 | // on some ECU first PID read attemts some time fails, changed to 3 attempts |
cswiger | 0:1092494506a3 | 418 | for (char i=0; i<3; i++) |
cswiger | 0:1092494506a3 | 419 | { |
cswiger | 0:1092494506a3 | 420 | pid01to20_support = (get_pid(PID_SUPPORT00, &tempLong)) ? tempLong : 0; |
cswiger | 0:1092494506a3 | 421 | if (pid01to20_support) |
cswiger | 0:1092494506a3 | 422 | break; |
cswiger | 0:1092494506a3 | 423 | } |
cswiger | 0:1092494506a3 | 424 | |
cswiger | 0:1092494506a3 | 425 | if(is_pid_supported(PID_SUPPORT20)) |
cswiger | 0:1092494506a3 | 426 | if (get_pid(PID_SUPPORT20, &tempLong)) |
cswiger | 0:1092494506a3 | 427 | pid21to40_support = tempLong; |
cswiger | 0:1092494506a3 | 428 | |
cswiger | 0:1092494506a3 | 429 | if(is_pid_supported(PID_SUPPORT40)) |
cswiger | 0:1092494506a3 | 430 | if (get_pid(PID_SUPPORT40, &tempLong)) |
cswiger | 0:1092494506a3 | 431 | pid41to60_support = tempLong; |
cswiger | 0:1092494506a3 | 432 | } |
cswiger | 0:1092494506a3 | 433 | |
cswiger | 0:1092494506a3 | 434 | char is_pid_supported(char pid) |
cswiger | 0:1092494506a3 | 435 | { |
cswiger | 0:1092494506a3 | 436 | if(pid==0) |
cswiger | 0:1092494506a3 | 437 | return 1; |
cswiger | 0:1092494506a3 | 438 | else |
cswiger | 0:1092494506a3 | 439 | if(pid<=0x20) |
cswiger | 0:1092494506a3 | 440 | { |
cswiger | 0:1092494506a3 | 441 | if(1L<<(char)(0x20-pid) & pid01to20_support) |
cswiger | 0:1092494506a3 | 442 | return 1; |
cswiger | 0:1092494506a3 | 443 | } |
cswiger | 0:1092494506a3 | 444 | else |
cswiger | 0:1092494506a3 | 445 | if(pid<=0x40) |
cswiger | 0:1092494506a3 | 446 | { |
cswiger | 0:1092494506a3 | 447 | if(1L<<(char)(0x40-pid) & pid21to40_support) |
cswiger | 0:1092494506a3 | 448 | return 1; |
cswiger | 0:1092494506a3 | 449 | } |
cswiger | 0:1092494506a3 | 450 | else |
cswiger | 0:1092494506a3 | 451 | if(pid<=0x60) |
cswiger | 0:1092494506a3 | 452 | { |
cswiger | 0:1092494506a3 | 453 | if(1L<<(char)(0x60-pid) & pid41to60_support) |
cswiger | 0:1092494506a3 | 454 | return 1; |
cswiger | 0:1092494506a3 | 455 | } |
cswiger | 0:1092494506a3 | 456 | |
cswiger | 0:1092494506a3 | 457 | return 0; |
cswiger | 0:1092494506a3 | 458 | } |
cswiger | 0:1092494506a3 | 459 | |
cswiger | 0:1092494506a3 | 460 | |
cswiger | 0:1092494506a3 | 461 | char get_pid(char pid, long *ret) |
cswiger | 0:1092494506a3 | 462 | { |
cswiger | 0:1092494506a3 | 463 | char cmd_str[6]; // to send to STN1110 |
cswiger | 0:1092494506a3 | 464 | char str[STRLEN]; // to receive from STN1110 |
cswiger | 0:1092494506a3 | 465 | char i; |
cswiger | 0:1092494506a3 | 466 | char buf[10]; // to receive the result |
cswiger | 0:1092494506a3 | 467 | char reslen; |
cswiger | 0:1092494506a3 | 468 | |
cswiger | 0:1092494506a3 | 469 | // check if PID is supported |
cswiger | 0:1092494506a3 | 470 | if(!is_pid_supported(pid)) |
cswiger | 0:1092494506a3 | 471 | { |
cswiger | 0:1092494506a3 | 472 | // Not Supported |
cswiger | 0:1092494506a3 | 473 | return 0; |
cswiger | 0:1092494506a3 | 474 | } |
cswiger | 0:1092494506a3 | 475 | |
cswiger | 0:1092494506a3 | 476 | // receive length depends on pid |
cswiger | 0:1092494506a3 | 477 | reslen=pid_reslen[pid]; |
cswiger | 0:1092494506a3 | 478 | |
cswiger | 0:1092494506a3 | 479 | sprintf(cmd_str, "01%02X\r", pid); |
cswiger | 0:1092494506a3 | 480 | pc.printf("get_pid - stn1110_write\n\r"); |
cswiger | 0:1092494506a3 | 481 | stn1110_write(cmd_str); |
cswiger | 0:1092494506a3 | 482 | pc.printf("get_pid - stn1110_read\n\r"); |
cswiger | 0:1092494506a3 | 483 | stn1110_read(str, STRLEN); |
cswiger | 0:1092494506a3 | 484 | |
cswiger | 0:1092494506a3 | 485 | if(stn1110_check_response(cmd_str, str)!=0) |
cswiger | 0:1092494506a3 | 486 | { |
cswiger | 0:1092494506a3 | 487 | return 0; |
cswiger | 0:1092494506a3 | 488 | } |
cswiger | 0:1092494506a3 | 489 | stn1110_compact_response(buf, str); |
cswiger | 0:1092494506a3 | 490 | |
cswiger | 0:1092494506a3 | 491 | *ret=buf[0]*256U+buf[1]; |
cswiger | 0:1092494506a3 | 492 | |
cswiger | 0:1092494506a3 | 493 | // Calculate different for each PID |
cswiger | 0:1092494506a3 | 494 | switch(pid) |
cswiger | 0:1092494506a3 | 495 | { |
cswiger | 0:1092494506a3 | 496 | case ENGINE_RPM: |
cswiger | 0:1092494506a3 | 497 | *ret=*ret/4U; |
cswiger | 0:1092494506a3 | 498 | break; |
cswiger | 0:1092494506a3 | 499 | case MAF_AIR_FLOW: |
cswiger | 0:1092494506a3 | 500 | break; |
cswiger | 0:1092494506a3 | 501 | case VEHICLE_SPEED: |
cswiger | 0:1092494506a3 | 502 | *ret=buf[0]; |
cswiger | 0:1092494506a3 | 503 | break; |
cswiger | 0:1092494506a3 | 504 | case FUEL_STATUS: |
cswiger | 0:1092494506a3 | 505 | case LOAD_VALUE: |
cswiger | 0:1092494506a3 | 506 | case THROTTLE_POS: |
cswiger | 0:1092494506a3 | 507 | case REL_THR_POS: |
cswiger | 0:1092494506a3 | 508 | case EGR: |
cswiger | 0:1092494506a3 | 509 | case EGR_ERROR: |
cswiger | 0:1092494506a3 | 510 | case FUEL_LEVEL: |
cswiger | 0:1092494506a3 | 511 | case ABS_THR_POS_B: |
cswiger | 0:1092494506a3 | 512 | case ABS_THR_POS_C: |
cswiger | 0:1092494506a3 | 513 | case ACCEL_PEDAL_D: |
cswiger | 0:1092494506a3 | 514 | case ACCEL_PEDAL_E: |
cswiger | 0:1092494506a3 | 515 | case ACCEL_PEDAL_F: |
cswiger | 0:1092494506a3 | 516 | case CMD_THR_ACTU: |
cswiger | 0:1092494506a3 | 517 | *ret=(buf[0]*100U)/255U; |
cswiger | 0:1092494506a3 | 518 | break; |
cswiger | 0:1092494506a3 | 519 | case ABS_LOAD_VAL: |
cswiger | 0:1092494506a3 | 520 | *ret=(*ret*100)/255; |
cswiger | 0:1092494506a3 | 521 | break; |
cswiger | 0:1092494506a3 | 522 | case B1S1_O2_V: |
cswiger | 0:1092494506a3 | 523 | case B1S2_O2_V: |
cswiger | 0:1092494506a3 | 524 | case B1S3_O2_V: |
cswiger | 0:1092494506a3 | 525 | case B1S4_O2_V: |
cswiger | 0:1092494506a3 | 526 | case B2S1_O2_V: |
cswiger | 0:1092494506a3 | 527 | case B2S2_O2_V: |
cswiger | 0:1092494506a3 | 528 | case B2S3_O2_V: |
cswiger | 0:1092494506a3 | 529 | case B2S4_O2_V: |
cswiger | 0:1092494506a3 | 530 | case O2S1_WR_V: |
cswiger | 0:1092494506a3 | 531 | case O2S2_WR_V: |
cswiger | 0:1092494506a3 | 532 | case O2S3_WR_V: |
cswiger | 0:1092494506a3 | 533 | case O2S4_WR_V: |
cswiger | 0:1092494506a3 | 534 | case O2S5_WR_V: |
cswiger | 0:1092494506a3 | 535 | case O2S6_WR_V: |
cswiger | 0:1092494506a3 | 536 | case O2S7_WR_V: |
cswiger | 0:1092494506a3 | 537 | case O2S8_WR_V: |
cswiger | 0:1092494506a3 | 538 | case O2S1_WR_C: |
cswiger | 0:1092494506a3 | 539 | case O2S2_WR_C: |
cswiger | 0:1092494506a3 | 540 | case O2S3_WR_C: |
cswiger | 0:1092494506a3 | 541 | case O2S4_WR_C: |
cswiger | 0:1092494506a3 | 542 | case O2S5_WR_C: |
cswiger | 0:1092494506a3 | 543 | case O2S6_WR_C: |
cswiger | 0:1092494506a3 | 544 | case O2S7_WR_C: |
cswiger | 0:1092494506a3 | 545 | case O2S8_WR_C: |
cswiger | 0:1092494506a3 | 546 | case CMD_EQUIV_R: |
cswiger | 0:1092494506a3 | 547 | case DIST_MIL_ON: |
cswiger | 0:1092494506a3 | 548 | case DIST_MIL_CLR: |
cswiger | 0:1092494506a3 | 549 | case TIME_MIL_ON: |
cswiger | 0:1092494506a3 | 550 | case TIME_MIL_CLR: |
cswiger | 0:1092494506a3 | 551 | case INT_AIR_TEMP: |
cswiger | 0:1092494506a3 | 552 | case AMBIENT_TEMP: |
cswiger | 0:1092494506a3 | 553 | case CAT_TEMP_B1S1: |
cswiger | 0:1092494506a3 | 554 | case CAT_TEMP_B2S1: |
cswiger | 0:1092494506a3 | 555 | case CAT_TEMP_B1S2: |
cswiger | 0:1092494506a3 | 556 | case CAT_TEMP_B2S2: |
cswiger | 0:1092494506a3 | 557 | case STFT_BANK1: |
cswiger | 0:1092494506a3 | 558 | case LTFT_BANK1: |
cswiger | 0:1092494506a3 | 559 | case STFT_BANK2: |
cswiger | 0:1092494506a3 | 560 | case LTFT_BANK2: |
cswiger | 0:1092494506a3 | 561 | case FUEL_PRESSURE: |
cswiger | 0:1092494506a3 | 562 | case MAN_PRESSURE: |
cswiger | 0:1092494506a3 | 563 | case BARO_PRESSURE: |
cswiger | 0:1092494506a3 | 564 | *ret=buf[0]; |
cswiger | 0:1092494506a3 | 565 | if(pid==FUEL_PRESSURE) |
cswiger | 0:1092494506a3 | 566 | *ret*=3U; |
cswiger | 0:1092494506a3 | 567 | break; |
cswiger | 0:1092494506a3 | 568 | case COOLANT_TEMP: |
cswiger | 0:1092494506a3 | 569 | *ret=buf[0]-40U; |
cswiger | 0:1092494506a3 | 570 | break; |
cswiger | 0:1092494506a3 | 571 | case EVAP_PRESSURE: |
cswiger | 0:1092494506a3 | 572 | *ret=((int)buf[0]*256+buf[1])/4; |
cswiger | 0:1092494506a3 | 573 | break; |
cswiger | 0:1092494506a3 | 574 | case TIMING_ADV: |
cswiger | 0:1092494506a3 | 575 | *ret=(buf[0]/2)-64; |
cswiger | 0:1092494506a3 | 576 | break; |
cswiger | 0:1092494506a3 | 577 | case CTRL_MOD_V: |
cswiger | 0:1092494506a3 | 578 | break; |
cswiger | 0:1092494506a3 | 579 | case RUNTIME_START: |
cswiger | 0:1092494506a3 | 580 | break; |
cswiger | 0:1092494506a3 | 581 | case OBD_STD: |
cswiger | 0:1092494506a3 | 582 | *ret=buf[0]; |
cswiger | 0:1092494506a3 | 583 | break; |
cswiger | 0:1092494506a3 | 584 | default: |
cswiger | 0:1092494506a3 | 585 | *ret=0; |
cswiger | 0:1092494506a3 | 586 | for(i=0; i<reslen; i++) |
cswiger | 0:1092494506a3 | 587 | { |
cswiger | 0:1092494506a3 | 588 | *ret*=256L; |
cswiger | 0:1092494506a3 | 589 | *ret+=buf[i]; |
cswiger | 0:1092494506a3 | 590 | } |
cswiger | 0:1092494506a3 | 591 | break; |
cswiger | 0:1092494506a3 | 592 | } |
cswiger | 0:1092494506a3 | 593 | |
cswiger | 0:1092494506a3 | 594 | return 1; |
cswiger | 0:1092494506a3 | 595 | } |
cswiger | 0:1092494506a3 | 596 | |
cswiger | 0:1092494506a3 | 597 | |
cswiger | 0:1092494506a3 | 598 | char verifyECUAlive() |
cswiger | 0:1092494506a3 | 599 | { |
cswiger | 0:1092494506a3 | 600 | char cmd_str[6]; |
cswiger | 0:1092494506a3 | 601 | char str[STRLEN]; |
cswiger | 0:1092494506a3 | 602 | sprintf(cmd_str, "01%02X\r", ENGINE_RPM); |
cswiger | 0:1092494506a3 | 603 | stn1110_write(cmd_str); |
cswiger | 0:1092494506a3 | 604 | stn1110_read(str, STRLEN); |
cswiger | 0:1092494506a3 | 605 | |
cswiger | 0:1092494506a3 | 606 | if(stn1110_check_response(cmd_str, str) == 0) |
cswiger | 0:1092494506a3 | 607 | { |
cswiger | 0:1092494506a3 | 608 | return 1; |
cswiger | 0:1092494506a3 | 609 | } |
cswiger | 0:1092494506a3 | 610 | else |
cswiger | 0:1092494506a3 | 611 | { |
cswiger | 0:1092494506a3 | 612 | return 0; |
cswiger | 0:1092494506a3 | 613 | } |
cswiger | 0:1092494506a3 | 614 | } |
cswiger | 0:1092494506a3 | 615 | |
cswiger | 0:1092494506a3 | 616 | |
cswiger | 0:1092494506a3 | 617 | char Refresh() |
cswiger | 0:1092494506a3 | 618 | { |
cswiger | 0:1092494506a3 | 619 | //debug |
cswiger | 0:1092494506a3 | 620 | isIgnitionOn = verifyECUAlive(); |
cswiger | 0:1092494506a3 | 621 | |
cswiger | 0:1092494506a3 | 622 | //If ignition is on, check for engine |
cswiger | 0:1092494506a3 | 623 | if (isIgnitionOn) { |
cswiger | 0:1092494506a3 | 624 | isEngineOn = (get_pid(ENGINE_RPM, &engineRPM) && engineRPM > 0) ? 1 : 0; |
cswiger | 0:1092494506a3 | 625 | } else { // else engine must be off |
cswiger | 0:1092494506a3 | 626 | isEngineOn = 0; |
cswiger | 0:1092494506a3 | 627 | } |
cswiger | 0:1092494506a3 | 628 | return 0; |
cswiger | 0:1092494506a3 | 629 | } |
cswiger | 0:1092494506a3 | 630 | |
cswiger | 0:1092494506a3 | 631 | |
cswiger | 0:1092494506a3 | 632 | |
cswiger | 0:1092494506a3 | 633 | bool dtc_clear(void) |
cswiger | 0:1092494506a3 | 634 | { |
cswiger | 0:1092494506a3 | 635 | char cmd_answer[DTC_BUFFER]=""; |
cswiger | 0:1092494506a3 | 636 | |
cswiger | 0:1092494506a3 | 637 | stn1110_write("04\r"); |
cswiger | 0:1092494506a3 | 638 | stn1110_read(cmd_answer,DTC_BUFFER); |
cswiger | 0:1092494506a3 | 639 | strip_answer(cmd_answer); |
cswiger | 0:1092494506a3 | 640 | |
cswiger | 0:1092494506a3 | 641 | if (strcmp(cmd_answer, "44")!=0) |
cswiger | 0:1092494506a3 | 642 | { |
cswiger | 0:1092494506a3 | 643 | return false; |
cswiger | 0:1092494506a3 | 644 | } else |
cswiger | 0:1092494506a3 | 645 | { |
cswiger | 0:1092494506a3 | 646 | has_dtc=false; |
cswiger | 0:1092494506a3 | 647 | return true; |
cswiger | 0:1092494506a3 | 648 | } |
cswiger | 0:1092494506a3 | 649 | } |
cswiger | 0:1092494506a3 | 650 | |
cswiger | 0:1092494506a3 | 651 | |
cswiger | 0:1092494506a3 | 652 | bool dtc_read(void) |
cswiger | 0:1092494506a3 | 653 | { |
cswiger | 0:1092494506a3 | 654 | char cmd_answer[DTC_BUFFER]=""; |
cswiger | 0:1092494506a3 | 655 | has_dtc=false; |
cswiger | 0:1092494506a3 | 656 | |
cswiger | 0:1092494506a3 | 657 | stn1110_write("03\r"); |
cswiger | 0:1092494506a3 | 658 | stn1110_read(cmd_answer,DTC_BUFFER); |
cswiger | 0:1092494506a3 | 659 | |
cswiger | 0:1092494506a3 | 660 | for (char i=0;i<MAX_DTC_READ;i++) |
cswiger | 0:1092494506a3 | 661 | { |
cswiger | 0:1092494506a3 | 662 | strcpy(DTC[i].code,""); |
cswiger | 0:1092494506a3 | 663 | } |
cswiger | 0:1092494506a3 | 664 | |
cswiger | 0:1092494506a3 | 665 | strip_answer(cmd_answer); |
cswiger | 0:1092494506a3 | 666 | |
cswiger | 0:1092494506a3 | 667 | if (strstr(cmd_answer, "NODATA")) |
cswiger | 0:1092494506a3 | 668 | { |
cswiger | 0:1092494506a3 | 669 | //No errors |
cswiger | 0:1092494506a3 | 670 | return true; |
cswiger | 0:1092494506a3 | 671 | } |
cswiger | 0:1092494506a3 | 672 | |
cswiger | 0:1092494506a3 | 673 | if (strncmp(cmd_answer, "43", 2)!=0) |
cswiger | 0:1092494506a3 | 674 | { |
cswiger | 0:1092494506a3 | 675 | //ERROR: Incorrect answer |
cswiger | 0:1092494506a3 | 676 | return false; |
cswiger | 0:1092494506a3 | 677 | } |
cswiger | 0:1092494506a3 | 678 | |
cswiger | 0:1092494506a3 | 679 | char *ss=cmd_answer+2; |
cswiger | 0:1092494506a3 | 680 | char dtclen=0; |
cswiger | 0:1092494506a3 | 681 | |
cswiger | 0:1092494506a3 | 682 | while (strlen(ss) >= 4) |
cswiger | 0:1092494506a3 | 683 | { |
cswiger | 0:1092494506a3 | 684 | const char *prefix[16]= |
cswiger | 0:1092494506a3 | 685 | { |
cswiger | 0:1092494506a3 | 686 | "P0", "P1", "P2", "P3", |
cswiger | 0:1092494506a3 | 687 | "C0", "C1", "C2", "C3", |
cswiger | 0:1092494506a3 | 688 | "B0", "B1", "B2", "B3", |
cswiger | 0:1092494506a3 | 689 | "U0", "U1", "U2", "U3", |
cswiger | 0:1092494506a3 | 690 | }; |
cswiger | 0:1092494506a3 | 691 | uint8_t p=0; |
cswiger | 0:1092494506a3 | 692 | if ( ((*ss)>='0') && ((*ss)<='9') ) p=(*ss)-'0'; else |
cswiger | 0:1092494506a3 | 693 | if ( ((*ss)>='A') && ((*ss)<='F') ) p=(*ss)-'A'+10; else |
cswiger | 0:1092494506a3 | 694 | if ( ((*ss)>='a') && ((*ss)<='f') ) p=(*ss)-'a'+10; |
cswiger | 0:1092494506a3 | 695 | char code[6]; |
cswiger | 0:1092494506a3 | 696 | strcpy(code, prefix[p]); |
cswiger | 0:1092494506a3 | 697 | code[2]=ss[1]; |
cswiger | 0:1092494506a3 | 698 | code[3]=ss[2]; |
cswiger | 0:1092494506a3 | 699 | code[4]=ss[3]; |
cswiger | 0:1092494506a3 | 700 | code[5]=0; |
cswiger | 0:1092494506a3 | 701 | |
cswiger | 0:1092494506a3 | 702 | if (strcmp(code, "P0000")!=0) |
cswiger | 0:1092494506a3 | 703 | { |
cswiger | 0:1092494506a3 | 704 | strcpy(DTC[dtclen].code,code); |
cswiger | 0:1092494506a3 | 705 | has_dtc=true; |
cswiger | 0:1092494506a3 | 706 | dtclen++; |
cswiger | 0:1092494506a3 | 707 | } |
cswiger | 0:1092494506a3 | 708 | ss+=4; |
cswiger | 0:1092494506a3 | 709 | } |
cswiger | 0:1092494506a3 | 710 | |
cswiger | 0:1092494506a3 | 711 | return true; |
cswiger | 0:1092494506a3 | 712 | } |
cswiger | 0:1092494506a3 | 713 | |
cswiger | 0:1092494506a3 | 714 | char *strip_answer(char *s) |
cswiger | 0:1092494506a3 | 715 | { |
cswiger | 0:1092494506a3 | 716 | char *ss; |
cswiger | 0:1092494506a3 | 717 | for (ss=s; *s; s++) |
cswiger | 0:1092494506a3 | 718 | { |
cswiger | 0:1092494506a3 | 719 | if ( ((*s)!=' ') && ((*s)!='\t') && ((*s)!='\n') && ((*s)!='\r') ) |
cswiger | 0:1092494506a3 | 720 | (*ss++)=(*s); |
cswiger | 0:1092494506a3 | 721 | } |
cswiger | 0:1092494506a3 | 722 | (*ss)=0; |
cswiger | 0:1092494506a3 | 723 | |
cswiger | 0:1092494506a3 | 724 | return s; |
cswiger | 0:1092494506a3 | 725 | } |
cswiger | 0:1092494506a3 | 726 |