zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

gps-1.3.1
yaapu 3 years ago committed by Peter Barker
parent
9f0d9a5652
commit
94aba33d10
4 changed files with 101 additions and 46 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 84
      libraries/AP_OSD/AP_OSD.cpp
  2. 34
      libraries/AP_OSD/AP_OSD.h
  3. 5
      libraries/AP_OSD/AP_OSD_MSP_DisplayPort.cpp
  4. 22
      libraries/AP_OSD/AP_OSD_Screen.cpp

84
libraries/AP_OSD/AP_OSD.cpp
Unescape View File

@ -35,6 +35,7 @@ @@ -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() @@ -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() @@ -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() @@ -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);
// 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;
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();
Vector2f v = ahrs.groundspeed_vector();
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
Unescape View File

@ -130,6 +130,7 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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
Unescape View File

@ -97,11 +97,8 @@ void AP_OSD_MSP_DisplayPort::flush(void) @@ -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
Unescape View File

@ -1324,15 +1324,15 @@ void AP_OSD_Screen::draw_current(uint8_t instance, uint8_t x, uint8_t y) @@ -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) @@ -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)

Write Preview
Loading…
Cancel
Save