Working OLED in test mode.

Makefile

@@ -190,6 +190,7 @@ HIGHEND_SRC  = flight/autotune.c \
 		   flight/gps_conversion.c \
 		   io/gps.c \
 		   io/ledstrip.c \
+		   io/display.c \
 		   telemetry/telemetry.c \
 		   telemetry/frsky.c \
 		   telemetry/hott.c \
@@ -231,6 +232,7 @@ NAZE_SRC	 = startup_stm32f10x_md_gcc.S \
 
 EUSTM32F103RC_SRC	 = startup_stm32f10x_hd_gcc.S \
 		   drivers/accgyro_mpu6050.c \
+		   drivers/accgyro_l3g4200d.c \
 		   drivers/adc.c \
 		   drivers/adc_stm32f10x.c \
 		   drivers/barometer_bmp085.c \

src/main/drivers/display_ug2864hsweg01.c

@@ -27,8 +27,7 @@
 #include "display_ug2864hsweg01.h"
 
 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, 0x00, 0x00, 0x00 }, { 0x00, 0x00, 0x4F, 0x00, 0x00 }, //   (  1)  ! - 0x0021 Exclamation Mark
-  { 0x00,0x00,0x4F,0x00,0x00}, //   (  1)  ! - 0x0021 Exclamation Mark
                 { 0x00, 0x07, 0x00, 0x07, 0x00 }, //   (  2)  " - 0x0022 Quotation Mark
                 { 0x14, 0x7F, 0x14, 0x7F, 0x14 }, //   (  3)  # - 0x0023 Number Sign
                 { 0x24, 0x2A, 0x7F, 0x2A, 0x12 }, //   (  4)  $ - 0x0024 Dollar Sign
@@ -158,22 +157,44 @@ static const uint8_t const multiWiiFont[][5] = { // Refer to "Times New Roman" F
         };
 
 #define OLED_address   0x3C     // OLED at address 0x3C in 7bit
-
+void i2c_OLED_send_cmd(uint8_t command)
-void i2c_OLED_send_cmd(uint8_t command) {
+{
     i2cWrite(OLED_address, 0x80, command);
 }
 
-void i2c_OLED_send_byte(uint8_t val) {
+static void i2c_OLED_send_byte(uint8_t val)
+{
     i2cWrite(OLED_address, 0x40, val);
 }
 
-void i2c_OLED_set_XY(uint8_t col, uint8_t row) {        //  Not used in MW V2.0 but its here anyway!
+void i2c_OLED_clear_display(void)
+{
+    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(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(0);  // 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)
+{
+    //  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(0x10 + ((8 * col >> 4) & 0x0f));  //set high col address
 }
 
-void i2c_OLED_set_line(uint8_t row) {   // goto the beginning of a single row, compattible with LCD_CONFIG
+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
@@ -183,7 +204,8 @@ 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){
+void i2c_OLED_send_char(unsigned char ascii)
+{
     unsigned char i;
     uint8_t buffer;
     for (i = 0; i < 5; i++) {
@@ -194,7 +216,8 @@ void i2c_OLED_send_char(unsigned char ascii){
     i2c_OLED_send_byte(CHAR_FORMAT);    // the gap
 }
 
-void i2c_OLED_send_string(const char *string){
+void i2c_OLED_send_string(const char *string)
+{
     // Sends a string of chars until null terminator
     unsigned char i = 0;
     uint8_t buffer;
@@ -205,13 +228,12 @@ void i2c_OLED_send_string(const char *string){
             i2c_OLED_send_byte((unsigned char) buffer);
         }
         i2c_OLED_send_byte(CHAR_FORMAT);    // the gap
-    *string++;
+        string++;
     }
 }
 
-
+void ug2864hsweg01InitI2C(void)
-
+{
-void ug2864hsweg01InitI2C(void){
     i2c_OLED_send_cmd(0xae);    //display off
     i2c_OLED_send_cmd(0xa4);          //SET All pixels OFF
 //  i2c_OLED_send_cmd(0xa5);            //SET ALL pixels ON
@@ -241,5 +263,6 @@ void ug2864hsweg01InitI2C(void){
     delay(20);
     i2c_OLED_send_cmd(0xaf);          //display on
     delay(20);
+    i2c_OLED_clear_display();
 }
 

src/main/drivers/display_ug2864hsweg01.h

@@ -19,5 +19,9 @@
 
 void ug2864hsweg01InitI2C(void);
 
+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);
+

src/main/io/display.c

@@ -0,0 +1,72 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "stdbool.h"
+#include "stdint.h"
+
+#include "platform.h"
+
+#include "build_config.h"
+
+#include "drivers/system.h"
+#include "drivers/display_ug2864hsweg01.h"
+
+#include "display.h"
+
+#define TEST_I2C_DISPLAY
+
+#ifdef TEST_I2C_DISPLAY
+static const char *messages[] = {
+        "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;
+
+void updateDisplay(void)
+{
+    uint32_t now = micros();
+
+    bool updateNow = (int32_t)(now - nextDisplayUpdateAt) >= 0L;
+    if (!updateNow) {
+        return;
+    }
+
+    nextDisplayUpdateAt = now + DISPLAY_STRIP_10HZ;
+
+#ifdef TEST_I2C_DISPLAY
+    i2c_OLED_set_line(rowIndex++);
+    i2c_OLED_send_string(messages[messageIndex++]);
+    messageIndex = messageIndex % messageCount;
+    rowIndex = rowIndex % rowCount;
+#endif
+}
+
+
+void displayInit(void)
+{
+    delay(20);
+    ug2864hsweg01InitI2C();
+}

src/main/io/display.h

@@ -0,0 +1,19 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+void updateDisplay(void);
+

src/main/main.c

@@ -37,7 +37,6 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/pwm_rx.h"
 #include "drivers/adc.h"
-#include "drivers/display_ug2864hsweg01.h"
 
 #include "flight/flight.h"
 #include "flight/mixer.h"
@@ -117,10 +116,6 @@ void productionDebug(void)
 }
 #endif
 
-void displayInit(void)
-{
-    ug2864hsweg01InitI2C();
-}
 void init(void)
 {
     uint8_t i;
@@ -144,6 +139,8 @@ void init(void)
 
     initBoardAlignment(&masterConfig.boardAlignment);
 
+    displayInit();
+
     // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
     sensorsSet(SENSORS_SET);
     // drop out any sensors that don't seem to work, init all the others. halt if gyro is dead.
@@ -300,13 +297,6 @@ int main(void) {
     init();
 
     while (1) {
-#if 1
-        delay(200);
-        displayInit();
-        delay(100);
-        i2c_OLED_set_line(0);
-        i2c_OLED_send_string("TEST");
-#endif
         loop();
         processLoopback();
     }

src/main/mw.c

@@ -44,6 +44,7 @@
 #include "sensors/gyro.h"
 #include "sensors/battery.h"
 #include "io/beeper.h"
+#include "io/display.h"
 #include "io/escservo.h"
 #include "flight/altitudehold.h"
 #include "flight/failsafe.h"
@@ -380,10 +381,11 @@ typedef enum {
 #ifdef SONAR
     UPDATE_SONAR_TASK,
 #endif
-	UPDATE_GPS_TASK
+    UPDATE_GPS_TASK,
+    UPDATE_DISPLAY_TASK
 } periodicTasks;
 
-#define PERIODIC_TASK_COUNT (UPDATE_GPS_TASK + 1)
+#define PERIODIC_TASK_COUNT (UPDATE_DISPLAY_TASK + 1)
 
 
 void executePeriodicTasks(void)
@@ -430,6 +432,9 @@ void executePeriodicTasks(void)
         }
         break;
 #endif
+    case UPDATE_DISPLAY_TASK:
+        updateDisplay();
+        break;
     }
 
     if (periodicTaskIndex >= PERIODIC_TASK_COUNT) {

src/main/sensors/initialisation.c

@@ -19,6 +19,9 @@
 #include <string.h>
 
 #include "platform.h"
+
+#include "build_config.h"
+
 #include "common/axis.h"
 
 #include "drivers/accgyro.h"
@@ -112,6 +115,7 @@
 #ifdef EUSTM32F103RC
 #define USE_FAKE_GYRO
 #define USE_FAKE_ACC
+#define USE_GYRO_L3G4200D
 #endif
 
 #ifdef STM32F3DISCOVERY
@@ -165,11 +169,16 @@ extern acc_t acc;
 
 #ifdef USE_FAKE_GYRO
 static void fakeGyroInit(void) {}
-static void fakeGyroRead(int16_t *gyroData) {}
+static void fakeGyroRead(int16_t *gyroData) {
-static void fakeGyroReadTemp(int16_t *tempData) {}
+    UNUSED(gyroData);
+}
+static void fakeGyroReadTemp(int16_t *tempData) {
+    UNUSED(tempData);
+}
 
 bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
 {
+    UNUSED(lpf);
     gyro->init = fakeGyroInit;
     gyro->read = fakeGyroRead;
     gyro->temperature = fakeGyroReadTemp;
@@ -179,7 +188,9 @@ bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
 
 #ifdef USE_FAKE_ACC
 static void fakeAccInit(void) {}
-static void fakeAccRead(int16_t *accData) {}
+static void fakeAccRead(int16_t *accData) {
+    UNUSED(accData);
+}
 
 bool fakeAccDetect(acc_t *acc)
 {