1 files changed, 92 insertions, 0 deletions
diff --git a/lpo/lpo.ino b/lpo/lpo.ino
new file mode 100644
index 0000000..85a595b
--- /dev/null
+++ b/lpo/lpo.ino
@@ -0,0 +1,92 @@
+/*************************************************************************
+* Inpired from simple_obd_display.ino by Stanley Huang <stanley@freematics.com.au>
+* (Simple OBD Data Display for Freematics OBD-II Adapter)
+*************************************************************************/
+#include "MyLCD.h"
+#include "MyOBD.h"
+
+MyLCD lcd;
+MyOBD obd;
+
+void setup()
+{
+  lcd.begin();
+  lcd.print("obd.begin()");
+  obd.begin();
+  lcd.println(" done");
+  reconnect();
+  showDTC();
+  initScreen();
+}
+
+void reconnect()
+{
+  lcd.setCursor(1,2);
+  lcd.print("obd.init()                       ");
+  lcd.setCursor(11,2);
+  while (!obd.init()) {
+    lcd.print(".");
+  }
+  lcd.println(" done");
+}
+
+void showDTC() {
+  static char buf[10];
+  static uint16_t codes[16];
+  byte c, count = obd.readDTC(codes, 16);
+  lcd.print("DTC: ");
+  if ( count==0 ) {
+    lcd.println("none");
+  } else {
+    for (c=0; c<count; c++) {
+        sprintf(buf, "P%04u ", codes[c]);
+        lcd.print(buf);
+    }
+    lcd.println();
+  }
+}
+
+void loop()
+{
+  static byte pids[]= {PID_RPM, PID_SPEED, PID_ENGINE_LOAD, PID_THROTTLE};
+  static byte index = 0, prev_err=obd.errors;
+  byte pid = pids[index];
+  int value;
+  // send a query to OBD adapter for specified OBD-II pid
+  if (obd.readPID(pid, value)) {
+      showData(pid, value);
+  }
+  index = (index + 1) % sizeof(pids);
+
+  if (obd.errors > prev_err) {
+      showData(0, obd.errors);
+  }
+  if ( obd.errors > 10 ) {
+    obd.errors=0;
+    reconnect();
+  }
+}
+
+void initScreen()
+{
+  lcd.setCursor(1,4);
+  lcd.println("comm. errors:       ");
+  lcd.println("         rpm:       ");
+  lcd.println("       speed:        km/h");
+  lcd.println("       throt:        %");
+  lcd.println("        load:       ");
+  showData(0, obd.errors);
+}
+
+void showData(byte pid, int value)
+{
+  static char buf[10];
+  int res = sprintf(buf, "%6d", value);
+  switch (pid) {
+  case 0:               lcd.setCursor(15,4); lcd.print(buf); break;
+  case PID_RPM:         lcd.setCursor(15,5); lcd.print(buf); break;
+  case PID_SPEED:       lcd.setCursor(15,6); lcd.print(buf); break;
+  case PID_THROTTLE:    lcd.setCursor(15,7); lcd.print(buf); break;
+  case PID_ENGINE_LOAD: lcd.setCursor(15,8); lcd.print(buf); break;
+  }
+}