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

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

This commit is contained in:
yaapu 2021-09-06 16:07:53 +02:00 committed by Peter Barker
parent 9f0d9a5652
commit 94aba33d10
4 changed files with 100 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
libraries/AP_OSD/AP_OSD.cpp
View File

@ -35,6 +35,7 @@
#include <utility>
#include <AP_Notify/AP_Notify.h>
#include <AP_Terrain/AP_Terrain.h>
#include <AP_RSSI/AP_RSSI.h>
// 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()
{

34
libraries/AP_OSD/AP_OSD.h
View File

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

5
libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp
View File

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

22
libraries/AP_OSD/AP_OSD_Screen.cpp
View File

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