Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OSD: added a new backend for msp displayport aka canvas mode support

This commit is contained in:
yaapu 2021-08-06 17:54:02 +02:00 committed by Andrew Tridgell
parent 188b7a50a9
commit 7e3395eebd
11 changed files with 751 additions and 223 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_OSD/AP_OSD.cpp
View File

@ -26,6 +26,7 @@
#include "AP_OSD_SITL.h"
#endif
#include "AP_OSD_MSP.h"
#include "AP_OSD_MSP_DisplayPort.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/Util.h>
#include <RC_Channel/RC_Channel.h>
@ -291,11 +292,23 @@ void AP_OSD::init()
hal.console->printf("Started MSP OSD\n");
break;
}
#if HAL_WITH_MSP_DISPLAYPORT
case OSD_MSP_DISPLAYPORT: {
backend = AP_OSD_MSP_DisplayPort::probe(*this);
if (backend == nullptr) {
break;
}
hal.console->printf("Started MSP DisplayPort OSD\n");
break;
}
#endif
}
#if OSD_ENABLED
if (backend != nullptr && (enum osd_types)osd_type.get() != OSD_MSP) {
// populate the fonts lookup table
backend->init_symbol_set(AP_OSD_AbstractScreen::symbols_lookup_table, AP_OSD_NUM_SYMBOLS);
// create thread as higher priority than IO for all backends but MSP which has its own
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1024, AP_HAL::Scheduler::PRIORITY_IO, 1);
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1280, AP_HAL::Scheduler::PRIORITY_IO, 1);
}
#endif
}

11
libraries/AP_OSD/AP_OSD.h
View File

@ -52,6 +52,7 @@ class AP_MSP;
#define PARAM_INDEX(key, idx, group) (uint32_t(uint32_t(key) << 23 | uint32_t(idx) << 18 | uint32_t(group)))
#define PARAM_TOKEN_INDEX(token) PARAM_INDEX(AP_Param::get_persistent_key(token.key), token.idx, token.group_element)
#define AP_OSD_NUM_SYMBOLS 79
/*
class to hold one setting
*/
@ -72,6 +73,7 @@ class AP_OSD;
class AP_OSD_AbstractScreen
{
friend class AP_OSD;
public:
// constructor
AP_OSD_AbstractScreen() {}
@ -101,6 +103,8 @@ protected:
AP_OSD_Backend *backend;
AP_OSD *osd;
static uint8_t symbols_lookup_table[AP_OSD_NUM_SYMBOLS];
};
#if OSD_ENABLED
@ -115,7 +119,7 @@ public:
// skip the drawing if we are not using a font based backend. This saves a lot of flash space when
// using the MSP OSD system on boards that don't have a MAX7456
#if HAL_WITH_OSD_BITMAP
#if HAL_WITH_OSD_BITMAP || HAL_WITH_MSP_DISPLAYPORT
void draw(void) override;
#endif
@ -363,7 +367,7 @@ public:
static const uint8_t NUM_PARAMS = 9;
static const uint8_t SAVE_PARAM = NUM_PARAMS + 1;
#if HAL_WITH_OSD_BITMAP
#if HAL_WITH_OSD_BITMAP || HAL_WITH_MSP_DISPLAYPORT
void draw(void) override;
#endif
void handle_write_msg(const mavlink_osd_param_config_t& packet, const GCS_MAVLINK& link);
@ -437,7 +441,8 @@ public:
OSD_MAX7456=1,
OSD_SITL=2,
OSD_MSP=3,
OSD_TXONLY=4
OSD_TXONLY=4,
OSD_MSP_DISPLAYPORT=5
};
enum switch_method {
TOGGLE=0,

6
libraries/AP_OSD/AP_OSD_Backend.cpp
View File

@ -19,6 +19,7 @@
#include <ctype.h>
extern const AP_HAL::HAL& hal;
constexpr uint8_t AP_OSD_Backend::symbols[AP_OSD_NUM_SYMBOLS];
#define SYM_DIG_OFS_1 0x90
#define SYM_DIG_OFS_2 0xA0
@ -80,3 +81,8 @@ FileData *AP_OSD_Backend::load_font_data(uint8_t font_num)
}
return fd;
}
void AP_OSD_Backend::init_symbol_set(uint8_t *lookup_table, const uint8_t size)
{
memcpy(lookup_table, symbols, size);
}

177
libraries/AP_OSD/AP_OSD_Backend.h
View File

@ -53,6 +53,9 @@ public:
blink_phase = (blink_phase+1)%4;
};
// copy the backend specific symbol set to the OSD lookup table
virtual void init_symbol_set(uint8_t *symbols, const uint8_t size);
AP_OSD * get_osd()
{
return &_osd;
@ -83,6 +86,176 @@ protected:
FORMAT_NTSC = 1,
FORMAT_PAL = 2,
} _format;
// default OSD symbols
static const uint8_t SYM_M = 0xB9;
static const uint8_t SYM_KM = 0xBA;
static const uint8_t SYM_FT = 0x0F;
static const uint8_t SYM_MI = 0xBB;
static const uint8_t SYM_ALT_M = 0xB1;
static const uint8_t SYM_ALT_FT = 0xB3;
static const uint8_t SYM_BATT_FULL = 0x90;
static const uint8_t SYM_RSSI = 0x01;
static const uint8_t SYM_VOLT = 0x06;
static const uint8_t SYM_AMP = 0x9A;
static const uint8_t SYM_MAH = 0x07;
static const uint8_t SYM_MS = 0x9F;
static const uint8_t SYM_FS = 0x99;
static const uint8_t SYM_KMH = 0xA1;
static const uint8_t SYM_MPH = 0xB0;
static const uint8_t SYM_DEGR = 0xA8;
static const uint8_t SYM_PCNT = 0x25;
static const uint8_t SYM_RPM = 0xE0;
static const uint8_t SYM_ASPD = 0xE1;
static const uint8_t SYM_GSPD = 0xE2;
static const uint8_t SYM_WSPD = 0xE3;
static const uint8_t SYM_VSPD = 0xE4;
static const uint8_t SYM_WPNO = 0xE5;
static const uint8_t SYM_WPDIR = 0xE6;
static const uint8_t SYM_WPDST = 0xE7;
static const uint8_t SYM_FTMIN = 0xE8;
static const uint8_t SYM_FTSEC = 0x99;
static const uint8_t SYM_SAT_L = 0x1E;
static const uint8_t SYM_SAT_R = 0x1F;
static const uint8_t SYM_HDOP_L = 0xBD;
static const uint8_t SYM_HDOP_R = 0xBE;
static const uint8_t SYM_HOME = 0xBF;
static const uint8_t SYM_WIND = 0x16;
static const uint8_t SYM_ARROW_START = 0x60;
static const uint8_t SYM_ARROW_COUNT = 16;
static const uint8_t SYM_AH_H_START = 0x80;
static const uint8_t SYM_AH_H_COUNT = 9;
static const uint8_t SYM_AH_V_START = 0xCA;
static const uint8_t SYM_AH_V_COUNT = 6;
static const uint8_t SYM_AH_CENTER_LINE_LEFT = 0x26;
static const uint8_t SYM_AH_CENTER_LINE_RIGHT = 0x27;
static const uint8_t SYM_AH_CENTER = 0x7E;
static const uint8_t SYM_HEADING_N = 0x18;
static const uint8_t SYM_HEADING_S = 0x19;
static const uint8_t SYM_HEADING_E = 0x1A;
static const uint8_t SYM_HEADING_W = 0x1B;
static const uint8_t SYM_HEADING_DIVIDED_LINE = 0x1C;
static const uint8_t SYM_HEADING_LINE = 0x1D;
static const uint8_t SYM_UP_UP = 0xA2;
static const uint8_t SYM_UP = 0xA3;
static const uint8_t SYM_DOWN = 0xA4;
static const uint8_t SYM_DOWN_DOWN = 0xA5;
static const uint8_t SYM_DEGREES_C = 0x0E;
static const uint8_t SYM_DEGREES_F = 0x0D;
static const uint8_t SYM_GPS_LAT = 0xA6;
static const uint8_t SYM_GPS_LONG = 0xA7;
static const uint8_t SYM_ARMED = 0x00;
static const uint8_t SYM_DISARMED = 0xE9;
static const uint8_t SYM_ROLL0 = 0x2D;
static const uint8_t SYM_ROLLR = 0xEA;
static const uint8_t SYM_ROLLL = 0xEB;
static const uint8_t SYM_PTCH0 = 0x7C;
static const uint8_t SYM_PTCHUP = 0xEC;
static const uint8_t SYM_PTCHDWN = 0xED;
static const uint8_t SYM_XERR = 0xEE;
static const uint8_t SYM_KN = 0xF0;
static const uint8_t SYM_NM = 0xF1;
static const uint8_t SYM_DIST = 0x22;
static const uint8_t SYM_FLY = 0x9C;
static const uint8_t SYM_EFF = 0xF2;
static const uint8_t SYM_AH = 0xF3;
static const uint8_t SYM_MW = 0xF4;
static const uint8_t SYM_CLK = 0xBC;
static const uint8_t SYM_KILO = 0x4B;
static const uint8_t SYM_TERALT = 0xEF;
static const uint8_t SYM_FENCE_ENABLED = 0xF5;
static const uint8_t SYM_FENCE_DISABLED = 0xF6;
static const uint8_t SYM_RNGFD = 0xF7;
static const uint8_t SYM_LQ = 0xF8;
static constexpr uint8_t symbols[AP_OSD_NUM_SYMBOLS] {
SYM_M,
SYM_KM,
SYM_FT,
SYM_MI,
SYM_ALT_M,
SYM_ALT_FT,
SYM_BATT_FULL,
SYM_RSSI,
SYM_VOLT,
SYM_AMP,
SYM_MAH,
SYM_MS,
SYM_FS,
SYM_KMH,
SYM_MPH,
SYM_DEGR,
SYM_PCNT,
SYM_RPM,
SYM_ASPD,
SYM_GSPD,
SYM_WSPD,
SYM_VSPD,
SYM_WPNO,
SYM_WPDIR,
SYM_WPDST,
SYM_FTMIN,
SYM_FTSEC,
SYM_SAT_L,
SYM_SAT_R,
SYM_HDOP_L,
SYM_HDOP_R,
SYM_HOME,
SYM_WIND,
SYM_ARROW_START,
SYM_ARROW_COUNT,
SYM_AH_H_START,
SYM_AH_H_COUNT,
SYM_AH_V_START,
SYM_AH_V_COUNT,
SYM_AH_CENTER_LINE_LEFT,
SYM_AH_CENTER_LINE_RIGHT,
SYM_AH_CENTER,
SYM_HEADING_N,
SYM_HEADING_S,
SYM_HEADING_E,
SYM_HEADING_W,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_UP_UP,
SYM_UP,
SYM_DOWN,
SYM_DOWN_DOWN,
SYM_DEGREES_C,
SYM_DEGREES_F,
SYM_GPS_LAT,
SYM_GPS_LONG,
SYM_ARMED,
SYM_DISARMED,
SYM_ROLL0,
SYM_ROLLR,
SYM_ROLLL,
SYM_PTCH0,
SYM_PTCHUP,
SYM_PTCHDWN,
SYM_XERR,
SYM_KN,
SYM_NM,
SYM_DIST,
SYM_FLY,
SYM_EFF,
SYM_AH,
SYM_MW,
SYM_CLK,
SYM_KILO,
SYM_TERALT,
SYM_FENCE_ENABLED,
SYM_FENCE_DISABLED,
SYM_RNGFD,
SYM_LQ,
};
};

1
libraries/AP_OSD/AP_OSD_MAX7456.cpp
View File

@ -131,7 +131,6 @@
#endif
#define BWBRIGHTNESS ((BLACKBRIGHTNESS << 2) | WHITEBRIGHTNESS)
extern const AP_HAL::HAL &hal;
AP_OSD_MAX7456::AP_OSD_MAX7456(AP_OSD &osd, AP_HAL::OwnPtr<AP_HAL::Device> dev):

9
libraries/AP_OSD/AP_OSD_MSP.cpp
View File

@ -164,7 +164,7 @@ bool AP_OSD_MSP::init(void)
}
// override built in positions with defaults for MSP OSD
void AP_OSD_MSP::setup_defaults(void)
void AP_OSD_MSP::setup_defaults(void)
{
AP_Param::set_defaults_from_table(defaults_table, ARRAY_SIZE(defaults_table));
}
@ -180,9 +180,4 @@ AP_OSD_Backend *AP_OSD_MSP::probe(AP_OSD &osd)
return nullptr;
}
return backend;
}
AP_OSD_MSP::AP_OSD_MSP(AP_OSD &osd):
AP_OSD_Backend(osd)
{
}
}

5
libraries/AP_OSD/AP_OSD_MSP.h
View File

@ -3,7 +3,7 @@
class AP_OSD_MSP : public AP_OSD_Backend
{
using AP_OSD_Backend::AP_OSD_Backend;
public:
static AP_OSD_Backend *probe(AP_OSD &osd);
@ -20,8 +20,5 @@ public:
void clear() override {};
private:
//constructor
AP_OSD_MSP(AP_OSD &osd);
void setup_defaults(void);
};

129
libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp Normal file
View File

@ -0,0 +1,129 @@
/*
* This file 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 file 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/>.
*
*/
/*
OSD backend for MSP
*/
#include <AP_MSP/AP_MSP.h>
#include <AP_MSP/msp.h>
#include "AP_OSD_MSP_DisplayPort.h"
#if HAL_WITH_MSP_DISPLAYPORT
static const struct AP_Param::defaults_table_struct defaults_table[] = {
/*
{ "PARAM_NAME", value_float }
*/
};
extern const AP_HAL::HAL &hal;
constexpr uint8_t AP_OSD_MSP_DisplayPort::symbols[AP_OSD_NUM_SYMBOLS];
// initialise backend
bool AP_OSD_MSP_DisplayPort::init(void)
{
// check if we have a DisplayPort backend to use
const AP_MSP *msp = AP::msp();
if (msp == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING,"MSP backend not available");
return false;
}
_displayport = msp->find_protocol(AP_SerialManager::SerialProtocol_MSP_DisplayPort);
if (_displayport == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING,"MSP DisplayPort uart not available");
return false;
}
// re-init port here for use in this thread
_displayport->init_uart();
return true;
}
void AP_OSD_MSP_DisplayPort::clear(void)
{
// clear remote MSP screen
_displayport->msp_displayport_clear_screen();
// toggle flashing @2Hz
const uint32_t now = AP_HAL::millis();
if (((now / 500) & 0x01) != _blink_on) {
_blink_on = !_blink_on;
}
}
void AP_OSD_MSP_DisplayPort::write(uint8_t x, uint8_t y, const char* text)
{
_displayport->msp_displayport_write_string(x, y, 0, text);
}
void AP_OSD_MSP_DisplayPort::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...)
{
if (blink && !_blink_on) {
return;
}
char buf[32+1]; // +1 for snprintf null-termination
va_list ap;
va_start(ap, fmt);
int res = hal.util->vsnprintf(buf, sizeof(buf), fmt, ap);
res = MIN(res, int(sizeof(buf)));
if (res < int(sizeof(buf))-1) {
_displayport->msp_displayport_write_string(x, y, blink, buf);
}
va_end(ap);
}
void AP_OSD_MSP_DisplayPort::flush(void)
{
// grab the screen and force a redraw
_displayport->msp_displayport_grab();
_displayport->msp_displayport_draw_screen();
// ok done processing displayport data
// let's process incoming MSP frames (and reply if needed)
_displayport->process_incoming_data();
}
void AP_OSD_MSP_DisplayPort::init_symbol_set(uint8_t *lookup_table, const uint8_t size)
{
const AP_MSP *msp = AP::msp();
if (msp == nullptr) {
return;
}
// do we use backend specific symbols table?
if (msp->check_option(AP_MSP::MspOption::OPTION_DISPLAYPORT_BTFL_SYMBOLS)) {
memcpy(lookup_table, symbols, size);
} else {
memcpy(lookup_table, AP_OSD_Backend::symbols, size);
}
}
// override built in positions with defaults for MSP OSD
void AP_OSD_MSP_DisplayPort::setup_defaults(void)
{
AP_Param::set_defaults_from_table(defaults_table, ARRAY_SIZE(defaults_table));
}
AP_OSD_Backend *AP_OSD_MSP_DisplayPort::probe(AP_OSD &osd)
{
AP_OSD_MSP_DisplayPort *backend = new AP_OSD_MSP_DisplayPort(osd);
if (!backend) {
return nullptr;
}
if (!backend->init()) {
delete backend;
return nullptr;
}
return backend;
}
#endif

210
libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h Normal file
View File

@ -0,0 +1,210 @@
#include <AP_OSD/AP_OSD_Backend.h>
#include <AP_MSP/AP_MSP.h>
#if HAL_WITH_MSP_DISPLAYPORT
class AP_OSD_MSP_DisplayPort : public AP_OSD_Backend
{
using AP_OSD_Backend::AP_OSD_Backend;
public:
static AP_OSD_Backend *probe(AP_OSD &osd);
//initialize display port and underlying hardware
bool init() override;
//draw given text to framebuffer
void write(uint8_t x, uint8_t y, const char* text) override;
//draw formatted text to framebuffer
void write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...) override FMT_PRINTF(5, 6);
//flush framebuffer to screen
void flush() override;
//clear framebuffer
void clear() override;
// copy the backend specific symbol set to the OSD lookup table
void init_symbol_set(uint8_t *lookup_table, const uint8_t size) override;
private:
void setup_defaults(void);
AP_MSP_Telem_Backend* _displayport;
// MSP DisplayPort symbols
static const uint8_t SYM_M = 0x0C;
static const uint8_t SYM_KM = 0x7D;
static const uint8_t SYM_FT = 0x0F;
static const uint8_t SYM_MI = 0x7E;
static const uint8_t SYM_ALT_M = 0x0C;
static const uint8_t SYM_ALT_FT = 0x0F;
static const uint8_t SYM_BATT_FULL = 0x90;
static const uint8_t SYM_RSSI = 0x01;
static const uint8_t SYM_VOLT = 0x06;
static const uint8_t SYM_AMP = 0x9A;
static const uint8_t SYM_MAH = 0x07;
static const uint8_t SYM_MS = 0x9F;
static const uint8_t SYM_FS = 0x99;
static const uint8_t SYM_KMH = 0x9E;
static const uint8_t SYM_MPH = 0x9D;
static const uint8_t SYM_DEGR = 0x1D;
static const uint8_t SYM_PCNT = 0x25;
static const uint8_t SYM_RPM = 0x12;
static const uint8_t SYM_ASPD = 0x41;
static const uint8_t SYM_GSPD = 0x70;
static const uint8_t SYM_WSPD = 0x1B;
static const uint8_t SYM_VSPD = 0x7F;
static const uint8_t SYM_WPNO = 0x23;
static const uint8_t SYM_WPDIR = 0xE6;
static const uint8_t SYM_WPDST = 0xE7;
static const uint8_t SYM_FTMIN = 0xE8;
static const uint8_t SYM_FTSEC = 0x99;
static const uint8_t SYM_SAT_L = 0x1E;
static const uint8_t SYM_SAT_R = 0x1F;
static const uint8_t SYM_HDOP_L = 0x11;
static const uint8_t SYM_HDOP_R = 0x08;
static const uint8_t SYM_HOME = 0x05;
static const uint8_t SYM_WIND = 0x57;
static const uint8_t SYM_ARROW_START = 0x60;
static const uint8_t SYM_ARROW_COUNT = 16;
static const uint8_t SYM_AH_H_START = 0x80;
static const uint8_t SYM_AH_H_COUNT = 9;
static const uint8_t SYM_AH_V_START = 0x82;
static const uint8_t SYM_AH_V_COUNT = 6;
static const uint8_t SYM_AH_CENTER_LINE_LEFT = 0x84;
static const uint8_t SYM_AH_CENTER_LINE_RIGHT = 0x84;
static const uint8_t SYM_AH_CENTER = 0x2B;
static const uint8_t SYM_HEADING_N = 0x18;
static const uint8_t SYM_HEADING_S = 0x19;
static const uint8_t SYM_HEADING_E = 0x1A;
static const uint8_t SYM_HEADING_W = 0x1B;
static const uint8_t SYM_HEADING_DIVIDED_LINE = 0x1C;
static const uint8_t SYM_HEADING_LINE = 0x1D;
static const uint8_t SYM_UP_UP = 0x68;
static const uint8_t SYM_UP = 0x68;
static const uint8_t SYM_DOWN = 0x60;
static const uint8_t SYM_DOWN_DOWN = 0x60;
static const uint8_t SYM_DEGREES_C = 0x0E;
static const uint8_t SYM_DEGREES_F = 0x0D;
static const uint8_t SYM_GPS_LAT = 0x68;
static const uint8_t SYM_GPS_LONG = 0x6C;
static const uint8_t SYM_ARMED = 0x08;
static const uint8_t SYM_DISARMED = 0x08;
static const uint8_t SYM_ROLL0 = 0x2D;
static const uint8_t SYM_ROLLR = 0x64;
static const uint8_t SYM_ROLLL = 0x6C;
static const uint8_t SYM_PTCH0 = 0x7C;
static const uint8_t SYM_PTCHUP = 0x68;
static const uint8_t SYM_PTCHDWN = 0x60;
static const uint8_t SYM_XERR = 0xEE;
static const uint8_t SYM_KN = 0xF0;
static const uint8_t SYM_NM = 0xF1;
static const uint8_t SYM_DIST = 0x22;
static const uint8_t SYM_FLY = 0x9C;
static const uint8_t SYM_EFF = 0xF2;
static const uint8_t SYM_AH = 0xF3;
static const uint8_t SYM_MW = 0xF4;
static const uint8_t SYM_CLK = 0x08;
static const uint8_t SYM_KILO = 0x4B;
static const uint8_t SYM_TERALT = 0xEF;
static const uint8_t SYM_FENCE_ENABLED = 0xF5;
static const uint8_t SYM_FENCE_DISABLED = 0xF6;
static const uint8_t SYM_RNGFD = 0x72;
static const uint8_t SYM_LQ = 0xF8;
static constexpr uint8_t symbols[AP_OSD_NUM_SYMBOLS] {
SYM_M,
SYM_KM,
SYM_FT,
SYM_MI,
SYM_ALT_M,
SYM_ALT_FT,
SYM_BATT_FULL,
SYM_RSSI,
SYM_VOLT,
SYM_AMP,
SYM_MAH,
SYM_MS,
SYM_FS,
SYM_KMH,
SYM_MPH,
SYM_DEGR,
SYM_PCNT,
SYM_RPM,
SYM_ASPD,
SYM_GSPD,
SYM_WSPD,
SYM_VSPD,
SYM_WPNO,
SYM_WPDIR,
SYM_WPDST,
SYM_FTMIN,
SYM_FTSEC,
SYM_SAT_L,
SYM_SAT_R,
SYM_HDOP_L,
SYM_HDOP_R,
SYM_HOME,
SYM_WIND,
SYM_ARROW_START,
SYM_ARROW_COUNT,
SYM_AH_H_START,
SYM_AH_H_COUNT,
SYM_AH_V_START,
SYM_AH_V_COUNT,
SYM_AH_CENTER_LINE_LEFT,
SYM_AH_CENTER_LINE_RIGHT,
SYM_AH_CENTER,
SYM_HEADING_N,
SYM_HEADING_S,
SYM_HEADING_E,
SYM_HEADING_W,
SYM_HEADING_DIVIDED_LINE,
SYM_HEADING_LINE,
SYM_UP_UP,
SYM_UP,
SYM_DOWN,
SYM_DOWN_DOWN,
SYM_DEGREES_C,
SYM_DEGREES_F,
SYM_GPS_LAT,
SYM_GPS_LONG,
SYM_ARMED,
SYM_DISARMED,
SYM_ROLL0,
SYM_ROLLR,
SYM_ROLLL,
SYM_PTCH0,
SYM_PTCHUP,
SYM_PTCHDWN,
SYM_XERR,
SYM_KN,
SYM_NM,
SYM_DIST,
SYM_FLY,
SYM_EFF,
SYM_AH,
SYM_MW,
SYM_CLK,
SYM_KILO,
SYM_TERALT,
SYM_FENCE_ENABLED,
SYM_FENCE_DISABLED,
SYM_RNGFD,
SYM_LQ,
};
bool _blink_on;
};
#endif

2
libraries/AP_OSD/AP_OSD_ParamScreen.cpp
View File

@ -568,7 +568,7 @@ void AP_OSD_ParamScreen::update_state_machine()
}
}
#if HAL_WITH_OSD_BITMAP
#if HAL_WITH_OSD_BITMAP || HAL_WITH_MSP_DISPLAYPORT
void AP_OSD_ParamScreen::draw(void)
{
if (!enabled || !backend) {

409
libraries/AP_OSD/AP_OSD_Screen.cpp
View File

@ -1019,6 +1019,101 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info2[] = {
AP_GROUPEND
};
uint8_t AP_OSD_AbstractScreen::symbols_lookup_table[AP_OSD_NUM_SYMBOLS];
// Symbol indexes to acces _symbols[index][set]
#define SYM_M 0
#define SYM_KM 1
#define SYM_FT 2
#define SYM_MI 3
#define SYM_ALT_M 4
#define SYM_ALT_FT 5
#define SYM_BATT_FULL 6
#define SYM_RSSI 7
#define SYM_VOLT 8
#define SYM_AMP 9
#define SYM_MAH 10
#define SYM_MS 11
#define SYM_FS 12
#define SYM_KMH 13
#define SYM_MPH 14
#define SYM_DEGR 15
#define SYM_PCNT 16
#define SYM_RPM 17
#define SYM_ASPD 18
#define SYM_GSPD 19
#define SYM_WSPD 20
#define SYM_VSPD 21
#define SYM_WPNO 22
#define SYM_WPDIR 23
#define SYM_WPDST 24
#define SYM_FTMIN 25
#define SYM_FTSEC 26
#define SYM_SAT_L 27
#define SYM_SAT_R 28
#define SYM_HDOP_L 29
#define SYM_HDOP_R 30
#define SYM_HOME 31
#define SYM_WIND 32
#define SYM_ARROW_START 33
#define SYM_ARROW_COUNT 34
#define SYM_AH_H_START 35
#define SYM_AH_H_COUNT 36
#define SYM_AH_V_START 37
#define SYM_AH_V_COUNT 38
#define SYM_AH_CENTER_LINE_LEFT 39
#define SYM_AH_CENTER_LINE_RIGHT 40
#define SYM_AH_CENTER 41
#define SYM_HEADING_N 42
#define SYM_HEADING_S 43
#define SYM_HEADING_E 44
#define SYM_HEADING_W 45
#define SYM_HEADING_DIVIDED_LINE 46
#define SYM_HEADING_LINE 47
#define SYM_UP_UP 48
#define SYM_UP 49
#define SYM_DOWN 50
#define SYM_DOWN_DOWN 51
#define SYM_DEGREES_C 52
#define SYM_DEGREES_F 53
#define SYM_GPS_LAT 54
#define SYM_GPS_LONG 55
#define SYM_ARMED 56
#define SYM_DISARMED 57
#define SYM_ROLL0 58
#define SYM_ROLLR 59
#define SYM_ROLLL 60
#define SYM_PTCH0 61
#define SYM_PTCHUP 62
#define SYM_PTCHDWN 63
#define SYM_XERR 64
#define SYM_KN 65
#define SYM_NM 66
#define SYM_DIST 67
#define SYM_FLY 68
#define SYM_EFF 69
#define SYM_AH 70
#define SYM_MW 71
#define SYM_CLK 72
#define SYM_KILO 73
#define SYM_TERALT 74
#define SYM_FENCE_ENABLED 75
#define SYM_FENCE_DISABLED 76
#define SYM_RNGFD 77
#define SYM_LQ 78
#define SYMBOL(n) AP_OSD_AbstractScreen::symbols_lookup_table[n]
// constructor
AP_OSD_Screen::AP_OSD_Screen()
{
@ -1026,99 +1121,6 @@ AP_OSD_Screen::AP_OSD_Screen()
AP_Param::setup_object_defaults(this, var_info2);
}
//Symbols
#define SYM_M 0xB9
#define SYM_KM 0xBA
#define SYM_FT 0x0F
#define SYM_MI 0xBB
#define SYM_ALT_M 0xB1
#define SYM_ALT_FT 0xB3
#define SYM_BATT_FULL 0x90
#define SYM_RSSI 0x01
#define SYM_VOLT 0x06
#define SYM_AMP 0x9A
#define SYM_MAH 0x07
#define SYM_MS 0x9F
#define SYM_FS 0x99
#define SYM_KMH 0xA1
#define SYM_MPH 0xB0
#define SYM_DEGR 0xA8
#define SYM_PCNT 0x25
#define SYM_RPM 0xE0
#define SYM_ASPD 0xE1
#define SYM_GSPD 0xE2
#define SYM_WSPD 0xE3
#define SYM_VSPD 0xE4
#define SYM_WPNO 0xE5
#define SYM_WPDIR 0xE6
#define SYM_WPDST 0xE7
#define SYM_FTMIN 0xE8
#define SYM_FTSEC 0x99
#define SYM_SAT_L 0x1E
#define SYM_SAT_R 0x1F
#define SYM_HDOP_L 0xBD
#define SYM_HDOP_R 0xBE
#define SYM_HOME 0xBF
#define SYM_WIND 0x16
#define SYM_ARROW_START 0x60
#define SYM_ARROW_COUNT 16
#define SYM_AH_H_START 0x80
#define SYM_AH_H_COUNT 9
#define SYM_AH_V_START 0xCA
#define SYM_AH_V_COUNT 6
#define SYM_AH_CENTER_LINE_LEFT 0x26
#define SYM_AH_CENTER_LINE_RIGHT 0x27
#define SYM_AH_CENTER 0x7E
#define SYM_HEADING_N 0x18
#define SYM_HEADING_S 0x19
#define SYM_HEADING_E 0x1A
#define SYM_HEADING_W 0x1B
#define SYM_HEADING_DIVIDED_LINE 0x1C
#define SYM_HEADING_LINE 0x1D
#define SYM_UP_UP 0xA2
#define SYM_UP 0xA3
#define SYM_DOWN 0xA4
#define SYM_DOWN_DOWN 0xA5
#define SYM_DEGREES_C 0x0E
#define SYM_DEGREES_F 0x0D
#define SYM_GPS_LAT 0xA6
#define SYM_GPS_LONG 0xA7
#define SYM_ARMED 0x00
#define SYM_DISARMED 0xE9
#define SYM_ROLL0 0x2D
#define SYM_ROLLR 0xEA
#define SYM_ROLLL 0xEB
#define SYM_PTCH0 0x7C
#define SYM_PTCHUP 0xEC
#define SYM_PTCHDWN 0xED
#define SYM_XERR 0xEE
#define SYM_KN 0xF0
#define SYM_NM 0xF1
#define SYM_DIST 0x22
#define SYM_FLY 0x9C
#define SYM_EFF 0xF2
#define SYM_AH 0xF3
#define SYM_MW 0xF4
#define SYM_CLK 0xBC
#define SYM_KILO 0x4B
#define SYM_TERALT 0xEF
#define SYM_FENCE_ENABLED 0xF5
#define SYM_FENCE_DISABLED 0xF6
#define SYM_RNGFD 0xF7
#define SYM_LQ 0xF8
void AP_OSD_AbstractScreen::set_backend(AP_OSD_Backend *_backend)
{
backend = _backend;
@ -1135,45 +1137,45 @@ bool AP_OSD_AbstractScreen::check_option(uint32_t option)
*/
char AP_OSD_AbstractScreen::u_icon(enum unit_type unit)
{
static const char icons_metric[UNIT_TYPE_LAST] {
(char)SYM_ALT_M, //ALTITUDE
(char)SYM_KMH, //SPEED
(char)SYM_MS, //VSPEED
(char)SYM_M, //DISTANCE
(char)SYM_KM, //DISTANCE_LONG
(char)SYM_DEGREES_C //TEMPERATURE
static const uint8_t icons_metric[UNIT_TYPE_LAST] {
SYM_ALT_M, //ALTITUDE
SYM_KMH, //SPEED
SYM_MS, //VSPEED
SYM_M, //DISTANCE
SYM_KM, //DISTANCE_LONG
SYM_DEGREES_C //TEMPERATURE
};
static const char icons_imperial[UNIT_TYPE_LAST] {
(char)SYM_ALT_FT, //ALTITUDE
(char)SYM_MPH, //SPEED
(char)SYM_FS, //VSPEED
(char)SYM_FT, //DISTANCE
(char)SYM_MI, //DISTANCE_LONG
(char)SYM_DEGREES_F //TEMPERATURE
static const uint8_t icons_imperial[UNIT_TYPE_LAST] {
SYM_ALT_FT, //ALTITUDE
SYM_MPH, //SPEED
SYM_FS, //VSPEED
SYM_FT, //DISTANCE
SYM_MI, //DISTANCE_LONG
SYM_DEGREES_F //TEMPERATURE
};
static const char icons_SI[UNIT_TYPE_LAST] {
(char)SYM_ALT_M, //ALTITUDE
(char)SYM_MS, //SPEED
(char)SYM_MS, //VSPEED
(char)SYM_M, //DISTANCE
(char)SYM_KM, //DISTANCE_LONG
(char)SYM_DEGREES_C //TEMPERATURE
static const uint8_t icons_SI[UNIT_TYPE_LAST] {
SYM_ALT_M, //ALTITUDE
SYM_MS, //SPEED
SYM_MS, //VSPEED
SYM_M, //DISTANCE
SYM_KM, //DISTANCE_LONG
SYM_DEGREES_C //TEMPERATURE
};
static const char icons_aviation[UNIT_TYPE_LAST] {
(char)SYM_ALT_FT, //ALTITUDE Ft
(char)SYM_KN, //SPEED Knots
(char)SYM_FTMIN, //VSPEED
(char)SYM_FT, //DISTANCE
(char)SYM_NM, //DISTANCE_LONG Nm
(char)SYM_DEGREES_C //TEMPERATURE
static const uint8_t icons_aviation[UNIT_TYPE_LAST] {
SYM_ALT_FT, //ALTITUDE Ft
SYM_KN, //SPEED Knots
SYM_FTMIN, //VSPEED
SYM_FT, //DISTANCE
SYM_NM, //DISTANCE_LONG Nm
SYM_DEGREES_C //TEMPERATURE
};
static const char *icons[AP_OSD::UNITS_LAST] = {
static const uint8_t* icons[AP_OSD::UNITS_LAST] = {
icons_metric,
icons_imperial,
icons_SI,
icons_aviation,
};
return icons[constrain_int16(osd->units, 0, AP_OSD::UNITS_LAST-1)][unit];
return (char)SYMBOL(icons[constrain_int16(osd->units, 0, AP_OSD::UNITS_LAST-1)][unit]);
}
/*
@ -1253,7 +1255,7 @@ void AP_OSD_Screen::draw_bat_volt(uint8_t x, uint8_t y)
uint8_t pct = battery.capacity_remaining_pct();
uint8_t p = (100 - pct) / 16.6;
float v = battery.voltage();
backend->write(x,y, v < osd->warn_batvolt, "%c%2.1f%c", SYM_BATT_FULL + p, (double)v, SYM_VOLT);
backend->write(x,y, v < osd->warn_batvolt, "%c%2.1f%c", SYMBOL(SYM_BATT_FULL) + p, (double)v, SYMBOL(SYM_VOLT));
}
void AP_OSD_Screen::draw_avgcellvolt(uint8_t x, uint8_t y)
@ -1266,12 +1268,12 @@ void AP_OSD_Screen::draw_avgcellvolt(uint8_t x, uint8_t y)
osd->max_battery_voltage = MAX(osd->max_battery_voltage,v);
if (osd->cell_count > 0) {
v = v / osd->cell_count;
backend->write(x,y, v < osd->warn_avgcellvolt, "%c%1.2f%c", SYM_BATT_FULL + p, v, SYM_VOLT);
backend->write(x,y, v < osd->warn_avgcellvolt, "%c%1.2f%c", SYMBOL(SYM_BATT_FULL) + p, v, SYMBOL(SYM_VOLT));
} else if (osd->cell_count < 0) { // user must decide on autodetect cell count or manually entered to display this panel since default is -1
backend->write(x,y, false, "%c---%c", SYM_BATT_FULL + p, SYM_VOLT);
backend->write(x,y, false, "%c---%c", SYMBOL(SYM_BATT_FULL) + p, SYMBOL(SYM_VOLT));
} else { // use autodetected cell count
v = v / (uint8_t)(osd->max_battery_voltage * 0.2381 + 1);
backend->write(x,y, v < osd->warn_avgcellvolt, "%c%1.2f%c", SYM_BATT_FULL + p, v, SYM_VOLT);
backend->write(x,y, v < osd->warn_avgcellvolt, "%c%1.2f%c", SYMBOL(SYM_BATT_FULL) + p, v, SYMBOL(SYM_VOLT));
}
}
@ -1281,7 +1283,7 @@ void AP_OSD_Screen::draw_restvolt(uint8_t x, uint8_t y)
uint8_t pct = battery.capacity_remaining_pct();
uint8_t p = (100 - pct) / 16.6;
float v = battery.voltage_resting_estimate();
backend->write(x,y, v < osd->warn_restvolt, "%c%2.1f%c", SYM_BATT_FULL + p, (double)v, SYM_VOLT);
backend->write(x,y, v < osd->warn_restvolt, "%c%2.1f%c", SYMBOL(SYM_BATT_FULL) + p, (double)v, SYMBOL(SYM_VOLT));
}
@ -1291,7 +1293,7 @@ void AP_OSD_Screen::draw_rssi(uint8_t x, uint8_t y)
AP_RSSI *ap_rssi = AP_RSSI::get_singleton();
if (ap_rssi) {
const uint8_t rssiv = ap_rssi->read_receiver_rssi() * 99;
backend->write(x, y, rssiv < osd->warn_rssi, "%c%2d", SYM_RSSI, rssiv);
backend->write(x, y, rssiv < osd->warn_rssi, "%c%2d", SYMBOL(SYM_RSSI), rssiv);
}
}
@ -1301,9 +1303,9 @@ void AP_OSD_Screen::draw_link_quality(uint8_t x, uint8_t y)
if (ap_rssi) {
const int16_t lqv = ap_rssi->read_receiver_link_quality();
if (lqv < 0){
backend->write(x, y, false, "%c--", SYM_LQ);
backend->write(x, y, false, "%c--", SYMBOL(SYM_LQ));
} else {
backend->write(x, y, false, "%c%2d", SYM_LQ, lqv);
backend->write(x, y, false, "%c%2d", SYMBOL(SYM_LQ), lqv);
}
}
}
@ -1317,10 +1319,10 @@ void AP_OSD_Screen::draw_current(uint8_t instance, uint8_t x, uint8_t y)
//filter current and display with autoranging for low values
osd->avg_current_a= osd->avg_current_a + (amps - osd->avg_current_a) * 0.33;
if (osd->avg_current_a < 10.0) {
backend->write(x, y, false, "%2.2f%c", osd->avg_current_a, SYM_AMP);
backend->write(x, y, false, "%2.2f%c", osd->avg_current_a, SYMBOL(SYM_AMP));
}
else {
backend->write(x, y, false, "%2.1f%c", osd->avg_current_a, SYM_AMP);
backend->write(x, y, false, "%2.1f%c", osd->avg_current_a, SYMBOL(SYM_AMP));
}
}
@ -1334,9 +1336,9 @@ void AP_OSD_Screen::draw_fltmode(uint8_t x, uint8_t y)
AP_Notify * notify = AP_Notify::get_singleton();
char arm;
if (AP_Notify::flags.armed) {
arm = SYM_ARMED;
arm = SYMBOL(SYM_ARMED);
} else {
arm = SYM_DISARMED;
arm = SYMBOL(SYM_DISARMED);
}
if (notify) {
backend->write(x, y, false, "%s%c", notify->get_flight_mode_str(), arm);
@ -1348,7 +1350,7 @@ void AP_OSD_Screen::draw_sats(uint8_t x, uint8_t y)
AP_GPS & gps = AP::gps();
uint8_t nsat = gps.num_sats();
bool flash = (nsat < osd->warn_nsat) || (gps.status() < AP_GPS::GPS_OK_FIX_3D);
backend->write(x, y, flash, "%c%c%2u", SYM_SAT_L, SYM_SAT_R, nsat);
backend->write(x, y, flash, "%c%c%2u", SYMBOL(SYM_SAT_L), SYMBOL(SYM_SAT_R), nsat);
}
void AP_OSD_Screen::draw_batused(uint8_t instance, uint8_t x, uint8_t y)
@ -1358,10 +1360,10 @@ void AP_OSD_Screen::draw_batused(uint8_t instance, uint8_t x, uint8_t y)
mah = 0;
}
if (mah <= 9999) {
backend->write(x,y, false, "%4d%c", (int)mah, SYM_MAH);
backend->write(x,y, false, "%4d%c", (int)mah, SYMBOL(SYM_MAH));
} else {
const float ah = mah * 1e-3f;
backend->write(x,y, false, "%2.2f%c", (double)ah, SYM_AH);
backend->write(x,y, false, "%2.2f%c", (double)ah, SYMBOL(SYM_AH));
}
}
@ -1427,8 +1429,8 @@ void AP_OSD_Screen::draw_message(uint8_t x, uint8_t y)
// draw a arrow at the given angle, and print the given magnitude
void AP_OSD_Screen::draw_speed(uint8_t x, uint8_t y, float angle_rad, float magnitude)
{
static const int32_t interval = 36000 / SYM_ARROW_COUNT;
char arrow = SYM_ARROW_START + ((int32_t(angle_rad*DEGX100) + interval / 2) / interval) % SYM_ARROW_COUNT;
static const int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT);
char arrow = SYMBOL(SYM_ARROW_START) + ((int32_t(angle_rad*DEGX100) + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT);
if (u_scale(SPEED, magnitude) < 9.95) {
backend->write(x, y, false, "%c %1.1f%c", arrow, u_scale(SPEED, magnitude), u_icon(SPEED));
} else {
@ -1441,7 +1443,7 @@ void AP_OSD_Screen::draw_gspeed(uint8_t x, uint8_t y)
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
Vector2f v = ahrs.groundspeed_vector();
backend->write(x, y, false, "%c", SYM_GSPD);
backend->write(x, y, false, "%c", SYMBOL(SYM_GSPD));
float angle = 0;
const float length = v.length();
@ -1477,9 +1479,9 @@ void AP_OSD_Screen::draw_horizon(uint8_t x, uint8_t y)
for (int dx = -4; dx <= 4; dx++) {
float fy = (ratio * dx) * (ky/kx) + pitch * ah_pitch_rad_to_char + 0.5f;
int dy = floorf(fy);
char c = (fy - dy) * SYM_AH_H_COUNT;
char c = (fy - dy) * SYMBOL(SYM_AH_H_COUNT);
//chars in font in reversed order
c = SYM_AH_H_START + ((SYM_AH_H_COUNT - 1) - c);
c = SYMBOL(SYM_AH_H_START) + ((SYMBOL(SYM_AH_H_COUNT) - 1) - c);
if (dy >= -4 && dy <= 4) {
backend->write(x + dx, y - dy, false, "%c", c);
}
@ -1488,8 +1490,8 @@ void AP_OSD_Screen::draw_horizon(uint8_t x, uint8_t y)
for (int dy=-4; dy<=4; dy++) {
float fx = ((dy / ratio) - pitch * ah_pitch_rad_to_char) * (kx/ky) + 0.5f;
int dx = floorf(fx);
char c = (fx - dx) * SYM_AH_V_COUNT;
c = SYM_AH_V_START + c;
char c = (fx - dx) * SYMBOL(SYM_AH_V_COUNT);
c = SYMBOL(SYM_AH_V_START) + c;
if (dx >= -4 && dx <=4) {
backend->write(x + dx, y - dy, false, "%c", c);
}
@ -1497,7 +1499,7 @@ void AP_OSD_Screen::draw_horizon(uint8_t x, uint8_t y)
}
if (!check_option(AP_OSD::OPTION_DISABLE_CROSSHAIR)) {
backend->write(x-1,y, false, "%c%c%c", SYM_AH_CENTER_LINE_LEFT, SYM_AH_CENTER, SYM_AH_CENTER_LINE_RIGHT);
backend->write(x-1,y, false, "%c%c%c", SYMBOL(SYM_AH_CENTER_LINE_LEFT), SYMBOL(SYM_AH_CENTER), SYMBOL(SYM_AH_CENTER_LINE_RIGHT));
}
}
@ -1535,16 +1537,16 @@ void AP_OSD_Screen::draw_home(uint8_t x, uint8_t y)
const Location &home_loc = ahrs.get_home();
float distance = home_loc.get_distance(loc);
int32_t angle = wrap_360_cd(loc.get_bearing_to(home_loc) - ahrs.yaw_sensor);
int32_t interval = 36000 / SYM_ARROW_COUNT;
int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT);
if (distance < 2.0f) {
//avoid fast rotating arrow at small distances
angle = 0;
}
char arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;
backend->write(x, y, false, "%c%c", SYM_HOME, arrow);
char arrow = SYMBOL(SYM_ARROW_START) + ((angle + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT);
backend->write(x, y, false, "%c%c", SYMBOL(SYM_HOME), arrow);
draw_distance(x+2, y, distance);
} else {
backend->write(x, y, true, "%c", SYM_HOME);
backend->write(x, y, true, "%c", SYMBOL(SYM_HOME));
}
}
@ -1552,19 +1554,19 @@ void AP_OSD_Screen::draw_heading(uint8_t x, uint8_t y)
{
AP_AHRS &ahrs = AP::ahrs();
uint16_t yaw = ahrs.yaw_sensor / 100;
backend->write(x, y, false, "%3d%c", yaw, SYM_DEGR);
backend->write(x, y, false, "%3d%c", yaw, SYMBOL(SYM_DEGR));
}
void AP_OSD_Screen::draw_throttle(uint8_t x, uint8_t y)
{
backend->write(x, y, false, "%3d%c", gcs().get_hud_throttle(), SYM_PCNT);
backend->write(x, y, false, "%3d%c", gcs().get_hud_throttle(), SYMBOL(SYM_PCNT));
}
//Thanks to betaflight/inav for simple and clean compass visual design
void AP_OSD_Screen::draw_compass(uint8_t x, uint8_t y)
{
const int8_t total_sectors = 16;
static const char compass_circle[total_sectors] = {
static const uint8_t compass_circle[total_sectors] = {
SYM_HEADING_N,
SYM_HEADING_LINE,
SYM_HEADING_DIVIDED_LINE,
@ -1589,7 +1591,7 @@ void AP_OSD_Screen::draw_compass(uint8_t x, uint8_t y)
for (int8_t i = -4; i <= 4; i++) {
int8_t sector = center_sector + i;
sector = (sector + total_sectors) % total_sectors;
backend->write(x + i, y, false, "%c", compass_circle[sector]);
backend->write(x + i, y, false, "%c", SYMBOL(compass_circle[sector]));
}
}
@ -1616,7 +1618,7 @@ void AP_OSD_Screen::draw_wind(uint8_t x, uint8_t y)
}
#endif
backend->write(x, y, false, "%c", SYM_WSPD);
backend->write(x, y, false, "%c", SYMBOL(SYM_WSPD));
}
void AP_OSD_Screen::draw_aspeed(uint8_t x, uint8_t y)
@ -1626,9 +1628,9 @@ void AP_OSD_Screen::draw_aspeed(uint8_t x, uint8_t y)
WITH_SEMAPHORE(ahrs.get_semaphore());
bool have_estimate = ahrs.airspeed_estimate(aspd);
if (have_estimate) {
backend->write(x, y, false, "%c%4d%c", SYM_ASPD, (int)u_scale(SPEED, aspd), u_icon(SPEED));
backend->write(x, y, false, "%c%4d%c", SYMBOL(SYM_ASPD), (int)u_scale(SPEED, aspd), u_icon(SPEED));
} else {
backend->write(x, y, false, "%c ---%c", SYM_ASPD, u_icon(SPEED));
backend->write(x, y, false, "%c ---%c", SYMBOL(SYM_ASPD), u_icon(SPEED));
}
}
@ -1648,13 +1650,13 @@ void AP_OSD_Screen::draw_vspeed(uint8_t x, uint8_t y)
}
char sym;
if (vspd > 3.0f) {
sym = SYM_UP_UP;
sym = SYMBOL(SYM_UP_UP);
} else if (vspd >=0.0f) {
sym = SYM_UP;
sym = SYMBOL(SYM_UP);
} else if (vspd >= -3.0f) {
sym = SYM_DOWN;
sym = SYMBOL(SYM_DOWN);
} else {
sym = SYM_DOWN_DOWN;
sym = SYMBOL(SYM_DOWN_DOWN);
}
vs_scaled = u_scale(VSPEED, fabsf(vspd));
if ((osd->units != AP_OSD::UNITS_AVIATION) && (vs_scaled < 9.95f)) {
@ -1686,7 +1688,7 @@ void AP_OSD_Screen::draw_esc_rpm(uint8_t x, uint8_t y)
}
float krpm = rpm * 0.001f;
const char *format = krpm < 9.995 ? "%.2f%c%c" : (krpm < 99.95 ? "%.1f%c%c" : "%.0f%c%c");
backend->write(x, y, false, format, krpm, SYM_KILO, SYM_RPM);
backend->write(x, y, false, format, krpm, SYMBOL(SYM_KILO), SYMBOL(SYM_RPM));
}
void AP_OSD_Screen::draw_esc_amps(uint8_t x, uint8_t y)
@ -1696,7 +1698,7 @@ void AP_OSD_Screen::draw_esc_amps(uint8_t x, uint8_t y)
if (!AP::esc_telem().get_current(0, amps)) {
return;
}
backend->write(x, y, false, "%4.1f%c", amps, SYM_AMP);
backend->write(x, y, false, "%4.1f%c", amps, SYMBOL(SYM_AMP));
}
#endif
@ -1710,7 +1712,7 @@ void AP_OSD_Screen::draw_gps_latitude(uint8_t x, uint8_t y)
dec_portion = loc.lat / 10000000L;
frac_portion = abs_lat - labs(dec_portion)*10000000UL;
backend->write(x, y, false, "%c%4ld.%07ld", SYM_GPS_LAT, (long)dec_portion,(long)frac_portion);
backend->write(x, y, false, "%c%4ld.%07ld", SYMBOL(SYM_GPS_LAT), (long)dec_portion,(long)frac_portion);
}
void AP_OSD_Screen::draw_gps_longitude(uint8_t x, uint8_t y)
@ -1723,7 +1725,7 @@ void AP_OSD_Screen::draw_gps_longitude(uint8_t x, uint8_t y)
dec_portion = loc.lng / 10000000L;
frac_portion = abs_lon - labs(dec_portion)*10000000UL;
backend->write(x, y, false, "%c%4ld.%07ld", SYM_GPS_LONG, (long)dec_portion,(long)frac_portion);
backend->write(x, y, false, "%c%4ld.%07ld", SYMBOL(SYM_GPS_LONG), (long)dec_portion,(long)frac_portion);
}
void AP_OSD_Screen::draw_roll_angle(uint8_t x, uint8_t y)
@ -1732,13 +1734,13 @@ void AP_OSD_Screen::draw_roll_angle(uint8_t x, uint8_t y)
uint16_t roll = abs(ahrs.roll_sensor) / 100;
char r;
if (ahrs.roll_sensor > 50) {
r = SYM_ROLLR;
r = SYMBOL(SYM_ROLLR);
} else if (ahrs.roll_sensor < -50) {
r = SYM_ROLLL;
r = SYMBOL(SYM_ROLLL);
} else {
r = SYM_ROLL0;
r = SYMBOL(SYM_ROLL0);
}
backend->write(x, y, false, "%c%3d%c", r, roll, SYM_DEGR);
backend->write(x, y, false, "%c%3d%c", r, roll, SYMBOL(SYM_DEGR));
}
void AP_OSD_Screen::draw_pitch_angle(uint8_t x, uint8_t y)
@ -1747,13 +1749,13 @@ void AP_OSD_Screen::draw_pitch_angle(uint8_t x, uint8_t y)
uint16_t pitch = abs(ahrs.pitch_sensor) / 100;
char p;
if (ahrs.pitch_sensor > 50) {
p = SYM_PTCHUP;
p = SYMBOL(SYM_PTCHUP);
} else if (ahrs.pitch_sensor < -50) {
p = SYM_PTCHDWN;
p = SYMBOL(SYM_PTCHDWN);
} else {
p = SYM_PTCH0;
p = SYMBOL(SYM_PTCH0);
}
backend->write(x, y, false, "%c%3d%c", p, pitch, SYM_DEGR);
backend->write(x, y, false, "%c%3d%c", p, pitch, SYMBOL(SYM_DEGR));
}
void AP_OSD_Screen::draw_temp(uint8_t x, uint8_t y)
@ -1768,45 +1770,45 @@ void AP_OSD_Screen::draw_hdop(uint8_t x, uint8_t y)
{
AP_GPS & gps = AP::gps();
float hdp = gps.get_hdop() / 100.0f;
backend->write(x, y, false, "%c%c%3.2f", SYM_HDOP_L, SYM_HDOP_R, (double)hdp);
backend->write(x, y, false, "%c%c%3.2f", SYMBOL(SYM_HDOP_L), SYMBOL(SYM_HDOP_R), (double)hdp);
}
void AP_OSD_Screen::draw_waypoint(uint8_t x, uint8_t y)
{
AP_AHRS &ahrs = AP::ahrs();
int32_t angle = wrap_360_cd(osd->nav_info.wp_bearing - ahrs.yaw_sensor);
int32_t interval = 36000 / SYM_ARROW_COUNT;
int32_t interval = 36000 / SYMBOL(SYM_ARROW_COUNT);
if (osd->nav_info.wp_distance < 2.0f) {
//avoid fast rotating arrow at small distances
angle = 0;
}
char arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;
backend->write(x,y, false, "%c%2u%c",SYM_WPNO, osd->nav_info.wp_number, arrow);
char arrow = SYMBOL(SYM_ARROW_START) + ((angle + interval / 2) / interval) % SYMBOL(SYM_ARROW_COUNT);
backend->write(x,y, false, "%c%2u%c",SYMBOL(SYM_WPNO), osd->nav_info.wp_number, arrow);
draw_distance(x+4, y, osd->nav_info.wp_distance);
}
void AP_OSD_Screen::draw_xtrack_error(uint8_t x, uint8_t y)
{
backend->write(x, y, false, "%c", SYM_XERR);
backend->write(x, y, false, "%c", SYMBOL(SYM_XERR));
draw_distance(x+1, y, osd->nav_info.wp_xtrack_error);
}
void AP_OSD_Screen::draw_stat(uint8_t x, uint8_t y)
{
backend->write(x+2, y, false, "%c%c%c", 0x4d,0x41,0x58);
backend->write(x, y+1, false, "%c",SYM_GSPD);
backend->write(x, y+1, false, "%c",SYMBOL(SYM_GSPD));
backend->write(x+1, y+1, false, "%4d%c", (int)u_scale(SPEED, osd->max_speed_mps), u_icon(SPEED));
backend->write(x, y+2, false, "%5.1f%c", (double)osd->max_current_a, SYM_AMP);
backend->write(x, y+2, false, "%5.1f%c", (double)osd->max_current_a, SYMBOL(SYM_AMP));
backend->write(x, y+3, false, "%5d%c", (int)u_scale(ALTITUDE, osd->max_alt_m), u_icon(ALTITUDE));
backend->write(x, y+4, false, "%c", SYM_HOME);
backend->write(x, y+4, false, "%c", SYMBOL(SYM_HOME));
draw_distance(x+1, y+4, osd->max_dist_m);
backend->write(x, y+5, false, "%c", SYM_DIST);
backend->write(x, y+5, false, "%c", SYMBOL(SYM_DIST));
draw_distance(x+1, y+5, osd->last_distance_m);
}
void AP_OSD_Screen::draw_dist(uint8_t x, uint8_t y)
{
backend->write(x, y, false, "%c", SYM_DIST);
backend->write(x, y, false, "%c", SYMBOL(SYM_DIST));
draw_distance(x+1, y, osd->last_distance_m);
}
@ -1815,7 +1817,7 @@ void AP_OSD_Screen::draw_flightime(uint8_t x, uint8_t y)
AP_Stats *stats = AP::stats();
if (stats) {
uint32_t t = stats->get_flight_time_s();
backend->write(x, y, false, "%c%3u:%02u", SYM_FLY, unsigned(t/60), unsigned(t%60));
backend->write(x, y, false, "%c%3u:%02u", SYMBOL(SYM_FLY), unsigned(t/60), unsigned(t%60));
}
}
@ -1831,9 +1833,9 @@ void AP_OSD_Screen::draw_eff(uint8_t x, uint8_t y)
float speed = u_scale(SPEED,v.length());
float current_amps;
if ((speed > 2.0) && battery.current_amps(current_amps)) {
backend->write(x, y, false, "%c%3d%c", SYM_EFF,int(1000.0f*current_amps/speed),SYM_MAH);
backend->write(x, y, false, "%c%3d%c", SYMBOL(SYM_EFF),int(1000.0f*current_amps/speed),SYMBOL(SYM_MAH));
} else {
backend->write(x, y, false, "%c---%c", SYM_EFF,SYM_MAH);
backend->write(x, y, false, "%c---%c", SYMBOL(SYM_EFF),SYMBOL(SYM_MAH));
}
}
@ -1861,9 +1863,9 @@ void AP_OSD_Screen::draw_climbeff(uint8_t x, uint8_t y)
AP_BattMonitor &battery = AP::battery();
float amps;
if (battery.current_amps(amps) && is_positive(amps)) {
backend->write(x, y, false,"%c%c%3.1f%c",SYM_PTCHUP,SYM_EFF,(double)(3.6f * u_scale(VSPEED,vspd)/amps),unit_icon);
backend->write(x, y, false,"%c%c%3.1f%c",SYMBOL(SYM_PTCHUP),SYMBOL(SYM_EFF),(double)(3.6f * u_scale(VSPEED,vspd)/amps),unit_icon);
} else {
backend->write(x, y, false,"%c%c---%c",SYM_PTCHUP,SYM_EFF,unit_icon);
backend->write(x, y, false,"%c%c---%c",SYMBOL(SYM_PTCHUP),SYMBOL(SYM_EFF),unit_icon);
}
}
@ -1895,7 +1897,7 @@ void AP_OSD_Screen::draw_bat2_vlt(uint8_t x, uint8_t y)
uint8_t pct2 = battery.capacity_remaining_pct(1);
uint8_t p2 = (100 - pct2) / 16.6;
float v2 = battery.voltage(1);
backend->write(x,y, v2 < osd->warn_bat2volt, "%c%2.1f%c", SYM_BATT_FULL + p2, (double)v2, SYM_VOLT);
backend->write(x,y, v2 < osd->warn_bat2volt, "%c%2.1f%c", SYMBOL(SYM_BATT_FULL) + p2, (double)v2, SYMBOL(SYM_VOLT));
}
void AP_OSD_Screen::draw_bat2used(uint8_t x, uint8_t y)
@ -1911,9 +1913,9 @@ void AP_OSD_Screen::draw_aspd1(uint8_t x, uint8_t y)
}
float asp1 = airspeed->get_airspeed();
if (airspeed != nullptr && airspeed->healthy()) {
backend->write(x, y, false, "%c%4d%c", SYM_ASPD, (int)u_scale(SPEED, asp1), u_icon(SPEED));
backend->write(x, y, false, "%c%4d%c", SYMBOL(SYM_ASPD), (int)u_scale(SPEED, asp1), u_icon(SPEED));
} else {
backend->write(x, y, false, "%c ---%c", SYM_ASPD, u_icon(SPEED));
backend->write(x, y, false, "%c ---%c", SYMBOL(SYM_ASPD), u_icon(SPEED));
}
}
@ -1925,9 +1927,9 @@ void AP_OSD_Screen::draw_aspd2(uint8_t x, uint8_t y)
}
float asp2 = airspeed->get_airspeed(1);
if (airspeed != nullptr && airspeed->healthy(1)) {
backend->write(x, y, false, "%c%4d%c", SYM_ASPD, (int)u_scale(SPEED, asp2), u_icon(SPEED));
backend->write(x, y, false, "%c%4d%c", SYMBOL(SYM_ASPD), (int)u_scale(SPEED, asp2), u_icon(SPEED));
} else {
backend->write(x, y, false, "%c ---%c", SYM_ASPD, u_icon(SPEED));
backend->write(x, y, false, "%c ---%c", SYMBOL(SYM_ASPD), u_icon(SPEED));
}
}
@ -1937,9 +1939,9 @@ void AP_OSD_Screen::draw_clk(uint8_t x, uint8_t y)
uint8_t hour, min, sec;
uint16_t ms;
if (!rtc.get_local_time(hour, min, sec, ms)) {
backend->write(x, y, false, "%c--:--", SYM_CLK);
backend->write(x, y, false, "%c--:--", SYMBOL(SYM_CLK));
} else {
backend->write(x, y, false, "%c%02u:%02u", SYM_CLK, hour, min);
backend->write(x, y, false, "%c%02u:%02u", SYMBOL(SYM_CLK), hour, min);
}
}
@ -1999,7 +2001,7 @@ void AP_OSD_Screen::draw_vtx_power(uint8_t x, uint8_t y)
if(!vtx->has_option(AP_VideoTX::VideoOptions::VTX_PITMODE)){
powr = vtx->get_power_mw();
}
backend->write(x, y, !vtx->is_configuration_finished(), "%4hu%c", powr, SYM_MW);
backend->write(x, y, !vtx->is_configuration_finished(), "%4hu%c", powr, SYMBOL(SYM_MW));
}
#if AP_TERRAIN_AVAILABLE
void AP_OSD_Screen::draw_hgt_abvterr(uint8_t x, uint8_t y)
@ -2009,9 +2011,9 @@ void AP_OSD_Screen::draw_hgt_abvterr(uint8_t x, uint8_t y)
float terrain_altitude;
if (terrain != nullptr && terrain->height_above_terrain(terrain_altitude,true)) {
bool blink = (osd->warn_terr != -1)? (terrain_altitude < osd->warn_terr) : false; //blink if warn_terr is not disabled and alt above terrain is below warning value
backend->write(x, y, blink, "%4d%c%c", (int)u_scale(ALTITUDE, terrain_altitude), u_icon(ALTITUDE), SYM_TERALT);
backend->write(x, y, blink, "%4d%c%c", (int)u_scale(ALTITUDE, terrain_altitude), u_icon(ALTITUDE), SYMBOL(SYM_TERALT));
} else {
backend->write(x, y, false, " ---%c%c", u_icon(ALTITUDE),SYM_TERALT);
backend->write(x, y, false, " ---%c%c", u_icon(ALTITUDE),SYMBOL(SYM_TERALT));
}
}
#endif
@ -2024,9 +2026,9 @@ void AP_OSD_Screen::draw_fence(uint8_t x, uint8_t y)
return;
}
if (fenceptr->enabled() && fenceptr->present()) {
backend->write(x, y, fenceptr->get_breaches(), "%c", SYM_FENCE_ENABLED);
backend->write(x, y, fenceptr->get_breaches(), "%c", SYMBOL(SYM_FENCE_ENABLED));
} else {
backend->write(x, y, false, "%c", SYM_FENCE_DISABLED);
backend->write(x, y, false, "%c", SYMBOL(SYM_FENCE_DISABLED));
}
}
@ -2037,21 +2039,20 @@ void AP_OSD_Screen::draw_rngf(uint8_t x, uint8_t y)
return;
}
if (rangefinder->status_orient(ROTATION_PITCH_270) <= RangeFinder::Status::NoData) {
backend->write(x, y, false, "%cNO DATA", SYM_RNGFD);
backend->write(x, y, false, "%cNO DATA", SYMBOL(SYM_RNGFD));
} else {
backend->write(x, y, false, "%c%2.2f%c", SYM_RNGFD, u_scale(DISTANCE, (rangefinder->distance_cm_orient(ROTATION_PITCH_270) * 0.01f)), u_icon(DISTANCE));
backend->write(x, y, false, "%c%2.2f%c", SYMBOL(SYM_RNGFD), u_scale(DISTANCE, (rangefinder->distance_cm_orient(ROTATION_PITCH_270) * 0.01f)), u_icon(DISTANCE));
}
}
#define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
#if HAL_WITH_OSD_BITMAP
#if HAL_WITH_OSD_BITMAP || HAL_WITH_MSP_DISPLAYPORT
void AP_OSD_Screen::draw(void)
{
if (!enabled || !backend) {
return;
}
//Note: draw order should be optimized.
//Big and less important items should be drawn first,
//so they will not overwrite more important ones.