diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index 58e2b2910d..2b8020d74f 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -26,6 +26,7 @@ #include "AP_OSD_SITL.h" #endif #include "AP_OSD_MSP.h" +#include "AP_OSD_MSP_DisplayPort.h" #include #include #include @@ -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 } diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index 8481e61aa3..0a1bb5bb54 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -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, diff --git a/libraries/AP_OSD/AP_OSD_Backend.cpp b/libraries/AP_OSD/AP_OSD_Backend.cpp index 2dd6ded8df..f53f818006 100644 --- a/libraries/AP_OSD/AP_OSD_Backend.cpp +++ b/libraries/AP_OSD/AP_OSD_Backend.cpp @@ -19,6 +19,7 @@ #include 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); +} diff --git a/libraries/AP_OSD/AP_OSD_Backend.h b/libraries/AP_OSD/AP_OSD_Backend.h index 3e2f82dc87..feacb55c07 100644 --- a/libraries/AP_OSD/AP_OSD_Backend.h +++ b/libraries/AP_OSD/AP_OSD_Backend.h @@ -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, + }; }; - - diff --git a/libraries/AP_OSD/AP_OSD_MAX7456.cpp b/libraries/AP_OSD/AP_OSD_MAX7456.cpp index c4677aa7a1..c5830e7078 100644 --- a/libraries/AP_OSD/AP_OSD_MAX7456.cpp +++ b/libraries/AP_OSD/AP_OSD_MAX7456.cpp @@ -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 dev): diff --git a/libraries/AP_OSD/AP_OSD_MSP.cpp b/libraries/AP_OSD/AP_OSD_MSP.cpp index 8c32c69fcb..e3c1960089 100644 --- a/libraries/AP_OSD/AP_OSD_MSP.cpp +++ b/libraries/AP_OSD/AP_OSD_MSP.cpp @@ -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) -{ -} +} \ No newline at end of file diff --git a/libraries/AP_OSD/AP_OSD_MSP.h b/libraries/AP_OSD/AP_OSD_MSP.h index d64a39729d..984756cc3f 100644 --- a/libraries/AP_OSD/AP_OSD_MSP.h +++ b/libraries/AP_OSD/AP_OSD_MSP.h @@ -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); }; diff --git a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp new file mode 100644 index 0000000000..cc275d1616 --- /dev/null +++ b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp @@ -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 . + * + */ +/* + OSD backend for MSP + */ +#include +#include +#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 \ No newline at end of file diff --git a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h new file mode 100644 index 0000000000..5c5a3856cf --- /dev/null +++ b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.h @@ -0,0 +1,210 @@ +#include +#include + +#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 \ No newline at end of file diff --git a/libraries/AP_OSD/AP_OSD_ParamScreen.cpp b/libraries/AP_OSD/AP_OSD_ParamScreen.cpp index 68d6961360..8602f34319 100644 --- a/libraries/AP_OSD/AP_OSD_ParamScreen.cpp +++ b/libraries/AP_OSD/AP_OSD_ParamScreen.cpp @@ -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) { diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index a2cb5ae6da..34c868e9ba 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -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.