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

AP_MSP: added multi screen and stats support to DJI FPV OSD

This commit is contained in:
yaapu 2021-09-06 16:07:39 +02:00 committed by Peter Barker
parent 934a4e26c9
commit 9f0d9a5652
10 changed files with 369 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
libraries/AP_MSP/AP_MSP.cpp
View File

@ -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()

17
libraries/AP_MSP/AP_MSP.h
View File

@ -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;

102
libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
View File

@ -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

16
libraries/AP_MSP/AP_MSP_Telem_Backend.h
View File

@ -17,6 +17,8 @@
#pragma once
#include <AP_RCTelemetry/AP_RCTelemetry.h>
#include <AP_OSD/AP_OSD.h>
#include "msp.h"
#include <time.h>
@ -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);

171
libraries/AP_MSP/AP_MSP_Telem_DJI.cpp
View File

@ -15,10 +15,14 @@
#include <AP_Common/AP_FWVersion.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Stats/AP_Stats.h>
#include <AP_RSSI/AP_RSSI.h>
#include "AP_MSP.h"
#include "AP_MSP_Telem_DJI.h"
#include <stdio.h>
#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

8
libraries/AP_MSP/AP_MSP_Telem_DJI.h
View File

@ -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;

2
libraries/AP_MSP/AP_MSP_Telem_DisplayPort.cpp
View File

@ -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);

4
libraries/AP_MSP/Tools/msposd.py
View File

@ -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))

76
libraries/AP_MSP/Tools/osdtest.parm
View File

@ -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

1
libraries/AP_MSP/Tools/pymsp.py
View File

@ -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):