From 94aba33d10e148101d439ce3316a3d79c6fce982 Mon Sep 17 00:00:00 2001 From: yaapu Date: Mon, 6 Sep 2021 16:07:53 +0200 Subject: [PATCH] AP_OSD: added multi screen and stats support to DJI FPV OSD --- libraries/AP_OSD/AP_OSD.cpp | 84 +++++++++++++++------ libraries/AP_OSD/AP_OSD.h | 34 +++++++-- libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp | 5 +- libraries/AP_OSD/AP_OSD_Screen.cpp | 22 +++--- 4 files changed, 100 insertions(+), 45 deletions(-) diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index 2b8020d74f..e8949b8072 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -35,6 +35,7 @@ #include #include #include +#include // macro for easy use of var_info2 #define AP_SUBGROUPINFO2(element, name, idx, thisclazz, elclazz) { AP_PARAM_GROUP, idx, name, AP_VAROFFSET(thisclazz, element), { group_info : elclazz::var_info2 }, AP_PARAM_FLAG_NESTED_OFFSET } @@ -253,7 +254,7 @@ AP_OSD::AP_OSD() void AP_OSD::init() { - switch ((enum osd_types)osd_type.get()) { + switch (osd_types(osd_type.get())) { case OSD_NONE: case OSD_TXONLY: default: @@ -304,10 +305,10 @@ void AP_OSD::init() #endif } #if OSD_ENABLED - if (backend != nullptr && (enum osd_types)osd_type.get() != OSD_MSP) { + if (backend != nullptr) { // 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 + // create thread as higher priority than IO for all backends hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1280, AP_HAL::Scheduler::PRIORITY_IO, 1); } #endif @@ -327,63 +328,102 @@ void AP_OSD::update_osd() backend->clear(); if (!_disable) { - stats(); + update_stats(); update_current_screen(); get_screen(current_screen).set_backend(backend); - get_screen(current_screen).draw(); + // skip drawing for MSP OSD backends to save some resources + if (osd_types(osd_type.get()) != OSD_MSP) { + get_screen(current_screen).draw(); + } } backend->flush(); } //update maximums and totals -void AP_OSD::stats() +void AP_OSD::update_stats() { + // allow other threads to consume stats info + WITH_SEMAPHORE(_sem); + uint32_t now = AP_HAL::millis(); if (!AP_Notify::flags.armed) { - last_update_ms = now; + _stats.last_update_ms = now; return; } // flight distance - uint32_t delta_ms = now - last_update_ms; - last_update_ms = now; + uint32_t delta_ms = now - _stats.last_update_ms; + _stats.last_update_ms = now; - AP_AHRS &ahrs = AP::ahrs(); - Vector2f v = ahrs.groundspeed_vector(); + Vector2f v; + Location loc {}; + Location home_loc; + bool home_is_set; + bool have_airspeed_estimate; + float alt; + float aspd_mps = 0.0f; + { + // minimize semaphore scope + AP_AHRS &ahrs = AP::ahrs(); + WITH_SEMAPHORE(ahrs.get_semaphore()); + v = ahrs.groundspeed_vector(); + home_is_set = ahrs.get_position(loc) && ahrs.home_is_set(); + if (home_is_set) { + home_loc = ahrs.get_home(); + } + ahrs.get_relative_position_D_home(alt); + have_airspeed_estimate = ahrs.airspeed_estimate(aspd_mps); + } float speed = v.length(); if (speed < 0.178) { speed = 0.0; } float dist_m = (speed * delta_ms)*0.001; - last_distance_m += dist_m; + _stats.last_distance_m += dist_m; // maximum ground speed - max_speed_mps = fmaxf(max_speed_mps,speed); + _stats.max_speed_mps = fmaxf(_stats.max_speed_mps,speed); // maximum distance - Location loc; - if (ahrs.get_position(loc) && ahrs.home_is_set()) { - const Location &home_loc = ahrs.get_home(); + if (home_is_set) { float distance = home_loc.get_distance(loc); - max_dist_m = fmaxf(max_dist_m, distance); + _stats.max_dist_m = fmaxf(_stats.max_dist_m, distance); } // maximum altitude - float alt; - AP::ahrs().get_relative_position_D_home(alt); alt = -alt; - max_alt_m = fmaxf(max_alt_m, alt); + _stats.max_alt_m = fmaxf(_stats.max_alt_m, alt); // maximum current AP_BattMonitor &battery = AP::battery(); float amps; if (battery.current_amps(amps)) { - max_current_a = fmaxf(max_current_a, amps); + _stats.max_current_a = fmaxf(_stats.max_current_a, amps); } + // minimum voltage + float voltage = battery.voltage(); + if (voltage > 0) { + _stats.min_voltage_v = fminf(_stats.min_voltage_v, voltage); + } + // minimum rssi + AP_RSSI *ap_rssi = AP_RSSI::get_singleton(); + if (ap_rssi) { + _stats.min_rssi = fminf(_stats.min_rssi, ap_rssi->read_receiver_rssi()); + } + // max airspeed either true or synthetic + if (have_airspeed_estimate) { + _stats.max_airspeed_mps = fmaxf(_stats.max_airspeed_mps, aspd_mps); + } +#if HAL_WITH_ESC_TELEM + // max esc temp + AP_ESC_Telem& telem = AP::esc_telem(); + int16_t highest_temperature = 0; + telem.get_highest_motor_temperature(highest_temperature); + _stats.max_esc_temp = MAX(_stats.max_esc_temp, highest_temperature); +#endif } - //Thanks to minimosd authors for the multiple osd screen idea void AP_OSD::update_current_screen() { diff --git a/libraries/AP_OSD/AP_OSD.h b/libraries/AP_OSD/AP_OSD.h index 0a1bb5bb54..b3dfadc9ac 100644 --- a/libraries/AP_OSD/AP_OSD.h +++ b/libraries/AP_OSD/AP_OSD.h @@ -130,6 +130,7 @@ public: private: friend class AP_MSP; friend class AP_MSP_Telem_Backend; + friend class AP_MSP_Telem_DJI; static const uint8_t message_visible_width = 26; static const uint8_t message_scroll_time_ms = 200; @@ -503,7 +504,22 @@ public: uint16_t wp_number; }; + struct StatsInfo { + uint32_t last_update_ms; + float last_distance_m; + float max_dist_m; + float max_alt_m; + float max_speed_mps; + float max_airspeed_mps; + float max_current_a; + float avg_current_a; + float min_voltage_v = FLT_MAX; + float min_rssi = FLT_MAX; // 0-1 + int16_t max_esc_temp; + }; + void set_nav_info(NavInfo &nav_info); + const volatile StatsInfo& get_stats_info() const {return _stats;}; // disable the display void disable() { _disable = true; @@ -525,6 +541,8 @@ public: // Check whether arming is allowed bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; bool is_readonly_screen() const { return current_screen < AP_OSD_NUM_DISPLAY_SCREENS; } + // get the current screen + uint8_t get_current_screen() const { return current_screen; }; #endif // OSD_ENABLED #if OSD_PARAM_ENABLED AP_OSD_ParamScreen param_screen[AP_OSD_NUM_PARAM_SCREENS] { 0, 1 }; @@ -538,12 +556,16 @@ public: #endif // handle OSD parameter configuration void handle_msg(const mavlink_message_t &msg, const GCS_MAVLINK& link); + // allow threads to lock against OSD update + HAL_Semaphore &get_semaphore(void) { + return _sem; + } private: void osd_thread(); #if OSD_ENABLED void update_osd(); - void stats(); + void update_stats(); void update_current_screen(); void next_screen(); @@ -559,17 +581,13 @@ private: bool was_failsafe; bool _disable; - uint32_t last_update_ms; - float last_distance_m; - float max_dist_m; - float max_alt_m; - float max_speed_mps; - float max_current_a; - float avg_current_a; + StatsInfo _stats; #endif AP_OSD_Backend *backend; static AP_OSD *_singleton; + // multi-thread access support + HAL_Semaphore _sem; }; namespace AP diff --git a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp index cc275d1616..1ea73116d9 100644 --- a/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp +++ b/libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp @@ -97,11 +97,8 @@ void AP_OSD_MSP_DisplayPort::flush(void) 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)) { + if (msp && msp->is_option_enabled(AP_MSP::Option::DISPLAYPORT_BTFL_SYMBOLS)) { memcpy(lookup_table, symbols, size); } else { memcpy(lookup_table, AP_OSD_Backend::symbols, size); diff --git a/libraries/AP_OSD/AP_OSD_Screen.cpp b/libraries/AP_OSD/AP_OSD_Screen.cpp index f485f58995..484d22033b 100644 --- a/libraries/AP_OSD/AP_OSD_Screen.cpp +++ b/libraries/AP_OSD/AP_OSD_Screen.cpp @@ -1324,15 +1324,15 @@ void AP_OSD_Screen::draw_current(uint8_t instance, uint8_t x, uint8_t y) { float amps; if (!AP::battery().current_amps(amps, instance)) { - osd->avg_current_a = 0; + osd->_stats.avg_current_a = 0; } //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, SYMBOL(SYM_AMP)); + osd->_stats.avg_current_a= osd->_stats.avg_current_a + (amps - osd->_stats.avg_current_a) * 0.33; + if (osd->_stats.avg_current_a < 10.0) { + backend->write(x, y, false, "%2.2f%c", osd->_stats.avg_current_a, SYMBOL(SYM_AMP)); } else { - backend->write(x, y, false, "%2.1f%c", osd->avg_current_a, SYMBOL(SYM_AMP)); + backend->write(x, y, false, "%2.1f%c", osd->_stats.avg_current_a, SYMBOL(SYM_AMP)); } } @@ -1807,19 +1807,19 @@ 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",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, 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+1, y+1, false, "%4d%c", (int)u_scale(SPEED, osd->_stats.max_speed_mps), u_icon(SPEED)); + backend->write(x, y+2, false, "%5.1f%c", (double)osd->_stats.max_current_a, SYM_AMP); + backend->write(x, y+3, false, "%5d%c", (int)u_scale(ALTITUDE, osd->_stats.max_alt_m), u_icon(ALTITUDE)); backend->write(x, y+4, false, "%c", SYMBOL(SYM_HOME)); - draw_distance(x+1, y+4, osd->max_dist_m); + draw_distance(x+1, y+4, osd->_stats.max_dist_m); backend->write(x, y+5, false, "%c", SYMBOL(SYM_DIST)); - draw_distance(x+1, y+5, osd->last_distance_m); + draw_distance(x+1, y+5, osd->_stats.last_distance_m); } void AP_OSD_Screen::draw_dist(uint8_t x, uint8_t y) { backend->write(x, y, false, "%c", SYMBOL(SYM_DIST)); - draw_distance(x+1, y, osd->last_distance_m); + draw_distance(x+1, y, osd->_stats.last_distance_m); } void AP_OSD_Screen::draw_flightime(uint8_t x, uint8_t y)