zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Notify: Add display support

mission-4.1.18
mirkix 9 years ago committed by Lucas De Marchi
parent
dee20a31f1
commit
2457558624
5 changed files with 622 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      libraries/AP_Notify/AP_Notify.h
  2. 450
      libraries/AP_Notify/Display.cpp
  3. 42
      libraries/AP_Notify/Display.h
  4. 107
      libraries/AP_Notify/Display_SSD1306_I2C.cpp
  5. 21
      libraries/AP_Notify/Display_SSD1306_I2C.h

2
libraries/AP_Notify/AP_Notify.h
Unescape View File

@ -33,6 +33,8 @@ @@ -33,6 +33,8 @@
#include "VRBoard_LED.h"
#include "OreoLED_PX4.h"
#include "RCOutputRGBLed.h"
#include "Display.h"
#include "Display_SSD1306_I2C.h"
#ifndef OREOLED_ENABLED
# define OREOLED_ENABLED 0 // set to 1 to enable OreoLEDs

450
libraries/AP_Notify/Display.cpp
Unescape View File

@ -0,0 +1,450 @@ @@ -0,0 +1,450 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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 <http://www.gnu.org/licenses/>.
*/
/* Notify display driver for 128 x 64 pixel displays */
#include "AP_Notify.h"
#include <AP_GPS/AP_GPS.h>
#include "Display.h"
static const uint8_t _font[] = {
0x00, 0x00, 0x00, 0x00, 0x00,
0x3E, 0x5B, 0x4F, 0x5B, 0x3E,
0x3E, 0x6B, 0x4F, 0x6B, 0x3E,
0x1C, 0x3E, 0x7C, 0x3E, 0x1C,
0x18, 0x3C, 0x7E, 0x3C, 0x18,
0x1C, 0x57, 0x7D, 0x57, 0x1C,
0x1C, 0x5E, 0x7F, 0x5E, 0x1C,
0x00, 0x18, 0x3C, 0x18, 0x00,
0xFF, 0xE7, 0xC3, 0xE7, 0xFF,
0x00, 0x18, 0x24, 0x18, 0x00,
0xFF, 0xE7, 0xDB, 0xE7, 0xFF,
0x30, 0x48, 0x3A, 0x06, 0x0E,
0x26, 0x29, 0x79, 0x29, 0x26,
0x40, 0x7F, 0x05, 0x05, 0x07,
0x40, 0x7F, 0x05, 0x25, 0x3F,
0x5A, 0x3C, 0xE7, 0x3C, 0x5A,
0x7F, 0x3E, 0x1C, 0x1C, 0x08,
0x08, 0x1C, 0x1C, 0x3E, 0x7F,
0x14, 0x22, 0x7F, 0x22, 0x14,
0x5F, 0x5F, 0x00, 0x5F, 0x5F,
0x06, 0x09, 0x7F, 0x01, 0x7F,
0x00, 0x66, 0x89, 0x95, 0x6A,
0x60, 0x60, 0x60, 0x60, 0x60,
0x94, 0xA2, 0xFF, 0xA2, 0x94,
0x08, 0x04, 0x7E, 0x04, 0x08,
0x10, 0x20, 0x7E, 0x20, 0x10,
0x08, 0x08, 0x2A, 0x1C, 0x08,
0x08, 0x1C, 0x2A, 0x08, 0x08,
0x1E, 0x10, 0x10, 0x10, 0x10,
0x0C, 0x1E, 0x0C, 0x1E, 0x0C,
0x30, 0x38, 0x3E, 0x38, 0x30,
0x06, 0x0E, 0x3E, 0x0E, 0x06,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x5F, 0x00, 0x00,
0x00, 0x07, 0x00, 0x07, 0x00,
0x14, 0x7F, 0x14, 0x7F, 0x14,
0x24, 0x2A, 0x7F, 0x2A, 0x12,
0x23, 0x13, 0x08, 0x64, 0x62,
0x36, 0x49, 0x56, 0x20, 0x50,
0x00, 0x08, 0x07, 0x03, 0x00,
0x00, 0x1C, 0x22, 0x41, 0x00,
0x00, 0x41, 0x22, 0x1C, 0x00,
0x2A, 0x1C, 0x7F, 0x1C, 0x2A,
0x08, 0x08, 0x3E, 0x08, 0x08,
0x00, 0x80, 0x70, 0x30, 0x00,
0x08, 0x08, 0x08, 0x08, 0x08,
0x00, 0x00, 0x60, 0x60, 0x00,
0x20, 0x10, 0x08, 0x04, 0x02,
0x3E, 0x51, 0x49, 0x45, 0x3E,
0x00, 0x42, 0x7F, 0x40, 0x00,
0x72, 0x49, 0x49, 0x49, 0x46,
0x21, 0x41, 0x49, 0x4D, 0x33,
0x18, 0x14, 0x12, 0x7F, 0x10,
0x27, 0x45, 0x45, 0x45, 0x39,
0x3C, 0x4A, 0x49, 0x49, 0x31,
0x41, 0x21, 0x11, 0x09, 0x07,
0x36, 0x49, 0x49, 0x49, 0x36,
0x46, 0x49, 0x49, 0x29, 0x1E,
0x00, 0x00, 0x14, 0x00, 0x00,
0x00, 0x40, 0x34, 0x00, 0x00,
0x00, 0x08, 0x14, 0x22, 0x41,
0x14, 0x14, 0x14, 0x14, 0x14,
0x00, 0x41, 0x22, 0x14, 0x08,
0x02, 0x01, 0x59, 0x09, 0x06,
0x3E, 0x41, 0x5D, 0x59, 0x4E,
0x7C, 0x12, 0x11, 0x12, 0x7C,
0x7F, 0x49, 0x49, 0x49, 0x36,
0x3E, 0x41, 0x41, 0x41, 0x22,
0x7F, 0x41, 0x41, 0x41, 0x3E,
0x7F, 0x49, 0x49, 0x49, 0x41,
0x7F, 0x09, 0x09, 0x09, 0x01,
0x3E, 0x41, 0x41, 0x51, 0x73,
0x7F, 0x08, 0x08, 0x08, 0x7F,
0x00, 0x41, 0x7F, 0x41, 0x00,
0x20, 0x40, 0x41, 0x3F, 0x01,
0x7F, 0x08, 0x14, 0x22, 0x41,
0x7F, 0x40, 0x40, 0x40, 0x40,
0x7F, 0x02, 0x1C, 0x02, 0x7F,
0x7F, 0x04, 0x08, 0x10, 0x7F,
0x3E, 0x41, 0x41, 0x41, 0x3E,
0x7F, 0x09, 0x09, 0x09, 0x06,
0x3E, 0x41, 0x51, 0x21, 0x5E,
0x7F, 0x09, 0x19, 0x29, 0x46,
0x26, 0x49, 0x49, 0x49, 0x32,
0x03, 0x01, 0x7F, 0x01, 0x03,
0x3F, 0x40, 0x40, 0x40, 0x3F,
0x1F, 0x20, 0x40, 0x20, 0x1F,
0x3F, 0x40, 0x38, 0x40, 0x3F,
0x63, 0x14, 0x08, 0x14, 0x63,
0x03, 0x04, 0x78, 0x04, 0x03,
0x61, 0x59, 0x49, 0x4D, 0x43,
0x00, 0x7F, 0x41, 0x41, 0x41,
0x02, 0x04, 0x08, 0x10, 0x20,
0x00, 0x41, 0x41, 0x41, 0x7F,
0x04, 0x02, 0x01, 0x02, 0x04,
0x40, 0x40, 0x40, 0x40, 0x40,
0x00, 0x03, 0x07, 0x08, 0x00,
0x20, 0x54, 0x54, 0x78, 0x40,
0x7F, 0x28, 0x44, 0x44, 0x38,
0x38, 0x44, 0x44, 0x44, 0x28,
0x38, 0x44, 0x44, 0x28, 0x7F,
0x38, 0x54, 0x54, 0x54, 0x18,
0x00, 0x08, 0x7E, 0x09, 0x02,
0x18, 0xA4, 0xA4, 0x9C, 0x78,
0x7F, 0x08, 0x04, 0x04, 0x78,
0x00, 0x44, 0x7D, 0x40, 0x00,
0x20, 0x40, 0x40, 0x3D, 0x00,
0x7F, 0x10, 0x28, 0x44, 0x00,
0x00, 0x41, 0x7F, 0x40, 0x00,
0x7C, 0x04, 0x78, 0x04, 0x78,
0x7C, 0x08, 0x04, 0x04, 0x78,
0x38, 0x44, 0x44, 0x44, 0x38,
0xFC, 0x18, 0x24, 0x24, 0x18,
0x18, 0x24, 0x24, 0x18, 0xFC,
0x7C, 0x08, 0x04, 0x04, 0x08,
0x48, 0x54, 0x54, 0x54, 0x24,
0x04, 0x04, 0x3F, 0x44, 0x24,
0x3C, 0x40, 0x40, 0x20, 0x7C,
0x1C, 0x20, 0x40, 0x20, 0x1C,
0x3C, 0x40, 0x30, 0x40, 0x3C,
0x44, 0x28, 0x10, 0x28, 0x44,
0x4C, 0x90, 0x90, 0x90, 0x7C,
0x44, 0x64, 0x54, 0x4C, 0x44,
0x00, 0x08, 0x36, 0x41, 0x00,
0x00, 0x00, 0x77, 0x00, 0x00,
0x00, 0x41, 0x36, 0x08, 0x00,
0x02, 0x01, 0x02, 0x04, 0x02,
0x3C, 0x26, 0x23, 0x26, 0x3C,
0x1E, 0xA1, 0xA1, 0x61, 0x12,
0x3A, 0x40, 0x40, 0x20, 0x7A,
0x38, 0x54, 0x54, 0x55, 0x59,
0x21, 0x55, 0x55, 0x79, 0x41,
0x22, 0x54, 0x54, 0x78, 0x42,
0x21, 0x55, 0x54, 0x78, 0x40,
0x20, 0x54, 0x55, 0x79, 0x40,
0x0C, 0x1E, 0x52, 0x72, 0x12,
0x39, 0x55, 0x55, 0x55, 0x59,
0x39, 0x54, 0x54, 0x54, 0x59,
0x39, 0x55, 0x54, 0x54, 0x58,
0x00, 0x00, 0x45, 0x7C, 0x41,
0x00, 0x02, 0x45, 0x7D, 0x42,
0x00, 0x01, 0x45, 0x7C, 0x40,
0x7D, 0x12, 0x11, 0x12, 0x7D,
0xF0, 0x28, 0x25, 0x28, 0xF0,
0x7C, 0x54, 0x55, 0x45, 0x00,
0x20, 0x54, 0x54, 0x7C, 0x54,
0x7C, 0x0A, 0x09, 0x7F, 0x49,
0x32, 0x49, 0x49, 0x49, 0x32,
0x3A, 0x44, 0x44, 0x44, 0x3A,
0x32, 0x4A, 0x48, 0x48, 0x30,
0x3A, 0x41, 0x41, 0x21, 0x7A,
0x3A, 0x42, 0x40, 0x20, 0x78,
0x00, 0x9D, 0xA0, 0xA0, 0x7D,
0x3D, 0x42, 0x42, 0x42, 0x3D,
0x3D, 0x40, 0x40, 0x40, 0x3D,
0x3C, 0x24, 0xFF, 0x24, 0x24,
0x48, 0x7E, 0x49, 0x43, 0x66,
0x2B, 0x2F, 0xFC, 0x2F, 0x2B,
0xFF, 0x09, 0x29, 0xF6, 0x20,
0xC0, 0x88, 0x7E, 0x09, 0x03,
0x20, 0x54, 0x54, 0x79, 0x41,
0x00, 0x00, 0x44, 0x7D, 0x41,
0x30, 0x48, 0x48, 0x4A, 0x32,
0x38, 0x40, 0x40, 0x22, 0x7A,
0x00, 0x7A, 0x0A, 0x0A, 0x72,
0x7D, 0x0D, 0x19, 0x31, 0x7D,
0x26, 0x29, 0x29, 0x2F, 0x28,
0x26, 0x29, 0x29, 0x29, 0x26,
0x30, 0x48, 0x4D, 0x40, 0x20,
0x38, 0x08, 0x08, 0x08, 0x08,
0x08, 0x08, 0x08, 0x08, 0x38,
0x2F, 0x10, 0xC8, 0xAC, 0xBA,
0x2F, 0x10, 0x28, 0x34, 0xFA,
0x00, 0x00, 0x7B, 0x00, 0x00,
0x08, 0x14, 0x2A, 0x14, 0x22,
0x22, 0x14, 0x2A, 0x14, 0x08,
0x55, 0x00, 0x55, 0x00, 0x55,
0xAA, 0x55, 0xAA, 0x55, 0xAA,
0xFF, 0x55, 0xFF, 0x55, 0xFF,
0x00, 0x00, 0x00, 0xFF, 0x00,
0x10, 0x10, 0x10, 0xFF, 0x00,
0x14, 0x14, 0x14, 0xFF, 0x00,
0x10, 0x10, 0xFF, 0x00, 0xFF,
0x10, 0x10, 0xF0, 0x10, 0xF0,
0x14, 0x14, 0x14, 0xFC, 0x00,
0x14, 0x14, 0xF7, 0x00, 0xFF,
0x00, 0x00, 0xFF, 0x00, 0xFF,
0x14, 0x14, 0xF4, 0x04, 0xFC,
0x14, 0x14, 0x17, 0x10, 0x1F,
0x10, 0x10, 0x1F, 0x10, 0x1F,
0x14, 0x14, 0x14, 0x1F, 0x00,
0x10, 0x10, 0x10, 0xF0, 0x00,
0x00, 0x00, 0x00, 0x1F, 0x10,
0x10, 0x10, 0x10, 0x1F, 0x10,
0x10, 0x10, 0x10, 0xF0, 0x10,
0x00, 0x00, 0x00, 0xFF, 0x10,
0x10, 0x10, 0x10, 0x10, 0x10,
0x10, 0x10, 0x10, 0xFF, 0x10,
0x00, 0x00, 0x00, 0xFF, 0x14,
0x00, 0x00, 0xFF, 0x00, 0xFF,
0x00, 0x00, 0x1F, 0x10, 0x17,
0x00, 0x00, 0xFC, 0x04, 0xF4,
0x14, 0x14, 0x17, 0x10, 0x17,
0x14, 0x14, 0xF4, 0x04, 0xF4,
0x00, 0x00, 0xFF, 0x00, 0xF7,
0x14, 0x14, 0x14, 0x14, 0x14,
0x14, 0x14, 0xF7, 0x00, 0xF7,
0x14, 0x14, 0x14, 0x17, 0x14,
0x10, 0x10, 0x1F, 0x10, 0x1F,
0x14, 0x14, 0x14, 0xF4, 0x14,
0x10, 0x10, 0xF0, 0x10, 0xF0,
0x00, 0x00, 0x1F, 0x10, 0x1F,
0x00, 0x00, 0x00, 0x1F, 0x14,
0x00, 0x00, 0x00, 0xFC, 0x14,
0x00, 0x00, 0xF0, 0x10, 0xF0,
0x10, 0x10, 0xFF, 0x10, 0xFF,
0x14, 0x14, 0x14, 0xFF, 0x14,
0x10, 0x10, 0x10, 0x1F, 0x00,
0x00, 0x00, 0x00, 0xF0, 0x10,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
0xFF, 0xFF, 0xFF, 0x00, 0x00,
0x00, 0x00, 0x00, 0xFF, 0xFF,
0x0F, 0x0F, 0x0F, 0x0F, 0x0F,
0x38, 0x44, 0x44, 0x38, 0x44,
0xFC, 0x4A, 0x4A, 0x4A, 0x34,
0x7E, 0x02, 0x02, 0x06, 0x06,
0x02, 0x7E, 0x02, 0x7E, 0x02,
0x63, 0x55, 0x49, 0x41, 0x63,
0x38, 0x44, 0x44, 0x3C, 0x04,
0x40, 0x7E, 0x20, 0x1E, 0x20,
0x06, 0x02, 0x7E, 0x02, 0x02,
0x99, 0xA5, 0xE7, 0xA5, 0x99,
0x1C, 0x2A, 0x49, 0x2A, 0x1C,
0x4C, 0x72, 0x01, 0x72, 0x4C,
0x30, 0x4A, 0x4D, 0x4D, 0x30,
0x30, 0x48, 0x78, 0x48, 0x30,
0xBC, 0x62, 0x5A, 0x46, 0x3D,
0x3E, 0x49, 0x49, 0x49, 0x00,
0x7E, 0x01, 0x01, 0x01, 0x7E,
0x2A, 0x2A, 0x2A, 0x2A, 0x2A,
0x44, 0x44, 0x5F, 0x44, 0x44,
0x40, 0x51, 0x4A, 0x44, 0x40,
0x40, 0x44, 0x4A, 0x51, 0x40,
0x00, 0x00, 0xFF, 0x01, 0x03,
0xE0, 0x80, 0xFF, 0x00, 0x00,
0x08, 0x08, 0x6B, 0x6B, 0x08,
0x36, 0x12, 0x36, 0x24, 0x36,
0x06, 0x0F, 0x09, 0x0F, 0x06,
0x00, 0x00, 0x18, 0x18, 0x00,
0x00, 0x00, 0x10, 0x10, 0x00,
0x30, 0x40, 0xFF, 0x01, 0x01,
0x00, 0x1F, 0x01, 0x01, 0x1E,
0x00, 0x19, 0x1D, 0x17, 0x12,
0x00, 0x3C, 0x3C, 0x3C, 0x3C,
0x00, 0x00, 0x00, 0x00, 0x00
};
bool Display::init(void)
{
memset(&_flags, 0, sizeof(_flags));
_healthy = hw_init();
if (!_healthy) {
return false;
}
// update all on display
update_all();
hw_update();
return true;
}
void Display::update()
{
static uint8_t timer = 0;
// return immediately if not enabled
if (!_healthy) {
return;
}
// max update frequency 2Hz
if (timer++ < 25) {
return;
}
timer = 0;
// check if status has changed
if (_flags.armed != AP_Notify::flags.armed) {
update_arm();
_flags.armed = AP_Notify::flags.armed;
}
if (_flags.pre_arm_check != AP_Notify::flags.pre_arm_check) {
update_prearm();
_flags.pre_arm_check = AP_Notify::flags.pre_arm_check;
}
if (_flags.gps_status != AP_Notify::flags.gps_status) {
update_gps();
_flags.gps_status = AP_Notify::flags.gps_status;
}
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();
}
void Display::draw_text(uint16_t x, uint16_t y, const char* c)
{
while (*c != 0) {
draw_char(x, y, *c);
x += 7;
c++;
}
}
void Display::draw_char(uint16_t x, uint16_t y, const char c)
{
uint8_t line;
// draw char to pixel
for (uint8_t i = 0; i < 6; i++) {
if (i == 5) {
line = 0;
} else {
line = _font[(c * 5) + i];
}
for (uint8_t j = 0; j < 8; j++) {
if (line & 1) {
set_pixel(x + i, y + j);
} else {
clear_pixel(x + i, y + j);
}
line >>= 1;
}
}
// update display
_need_update = true;
}
void Display::update_arm()
{
if (AP_Notify::flags.armed) {
draw_text(COLUMN(0), ROW(0), ">>>>> ARMED! <<<<<");
} else {
draw_text(COLUMN(0), ROW(0), " disarmed ");
}
}
void Display::update_prearm()
{
if (AP_Notify::flags.pre_arm_check) {
draw_text(COLUMN(0), ROW(2), "Prearm: passed ");
} else {
draw_text(COLUMN(0), ROW(2), "Prearm: failed ");
}
}
void Display::update_gps()
{
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;
}
}
void Display::update_gps_sats()
{
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);
}
void Display::update_ekf()
{
if (AP_Notify::flags.ekf_bad) {
draw_text(COLUMN(0), ROW(5), "EKF: fail");
} else {
draw_text(COLUMN(0), ROW(5), "EKF: ok ");
}
}

42
libraries/AP_Notify/Display.h
Unescape View File

@ -0,0 +1,42 @@ @@ -0,0 +1,42 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once
#include "NotifyDevice.h"
#define ROW(Y) ((Y * 11) + 2)
#define COLUMN(X) ((X * 7) + 1)
class Display: public NotifyDevice {
public:
bool init(void);
void update();
protected:
virtual bool hw_init() = 0;
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;
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();
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;
};

107
libraries/AP_Notify/Display_SSD1306_I2C.cpp
Unescape View File

@ -0,0 +1,107 @@ @@ -0,0 +1,107 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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 <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include "Display_SSD1306_I2C.h"
static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
bool Display_SSD1306_I2C::hw_init()
{
// display init sequence
uint8_t init_seq[] = {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
};
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
// disable recording of i2c lockup errors
hal.i2c->ignore_errors(true);
// init display
hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x0, sizeof(init_seq), init_seq);
// re-enable recording of i2c lockup errors
hal.i2c->ignore_errors(false);
// give back i2c semaphore
i2c_sem->give();
// clear display
hw_update();
return true;
}
bool Display_SSD1306_I2C::hw_update()
{
uint8_t command[] = {0x21, 0, 127, 0x22, 0, 7};
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// exit immediately if we can't take the semaphore
if (i2c_sem == NULL || !i2c_sem->take(5)) {
return false;
}
// write buffer to display
for (uint8_t i = 0; i < (SSD1306_COLUMNS / SSD1306_COLUMNS_PER_PAGE); i++) {
command[4] = i;
hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x0, sizeof(command), command);
hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x40, SSD1306_ROWS, &_displaybuffer[i * SSD1306_ROWS]);
}
// give back i2c semaphore
i2c_sem->give();
return true;
}
bool Display_SSD1306_I2C::set_pixel(uint16_t x, uint16_t y)
{
// check x, y range
if ((x >= SSD1306_ROWS) || (y >= SSD1306_COLUMNS)) {
return false;
}
// set pixel in buffer
_displaybuffer[x + (y / 8 * SSD1306_ROWS)] |= 1 << (y % 8);
return true;
}
bool Display_SSD1306_I2C::clear_pixel(uint16_t x, uint16_t y)
{
// check x, y range
if ((x >= SSD1306_ROWS) || (y >= SSD1306_COLUMNS)) {
return false;
}
// clear pixel in buffer
_displaybuffer[x + (y / 8 * SSD1306_ROWS)] &= ~(1 << (y % 8));
return true;
}

21
libraries/AP_Notify/Display_SSD1306_I2C.h
Unescape View File

@ -0,0 +1,21 @@ @@ -0,0 +1,21 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once
#include "Display.h"
#define SSD1306_I2C_ADDRESS 0x3C // default I2C bus address
#define SSD1306_ROWS 128 // display rows
#define SSD1306_COLUMNS 64 // display columns
#define SSD1306_COLUMNS_PER_PAGE 8
class Display_SSD1306_I2C: public Display {
public:
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);
private:
uint8_t _displaybuffer[SSD1306_ROWS * SSD1306_COLUMNS_PER_PAGE];
};
Write Preview
Loading…
Cancel
Save