2 files changed, 53 insertions, 8 deletions

diff --git a/obdlogger/MultiLCD.cpp b/obdlogger/MultiLCD.cpp
index 22112d1..c611b14 100644
--- a/obdlogger/MultiLCD.cpp
+++ b/obdlogger/MultiLCD.cpp
@@ -118,12 +118,19 @@ void LCD_OLED::write(char c)
 {
     char s[2] = {c};
     ScI2cMxDisplay8x16Str(OLED_ADDRESS, m_line, m_column, s);
-    m_column = (m_column + 8) & 0x7f;
+    m_column += 8;
+    if (m_column >= 128) {
+        m_column = 0;
+        m_line++;
+    }
 }
 
 void LCD_OLED::print(const char* s)
 {
     ScI2cMxDisplay8x16Str(OLED_ADDRESS, m_line, m_column, s);
+    m_column += (strlen(s) << 3);
+    m_line += (m_column >> 7) << 1;
+    m_column %= 0x7f;
 }
 
 void LCD_OLED::printLarge(const char* s)
@@ -137,7 +144,11 @@ void LCD_OLED::printLarge(const char* s)
         } else {
             ScI2cMxFillArea(OLED_ADDRESS, m_line, m_line + 1, m_column, m_column + 16, 0);
         }
-        m_column = (m_column + 16) & 0x7f;
+        m_column += 16;
+        if (m_column >= 128) {
+            m_column = 0;
+            m_line++;
+        }
         s++;
     }
 }
diff --git a/obdlogger/MultiLCD.h b/obdlogger/MultiLCD.h
index 2a10356..b8adfeb 100644
--- a/obdlogger/MultiLCD.h
+++ b/obdlogger/MultiLCD.h
@@ -3,23 +3,42 @@ extern const PROGMEM unsigned char font5x8[][5];
 
 #include "PCD8544.h"
 
-class LCD_PCD8544 : public PCD8544 {
+class LCD_Common
+{
 public:
+    virtual void backlight(bool on) {}
+    virtual byte getLines() = 0;
+    virtual byte getCols() = 0;
+};
+
+class LCD_PCD8544 : public LCD_Common, public PCD8544
+{
+public:
+    byte getLines() { return 6; }
+    byte getCols() { return 14; }
     void printLarge(const char* s);
     void backlight(bool on)
     {
         pinMode(7, OUTPUT);
         digitalWrite(7, on ? HIGH : LOW);
     }
+    void clearLine(byte line)
+    {
+        setCursor(0, line);
+        for (byte i = 14; i > 0; i--) write(' ');
+    }
 };
 
 #include "ZtLib.h"
 
 #define OLED_ADDRESS 0x27
 
-class LCD_OLED : public ZtLib {
+class LCD_OLED : public LCD_Common, public ZtLib
+{
 public:
-    void setCursor(unsigned char column, unsigned char line)
+    byte getLines() { return 4; }
+    byte getCols() { return 16; }
+    void setCursor(byte column, byte line)
     {
         m_column = column << 3;
         m_line = line << 1;
@@ -30,15 +49,30 @@ public:
     void clear();
     void begin();
     void backlight(bool on) {}
+    void clearLine(byte line)
+    {
+        setCursor(0, line);
+        for (byte i = 16; i > 0; i--) write(' ');
+    }
 private:
     unsigned char m_column;
     unsigned char m_line;
 };
 
 #include "LCD4Bit_mod.h"
-class LCD_1602 : public LCD4Bit_mod {
+class LCD_1602 : public LCD_Common, public LCD4Bit_mod
+{
 public:
-    void printLarge(const char* s) { print(s); }
-    void backlight(bool on) {}
+    byte getLines() { return 2; }
+    byte getCols() { return 16; }
+    void printLarge(const char* s)
+    {
+        print(s);
+    }
+    void clearLine(byte line)
+    {
+        setCursor(0, line);
+        for (byte i = 16; i > 0; i--) write(' ');
+    }
 };