From ed4115c4e36efdbe99b34c6f7db8510c9abb005d Mon Sep 17 00:00:00 2001 From: kozinalexey Date: Fri, 30 Sep 2016 11:21:28 +0400 Subject: [PATCH] AP_Notify: Support for OLED display by Alexey Kozin --- libraries/AP_Notify/AP_Notify.cpp | 32 ++- libraries/AP_Notify/AP_Notify.h | 27 ++- libraries/AP_Notify/Display.cpp | 245 ++++++++++++++------ libraries/AP_Notify/Display.h | 29 +-- libraries/AP_Notify/Display_OLED_I2C.cpp | 43 ++++ libraries/AP_Notify/Display_OLED_I2C.h | 27 +++ libraries/AP_Notify/Display_SH1106_I2C.cpp | 135 +++++++++++ libraries/AP_Notify/Display_SH1106_I2C.h | 28 +++ libraries/AP_Notify/Display_SSD1306_I2C.cpp | 69 ++++-- libraries/AP_Notify/Display_SSD1306_I2C.h | 17 +- 10 files changed, 533 insertions(+), 119 deletions(-) create mode 100644 libraries/AP_Notify/Display_OLED_I2C.cpp create mode 100644 libraries/AP_Notify/Display_OLED_I2C.h create mode 100644 libraries/AP_Notify/Display_SH1106_I2C.cpp create mode 100644 libraries/AP_Notify/Display_SH1106_I2C.h diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index a1f03781be..ccb746c31a 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -17,7 +17,7 @@ #include "AP_BoardLED.h" #include "Buzzer.h" -#include "Display_SSD1306_I2C.h" +#include "Display_OLED_I2C.h" #include "ExternalLED.h" #include "NavioLED_I2C.h" #include "OreoLED_PX4.h" @@ -31,6 +31,8 @@ #include "VRBoard_LED.h" #include "DiscreteRGBLed.h" #include "DiscoLED.h" +#include // for strncpy + // table of user settable parameters const AP_Param::GroupInfo AP_Notify::var_info[] = { @@ -55,7 +57,14 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = { // @Values: 0:Disable,1:Enable // @User: Advanced AP_GROUPINFO("LED_OVERRIDE", 2, AP_Notify, _rgb_led_override, 0), - + + // @Param: DISPLAY_TYPE + // @DisplayName: Type of on-board I2C display + // @Description: This sets up the type of on-board I2C display. Disabled by default. + // @Values: 0:Disable,1:ssd1306,2:sh1106 + // @User: Advanced + AP_GROUPINFO("DISPLAY_TYPE", 3, AP_Notify, _display_type, 0), + AP_GROUPEND }; @@ -66,12 +75,14 @@ AP_Notify::AP_Notify() } // static flags, to allow for direct class update from device drivers -struct AP_Notify::notify_flags_type AP_Notify::flags; +struct AP_Notify::notify_flags_and_values_type AP_Notify::flags; struct AP_Notify::notify_events_type AP_Notify::events; +char AP_Notify::_send_text[NOTIFY_TEXT_BUFFER_SIZE] {}; #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 AP_BoardLED boardled; ToshibaLED_PX4 toshibaled; + Display_OLED_I2C display; #if AP_NOTIFY_SOLO_TONES == 1 ToneAlarm_PX4_Solo tonealarm; @@ -81,9 +92,9 @@ struct AP_Notify::notify_events_type AP_Notify::events; #if AP_NOTIFY_OREOLED == 1 OreoLED_PX4 oreoled; - NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &tonealarm, &oreoled}; + NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &tonealarm, &oreoled, &display}; #else - NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &tonealarm}; + NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &tonealarm, &display}; #endif #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN @@ -108,7 +119,7 @@ struct AP_Notify::notify_events_type AP_Notify::events; NotifyDevice *AP_Notify::_devices[] = {&navioled, &toshibaled}; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI Buzzer buzzer; - Display_SSD1306_I2C display; + Display_OLED_I2C display; NotifyDevice *AP_Notify::_devices[] = {&display, &buzzer}; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT ToshibaLED_I2C toshibaled; @@ -150,6 +161,9 @@ void AP_Notify::init(bool enable_external_leds) memset(&AP_Notify::flags, 0, sizeof(AP_Notify::flags)); memset(&AP_Notify::events, 0, sizeof(AP_Notify::events)); + // clear text buffer + memset(_send_text, 0, sizeof(_send_text)); + AP_Notify::flags.external_leds = enable_external_leds; for (uint8_t i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { @@ -184,3 +198,9 @@ void AP_Notify::handle_play_tune(mavlink_message_t *msg) _devices[i]->handle_play_tune(msg); } } + +void AP_Notify::send_text(const char *str) +{ + strncpy(_send_text, str, sizeof(_send_text)); + _send_text[sizeof(_send_text)-1] = 0; +} diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 80ee4f9afc..eb5f637726 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -37,18 +37,28 @@ #define BUZZER_ON 1 #define BUZZER_OFF 0 +#define NOTIFY_TEXT_BUFFER_SIZE 51 + +//Type of on-board display +#define DISPLAY_OFF 0 +#define DISPLAY_SSD1306 1 +#define DISPLAY_SH1106 2 + class AP_Notify { - friend class RGBLed; // RGBLed needs access to notify parameters + friend class RGBLed; // RGBLed needs access to notify parameters + friend class Display_OLED_I2C; // Display_OLED_I2C needs access to notify parameters public: // Constructor AP_Notify(); /// notify_flags_type - bitmask of notification flags - struct notify_flags_type { + struct notify_flags_and_values_type { uint32_t initialising : 1; // 1 if initialising and copter should not be moved uint32_t gps_status : 3; // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock uint32_t gps_num_sats : 6; // number of sats + float battery_voltage ; // battery voltage + uint32_t flight_mode : 8; // flight mode uint32_t armed : 1; // 0 = disarmed, 1 = armed uint32_t pre_arm_check : 1; // 0 = failing checks, 1 = passed uint32_t pre_arm_gps_check : 1; // 0 = failing pre-arm GPS checks, 1 = passed @@ -91,9 +101,9 @@ public: uint32_t tune_error : 1; // tuning controller error }; - // the notify flags are static to allow direct class access - // without declaring the object - static struct notify_flags_type flags; + // The notify flags and values are static to allow direct class access + // without declaring the object. + static struct notify_flags_and_values_type flags; static struct notify_events_type events; // initialisation @@ -111,10 +121,17 @@ public: static const struct AP_Param::GroupInfo var_info[]; bool buzzer_enabled() const { return _buzzer_enable; } + + static void send_text(const char *str); + static char* get_text() { return _send_text; } + private: static NotifyDevice* _devices[]; + static char _send_text[NOTIFY_TEXT_BUFFER_SIZE]; AP_Int8 _rgb_led_brightness; AP_Int8 _rgb_led_override; AP_Int8 _buzzer_enable; + AP_Int8 _display_type; + }; diff --git a/libraries/AP_Notify/Display.cpp b/libraries/AP_Notify/Display.cpp index c3102e29f5..a69bb52078 100644 --- a/libraries/AP_Notify/Display.cpp +++ b/libraries/AP_Notify/Display.cpp @@ -18,6 +18,7 @@ #include "AP_Notify.h" +#include // for snprintf #include static const uint8_t _font[] = { @@ -279,9 +280,82 @@ static const uint8_t _font[] = { 0x00, 0x00, 0x00, 0x00, 0x00 }; +#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) +static const char * _modename[] = { + "STAB", // STABILIZE = 0, // manual airframe angle with manual throttle + "ACRO", // ACRO = 1, // manual body-frame angular rate with manual throttle + "ALTH", // ALT_HOLD = 2, // manual airframe angle with automatic throttle + "AUTO", // AUTO = 3, // fully automatic waypoint control using mission commands + "GUID", // GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands + "LOIT", // LOITER = 5, // automatic horizontal acceleration with automatic throttle + "RTL ", // RTL = 6, // automatic return to launching point + "CIRC", // CIRCLE = 7, // automatic circular flight with automatic throttle + "----", //8 no mode for copter + "LAND", // LAND = 9, // automatic landing with horizontal position control + "----", //10 no mode for copter + "DRIF", // DRIFT = 11, // semi-automous position, yaw and throttle control + "----", //12 no mode for copter + "SPRT", // SPORT = 13, // manual earth-frame angular rate control with manual throttle + "FLIP", // FLIP = 14, // automatically flip the vehicle on the roll axis + "ATUN", // AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains + "PHLD", // POSHOLD = 16, // automatic position hold with manual override, with automatic throttle + "BRAK", // BRAKE = 17, // full-brake using inertial/GPS system, no pilot input + "THRW", // THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input + "AVOI", // AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft + "GNGP" // GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude + +}; +#elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) +static const char * _modename[] = { + "MANU", // = 0, + "CIRC", // = 1, + "STAB", // = 2, + "TRAN", // = 3, + "ACRO", // = 4, + "FBWA", // = 5, + "FBWB", // = 6, + "CRUS", // = 7, + "ATUN", // = 8, + "----", //9 no mode for plane + "AUTO", // = 10, + "RTL ", // = 11, + "LOIT", // = 12, + "----", //13 no mode for plane + "----", //14 no mode for plane + "GUID", // = 15, + "INIT", // = 16, + "QSTB", // = 17, + "QHVR", // = 18, + "QLIT", // = 19, + "QLND", // = 20, + "QRTL" // = 21 +}; +#else //rover +static const char * _modename[] = { + "MANU", + "----", + "LERN", + "STER", + "HOLD", + "----", + "----", + "----", + "----", + "----", + "AUTO", + "RTL", + "----", + "----", + "----", + "GUID", + "INIT" +}; +#endif + bool Display::init(void) { - memset(&_flags, 0, sizeof(_flags)); + _mstartpos = 0; // ticker shift position + _movedelay = 4; // ticker delay before shifting after new message displayed _healthy = hw_init(); @@ -299,6 +373,7 @@ bool Display::init(void) void Display::update() { static uint8_t timer = 0; + static uint8_t screenpage =0; // return immediately if not enabled if (!_healthy) { @@ -312,46 +387,37 @@ void Display::update() timer = 0; - // check if status has changed - if (_flags.armed != AP_Notify::flags.armed) { - update_arm(); - _flags.armed = AP_Notify::flags.armed; + if (AP_Notify::flags.armed) { + if (screenpage != 1) { + clear_screen(); + update_arm(3); + screenpage = 1; + hw_update(); //update hw once , do not transmition to display in fly + } + return; } - if (_flags.pre_arm_check != AP_Notify::flags.pre_arm_check) { - update_prearm(); - _flags.pre_arm_check = AP_Notify::flags.pre_arm_check; + + if (screenpage != 2) { + clear_screen(); //once clear screen when page changed + screenpage = 2; } - if (_flags.gps_status != AP_Notify::flags.gps_status) { - update_gps(); - _flags.gps_status = AP_Notify::flags.gps_status; - } + update_all(); + hw_update(); //update at 2 Hz in disarmed mode - if (_flags.gps_num_sats != AP_Notify::flags.gps_num_sats) { - update_gps_sats(); - _flags.gps_num_sats = AP_Notify::flags.gps_num_sats; - } - - if (_flags.ekf_bad != AP_Notify::flags.ekf_bad) { - update_ekf(); - _flags.ekf_bad = AP_Notify::flags.ekf_bad; - } - - // if somethings has changed, update display - if (_need_update) { - hw_update(); - _need_update = false; - } } void Display::update_all() { - update_arm(); - update_prearm(); - update_gps(); - update_gps_sats(); - update_ekf(); + + update_text(0); + update_mode(1); + update_battery(2); + update_gps(3); + //update_gps_sats(4); + update_prearm(4); + update_ekf(5); } void Display::draw_text(uint16_t x, uint16_t y, const char* c) @@ -387,66 +453,109 @@ void Display::draw_char(uint16_t x, uint16_t y, const char c) line >>= 1; } } - - // update display - _need_update = true; } -void Display::update_arm() +void Display::update_arm(uint8_t r) { if (AP_Notify::flags.armed) { - draw_text(COLUMN(0), ROW(0), ">>>>> ARMED! <<<<<"); + draw_text(COLUMN(0), ROW(r), ">>>>> ARMED! <<<<<"); } else { - draw_text(COLUMN(0), ROW(0), " disarmed "); + draw_text(COLUMN(0), ROW(r), " disarmed "); } } -void Display::update_prearm() +void Display::update_prearm(uint8_t r) { if (AP_Notify::flags.pre_arm_check) { - draw_text(COLUMN(0), ROW(2), "Prearm: passed "); + draw_text(COLUMN(0), ROW(r), "Prearm: passed "); } else { - draw_text(COLUMN(0), ROW(2), "Prearm: failed "); + draw_text(COLUMN(0), ROW(r), "Prearm: failed "); } } -void Display::update_gps() +void Display::update_gps(uint8_t r) { - switch (AP_Notify::flags.gps_status) { - case AP_GPS::NO_GPS: - draw_text(COLUMN(0), ROW(3), "GPS: no GPS"); - break; - case AP_GPS::NO_FIX: - draw_text(COLUMN(0), ROW(3), "GPS: no fix"); - break; - case AP_GPS::GPS_OK_FIX_2D: - draw_text(COLUMN(0), ROW(3), "GPS: 2D "); - break; - case AP_GPS::GPS_OK_FIX_3D: - draw_text(COLUMN(0), ROW(3), "GPS: 3D "); - break; - case AP_GPS::GPS_OK_FIX_3D_DGPS: - draw_text(COLUMN(0), ROW(3), "GPS: DGPS "); - break; - case AP_GPS::GPS_OK_FIX_3D_RTK: - draw_text(COLUMN(0), ROW(3), "GPS: RTK "); - break; + static const char * gpsfixname[] = {"Other", "NoGPS","NoFix","2D ","3D ","DGPS " ,"RTK "}; + char msg [DISPLAY_MESSAGE_SIZE]; + const char * fixname; + switch (AP_Notify::flags.gps_status) { + case AP_GPS::NO_GPS: + fixname = gpsfixname[1]; + break; + case AP_GPS::NO_FIX: + fixname = gpsfixname[2]; + break; + case AP_GPS::GPS_OK_FIX_2D: + fixname = gpsfixname[3]; + break; + case AP_GPS::GPS_OK_FIX_3D: + fixname = gpsfixname[4]; + break; + case AP_GPS::GPS_OK_FIX_3D_DGPS: + fixname = gpsfixname[5]; + break; + case AP_GPS::GPS_OK_FIX_3D_RTK: + fixname = gpsfixname[6]; + break; + default: + fixname = gpsfixname[0]; } + snprintf(msg, DISPLAY_MESSAGE_SIZE, "GPS:%s Sats:%u", fixname, AP_Notify::flags.gps_num_sats) ; + draw_text(COLUMN(0), ROW(r), msg); } -void Display::update_gps_sats() +void Display::update_gps_sats(uint8_t r) { - draw_text(COLUMN(0), ROW(4), "Sats:"); - draw_char(COLUMN(8), ROW(4), (AP_Notify::flags.gps_num_sats / 10) + 48); - draw_char(COLUMN(9), ROW(4), (AP_Notify::flags.gps_num_sats % 10) + 48); + draw_text(COLUMN(0), ROW(r), "Sats:"); + draw_char(COLUMN(8), ROW(r), (AP_Notify::flags.gps_num_sats / 10) + 48); + draw_char(COLUMN(9), ROW(r), (AP_Notify::flags.gps_num_sats % 10) + 48); } -void Display::update_ekf() +void Display::update_ekf(uint8_t r) { if (AP_Notify::flags.ekf_bad) { - draw_text(COLUMN(0), ROW(5), "EKF: fail"); + draw_text(COLUMN(0), ROW(r), "EKF: fail"); } else { - draw_text(COLUMN(0), ROW(5), "EKF: ok "); + draw_text(COLUMN(0), ROW(r), "EKF: ok "); } } +void Display::update_battery(uint8_t r) +{ + char msg [DISPLAY_MESSAGE_SIZE]; + snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT1: %4.2fV", AP_Notify::flags.battery_voltage) ; + draw_text(COLUMN(0), ROW(r), msg); + } +void Display::update_mode(uint8_t r) +{ + char msg [DISPLAY_MESSAGE_SIZE]; + snprintf(msg, DISPLAY_MESSAGE_SIZE, "Mode: %s", _modename[AP_Notify::flags.flight_mode]) ; + draw_text(COLUMN(0), ROW(r), msg); +} + +void Display::update_text(uint8_t r) +{ + char msg [DISPLAY_MESSAGE_SIZE]; + char txt [NOTIFY_TEXT_BUFFER_SIZE]; + + snprintf(txt, NOTIFY_TEXT_BUFFER_SIZE, "%s", AP_Notify::get_text()); + _mstartpos++; + for (uint8_t i = 0; i < sizeof(msg); i++) { + if (txt[i + _mstartpos - 1] != 0) { + msg[i] = txt[i + _mstartpos - 1]; + } else { + msg[i] = ' '; + _movedelay = 4; + _mstartpos = 0; + } + } + + if (_mstartpos > sizeof(txt) - sizeof(msg)) { + _mstartpos = 0; + } + if (_movedelay > 0) { + _movedelay--; + _mstartpos = 0; + } + draw_text(COLUMN(0), ROW(0), msg); + } diff --git a/libraries/AP_Notify/Display.h b/libraries/AP_Notify/Display.h index 0172fd8212..4a16f05b2c 100644 --- a/libraries/AP_Notify/Display.h +++ b/libraries/AP_Notify/Display.h @@ -2,8 +2,10 @@ #include "NotifyDevice.h" -#define ROW(Y) ((Y * 11) + 2) -#define COLUMN(X) ((X * 7) + 1) +#define ROW(Y) ((Y * 10) + 6) +#define COLUMN(X) ((X * 7) + 0) + +#define DISPLAY_MESSAGE_SIZE 18 class Display: public NotifyDevice { public: @@ -15,26 +17,25 @@ protected: virtual bool hw_update() = 0; virtual bool set_pixel(uint16_t x, uint16_t y) = 0; virtual bool clear_pixel(uint16_t x, uint16_t y) = 0; + virtual bool clear_screen() = 0; private: void draw_char(uint16_t x, uint16_t y, const char c); void draw_text(uint16_t x, uint16_t y, const char *c); void update_all(); - void update_arm(); - void update_prearm(); - void update_gps(); - void update_gps_sats(); - void update_ekf(); + void update_arm(uint8_t r); + void update_prearm(uint8_t r); + void update_gps(uint8_t r); + void update_gps_sats(uint8_t r); + void update_ekf(uint8_t r); + void update_battery(uint8_t r); + void update_mode(uint8_t r); + void update_text(uint8_t r); bool _healthy; bool _need_update; - struct display_state { - uint32_t gps_status : 3; // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock - uint32_t gps_num_sats : 6; // number of sats - uint32_t armed : 1; // 0 = disarmed, 1 = armed - uint32_t pre_arm_check : 1; // 0 = failing checks, 1 = passed - uint32_t ekf_bad : 1; // 1 if ekf is reporting problems - } _flags; + uint8_t _mstartpos; + uint8_t _movedelay; }; diff --git a/libraries/AP_Notify/Display_OLED_I2C.cpp b/libraries/AP_Notify/Display_OLED_I2C.cpp new file mode 100644 index 0000000000..6bb40cad6c --- /dev/null +++ b/libraries/AP_Notify/Display_OLED_I2C.cpp @@ -0,0 +1,43 @@ +/* + This program 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. + + This program 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 this program. If not, see . + */ +#include "Display_OLED_I2C.h" +#include "Display_SH1106_I2C.h" +#include "Display_SSD1306_I2C.h" + +#include + +#include +#include +#include "AP_Notify.h" + +Display_OLED_I2C* Display_OLED_I2C::getInstance() +{ + if (nullptr == _instance.get()) { + switch (pNotify->_display_type) { + case DISPLAY_SSD1306: + _instance = new Display_SSD1306_I2C(); + break; + + case DISPLAY_SH1106: + _instance = new Display_SH1106_I2C(); + break; + + case DISPLAY_OFF: + default: + break; + } + } + return _instance.get(); +} \ No newline at end of file diff --git a/libraries/AP_Notify/Display_OLED_I2C.h b/libraries/AP_Notify/Display_OLED_I2C.h new file mode 100644 index 0000000000..fc564ea886 --- /dev/null +++ b/libraries/AP_Notify/Display_OLED_I2C.h @@ -0,0 +1,27 @@ +#pragma once + +#include "Display.h" + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI +#define OLED_I2C_BUS 2 +#else +#define OLED_I2C_BUS 1 +#endif + +class Display_OLED_I2C: public Display { + +protected: + virtual bool hw_init() { return hasInstance() && getInstance()->hw_init(); } + virtual bool hw_update() { return hasInstance() && getInstance()->hw_update(); } + virtual bool set_pixel(uint16_t x, uint16_t y) { return hasInstance() && getInstance()->set_pixel(x, y); } + virtual bool clear_pixel(uint16_t x, uint16_t y) { return hasInstance() && getInstance()->clear_pixel(x, y); } + virtual bool clear_screen() { return hasInstance() && getInstance()->clear_screen(); } + + virtual void _update_timer() {}; + +private: + bool hasInstance() { return getInstance() != nullptr; } + + AP_HAL::OwnPtr _instance; + Display_OLED_I2C* getInstance(); +}; diff --git a/libraries/AP_Notify/Display_SH1106_I2C.cpp b/libraries/AP_Notify/Display_SH1106_I2C.cpp new file mode 100644 index 0000000000..071df8a1d0 --- /dev/null +++ b/libraries/AP_Notify/Display_SH1106_I2C.cpp @@ -0,0 +1,135 @@ +/* + This program 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. + + This program 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 this program. If not, see . + */ +#include "Display_SH1106_I2C.h" + +#include + +#include +#include + +#define SH1106_I2C_ADDR 0x3C // default I2C bus address + +static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); + +bool Display_SH1106_I2C::hw_init() +{ + struct { + uint8_t reg; + uint8_t seq[26]; + } init_seq = { 0x0, { + 0xAE, // Display OFF + 0xA1, // Segment re-map + 0xC8, // COM Output Scan Direction + 0xA8, 0x3F, // MUX Ratio + 0xD5, 0x50, // Display Clock Divide Ratio and Oscillator Frequency: (== +0%) + 0xD3, 0x00, // Display Offset + 0xDB, 0x40, // VCOMH Deselect Level + 0x81, 0xCF, // Contrast Control + 0xAD, 0x8B, // DC-DC Control Mode: 1b (== internal DC-DC enabled) (AKA: Enable charge pump regulator) + 0x40, // Display Start Line + 0xDA, 0x12, // +++ COM Pins hardware configuration + 0xD9, 0xF1, // +++ Pre-charge Period + 0xA4, // +++ Entire Display ON (ignoring RAM): 0b (== OFF) + 0xA6, // +++ Normal/Inverse Display: 0b (== Normal) + 0xAF, // Display ON + 0xB0, // Page Address + 0x02, 0x10 // Column Address + } }; + + _dev = std::move(hal.i2c_mgr->get_device(OLED_I2C_BUS, SH1106_I2C_ADDR)); + memset(_displaybuffer, 0, SH1106_COLUMNS * SH1106_ROWS_PER_PAGE); + + // take i2c bus semaphore + if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + + // init display + _dev->transfer((uint8_t *)&init_seq, sizeof(init_seq), nullptr, 0); + + // give back i2c semaphore + _dev->get_semaphore()->give(); + + _need_hw_update = true; + + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SH1106_I2C::_update_timer, void)); + + return true; +} + +bool Display_SH1106_I2C::hw_update() +{ + _need_hw_update = true; + return true; +} + +void Display_SH1106_I2C::_update_timer() +{ + if (!_need_hw_update) { + return; + } + _need_hw_update = false; + + struct PACKED { + uint8_t reg; + uint8_t cmd[3]; + } command = { 0x0, {0x02 /* |= column[0:3] */, 0x10 /* |= column[4:7] */, 0xB0 /* |= page */} }; + + struct PACKED { + uint8_t reg; + uint8_t db[SH1106_COLUMNS/2]; + } display_buffer = { 0x40, {} }; + + // write buffer to display + for (uint8_t i = 0; i < (SH1106_ROWS / SH1106_ROWS_PER_PAGE); i++) { + command.cmd[2] = 0xB0 | (i & 0x0F); + _dev->transfer((uint8_t *)&command, sizeof(command), nullptr, 0); + + memcpy(&display_buffer.db[0], &_displaybuffer[i * SH1106_COLUMNS], SH1106_COLUMNS/2); + _dev->transfer((uint8_t *)&display_buffer, SH1106_COLUMNS/2 + 1, nullptr, 0); + + memcpy(&display_buffer.db[0], &_displaybuffer[i * SH1106_COLUMNS + SH1106_COLUMNS/2 ], SH1106_COLUMNS/2); + _dev->transfer((uint8_t *)&display_buffer, SH1106_COLUMNS/2 + 1, nullptr, 0); + } +} + +bool Display_SH1106_I2C::set_pixel(uint16_t x, uint16_t y) +{ + // check x, y range + if ((x >= SH1106_COLUMNS) || (y >= SH1106_ROWS)) { + return false; + } + // set pixel in buffer + _displaybuffer[x + (y / 8 * SH1106_COLUMNS)] |= 1 << (y % 8); + + return true; +} + +bool Display_SH1106_I2C::clear_pixel(uint16_t x, uint16_t y) +{ + // check x, y range + if ((x >= SH1106_COLUMNS) || (y >= SH1106_ROWS)) { + return false; + } + // clear pixel in buffer + _displaybuffer[x + (y / 8 * SH1106_COLUMNS)] &= ~(1 << (y % 8)); + + return true; +} +bool Display_SH1106_I2C::clear_screen() +{ + memset(_displaybuffer, 0, SH1106_COLUMNS * SH1106_ROWS_PER_PAGE); + return true; +} diff --git a/libraries/AP_Notify/Display_SH1106_I2C.h b/libraries/AP_Notify/Display_SH1106_I2C.h new file mode 100644 index 0000000000..16cbc9dddd --- /dev/null +++ b/libraries/AP_Notify/Display_SH1106_I2C.h @@ -0,0 +1,28 @@ +#pragma once + +#include "Display.h" +#include "Display_OLED_I2C.h" +#include + +#define SH1106_COLUMNS 132 // display columns +#define SH1106_ROWS 64 // display rows +#define SH1106_ROWS_PER_PAGE 8 + +class Display_SH1106_I2C: public Display_OLED_I2C { +public: + static bool hw_autodetect() { return true; } + +protected: + virtual bool hw_init(); + virtual bool hw_update(); + virtual bool set_pixel(uint16_t x, uint16_t y); + virtual bool clear_pixel(uint16_t x, uint16_t y); + virtual bool clear_screen(); + + void _update_timer() override; + +private: + AP_HAL::OwnPtr _dev; + uint8_t _displaybuffer[SH1106_COLUMNS * SH1106_ROWS_PER_PAGE]; + bool _need_hw_update; +}; diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.cpp b/libraries/AP_Notify/Display_SSD1306_I2C.cpp index 7c417a2f08..f57e49d477 100644 --- a/libraries/AP_Notify/Display_SSD1306_I2C.cpp +++ b/libraries/AP_Notify/Display_SSD1306_I2C.cpp @@ -19,7 +19,6 @@ #include #include -#define SSD1306_I2C_BUS 2 #define SSD1306_I2C_ADDR 0x3C // default I2C bus address static const AP_HAL::HAL& hal = AP_HAL::get_HAL(); @@ -30,15 +29,38 @@ bool Display_SSD1306_I2C::hw_init() struct { uint8_t reg; uint8_t seq[31]; - } init_seq = { 0x0, {0xAE, 0xD5, 0x80, 0xA8, 0x3F, 0xD3, - 0x00, 0x40, 0x8D, 0x14, 0x20, 0x00, - 0xA1, 0xC8, 0xDA, 0x12, 0x81, 0xCF, - 0xD9, 0xF1, 0xDB, 0x40, 0xA4, 0xA6, - 0xAF, 0x21, 0, 127, 0x22, 0, 7 } }; + } init_seq = { 0x0, { + // LEGEND: + // *** is out of sequence for init steps recommended in datasheet + // +++ not listed in sequence for init steps recommended in datasheet - _dev = std::move(hal.i2c_mgr->get_device(SSD1306_I2C_BUS, SSD1306_I2C_ADDR)); + 0xAE, // Display OFF + 0xD5, 0x80, // *** Set Display Clock Divide Ratio and Oscillator Frequency + // Clock Divide Ratio: 0b (== 1) + // Oscillator Frequency: 1000b (== +0%) + 0xA8, 0x3F, // MUX Ratio: 111111b (== 64MUX) + 0xD3, 0x00, // Display Offset: 0b (== 0) + 0x40, // Display Start Line: 0b (== 0) + 0x8D, 0x14, // *** Enable charge pump regulator: 1b (== Enable) + 0x20, 0x00, // *** Memory Addressing Mode: 00b (== Horizontal Addressing Mode) + 0xA1, // Segment re-map: 1b (== column address 127 is mapped to SEG0) + 0xC8, // COM Output Scan Direction: 1b (== remapped mode. Scan from COM[N-1] to COM0) + 0xDA, 0x12, // COM Pins hardware configuration: 01b (POR) + // (== Alternative COM pin configuration + Disable COM Left/Right remap) + 0x81, 0xCF, // Contrast Control: 0xCF (== 207 decimal, range 0..255) + 0xD9, 0xF1, // +++ Pre-charge Period: 0xF1 (== 1 DCLK P1 + 15 DCLK P2) + 0xDB, 0x40, // +++ VCOMH Deselect Level: 100b (INVALID?!) (== ?!) + 0xA4, // Entire Display ON (ignoring RAM): (== OFF) + 0xA6, // Normal/Inverse Display: 0b (== Normal) + 0xAF, // Display ON: 1b (== ON) + 0x21, 0, 127, // +++ Column Address: (== start:0, end:127) + 0x22, 0, 7 // +++ Page Address: (== start:0, end:7) + } }; - // take i2c bus sempahore + _dev = std::move(hal.i2c_mgr->get_device(OLED_I2C_BUS, SSD1306_I2C_ADDR)); + memset(_displaybuffer, 0, SSD1306_COLUMNS * SSD1306_ROWS_PER_PAGE); + + // take i2c bus semaphore if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { return false; } @@ -51,7 +73,7 @@ bool Display_SSD1306_I2C::hw_init() _need_hw_update = true; - _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SSD1306_I2C::_timer, void)); + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&Display_SSD1306_I2C::_update_timer, void)); return true; } @@ -62,13 +84,13 @@ bool Display_SSD1306_I2C::hw_update() return true; } -void Display_SSD1306_I2C::_timer() +void Display_SSD1306_I2C::_update_timer() { if (!_need_hw_update) { return; } _need_hw_update = false; - + struct PACKED { uint8_t reg; uint8_t cmd[6]; @@ -76,28 +98,30 @@ void Display_SSD1306_I2C::_timer() struct PACKED { uint8_t reg; - uint8_t db[SSD1306_ROWS]; + uint8_t db[SSD1306_COLUMNS/2]; } display_buffer = { 0x40, {} }; // write buffer to display - for (uint8_t i = 0; i < (SSD1306_COLUMNS / SSD1306_COLUMNS_PER_PAGE); i++) { + for (uint8_t i = 0; i < (SSD1306_ROWS / SSD1306_ROWS_PER_PAGE); i++) { command.cmd[4] = i; - _dev->transfer((uint8_t *)&command, sizeof(command), nullptr, 0); - memcpy(&display_buffer.db[0], &_displaybuffer[i * SSD1306_ROWS], SSD1306_ROWS); - _dev->transfer((uint8_t *)&display_buffer, sizeof(display_buffer), nullptr, 0); + memcpy(&display_buffer.db[0], &_displaybuffer[i * SSD1306_COLUMNS], SSD1306_COLUMNS/2); + _dev->transfer((uint8_t *)&display_buffer, SSD1306_COLUMNS/2 + 1, nullptr, 0); + + memcpy(&display_buffer.db[0], &_displaybuffer[i * SSD1306_COLUMNS + SSD1306_COLUMNS/2 ], SSD1306_COLUMNS/2); + _dev->transfer((uint8_t *)&display_buffer, SSD1306_COLUMNS/2 + 1, nullptr, 0); } } bool Display_SSD1306_I2C::set_pixel(uint16_t x, uint16_t y) { // check x, y range - if ((x >= SSD1306_ROWS) || (y >= SSD1306_COLUMNS)) { + if ((x >= SSD1306_COLUMNS) || (y >= SSD1306_ROWS)) { return false; } // set pixel in buffer - _displaybuffer[x + (y / 8 * SSD1306_ROWS)] |= 1 << (y % 8); + _displaybuffer[x + (y / 8 * SSD1306_COLUMNS)] |= 1 << (y % 8); return true; } @@ -105,11 +129,16 @@ bool Display_SSD1306_I2C::set_pixel(uint16_t x, uint16_t y) bool Display_SSD1306_I2C::clear_pixel(uint16_t x, uint16_t y) { // check x, y range - if ((x >= SSD1306_ROWS) || (y >= SSD1306_COLUMNS)) { + if ((x >= SSD1306_COLUMNS) || (y >= SSD1306_ROWS)) { return false; } // clear pixel in buffer - _displaybuffer[x + (y / 8 * SSD1306_ROWS)] &= ~(1 << (y % 8)); + _displaybuffer[x + (y / 8 * SSD1306_COLUMNS)] &= ~(1 << (y % 8)); return true; } +bool Display_SSD1306_I2C::clear_screen() +{ + memset(_displaybuffer, 0, SSD1306_COLUMNS * SSD1306_ROWS_PER_PAGE); + return true; +} diff --git a/libraries/AP_Notify/Display_SSD1306_I2C.h b/libraries/AP_Notify/Display_SSD1306_I2C.h index 2568af931b..532796ff26 100644 --- a/libraries/AP_Notify/Display_SSD1306_I2C.h +++ b/libraries/AP_Notify/Display_SSD1306_I2C.h @@ -1,22 +1,27 @@ #pragma once #include "Display.h" +#include "Display_OLED_I2C.h" #include -#define SSD1306_ROWS 128 // display rows -#define SSD1306_COLUMNS 64 // display columns -#define SSD1306_COLUMNS_PER_PAGE 8 +#define SSD1306_COLUMNS 128 // display columns +#define SSD1306_ROWS 64 // display rows +#define SSD1306_ROWS_PER_PAGE 8 -class Display_SSD1306_I2C: public Display { -public: +class Display_SSD1306_I2C: public Display_OLED_I2C { + +protected: virtual bool hw_init(); virtual bool hw_update(); virtual bool set_pixel(uint16_t x, uint16_t y); virtual bool clear_pixel(uint16_t x, uint16_t y); + virtual bool clear_screen(); + + void _update_timer() override; private: AP_HAL::OwnPtr _dev; - uint8_t _displaybuffer[SSD1306_ROWS * SSD1306_COLUMNS_PER_PAGE]; + uint8_t _displaybuffer[SSD1306_COLUMNS * SSD1306_ROWS_PER_PAGE]; bool _need_hw_update; void _timer(void); };