From 9f0d9a5652fc6f47b09e79185153c4d3a533d1c4 Mon Sep 17 00:00:00 2001 From: yaapu Date: Mon, 6 Sep 2021 16:07:39 +0200 Subject: [PATCH] AP_MSP: added multi screen and stats support to DJI FPV OSD --- libraries/AP_MSP/AP_MSP.cpp | 88 +++++---- libraries/AP_MSP/AP_MSP.h | 17 +- libraries/AP_MSP/AP_MSP_Telem_Backend.cpp | 102 ++++++++--- libraries/AP_MSP/AP_MSP_Telem_Backend.h | 16 +- libraries/AP_MSP/AP_MSP_Telem_DJI.cpp | 171 ++++++++++++++++-- libraries/AP_MSP/AP_MSP_Telem_DJI.h | 8 + libraries/AP_MSP/AP_MSP_Telem_DisplayPort.cpp | 2 +- libraries/AP_MSP/Tools/msposd.py | 4 +- libraries/AP_MSP/Tools/osdtest.parm | 76 ++++++-- libraries/AP_MSP/Tools/pymsp.py | 1 + 10 files changed, 369 insertions(+), 116 deletions(-) diff --git a/libraries/AP_MSP/AP_MSP.cpp b/libraries/AP_MSP/AP_MSP.cpp index 95dab343d2..31ef2084fd 100644 --- a/libraries/AP_MSP/AP_MSP.cpp +++ b/libraries/AP_MSP/AP_MSP.cpp @@ -43,9 +43,9 @@ const AP_Param::GroupInfo AP_MSP::var_info[] = { // @Param: _OPTIONS // @DisplayName: MSP OSD Options // @Description: A bitmask to set some MSP specific options - // @Bitmask: 0:EnableTelemetryMode, 1: DJIWorkarounds + // @Bitmask: 0:EnableTelemetryMode, 1: DisableDJIWorkarounds, 2:EnableBTFLFonts // @User: Standard - AP_GROUPINFO("_OPTIONS", 2, AP_MSP, _options, (uint8_t)MspOption::OPTION_TELEMETRY_DJI_WORKAROUNDS), + AP_GROUPINFO("_OPTIONS", 2, AP_MSP, _options, 0), AP_GROUPEND }; @@ -93,10 +93,6 @@ void AP_MSP::init() if (!init_backend(_msp_status.backend_count, uart, AP_SerialManager::SerialProtocol_DJI_FPV)) { break; } - // initialize osd settings from OSD backend - if (!_msp_status.osd_initialized) { - init_osd(); - } if (_backends[_msp_status.backend_count]->use_msp_thread()) { backends_using_msp_thread++; } @@ -141,7 +137,7 @@ void AP_MSP::init() } } -void AP_MSP::init_osd() +void AP_MSP::update_osd_item_settings() { #if OSD_ENABLED AP_OSD* osd = AP::osd(); @@ -149,35 +145,34 @@ void AP_MSP::init_osd() if (osd == nullptr) { return; } - - _osd_item_settings[OSD_RSSI_VALUE] = &osd->screen[0].rssi; - _osd_item_settings[OSD_MAIN_BATT_VOLTAGE] = &osd->screen[0].bat_volt; - _osd_item_settings[OSD_CROSSHAIRS] = &osd->screen[0].crosshair; - _osd_item_settings[OSD_ARTIFICIAL_HORIZON] = &osd->screen[0].horizon; - _osd_item_settings[OSD_HORIZON_SIDEBARS] = &osd->screen[0].sidebars; - _osd_item_settings[OSD_CRAFT_NAME] = &osd->screen[0].message; - _osd_item_settings[OSD_FLYMODE] = &osd->screen[0].fltmode; - _osd_item_settings[OSD_CURRENT_DRAW] = &osd->screen[0].current; - _osd_item_settings[OSD_MAH_DRAWN] = &osd->screen[0].batused; - _osd_item_settings[OSD_GPS_SPEED] = &osd->screen[0].gspeed; - _osd_item_settings[OSD_GPS_SATS] = &osd->screen[0].sats; - _osd_item_settings[OSD_ALTITUDE] = &osd->screen[0].altitude; - _osd_item_settings[OSD_POWER] = &osd->screen[0].power; - _osd_item_settings[OSD_AVG_CELL_VOLTAGE] = &osd->screen[0].cell_volt; - _osd_item_settings[OSD_GPS_LON] = &osd->screen[0].gps_longitude; - _osd_item_settings[OSD_GPS_LAT] = &osd->screen[0].gps_latitude; - _osd_item_settings[OSD_PITCH_ANGLE] = &osd->screen[0].pitch_angle; - _osd_item_settings[OSD_ROLL_ANGLE] = &osd->screen[0].roll_angle; - _osd_item_settings[OSD_MAIN_BATT_USAGE] = &osd->screen[0].batt_bar; - _osd_item_settings[OSD_DISARMED] = &osd->screen[0].arming; - _osd_item_settings[OSD_HOME_DIR] = &osd->screen[0].home_dir; - _osd_item_settings[OSD_HOME_DIST] = &osd->screen[0].home_dist; - _osd_item_settings[OSD_NUMERICAL_HEADING] = &osd->screen[0].heading; - _osd_item_settings[OSD_NUMERICAL_VARIO] = &osd->screen[0].vspeed; + _osd_item_settings[OSD_RSSI_VALUE] = &osd->screen[_msp_status.current_screen].rssi; // OSDn_RSSI + _osd_item_settings[OSD_MAIN_BATT_VOLTAGE] = &osd->screen[_msp_status.current_screen].bat_volt; // OSDn_BAT_VOLT + _osd_item_settings[OSD_CROSSHAIRS] = &osd->screen[_msp_status.current_screen].crosshair; // OSDn_CRSSHAIR + _osd_item_settings[OSD_ARTIFICIAL_HORIZON] = &osd->screen[_msp_status.current_screen].horizon; // OSDn_HORIZON + _osd_item_settings[OSD_HORIZON_SIDEBARS] = &osd->screen[_msp_status.current_screen].sidebars; // OSDn_SIDEBARS + _osd_item_settings[OSD_CRAFT_NAME] = &osd->screen[_msp_status.current_screen].message; // OSDn_MESSAGE + _osd_item_settings[OSD_FLYMODE] = &osd->screen[_msp_status.current_screen].fltmode; // OSDn_FLTMODE + _osd_item_settings[OSD_CURRENT_DRAW] = &osd->screen[_msp_status.current_screen].current; // OSDn_CURRENT + _osd_item_settings[OSD_MAH_DRAWN] = &osd->screen[_msp_status.current_screen].batused; // OSDn_BATUSED + _osd_item_settings[OSD_GPS_SPEED] = &osd->screen[_msp_status.current_screen].gspeed; // OSDn_GSPEED + _osd_item_settings[OSD_GPS_SATS] = &osd->screen[_msp_status.current_screen].sats; // OSDn_SATS + _osd_item_settings[OSD_ALTITUDE] = &osd->screen[_msp_status.current_screen].altitude; // OSDn_ALTITUDE + _osd_item_settings[OSD_POWER] = &osd->screen[_msp_status.current_screen].power; // OSDn_POWER + _osd_item_settings[OSD_AVG_CELL_VOLTAGE] = &osd->screen[_msp_status.current_screen].cell_volt; // OSDn_CELLVOLT + _osd_item_settings[OSD_GPS_LON] = &osd->screen[_msp_status.current_screen].gps_longitude; // OSDn_GPSLONG + _osd_item_settings[OSD_GPS_LAT] = &osd->screen[_msp_status.current_screen].gps_latitude; // OSDn_GPSLAT + _osd_item_settings[OSD_PITCH_ANGLE] = &osd->screen[_msp_status.current_screen].pitch_angle; // OSDn_PITCH + _osd_item_settings[OSD_ROLL_ANGLE] = &osd->screen[_msp_status.current_screen].roll_angle; // OSDn_ROLL + _osd_item_settings[OSD_MAIN_BATT_USAGE] = &osd->screen[_msp_status.current_screen].batt_bar; // OSDn_BATBAR + _osd_item_settings[OSD_DISARMED] = &osd->screen[_msp_status.current_screen].arming; // OSDn_ARMING + _osd_item_settings[OSD_HOME_DIR] = &osd->screen[_msp_status.current_screen].home_dir; // OSDn_HOMEDIR + _osd_item_settings[OSD_HOME_DIST] = &osd->screen[_msp_status.current_screen].home_dist; // OSDn_HOMEDIST + _osd_item_settings[OSD_NUMERICAL_HEADING] = &osd->screen[_msp_status.current_screen].heading; // OSDn_HEADING + _osd_item_settings[OSD_NUMERICAL_VARIO] = &osd->screen[_msp_status.current_screen].vspeed; // OSDn_VSPEED #if HAL_WITH_ESC_TELEM - _osd_item_settings[OSD_ESC_TMP] = &osd->screen[0].esc_temp; + _osd_item_settings[OSD_ESC_TMP] = &osd->screen[_msp_status.current_screen].esc_temp; // OSDn_ESCTEMP #endif - _osd_item_settings[OSD_RTC_DATETIME] = &osd->screen[0].clk; + _osd_item_settings[OSD_RTC_DATETIME] = &osd->screen[_msp_status.current_screen].clk; // OSDn_CLK #endif // OSD_ENABLED _msp_status.osd_initialized = true; } @@ -196,10 +191,13 @@ void AP_MSP::loop(void) hal.scheduler->delay(10); // 115200 baud, 18 MSP packets @4Hz, 100Hz should be OK const uint32_t now = AP_HAL::millis(); - // toggle flashing every 0.7 seconds - if (((now / 700) & 0x01) != _msp_status.flashing_on) { + // toggle flashing every 0.7 seconds and every 2 seconds + if ((uint32_t(now * 0.00143) & 0x01) != _msp_status.flashing_on) { _msp_status.flashing_on = !_msp_status.flashing_on; } + if ((uint32_t(now * 0.0005) & 0x01) != _msp_status.slow_flashing_on) { + _msp_status.slow_flashing_on = !_msp_status.slow_flashing_on; + } // detect flight mode changes and steal focus from text messages if (AP::notify().flags.flight_mode != _msp_status.last_flight_mode) { @@ -210,6 +208,19 @@ void AP_MSP::loop(void) _msp_status.flight_mode_focus = false; } +#if OSD_ENABLED + // check if we had a screen change + AP_OSD* osd = AP::osd(); + + if (osd != nullptr) { + const uint8_t screen = osd->is_readonly_screen() ? osd->get_current_screen() : _msp_status.current_screen; + if (_msp_status.current_screen != screen || !_msp_status.osd_initialized) { + _msp_status.current_screen = screen; + update_osd_item_settings(); + } + } +#endif // OSD_ENABLED + for (uint8_t i=0; i< _msp_status.backend_count; i++) { // note: we do not access a uart for a backend handled by another thread if (_backends[i] != nullptr && _backends[i]->use_msp_thread()) { @@ -233,6 +244,11 @@ AP_MSP_Telem_Backend* AP_MSP::find_protocol(const AP_SerialManager::SerialProtoc return nullptr; } +bool AP_MSP::is_option_enabled(Option option) const +{ + return (_options & (uint8_t)option) != 0; +} + namespace AP { AP_MSP *msp() diff --git a/libraries/AP_MSP/AP_MSP.h b/libraries/AP_MSP/AP_MSP.h index 3b749365d1..0a7f87dd07 100644 --- a/libraries/AP_MSP/AP_MSP.h +++ b/libraries/AP_MSP/AP_MSP.h @@ -53,13 +53,13 @@ public: // init - perform required initialisation void init(); - enum class MspOption : uint8_t { - OPTION_TELEMETRY_MODE = 1U<<0, - OPTION_TELEMETRY_DJI_WORKAROUNDS = 1U<<1, - OPTION_DISPLAYPORT_BTFL_SYMBOLS = 1U<<2, + enum class Option : uint8_t { + TELEMETRY_MODE = 1U<<0, + TELEMETRY_DISABLE_DJI_WORKAROUNDS = 1U<<1, + DISPLAYPORT_BTFL_SYMBOLS = 1U<<2, }; - bool check_option(const MspOption option) const { return (_options & (uint8_t)option) != 0; }; + bool is_option_enabled(const Option option) const; static AP_MSP *get_singleton(void) { @@ -67,7 +67,6 @@ public: } private: - AP_MSP_Telem_Backend *_backends[MSP_MAX_INSTANCES]; AP_Int8 _options; @@ -78,16 +77,18 @@ private: MSP::osd_config_t _osd_config; struct { - bool flashing_on; // OSD item flashing support + bool flashing_on; // OSD item flashing support @1.4Hz + bool slow_flashing_on; // OSD item flashing support @0.5H uint8_t last_flight_mode = 255; uint32_t last_flight_mode_change_ms; bool flight_mode_focus; // do we need to steal focus from text messages bool osd_initialized; // for one time osd initialization uint8_t backend_count; // actual count of active bacends + uint8_t current_screen; // defaults to screen 0 } _msp_status; bool init_backend(uint8_t backend_idx, AP_HAL::UARTDriver *uart, AP_SerialManager::SerialProtocol protocol); - void init_osd(); + void update_osd_item_settings(); void loop(void); AP_MSP_Telem_Backend* find_protocol(const AP_SerialManager::SerialProtocol protocol) const; diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index 599fa40947..2eab6217b3 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -158,7 +158,7 @@ uint8_t AP_MSP_Telem_Backend::calc_cell_count(const float battery_voltage) return floorf((battery_voltage / CELLFULL) + 1); } -float AP_MSP_Telem_Backend::get_vspeed_ms(void) +float AP_MSP_Telem_Backend::get_vspeed_ms(void) const { { // release semaphore as soon as possible @@ -254,7 +254,7 @@ void AP_MSP_Telem_Backend::update_airspeed(airspeed_state_t &airspeed_state) MSP OSDs can display up to MSP_TXT_VISIBLE_CHARS chars (UTF8 characters are supported) We display the flight mode string either with or without wind state */ -void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wind_enabled) +void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, uint8_t size, bool wind_enabled) { #if OSD_ENABLED AP_OSD *osd = AP::osd(); @@ -267,7 +267,7 @@ void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wi return; } // clear - memset(flight_mode_str, 0, MSP_TXT_BUFFER_SIZE); + memset(flight_mode_str, 0, size); if (wind_enabled) { /* @@ -282,7 +282,12 @@ void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wi } bool invert_wind = false; #if OSD_ENABLED - invert_wind = osd->screen[0].check_option(AP_OSD::OPTION_INVERTED_WIND); + AP_MSP *msp = AP::msp(); + if (msp == nullptr) { + return; + } + + invert_wind = osd->screen[msp->_msp_status.current_screen].check_option(AP_OSD::OPTION_INVERTED_WIND); #endif if (invert_wind) { v = -v; @@ -299,9 +304,9 @@ void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wi const int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.y, v.x) - ahrs.yaw_sensor); const int32_t interval = 36000 / ARRAY_SIZE(arrows); uint8_t arrow = arrows[((angle + interval / 2) / interval) % ARRAY_SIZE(arrows)]; - snprintf(flight_mode_str, MSP_TXT_BUFFER_SIZE, "%s %d%s%c%c%c", notify->get_flight_mode_str(), (uint8_t)roundf(v_length), unit, 0xE2, 0x86, arrow); + snprintf(flight_mode_str, size, "%s %d%s%c%c%c", notify->get_flight_mode_str(), (uint8_t)roundf(v_length), unit, 0xE2, 0x86, arrow); } else { - snprintf(flight_mode_str, MSP_TXT_BUFFER_SIZE, "%s ---%s", notify->get_flight_mode_str(), unit); + snprintf(flight_mode_str, size, "%s ---%s", notify->get_flight_mode_str(), unit); } } else { /* @@ -312,16 +317,10 @@ void AP_MSP_Telem_Backend::update_flight_mode_str(char *flight_mode_str, bool wi MANU [SS] */ #ifndef HAL_NO_GCS - const bool simple_mode = gcs().simple_input_active(); - const bool supersimple_mode = gcs().supersimple_input_active(); - const char* simple_mode_str = simple_mode ? " [S]" : (supersimple_mode ? " [SS]" : ""); - - char buffer[MSP_TXT_BUFFER_SIZE] {}; - // flightmode - const uint8_t used = snprintf(buffer, ARRAY_SIZE(buffer), "%s%s", notify->get_flight_mode_str(), simple_mode_str); - // left pad - uint8_t left_padded_len = MSP_TXT_VISIBLE_CHARS - (MSP_TXT_VISIBLE_CHARS - used)/2; - snprintf(flight_mode_str, MSP_TXT_BUFFER_SIZE, "%*s", left_padded_len, buffer); + const char* simple_mode_str = gcs().simple_input_active() ? " [S]" : (gcs().supersimple_input_active() ? " [SS]" : ""); + snprintf(flight_mode_str, size, "%s%s", notify->get_flight_mode_str(), simple_mode_str); +#else + snprintf(flight_mode_str, size, "%s", notify->get_flight_mode_str()); #endif } } @@ -628,7 +627,12 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_raw_gps(sbuf_t *dst) // handle airspeed override bool airspeed_en = false; #if OSD_ENABLED - airspeed_en = osd->screen[0].aspeed.enabled; + AP_MSP *msp = AP::msp(); + if (msp == nullptr) { + return MSP_RESULT_ERROR; + } + + airspeed_en = osd->screen[msp->_msp_status.current_screen].aspeed.enabled; #endif if (airspeed_en) { airspeed_state_t airspeed_state; @@ -736,9 +740,9 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_name(sbuf_t *dst) bool wind_en = false; char flight_mode_str[MSP_TXT_BUFFER_SIZE]; #if OSD_ENABLED - wind_en = osd->screen[0].wind.enabled; + wind_en = osd->screen[msp->_msp_status.current_screen].wind.enabled; #endif - update_flight_mode_str(flight_mode_str, wind_en); + update_flight_mode_str(flight_mode_str, ARRAY_SIZE(flight_mode_str), wind_en); sbuf_write_data(dst, flight_mode_str, ARRAY_SIZE(flight_mode_str)); // rendered as up to MSP_TXT_VISIBLE_CHARS chars with UTF8 support } } @@ -889,28 +893,25 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_altitude(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_analog(sbuf_t *dst) { - AP_RSSI* rssi = AP::rssi(); - if (rssi == nullptr) { - return MSP_RESULT_ERROR; - } battery_state_t battery_state; update_battery_state(battery_state); + float rssi; const struct PACKED { uint8_t voltage_dv; uint16_t mah; uint16_t rssi; int16_t current_ca; uint16_t voltage_cv; - } battery { + } analog { voltage_dv : (uint8_t)constrain_int16(battery_state.batt_voltage_v * 10, 0, 255), // battery voltage V to dV mah : (uint16_t)constrain_int32(battery_state.batt_consumed_mah, 0, 0xFFFF), // milliamp hours drawn from battery - rssi : uint16_t(rssi->enabled() ? rssi->read_receiver_rssi() * 1023 : 0), // rssi 0-1 to 0-1023 + rssi : uint16_t(get_rssi(rssi) ? (uint16_t)constrain_float(rssi,0,1) * 1023 : 0), // rssi 0-1 to 0-1023), // rssi 0-1 to 0-1023 current_ca : (int16_t)constrain_int32(battery_state.batt_current_a * 100, -0x8000, 0x7FFF), // current A to cA (0.01 steps, range is -320A to 320A) voltage_cv : (uint16_t)constrain_int32(battery_state.batt_voltage_v * 100,0,0xFFFF) // battery voltage in 0.01V steps }; - sbuf_write_data(dst, &battery, sizeof(battery)); + sbuf_write_data(dst, &analog, sizeof(analog)); return MSP_RESULT_ACK; } @@ -962,8 +963,8 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(sbuf_t *d for (uint8_t i = 0; i < esc_sensor.num_motors; i++) { int16_t temp = 0; float rpm = 0.0f; - telem.get_rpm(i, rpm); - telem.get_temperature(i, temp); + IGNORE_RETURN(telem.get_rpm(i, rpm)); + IGNORE_RETURN(telem.get_temperature(i, temp)); esc_sensor.data[i].temp = uint8_t(temp * 0.01f); esc_sensor.data[i].rpm = uint16_t(rpm * 0.1f); } @@ -1098,6 +1099,7 @@ void AP_MSP_Telem_Backend::hide_osd_items(void) BIT_CLEAR(osd_hidden_items_bitmask, OSD_AVG_CELL_VOLTAGE); BIT_CLEAR(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE); BIT_CLEAR(osd_hidden_items_bitmask, OSD_RTC_DATETIME); + BIT_CLEAR(osd_hidden_items_bitmask, OSD_RSSI_VALUE); if (msp->_msp_status.flashing_on) { // flash satcount when no 3D Fix @@ -1116,7 +1118,7 @@ void AP_MSP_Telem_Backend::hide_osd_items(void) // flash airspeed if there's no estimate bool airspeed_en = false; #if OSD_ENABLED - airspeed_en = osd->screen[0].aspeed.enabled; + airspeed_en = osd->screen[msp->_msp_status.current_screen].aspeed.enabled; #endif if (airspeed_en) { airspeed_state_t airspeed_state; @@ -1139,6 +1141,19 @@ void AP_MSP_Telem_Backend::hide_osd_items(void) if (!AP::rtc().get_utc_usec(time_usec)) { BIT_SET(osd_hidden_items_bitmask, OSD_RTC_DATETIME); } + // flash rssi if disabled + float rssi; + if (!get_rssi(rssi)) { + BIT_SET(osd_hidden_items_bitmask, OSD_RSSI_VALUE); + } + } + // disable flashing for min/max items + if (displaying_stats_screen()) { + BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIST); + BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SPEED); + BIT_CLEAR(osd_hidden_items_bitmask, OSD_CRAFT_NAME); + BIT_CLEAR(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE); + BIT_CLEAR(osd_hidden_items_bitmask, OSD_RSSI_VALUE); } } @@ -1202,4 +1217,33 @@ void AP_MSP_Telem_Backend::msp_displayport_write_string(uint8_t col, uint8_t row msp_send_packet(MSP_DISPLAYPORT, MSP::MSP_V1, &packet, 4 + len, false); } #endif //HAL_WITH_MSP_DISPLAYPORT +bool AP_MSP_Telem_Backend::displaying_stats_screen() const +{ +#if OSD_ENABLED + AP_OSD *osd = AP::osd(); + if (osd == nullptr) { + return false; + } + AP_MSP *msp = AP::msp(); + if (msp == nullptr) { + return false; + } + return osd->screen[msp->_msp_status.current_screen].stat.enabled; +#else + return false; +#endif +} + +bool AP_MSP_Telem_Backend::get_rssi(float &rssi) const +{ + AP_RSSI* ap_rssi = AP::rssi(); + if (ap_rssi == nullptr) { + return false; + } + if (!ap_rssi->enabled()) { + return false; + } + rssi = ap_rssi->read_receiver_rssi(); // range is [0-1] + return true; +} #endif //HAL_MSP_ENABLED diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.h b/libraries/AP_MSP/AP_MSP_Telem_Backend.h index 0ff57b5e1d..a6e70a25d8 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.h +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.h @@ -17,6 +17,8 @@ #pragma once #include +#include + #include "msp.h" #include @@ -155,12 +157,13 @@ protected: // telemetry helpers uint8_t calc_cell_count(float battery_voltage); - float get_vspeed_ms(void); - void update_home_pos(home_state_t &home_state); - void update_battery_state(battery_state_t &_battery_state); - void update_gps_state(gps_state_t &gps_state); - void update_airspeed(airspeed_state_t &airspeed_state); - void update_flight_mode_str(char *flight_mode_str, bool wind_enabled); + virtual float get_vspeed_ms(void) const; + virtual bool get_rssi(float &rssi) const; + virtual void update_home_pos(home_state_t &home_state); + virtual void update_battery_state(battery_state_t &_battery_state); + virtual void update_gps_state(gps_state_t &gps_state); + virtual void update_airspeed(airspeed_state_t &airspeed_state); + virtual void update_flight_mode_str(char *flight_mode_str, uint8_t size, bool wind_enabled); // MSP parsing void msp_process_received_command(); @@ -189,6 +192,7 @@ protected: virtual bool is_scheduler_enabled() const = 0; // only osd backends should allow a push type telemetry virtual bool use_msp_thread() const {return true;}; // is this backend hanlded by the MSP thread? virtual AP_SerialManager::SerialProtocol get_serial_protocol() const = 0; + virtual bool displaying_stats_screen() const; // implementation specific MSP out command processing virtual MSP::MSPCommandResult msp_process_out_api_version(MSP::sbuf_t *dst); diff --git a/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp b/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp index d062a8d131..c92b346fc4 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_DJI.cpp @@ -15,10 +15,14 @@ #include #include +#include +#include #include "AP_MSP.h" #include "AP_MSP_Telem_DJI.h" +#include + #if HAL_MSP_ENABLED extern const AP_HAL::HAL& hal; @@ -37,10 +41,7 @@ bool AP_MSP_Telem_DJI::init_uart() bool AP_MSP_Telem_DJI::is_scheduler_enabled() const { const AP_MSP *msp = AP::msp(); - if (msp == nullptr) { - return false; - } - return msp->check_option(AP_MSP::MspOption::OPTION_TELEMETRY_MODE); + return msp && msp->is_option_enabled(AP_MSP::Option::TELEMETRY_MODE); } void AP_MSP_Telem_DJI::hide_osd_items(void) @@ -91,26 +92,158 @@ MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_fc_variant(sbuf_t *dst) MSPCommandResult AP_MSP_Telem_DJI::msp_process_out_esc_sensor_data(sbuf_t *dst) { #if HAL_WITH_ESC_TELEM - const auto msp = AP::msp(); - if (msp && msp->check_option(AP_MSP::MspOption::OPTION_TELEMETRY_DJI_WORKAROUNDS)) { - AP_ESC_Telem& telem = AP::esc_telem(); - int16_t highest_temperature = 0; + int16_t highest_temperature = 0; + AP_ESC_Telem& telem = AP::esc_telem(); + if (!displaying_stats_screen()) { telem.get_highest_motor_temperature(highest_temperature); - - const struct PACKED { - uint8_t temp; - uint16_t rpm; - } esc_sensor_data { - temp : uint8_t(highest_temperature * 0.01f), // deg, report max temperature - rpm : uint16_t(telem.get_average_motor_rpm() * 0.1f) // rpm, report average RPM across all motors - }; - - sbuf_write_data(dst, &esc_sensor_data, sizeof(esc_sensor_data)); } else { - return AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(dst); + AP_OSD *osd = AP::osd(); + if (osd == nullptr) { + return MSP_RESULT_ERROR; + } + WITH_SEMAPHORE(osd->get_semaphore()); + highest_temperature = osd->get_stats_info().max_esc_temp; } + const struct PACKED { + uint8_t temp; + uint16_t rpm; + } esc_sensor_data { + temp : uint8_t(highest_temperature * 0.01f), // deg, report max temperature + rpm : uint16_t(telem.get_average_motor_rpm() * 0.1f) // rpm, report average RPM across all motors + }; + + sbuf_write_data(dst, &esc_sensor_data, sizeof(esc_sensor_data)); #endif return MSP_RESULT_ACK; } +void AP_MSP_Telem_DJI::update_home_pos(home_state_t &home_state) +{ + AP_MSP_Telem_Backend::update_home_pos(home_state); +#if OSD_ENABLED + if (!displaying_stats_screen()) { + return; + } + AP_MSP *msp = AP::msp(); + if (msp == nullptr) { + return; + } + AP_OSD *osd = AP::osd(); + if (osd == nullptr) { + return; + } + WITH_SEMAPHORE(osd->get_semaphore()); + // override telemetry with max distance and altitude info + // alternate max distance with traveled distance every 2 seconds + if (msp->_msp_status.slow_flashing_on) { + home_state.home_distance_m = osd->get_stats_info().max_dist_m; + } else { + home_state.home_distance_m = osd->get_stats_info().last_distance_m; + } + home_state.rel_altitude_cm = osd->get_stats_info().max_alt_m * 100; +#endif +} + +void AP_MSP_Telem_DJI::update_battery_state(battery_state_t &_battery_state) +{ + AP_MSP_Telem_Backend::update_battery_state(_battery_state); +#if OSD_ENABLED + if (!displaying_stats_screen()) { + return; + } + AP_OSD *osd = AP::osd(); + if (osd == nullptr) { + return; + } + WITH_SEMAPHORE(osd->get_semaphore()); + // override telemetry with max current and voltage info + _battery_state.batt_current_a = osd->get_stats_info().max_current_a; + _battery_state.batt_voltage_v = osd->get_stats_info().min_voltage_v; +#endif +} + +void AP_MSP_Telem_DJI::update_gps_state(gps_state_t &gps_state) +{ + AP_MSP_Telem_Backend::update_gps_state(gps_state); +#if OSD_ENABLED + if (!displaying_stats_screen()) { + return; + } + AP_OSD *osd = AP::osd(); + if (osd == nullptr) { + return; + } + WITH_SEMAPHORE(osd->get_semaphore()); + // override telemetry with max speed info + gps_state.speed_cms = osd->get_stats_info().max_speed_mps * 100; +#endif +} + +void AP_MSP_Telem_DJI::update_airspeed(airspeed_state_t &airspeed_state) +{ + AP_MSP_Telem_Backend::update_airspeed(airspeed_state); +#if OSD_ENABLED + if (!displaying_stats_screen()) { + return; + } + AP_OSD *osd = AP::osd(); + if (osd == nullptr) { + return; + } + WITH_SEMAPHORE(osd->get_semaphore()); + // override telemetry with max speed info + airspeed_state.airspeed_estimate_ms = osd->get_stats_info().max_airspeed_mps; +#endif +} + +void AP_MSP_Telem_DJI::update_flight_mode_str(char *flight_mode_str, uint8_t size, bool wind_enabled) +{ + AP_MSP_Telem_Backend::update_flight_mode_str(flight_mode_str, size, wind_enabled); +#if OSD_ENABLED + if (!displaying_stats_screen()) { + return; + } + AP_Stats *stats = AP::stats(); + if (stats != nullptr) { + uint32_t t = stats->get_flight_time_s(); + // need to check snprintf return value to prevent format-truncation warning in GCC because of -Werror=format-truncation + if (snprintf(flight_mode_str, size, "%s %3u:%02u", "STATS", unsigned(t/60), unsigned(t%60)) < 0) { + snprintf(flight_mode_str, size, "%s", "STATS --:--"); + } + } else { + snprintf(flight_mode_str, size, "%s", "STATS"); + } +#endif +} + +bool AP_MSP_Telem_DJI::get_rssi(float &rssi) const +{ + if (!AP_MSP_Telem_Backend::get_rssi(rssi)) { + return false; + } +#if OSD_ENABLED + if (!displaying_stats_screen()) { + return true; + } + AP_RSSI* ap_rssi = AP::rssi(); + if (ap_rssi == nullptr) { + return false; + } + if (!ap_rssi->enabled()) { + return false; + } + AP_OSD *osd = AP::osd(); + if (osd == nullptr) { + return false; + } + WITH_SEMAPHORE(osd->get_semaphore()); + // override telemetry with min rssi info + // Note: return false when min_rssi has not been updated yet by AP_OSD::update_stats() + if (is_equal(osd->get_stats_info().min_rssi, FLT_MAX)) { + return false; + } + rssi = osd->get_stats_info().min_rssi; // range is [0-1] +#endif + return true; +} #endif //HAL_MSP_ENABLED diff --git a/libraries/AP_MSP/AP_MSP_Telem_DJI.h b/libraries/AP_MSP/AP_MSP_Telem_DJI.h index 60b4fb3b81..0ce8aa9b84 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_DJI.h +++ b/libraries/AP_MSP/AP_MSP_Telem_DJI.h @@ -55,6 +55,14 @@ public: AP_SerialManager::SerialProtocol get_serial_protocol() const override { return AP_SerialManager::SerialProtocol::SerialProtocol_DJI_FPV; }; uint32_t get_osd_flight_mode_bitmask(void) override; void hide_osd_items(void) override; + + bool get_rssi(float &rssi) const override; + void update_home_pos(home_state_t &home_state) override; + void update_battery_state(battery_state_t &_battery_state) override; + void update_gps_state(gps_state_t &gps_state) override; + void update_airspeed(airspeed_state_t &airspeed_state) override; + void update_flight_mode_str(char *flight_mode_str, uint8_t size, bool wind_enabled) override; + MSP::MSPCommandResult msp_process_out_fc_variant(MSP::sbuf_t *dst) override; MSP::MSPCommandResult msp_process_out_esc_sensor_data(MSP::sbuf_t *dst) override; diff --git a/libraries/AP_MSP/AP_MSP_Telem_DisplayPort.cpp b/libraries/AP_MSP/AP_MSP_Telem_DisplayPort.cpp index d95d009bda..7734724b12 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_DisplayPort.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_DisplayPort.cpp @@ -32,7 +32,7 @@ MSPCommandResult AP_MSP_Telem_DisplayPort::msp_process_out_fc_variant(sbuf_t *ds return MSP_RESULT_ERROR; } // do we use backend specific symbols table? - if (msp->check_option(AP_MSP::MspOption::OPTION_DISPLAYPORT_BTFL_SYMBOLS)) { + if (msp->is_option_enabled(AP_MSP::Option::DISPLAYPORT_BTFL_SYMBOLS)) { sbuf_write_data(dst, BETAFLIGHT_IDENTIFIER, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); } else { sbuf_write_data(dst, ARDUPILOT_IDENTIFIER, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); diff --git a/libraries/AP_MSP/Tools/msposd.py b/libraries/AP_MSP/Tools/msposd.py index ebb8c9e3bf..7a06fbda13 100755 --- a/libraries/AP_MSP/Tools/msposd.py +++ b/libraries/AP_MSP/Tools/msposd.py @@ -212,10 +212,10 @@ def display_all(): display_text(msp.OSD_GPS_LAT, "Lat:%.07f" % (msp.get('RAW_GPS.Lat')*0.0000001)) display_text(msp.OSD_GPS_LON, "Lon:%.07f" % (msp.get('RAW_GPS.Lon')*0.0000001)) display_text(msp.OSD_RTC_DATETIME, "%04d-%02d-%02d %02d:%02d:%02d" % (msp.get('RTC.year'),msp.get('RTC.mon'),msp.get('RTC.mday'),msp.get('RTC.hour'),msp.get('RTC.min'),msp.get('RTC.sec'))) - display_text(msp.OSD_DISARMED, "%s" % ("ARMED" if msp.get('STATUS.mode_flags')&0X01==1 else "DISARMED")) + display_text(msp.OSD_DISARMED, "%s" % ("" if msp.get('STATUS.mode_flags')&0X01==1 else "DISARMED")) display_homedir() display_text(msp.OSD_HOME_DIST, "Dist: %dm" % (msp.get('COMP_GPS.distanceToHome'))) - display_text(msp.OSD_RSSI_VALUE, "Rssi:%02d" % (msp.get('ANALOG.rssi'))) + display_text(msp.OSD_RSSI_VALUE, "Rssi:%02d" % (msp.get('ANALOG.rssi') * 0.097)) display_text(msp.OSD_GPS_SPEED, "%.1fm/s" % (msp.get('RAW_GPS.Speed')*0.01)) display_text(msp.OSD_GPS_SATS, "Sats:%u" % msp.get("RAW_GPS.numSat")) display_text(msp.OSD_ROLL_ANGLE, "Roll:%.1f" % (msp.get("ATTITUDE.roll")*0.1)) diff --git a/libraries/AP_MSP/Tools/osdtest.parm b/libraries/AP_MSP/Tools/osdtest.parm index 7164e8857a..b72ef734a2 100644 --- a/libraries/AP_MSP/Tools/osdtest.parm +++ b/libraries/AP_MSP/Tools/osdtest.parm @@ -1,3 +1,4 @@ +OSD_CHAN 16.000000 OSD1_ALTITUDE_EN 1.000000 OSD1_ALTITUDE_X 24.000000 OSD1_ALTITUDE_Y 8.000000 @@ -10,18 +11,12 @@ OSD1_ASPD1_Y 0.000000 OSD1_ASPD2_EN 0.000000 OSD1_ASPD2_X 0.000000 OSD1_ASPD2_Y 0.000000 -OSD1_ASPEED_EN 0.000000 -OSD1_ASPEED_X 2.000000 -OSD1_ASPEED_Y 13.000000 +OSD1_ASPEED_EN 1.000000 OSD1_ATEMP_EN 0.000000 OSD1_ATEMP_X 0.000000 OSD1_ATEMP_Y 0.000000 OSD1_BAT2USED_EN 0.000000 -OSD1_BAT2USED_X 0.000000 -OSD1_BAT2USED_Y 0.000000 OSD1_BAT2_VLT_EN 0.000000 -OSD1_BAT2_VLT_X 0.000000 -OSD1_BAT2_VLT_Y 0.000000 OSD1_BATTBAR_EN 1.000000 OSD1_BATTBAR_X 13.000000 OSD1_BATTBAR_Y 15.000000 @@ -79,18 +74,18 @@ OSD1_GSPEED_Y 8.000000 OSD1_HDOP_EN 0.000000 OSD1_HDOP_X 0.000000 OSD1_HDOP_Y 0.000000 -OSD1_HEADING_EN 1.000000 -OSD1_HEADING_X 13.000000 -OSD1_HEADING_Y 2.000000 +OSD1_HEADING_EN 0.000000 +OSD1_HEADING_X 0.000000 +OSD1_HEADING_Y 0.000000 OSD1_HOMEDIR_EN 1.000000 OSD1_HOMEDIR_X 1.000000 OSD1_HOMEDIR_Y 2.000000 OSD1_HOMEDIST_EN 1.000000 OSD1_HOMEDIST_X 1.000000 OSD1_HOMEDIST_Y 1.000000 -OSD1_HOME_EN 1.000000 -OSD1_HOME_X 14.000000 -OSD1_HOME_Y 1.000000 +OSD1_HOME_EN 0.000000 +OSD1_HOME_X 0.000000 +OSD1_HOME_Y 0.000000 OSD1_HORIZON_EN 1.000000 OSD1_HORIZON_X 14.000000 OSD1_HORIZON_Y 8.000000 @@ -131,8 +126,59 @@ OSD1_WAYPOINT_EN 0.000000 OSD1_WAYPOINT_X 0.000000 OSD1_WAYPOINT_Y 0.000000 OSD1_WIND_EN 1.000000 -OSD1_WIND_X 2.000000 -OSD1_WIND_Y 12.000000 OSD1_XTRACK_EN 0.000000 OSD1_XTRACK_X 0.000000 OSD1_XTRACK_Y 0.000000 +OSD2_ENABLE 1.000000 +OSD2_STATS_EN 1.000000 +OSD2_ALTITUDE_EN 1.000000 +OSD2_ALTITUDE_X 13.000000 +OSD2_ALTITUDE_Y 6.000000 +OSD2_ARMING_EN 1.000000 +OSD2_ARMING_X 12.000000 +OSD2_ARMING_Y 14.000000 +OSD2_ASPEED_EN 1.000000 +OSD2_BATTBAR_EN 1.000000 +OSD2_BATTBAR_X 13.000000 +OSD2_BATTBAR_Y 15.000000 +OSD2_BATUSED_EN 1.000000 +OSD2_BATUSED_X 10.000000 +OSD2_BATUSED_Y 15.000000 +OSD2_BAT_VOLT_EN 1.000000 +OSD2_BAT_VOLT_X 13.000000 +OSD2_BAT_VOLT_Y 8.000000 +OSD2_CELLVOLT_EN 0.000000 +OSD2_CLK_EN 1.000000 +OSD2_CLK_X 22.000000 +OSD2_CLK_Y 0.000000 +OSD2_CURRENT_EN 1.000000 +OSD2_CURRENT_X 13.000000 +OSD2_CURRENT_Y 9.000000 +OSD2_GPSLAT_EN 1.000000 +OSD2_GPSLAT_X 13.000000 +OSD2_GPSLAT_Y 0.000000 +OSD2_GPSLONG_EN 1.000000 +OSD2_GPSLONG_X 13.000000 +OSD2_GPSLONG_Y 1.000000 +OSD2_GSPEED_EN 1.000000 +OSD2_GSPEED_X 13.000000 +OSD2_GSPEED_Y 5.000000 +OSD2_HDOP_EN 0.000000 +OSD2_HOMEDIR_EN 0.000000 +OSD2_HOMEDIST_EN 1.000000 +OSD2_HOMEDIST_X 13.000000 +OSD2_HOMEDIST_Y 7.000000 +OSD2_MESSAGE_EN 1.000000 +OSD2_MESSAGE_X 12.000000 +OSD2_MESSAGE_Y 3.000000 +OSD2_POWER_EN 1.000000 +OSD2_POWER_X 13.000000 +OSD2_POWER_Y 11.000000 +OSD2_RSSI_EN 1.000000 +OSD2_RSSI_X 13.000000 +OSD2_RSSI_Y 10.000000 +OSD2_SATS_EN 1.000000 +OSD2_SATS_X 1.000000 +OSD2_SATS_Y 0.000000 +OSD2_VSPEED_EN 0.000000 + diff --git a/libraries/AP_MSP/Tools/pymsp.py b/libraries/AP_MSP/Tools/pymsp.py index 812fb3fb8d..717ff707c7 100755 --- a/libraries/AP_MSP/Tools/pymsp.py +++ b/libraries/AP_MSP/Tools/pymsp.py @@ -236,6 +236,7 @@ class PyMSP: MSP_MOTOR_PINS: MSPItem("MOTOR_PINS", "8H","MP"), MSP_ANALOG: MSPItem("ANALOG", "BHHHH", "dV,consumed_mah,rssi,current,volt"), MSP_STATUS: MSPItem("STATUS", "HHHIBHHBBIB", "task_delta,i2c_err_count,sensor_status,mode_flags,nop_1,system_load,gyro_time,nop_2,nop_3,armed,extra"), + MSP_ESC_SENSOR_DATA: MSPItem('ESC', "BH", "temp1,rpm1"), } def __init__(self):