Display sensor readings (acc/gyro/compass) and battery readings on OLED

screen.

src/main/common/printf.c

@@ -170,6 +170,7 @@ static void putcp(void *p, char c)
 void tfp_sprintf(char *s, char *fmt, ...)
 {
     va_list va;
+
     va_start(va, fmt);
     tfp_format(&s, putcp, fmt, va);
     putcp(&s, 0);

src/main/drivers/display_ug2864hsweg01.c

@@ -26,6 +26,11 @@
 
 #include "display_ug2864hsweg01.h"
 
+#define INVERSE_CHAR_FORMAT 0x7f // 0b01111111
+#define NORMAL_CHAR_FORMAT  0x00 // 0b00000000
+
+unsigned char CHAR_FORMAT = NORMAL_CHAR_FORMAT;
+
 static const uint8_t const multiWiiFont[][5] = { // Refer to "Times New Roman" Font Database... 5 x 7 font
         { 0x00, 0x00, 0x00, 0x00, 0x00 }, { 0x00, 0x00, 0x4F, 0x00, 0x00 }, //   (  1)  ! - 0x0021 Exclamation Mark
                 { 0x00, 0x07, 0x00, 0x07, 0x00 }, //   (  2)  " - 0x0022 Quotation Mark
@@ -169,41 +174,47 @@ static void i2c_OLED_send_byte(uint8_t val)
 
 void i2c_OLED_clear_display(void)
 {
-    i2c_OLED_send_cmd(0xa6);              //Set Normal Display
+    i2c_OLED_send_cmd(0xa6);              // Set Normal Display
     i2c_OLED_send_cmd(0xae);              // Display OFF
     i2c_OLED_send_cmd(0x20);              // Set Memory Addressing Mode
     i2c_OLED_send_cmd(0x00);              // Set Memory Addressing Mode to Horizontal addressing mode
     i2c_OLED_send_cmd(0xb0);              // set page address to 0
-    i2c_OLED_send_cmd(0X40);              // Display start line register to 0
+    i2c_OLED_send_cmd(0x40);              // Display start line register to 0
     i2c_OLED_send_cmd(0);                 // Set low col address to 0
     i2c_OLED_send_cmd(0x10);              // Set high col address to 0
-    for(uint16_t i=0; i<1024; i++) {           // fill the display's RAM with graphic... 128*64 pixel picture
+    for(uint16_t i=0; i<1024; i++) {      // fill the display's RAM with graphic... 128*64 pixel picture
-        i2c_OLED_send_byte(0);  // clear
+        i2c_OLED_send_byte(0x00);  // clear
     }
     i2c_OLED_send_cmd(0x81);              // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
     i2c_OLED_send_cmd(200);               // Here you can set the brightness 1 = dull, 255 is very bright
     i2c_OLED_send_cmd(0xaf);              // display on
 }
-void i2c_OLED_set_XY(uint8_t col, uint8_t row)
+
+void i2c_OLED_clear_display_quick(void)
+{
+    i2c_OLED_send_cmd(0xb0);              // set page address to 0
+    i2c_OLED_send_cmd(0x40);              // Display start line register to 0
+    i2c_OLED_send_cmd(0);                 // Set low col address to 0
+    i2c_OLED_send_cmd(0x10);              // Set high col address to 0
+    for(uint16_t i=0; i<1024; i++) {      // fill the display's RAM with graphic... 128*64 pixel picture
+        i2c_OLED_send_byte(0x00);  // clear
+    }
+}
+
+void i2c_OLED_set_xy(uint8_t col, uint8_t row)
 {
-    //  Not used in MW V2.0 but its here anyway!
     i2c_OLED_send_cmd(0xb0 + row);                      //set page address
-    i2c_OLED_send_cmd(0x00 + (8 * col & 0x0f));         //set low col address
+    i2c_OLED_send_cmd(0x00 + ((CHARACTER_WIDTH_TOTAL * col) & 0x0f));         //set low col address
-    i2c_OLED_send_cmd(0x10 + ((8 * col >> 4) & 0x0f));  //set high col address
+    i2c_OLED_send_cmd(0x10 + (((CHARACTER_WIDTH_TOTAL * col) >> 4) & 0x0f));  //set high col address
 }
 
 void i2c_OLED_set_line(uint8_t row)
 {
-    // goto the beginning of a single row, compatible with LCD_CONFIG
     i2c_OLED_send_cmd(0xb0 + row); //set page address
     i2c_OLED_send_cmd(0);          //set low col address
     i2c_OLED_send_cmd(0x10);       //set high col address
 }
 
-unsigned char CHAR_FORMAT = 0;      // use to INVERSE characters
-// use INVERSE    CHAR_FORMAT = 0b01111111;
-// use NORMAL     CHAR_FORMAT = 0;
-
 void i2c_OLED_send_char(unsigned char ascii)
 {
     unsigned char i;
@@ -219,15 +230,8 @@ void i2c_OLED_send_char(unsigned char ascii)
 void i2c_OLED_send_string(const char *string)
 {
     // Sends a string of chars until null terminator
-    unsigned char i = 0;
-    uint8_t buffer;
     while (*string) {
-        for (i = 0; i < 5; i++) {
+        i2c_OLED_send_char(*string);
-            buffer = multiWiiFont[(*string) - 32][i];
-            buffer ^= CHAR_FORMAT;
-            i2c_OLED_send_byte((unsigned char) buffer);
-        }
-        i2c_OLED_send_byte(CHAR_FORMAT);    // the gap
         string++;
     }
 }
@@ -238,6 +242,9 @@ void ug2864hsweg01InitI2C(void)
     i2c_OLED_send_cmd(0xa4);          //SET All pixels OFF
 //  i2c_OLED_send_cmd(0xa5);            //SET ALL pixels ON
     delay(50);
+
+//    i2c_OLED_send_cmd(0x8D); // charge pump
+//    i2c_OLED_send_cmd(0x14); // enable
     i2c_OLED_send_cmd(0x20);            //Set Memory Addressing Mode
     i2c_OLED_send_cmd(0x02); //Set Memory Addressing Mode to Page addressing mode(RESET)
 //  i2c_OLED_send_cmd(0xa0);      //colum address 0 mapped to SEG0 (POR)*** wires at bottom

src/main/drivers/display_ug2864hsweg01.h

@@ -17,11 +17,26 @@
 
 #pragma once
 
+#define SCREEN_WIDTH 128
+#define SCREEN_HEIGHT 64
+
+#define FONT_WIDTH 5
+#define FONT_HEIGHT 7
+#define HORIZONTAL_PADDING 1
+#define VERTICAL_PADDING 1
+
+#define CHARACTER_WIDTH_TOTAL (FONT_WIDTH + HORIZONTAL_PADDING)
+#define CHARACTER_HEIGHT_TOTAL (FONT_HEIGHT + VERTICAL_PADDING)
+
+#define SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_WIDTH / CHARACTER_WIDTH_TOTAL)
+#define SCREEN_CHARACTER_ROW_COUNT (SCREEN_HEIGHT / CHARACTER_HEIGHT_TOTAL)
+
 void ug2864hsweg01InitI2C(void);
 
-void i2c_OLED_set_XY(uint8_t col, uint8_t row);
+void i2c_OLED_set_xy(uint8_t col, uint8_t row);
 void i2c_OLED_set_line(uint8_t row);
 void i2c_OLED_send_char(unsigned char ascii);
 void i2c_OLED_send_string(const char *string);
 void i2c_OLED_clear_display(void);
+void i2c_OLED_clear_display_quick(void);
 

src/main/io/display.c

@@ -15,37 +15,157 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "stdbool.h"
+#include <stdbool.h>
-#include "stdint.h"
+#include <stdint.h>
+#include <stdarg.h>
+#include <string.h>
 
 #include "platform.h"
 
 #include "build_config.h"
 
+#include "drivers/serial.h"
+#include "common/printf.h"
+
 #ifdef DISPLAY
 
 #include "drivers/system.h"
 #include "drivers/display_ug2864hsweg01.h"
 
+#include "sensors/battery.h"
+
+#include "common/axis.h"
+#include "flight/flight.h"
+#include "sensors/sensors.h"
+#include "sensors/compass.h"
+
+#include "config/runtime_config.h"
+
 #include "display.h"
 
-#define TEST_I2C_DISPLAY
+#define MILLISECONDS_IN_A_SECOND (1000 * 1000)
 
-#ifdef TEST_I2C_DISPLAY
+#define DISPLAY_UPDATE_FREQUENCY (MILLISECONDS_IN_A_SECOND / 10)
-static const char *messages[] = {
+#define PAGE_CYCLE_FREQUENCY (MILLISECONDS_IN_A_SECOND * 5)
-        "CLEANFLIGHT",
-        " DISPLAY   ",
-        "  TESTING  "
-};
-static uint8_t messageIndex = 0;
-static uint8_t messageCount = 3;
-static uint8_t rowIndex = 0;
-static uint8_t rowCount = 8;
-#endif
-
-#define DISPLAY_STRIP_10HZ ((1000 * 1000) / 10)
 
 uint32_t nextDisplayUpdateAt = 0;
+uint32_t nextPageAt = 0;
+
+char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT];
+
+typedef enum {
+    PAGE_BATTERY,
+    PAGE_SENSORS
+} pageId_e;
+
+const char* pageTitles[] = {
+    "BATTERY",
+    "SENSORS"
+};
+
+#define PAGE_COUNT (PAGE_SENSORS + 1)
+
+static const char* tickerCharacters = "|/-\\";
+#define TICKER_CHARACTER_COUNT (sizeof(tickerCharacters) / sizeof(char))
+
+typedef struct pageState_s {
+    bool pageChanging;
+    pageId_e pageId;
+} pageState_t;
+
+static pageState_t pageState;
+
+void LCDprint(uint8_t i) {
+   i2c_OLED_send_char(i);
+}
+
+// LCDbar(n,v) : draw a bar graph - n number of chars for width, v value in % to display
+void drawHorizonalPercentageBar(uint8_t width,uint8_t percent) {
+    uint8_t i, j;
+
+    if (percent > 100)
+        percent = 100;
+
+    j = (width * percent) / 100;
+
+    for (i = 0; i < j; i++)
+        LCDprint(159); // full
+
+    if (j < width)
+        LCDprint(154 + (percent * width * 5 / 100 - 5 * j)); // partial fill
+
+    for (i = j + 1; i < width; i++)
+        LCDprint(154); // empty
+}
+
+#if 0
+void fillScreenWithCharacters()
+{
+    for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
+        for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
+            i2c_OLED_set_xy(column, row);
+            i2c_OLED_send_char('A' + column);
+        }
+    }
+}
+#endif
+
+void updateTicker(void)
+{
+    static uint8_t tickerIndex = 0;
+    i2c_OLED_set_xy(SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
+    i2c_OLED_send_char(tickerCharacters[tickerIndex]);
+    tickerIndex++;
+    tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
+}
+
+void showTitle()
+{
+    i2c_OLED_set_line(0);
+    i2c_OLED_send_string(pageTitles[pageState.pageId]);
+}
+
+void handlePageChange(void)
+{
+    i2c_OLED_clear_display_quick();
+    showTitle();
+}
+
+void showBatteryPage(void)
+{
+    tfp_sprintf(lineBuffer, "volts: %d.%d, cells: %d", vbat / 10, vbat % 10, batteryCellCount);
+    i2c_OLED_set_line(1);
+    i2c_OLED_send_string(lineBuffer);
+
+    uint32_t batteryPercentage = calculateBatteryPercentage();
+    i2c_OLED_set_line(2);
+    drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
+}
+
+void showSensorsPage(void)
+{
+    uint8_t rowIndex = 1;
+
+    i2c_OLED_set_line(rowIndex++);
+    i2c_OLED_send_string(   "         X    Y    Z");
+    if (sensors(SENSOR_ACC)) {
+        tfp_sprintf(lineBuffer, "Acc : %4d %4d %4d", accSmooth[X], accSmooth[Y], accSmooth[Z]);
+        i2c_OLED_set_line(rowIndex++);
+        i2c_OLED_send_string(lineBuffer);
+    }
+    if (sensors(SENSOR_GYRO)) {
+        tfp_sprintf(lineBuffer, "Gryo: %4d %4d %4d", gyroADC[X], gyroADC[Y], gyroADC[Z]);
+        i2c_OLED_set_line(rowIndex++);
+        i2c_OLED_send_string(lineBuffer);
+    }
+#ifdef MAG
+    if (sensors(SENSOR_MAG)) {
+        tfp_sprintf(lineBuffer, "Comp: %4d %4d %4d", magADC[X], magADC[Y], magADC[Z]);
+        i2c_OLED_set_line(rowIndex++);
+        i2c_OLED_send_string(lineBuffer);
+    }
+    #endif
+}
 
 void updateDisplay(void)
 {
@@ -56,14 +176,32 @@ void updateDisplay(void)
         return;
     }
 
-    nextDisplayUpdateAt = now + DISPLAY_STRIP_10HZ;
+    nextDisplayUpdateAt = now + DISPLAY_UPDATE_FREQUENCY;
 
-#ifdef TEST_I2C_DISPLAY
+    if (ARMING_FLAG(ARMED)) {
-    i2c_OLED_set_line(rowIndex++);
+        return;
-    i2c_OLED_send_string(messages[messageIndex++]);
+    }
-    messageIndex = messageIndex % messageCount;
+
-    rowIndex = rowIndex % rowCount;
+    pageState.pageChanging = (int32_t)(now - nextPageAt) >= 0L;
-#endif
+    if (pageState.pageChanging) {
+        nextPageAt = now + PAGE_CYCLE_FREQUENCY;
+        pageState.pageId++;
+        pageState.pageId = pageState.pageId % PAGE_COUNT;
+
+        handlePageChange();
+    }
+
+
+    switch(pageState.pageId) {
+        case PAGE_BATTERY:
+            showBatteryPage();
+            break;
+        case PAGE_SENSORS:
+            showSensorsPage();
+            break;
+    }
+
+    updateTicker();
 }
 
 
@@ -71,6 +209,7 @@ void displayInit(void)
 {
     delay(20);
     ug2864hsweg01InitI2C();
+    memset(&pageState, 0, sizeof(pageState));
 }
 
 #endif

src/main/sensors/battery.c

@@ -109,3 +109,8 @@ void updateCurrentMeter(int32_t lastUpdateAt)
 	mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
 	mAhDrawn = mAhdrawnRaw / (3600 * 100);
 }
+
+uint32_t calculateBatteryPercentage(void)
+{
+    return ((uint32_t)(vbat) * 100) / (batteryConfig->vbatmaxcellvoltage * batteryCellCount);
+}

src/main/sensors/battery.h

@@ -42,3 +42,5 @@ void batteryInit(batteryConfig_t *initialBatteryConfig);
 
 void updateCurrentMeter(int32_t lastUpdateAt);
 int32_t currentMeterToCentiamps(uint16_t src);
+
+uint32_t calculateBatteryPercentage(void);