AP_OSD: refactor to enable a TX-only set of parameter screens

add accessor for getting parameter names
add camel-case conversion for parameters names
enable OSD parameters unless HAL_MINIMIZE_FEATURES
correct OSD parameter documentation
don't default to TXONLY
This commit is contained in:
Andy Piper 2020-10-19 21:30:08 +01:00 committed by Andrew Tridgell
parent 4d1c660ebc
commit 8cd9af5ddd
8 changed files with 158 additions and 57 deletions

View File

@ -19,7 +19,7 @@
#include "AP_OSD.h" #include "AP_OSD.h"
#if OSD_ENABLED #if OSD_ENABLED || OSD_PARAM_ENABLED
#include "AP_OSD_MAX7456.h" #include "AP_OSD_MAX7456.h"
#ifdef WITH_SITL_OSD #ifdef WITH_SITL_OSD
@ -38,12 +38,13 @@ const AP_Param::GroupInfo AP_OSD::var_info[] = {
// @Param: _TYPE // @Param: _TYPE
// @DisplayName: OSD type // @DisplayName: OSD type
// @Description: OSD type // @Description: OSD type. TXONLY makes the OSD parameter selection available to other modules even if there is no native OSD support on the board, for instance CRSF.
// @Values: 0:None,1:MAX7456,2:SITL,3:MSP // @Values: 0:None,1:MAX7456,2:SITL,3:MSP,4:TXONLY
// @User: Standard // @User: Standard
// @RebootRequired: True // @RebootRequired: True
AP_GROUPINFO_FLAGS("_TYPE", 1, AP_OSD, osd_type, 0, AP_PARAM_FLAG_ENABLE), AP_GROUPINFO_FLAGS("_TYPE", 1, AP_OSD, osd_type, 0, AP_PARAM_FLAG_ENABLE),
#if OSD_ENABLED
// @Param: _CHAN // @Param: _CHAN
// @DisplayName: Screen switch transmitter channel // @DisplayName: Screen switch transmitter channel
// @Description: This sets the channel used to switch different OSD screens. // @Description: This sets the channel used to switch different OSD screens.
@ -167,7 +168,10 @@ const AP_Param::GroupInfo AP_OSD::var_info[] = {
// @Range: 0 3000 // @Range: 0 3000
// @User: Advanced // @User: Advanced
AP_GROUPINFO("_BTN_DELAY", 20, AP_OSD, button_delay_ms, 300), AP_GROUPINFO("_BTN_DELAY", 20, AP_OSD, button_delay_ms, 300),
#endif
#endif
#if OSD_PARAM_ENABLED
// @Group: 5_ // @Group: 5_
// @Path: AP_OSD_ParamScreen.cpp // @Path: AP_OSD_ParamScreen.cpp
AP_SUBGROUPINFO(param_screen[0], "5_", 21, AP_OSD, AP_OSD_ParamScreen), AP_SUBGROUPINFO(param_screen[0], "5_", 21, AP_OSD, AP_OSD_ParamScreen),
@ -190,16 +194,18 @@ AP_OSD::AP_OSD()
AP_HAL::panic("AP_OSD must be singleton"); AP_HAL::panic("AP_OSD must be singleton");
} }
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
#if OSD_ENABLED
// default first screen enabled // default first screen enabled
screen[0].enabled = 1; screen[0].enabled = 1;
previous_pwm_screen = -1;
#endif
#ifdef WITH_SITL_OSD #ifdef WITH_SITL_OSD
osd_type.set_default(2); osd_type.set_default(OSD_SITL);
#endif #endif
#ifdef HAL_OSD_TYPE_DEFAULT #ifdef HAL_OSD_TYPE_DEFAULT
osd_type.set_default(HAL_OSD_TYPE_DEFAULT); osd_type.set_default(HAL_OSD_TYPE_DEFAULT);
#endif #endif
previous_pwm_screen = -1;
_singleton = this; _singleton = this;
} }
@ -207,6 +213,7 @@ void AP_OSD::init()
{ {
switch ((enum osd_types)osd_type.get()) { switch ((enum osd_types)osd_type.get()) {
case OSD_NONE: case OSD_NONE:
case OSD_TXONLY:
default: default:
break; break;
@ -244,12 +251,15 @@ void AP_OSD::init()
break; break;
} }
} }
#if OSD_ENABLED
if (backend != nullptr && (enum osd_types)osd_type.get() != OSD_MSP) { if (backend != nullptr && (enum osd_types)osd_type.get() != OSD_MSP) {
// 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 but MSP which has its own
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1024, AP_HAL::Scheduler::PRIORITY_IO, 1); hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_OSD::osd_thread, void), "OSD", 1024, AP_HAL::Scheduler::PRIORITY_IO, 1);
} }
#endif
} }
#if OSD_ENABLED
void AP_OSD::osd_thread() void AP_OSD::osd_thread()
{ {
while (true) { while (true) {
@ -424,6 +434,7 @@ void AP_OSD::set_nav_info(NavInfo &navinfo)
// do this without a lock for now // do this without a lock for now
nav_info = navinfo; nav_info = navinfo;
} }
#endif // OSD_ENABLED
// handle OSD parameter configuration // handle OSD parameter configuration
void AP_OSD::handle_msg(const mavlink_message_t &msg, const GCS_MAVLINK& link) void AP_OSD::handle_msg(const mavlink_message_t &msg, const GCS_MAVLINK& link)
@ -475,4 +486,4 @@ AP_OSD *AP::osd() {
return AP_OSD::get_singleton(); return AP_OSD::get_singleton();
} }
#endif // OSD_ENABLED #endif // OSD_ENABLED || OSD_PARAM_ENABLED

View File

@ -30,11 +30,11 @@
#endif #endif
#ifndef HAL_WITH_OSD_BITMAP #ifndef HAL_WITH_OSD_BITMAP
#define HAL_WITH_OSD_BITMAP defined(HAL_WITH_SPI_OSD) || defined(WITH_SITL_OSD) #define HAL_WITH_OSD_BITMAP OSD_ENABLED && (defined(HAL_WITH_SPI_OSD) || defined(WITH_SITL_OSD))
#endif #endif
#ifndef OSD_PARAM_ENABLED #ifndef OSD_PARAM_ENABLED
#define OSD_PARAM_ENABLED HAL_WITH_OSD_BITMAP && !HAL_MINIMIZE_FEATURES #define OSD_PARAM_ENABLED !HAL_MINIMIZE_FEATURES
#endif #endif
class AP_OSD_Backend; class AP_OSD_Backend;
@ -102,6 +102,7 @@ protected:
AP_OSD *osd; AP_OSD *osd;
}; };
#if OSD_ENABLED
/* /*
class to hold one screen of settings class to hold one screen of settings
*/ */
@ -243,6 +244,7 @@ private:
void draw_bat2used(uint8_t x, uint8_t y); void draw_bat2used(uint8_t x, uint8_t y);
void draw_clk(uint8_t x, uint8_t y); void draw_clk(uint8_t x, uint8_t y);
}; };
#endif // OSD_ENABLED
#if OSD_PARAM_ENABLED #if OSD_PARAM_ENABLED
/* /*
@ -291,7 +293,13 @@ public:
// initialize the setting from the configured information // initialize the setting from the configured information
void update(); void update();
// grab the parameter name
void copy_name(char* name, size_t len) {
_param->copy_name_token(_current_token, name, len);
if (len > 16) name[16] = 0;
}
// copy the name converting FOO_BAR_BAZ to FooBarBaz
void copy_name_camel_case(char* name, size_t len);
// set the ranges from static metadata // set the ranges from static metadata
bool set_from_metadata(); bool set_from_metadata();
bool set_by_name(const char* name, uint8_t config_type, float pmin=0, float pmax=0, float pincr=0); bool set_by_name(const char* name, uint8_t config_type, float pmin=0, float pmax=0, float pincr=0);
@ -333,10 +341,13 @@ public:
static const uint8_t NUM_PARAMS = 9; static const uint8_t NUM_PARAMS = 9;
static const uint8_t SAVE_PARAM = NUM_PARAMS + 1; static const uint8_t SAVE_PARAM = NUM_PARAMS + 1;
#if HAL_WITH_OSD_BITMAP
void draw(void) override; void draw(void) override;
#endif
void handle_write_msg(const mavlink_osd_param_config_t& packet, const GCS_MAVLINK& link); void handle_write_msg(const mavlink_osd_param_config_t& packet, const GCS_MAVLINK& link);
void handle_read_msg(const mavlink_osd_param_show_config_t& packet, const GCS_MAVLINK& link); void handle_read_msg(const mavlink_osd_param_show_config_t& packet, const GCS_MAVLINK& link);
// get a setting and associated metadata
AP_OSD_ParamSetting* get_setting(uint8_t param_idx);
// Save button co-ordinates // Save button co-ordinates
AP_Int8 save_x; AP_Int8 save_x;
AP_Int8 save_y; AP_Int8 save_y;
@ -347,17 +358,17 @@ public:
private: private:
AP_OSD_ParamSetting params[NUM_PARAMS] { 1, 2, 3, 4, 5, 6, 7, 8, 9 }; AP_OSD_ParamSetting params[NUM_PARAMS] { 1, 2, 3, 4, 5, 6, 7, 8, 9 };
void save_parameters();
#if OSD_ENABLED
void update_state_machine(); void update_state_machine();
void draw_parameter(uint8_t param_number, uint8_t x, uint8_t y); void draw_parameter(uint8_t param_number, uint8_t x, uint8_t y);
void modify_parameter(uint8_t number, Event ev); void modify_parameter(uint8_t number, Event ev);
void modify_configured_parameter(uint8_t number, Event ev); void modify_configured_parameter(uint8_t number, Event ev);
void save_parameters();
Event map_rc_input_to_event() const; Event map_rc_input_to_event() const;
RC_Channel::AuxSwitchPos get_channel_pos(uint8_t rcmapchan) const; RC_Channel::AuxSwitchPos get_channel_pos(uint8_t rcmapchan) const;
uint8_t _selected_param = 1; uint8_t _selected_param = 1;
uint16_t _requires_save;
MenuState _menu_state = MenuState::PARAM_SELECT; MenuState _menu_state = MenuState::PARAM_SELECT;
Event _last_rc_event = Event::NONE; Event _last_rc_event = Event::NONE;
@ -367,9 +378,11 @@ private:
uint32_t _transition_timeout_ms; uint32_t _transition_timeout_ms;
// number of consecutive times the current transition has happened // number of consecutive times the current transition has happened
uint32_t _transition_count; uint32_t _transition_count;
#endif
uint16_t _requires_save;
}; };
#endif #endif // OSD_PARAM_ENABLED
class AP_OSD class AP_OSD
{ {
@ -402,6 +415,7 @@ public:
OSD_MAX7456=1, OSD_MAX7456=1,
OSD_SITL=2, OSD_SITL=2,
OSD_MSP=3, OSD_MSP=3,
OSD_TXONLY=4
}; };
enum switch_method { enum switch_method {
TOGGLE=0, TOGGLE=0,
@ -410,9 +424,12 @@ public:
}; };
AP_Int8 osd_type; AP_Int8 osd_type;
AP_Int8 font_num;
AP_Int32 options;
#if OSD_ENABLED
AP_Int8 rc_channel; AP_Int8 rc_channel;
AP_Int8 sw_method; AP_Int8 sw_method;
AP_Int8 font_num;
AP_Int8 v_offset; AP_Int8 v_offset;
AP_Int8 h_offset; AP_Int8 h_offset;
@ -433,8 +450,6 @@ public:
OPTION_INVERTED_AH_ROLL = 1U<<2, OPTION_INVERTED_AH_ROLL = 1U<<2,
}; };
AP_Int32 options;
enum { enum {
UNITS_METRIC=0, UNITS_METRIC=0,
UNITS_IMPERIAL=1, UNITS_IMPERIAL=1,
@ -446,9 +461,7 @@ public:
AP_Int8 units; AP_Int8 units;
AP_OSD_Screen screen[AP_OSD_NUM_DISPLAY_SCREENS]; AP_OSD_Screen screen[AP_OSD_NUM_DISPLAY_SCREENS];
#if OSD_PARAM_ENABLED
AP_OSD_ParamScreen param_screen[AP_OSD_NUM_PARAM_SCREENS] { 0, 1 };
#endif
struct NavInfo { struct NavInfo {
float wp_distance; float wp_distance;
int32_t wp_bearing; int32_t wp_bearing;
@ -478,16 +491,27 @@ public:
// Check whether arming is allowed // Check whether arming is allowed
bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const; 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; } bool is_readonly_screen() const { return current_screen < AP_OSD_NUM_DISPLAY_SCREENS; }
#endif // OSD_ENABLED
#if OSD_PARAM_ENABLED
AP_OSD_ParamScreen param_screen[AP_OSD_NUM_PARAM_SCREENS] { 0, 1 };
// return a setting for use by TX based OSD
AP_OSD_ParamSetting* get_setting(uint8_t screen_idx, uint8_t param_idx) {
if (screen_idx >= AP_OSD_NUM_PARAM_SCREENS) {
return nullptr;
}
return param_screen[screen_idx].get_setting(param_idx);
}
#endif
// handle OSD parameter configuration // handle OSD parameter configuration
void handle_msg(const mavlink_message_t &msg, const GCS_MAVLINK& link); void handle_msg(const mavlink_message_t &msg, const GCS_MAVLINK& link);
private: private:
void osd_thread(); void osd_thread();
#if OSD_ENABLED
void update_osd(); void update_osd();
void stats(); void stats();
void update_current_screen(); void update_current_screen();
void next_screen(); void next_screen();
AP_OSD_Backend *backend;
//variables for screen switching //variables for screen switching
uint8_t current_screen; uint8_t current_screen;
@ -508,6 +532,8 @@ private:
float max_speed_mps; float max_speed_mps;
float max_current_a; float max_current_a;
float avg_current_a; float avg_current_a;
#endif
AP_OSD_Backend *backend;
static AP_OSD *_singleton; static AP_OSD *_singleton;
}; };

View File

@ -25,6 +25,7 @@ extern const AP_HAL::HAL& hal;
void AP_OSD_Backend::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...) void AP_OSD_Backend::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ...)
{ {
#if OSD_ENABLED
if (blink && (blink_phase < 2)) { if (blink && (blink_phase < 2)) {
return; return;
} }
@ -48,4 +49,5 @@ void AP_OSD_Backend::write(uint8_t x, uint8_t y, bool blink, const char *fmt, ..
write(x, y, buff); write(x, y, buff);
} }
va_end(ap); va_end(ap);
#endif
} }

View File

@ -18,6 +18,7 @@
#include <AP_OSD/AP_OSD_MAX7456.h> #include <AP_OSD/AP_OSD_MAX7456.h>
#if HAL_WITH_OSD_BITMAP
#include <AP_HAL/Util.h> #include <AP_HAL/Util.h>
#include <AP_HAL/Semaphores.h> #include <AP_HAL/Semaphores.h>
#include <AP_HAL/Scheduler.h> #include <AP_HAL/Scheduler.h>
@ -478,3 +479,4 @@ float AP_OSD_MAX7456::get_aspect_ratio_correction() const
return 1.0f; return 1.0f;
}; };
} }
#endif // HAL_WITH_OSD_BITMAP

View File

@ -18,6 +18,7 @@
#include <AP_OSD/AP_OSD_Backend.h> #include <AP_OSD/AP_OSD_Backend.h>
#include <AP_Common/Bitmask.h> #include <AP_Common/Bitmask.h>
#if HAL_WITH_OSD_BITMAP
class AP_OSD_MAX7456 : public AP_OSD_Backend class AP_OSD_MAX7456 : public AP_OSD_Backend
{ {
@ -88,3 +89,4 @@ private:
uint16_t video_lines; uint16_t video_lines;
}; };
#endif // HAL_WITH_OSD_BITMAP

View File

@ -71,10 +71,13 @@ const AP_Param::GroupInfo AP_OSD_ParamScreen::var_info[] = {
// @DisplayName: PARAM1_Y // @DisplayName: PARAM1_Y
// @Description: Vertical position on screen // @Description: Vertical position on screen
// @Range: 0 15 // @Range: 0 15
// @Group: PARAM1
// @Path: AP_OSD_ParamSetting.cpp
AP_SUBGROUPINFO(params[0], "PARAM1", 4, AP_OSD_ParamScreen, AP_OSD_ParamSetting), AP_SUBGROUPINFO(params[0], "PARAM1", 4, AP_OSD_ParamScreen, AP_OSD_ParamSetting),
// @Param: PARAM2_EN // @Param: PARAM2_EN
// @DisplayName: PARAM21_EN // @DisplayName: PARAM2_EN
// @Description: Enables display of parameter 2 // @Description: Enables display of parameter 2
// @Values: 0:Disabled,1:Enabled // @Values: 0:Disabled,1:Enabled
@ -87,6 +90,9 @@ const AP_Param::GroupInfo AP_OSD_ParamScreen::var_info[] = {
// @DisplayName: PARAM2_Y // @DisplayName: PARAM2_Y
// @Description: Vertical position on screen // @Description: Vertical position on screen
// @Range: 0 15 // @Range: 0 15
// @Group: PARAM2
// @Path: AP_OSD_ParamSetting.cpp
AP_SUBGROUPINFO(params[1], "PARAM2", 5, AP_OSD_ParamScreen, AP_OSD_ParamSetting), AP_SUBGROUPINFO(params[1], "PARAM2", 5, AP_OSD_ParamScreen, AP_OSD_ParamSetting),
// @Param: PARAM3_EN // @Param: PARAM3_EN
@ -103,6 +109,9 @@ const AP_Param::GroupInfo AP_OSD_ParamScreen::var_info[] = {
// @DisplayName: PARAM3_Y // @DisplayName: PARAM3_Y
// @Description: Vertical position on screen // @Description: Vertical position on screen
// @Range: 0 15 // @Range: 0 15
// @Group: PARAM3
// @Path: AP_OSD_ParamSetting.cpp
AP_SUBGROUPINFO(params[2], "PARAM3", 6, AP_OSD_ParamScreen, AP_OSD_ParamSetting), AP_SUBGROUPINFO(params[2], "PARAM3", 6, AP_OSD_ParamScreen, AP_OSD_ParamSetting),
// @Param: PARAM4_EN // @Param: PARAM4_EN
@ -119,6 +128,9 @@ const AP_Param::GroupInfo AP_OSD_ParamScreen::var_info[] = {
// @DisplayName: PARAM4_Y // @DisplayName: PARAM4_Y
// @Description: Vertical position on screen // @Description: Vertical position on screen
// @Range: 0 15 // @Range: 0 15
// @Group: PARAM4
// @Path: AP_OSD_ParamSetting.cpp
AP_SUBGROUPINFO(params[3], "PARAM4", 7, AP_OSD_ParamScreen, AP_OSD_ParamSetting), AP_SUBGROUPINFO(params[3], "PARAM4", 7, AP_OSD_ParamScreen, AP_OSD_ParamSetting),
// @Param: PARAM5_EN // @Param: PARAM5_EN
@ -135,6 +147,9 @@ const AP_Param::GroupInfo AP_OSD_ParamScreen::var_info[] = {
// @DisplayName: PARAM5_Y // @DisplayName: PARAM5_Y
// @Description: Vertical position on screen // @Description: Vertical position on screen
// @Range: 0 15 // @Range: 0 15
// @Group: PARAM5
// @Path: AP_OSD_ParamSetting.cpp
AP_SUBGROUPINFO(params[4], "PARAM5", 8, AP_OSD_ParamScreen, AP_OSD_ParamSetting), AP_SUBGROUPINFO(params[4], "PARAM5", 8, AP_OSD_ParamScreen, AP_OSD_ParamSetting),
// @Param: PARAM6_EN // @Param: PARAM6_EN
@ -151,6 +166,9 @@ const AP_Param::GroupInfo AP_OSD_ParamScreen::var_info[] = {
// @DisplayName: PARAM6_Y // @DisplayName: PARAM6_Y
// @Description: Vertical position on screen // @Description: Vertical position on screen
// @Range: 0 15 // @Range: 0 15
// @Group: PARAM6
// @Path: AP_OSD_ParamSetting.cpp
AP_SUBGROUPINFO(params[5], "PARAM6", 9, AP_OSD_ParamScreen, AP_OSD_ParamSetting), AP_SUBGROUPINFO(params[5], "PARAM6", 9, AP_OSD_ParamScreen, AP_OSD_ParamSetting),
// @Param: PARAM7_EN // @Param: PARAM7_EN
@ -167,6 +185,9 @@ const AP_Param::GroupInfo AP_OSD_ParamScreen::var_info[] = {
// @DisplayName: PARAM7_Y // @DisplayName: PARAM7_Y
// @Description: Vertical position on screen // @Description: Vertical position on screen
// @Range: 0 15 // @Range: 0 15
// @Group: PARAM7
// @Path: AP_OSD_ParamSetting.cpp
AP_SUBGROUPINFO(params[6], "PARAM7", 10, AP_OSD_ParamScreen, AP_OSD_ParamSetting), AP_SUBGROUPINFO(params[6], "PARAM7", 10, AP_OSD_ParamScreen, AP_OSD_ParamSetting),
// @Param: PARAM8_EN // @Param: PARAM8_EN
@ -183,6 +204,9 @@ const AP_Param::GroupInfo AP_OSD_ParamScreen::var_info[] = {
// @DisplayName: PARAM8_Y // @DisplayName: PARAM8_Y
// @Description: Vertical position on screen // @Description: Vertical position on screen
// @Range: 0 15 // @Range: 0 15
// @Group: PARAM8
// @Path: AP_OSD_ParamSetting.cpp
AP_SUBGROUPINFO(params[7], "PARAM8", 11, AP_OSD_ParamScreen, AP_OSD_ParamSetting), AP_SUBGROUPINFO(params[7], "PARAM8", 11, AP_OSD_ParamScreen, AP_OSD_ParamSetting),
// @Param: PARAM9_EN // @Param: PARAM9_EN
@ -199,6 +223,9 @@ const AP_Param::GroupInfo AP_OSD_ParamScreen::var_info[] = {
// @DisplayName: PARAM9_Y // @DisplayName: PARAM9_Y
// @Description: Vertical position on screen // @Description: Vertical position on screen
// @Range: 0 15 // @Range: 0 15
// @Group: PARAM9
// @Path: AP_OSD_ParamSetting.cpp
AP_SUBGROUPINFO(params[8], "PARAM9", 12, AP_OSD_ParamScreen, AP_OSD_ParamSetting), AP_SUBGROUPINFO(params[8], "PARAM9", 12, AP_OSD_ParamScreen, AP_OSD_ParamSetting),
// @Param: SAVE_X // @Param: SAVE_X
@ -297,6 +324,16 @@ AP_OSD_ParamScreen::AP_OSD_ParamScreen(uint8_t index)
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
AP_OSD_ParamSetting* AP_OSD_ParamScreen::get_setting(uint8_t param_idx)
{
if (param_idx >= NUM_PARAMS) {
return nullptr;
}
params[param_idx].update(); // make sure we are fresh
return &params[param_idx];
}
#if OSD_ENABLED
void AP_OSD_ParamScreen::draw_parameter(uint8_t number, uint8_t x, uint8_t y) void AP_OSD_ParamScreen::draw_parameter(uint8_t number, uint8_t x, uint8_t y)
{ {
bool param_blink = false; bool param_blink = false;
@ -328,8 +365,7 @@ void AP_OSD_ParamScreen::draw_parameter(uint8_t number, uint8_t x, uint8_t y)
if (p != nullptr) { if (p != nullptr) {
// grab the name of the parameter // grab the name of the parameter
char name[17]; char name[17];
p->copy_name_token(setting._current_token, name, 16); setting.copy_name(name, 17);
name[16] = 0;
const AP_OSD_ParamSetting::ParamMetadata* metadata = setting.get_custom_metadata(); const AP_OSD_ParamSetting::ParamMetadata* metadata = setting.get_custom_metadata();
@ -437,7 +473,7 @@ void AP_OSD_ParamScreen::modify_parameter(uint8_t number, Event ev)
} }
} }
// modify which parameter is configued for the given selection // modify which parameter is configured for the given selection
void AP_OSD_ParamScreen::modify_configured_parameter(uint8_t number, Event ev) void AP_OSD_ParamScreen::modify_configured_parameter(uint8_t number, Event ev)
{ {
if (number > NUM_PARAMS) { if (number > NUM_PARAMS) {
@ -487,25 +523,6 @@ void AP_OSD_ParamScreen::modify_configured_parameter(uint8_t number, Event ev)
} }
} }
// save all of the parameters
void AP_OSD_ParamScreen::save_parameters()
{
if (!_requires_save) {
return;
}
for (uint8_t i = 0; i < NUM_PARAMS; i++) {
if (params[i].enabled && (_requires_save & (1 << i))) {
AP_Param* p = params[i]._param;
if (p != nullptr) {
p->save();
}
params[i].save_as_new();
}
}
_requires_save = 0;
}
// return radio values as LOW, MIDDLE, HIGH // return radio values as LOW, MIDDLE, HIGH
// this function uses different threshold values to RC_Chanel::get_channel_pos() // this function uses different threshold values to RC_Chanel::get_channel_pos()
// to avoid glitching on the stick travel // to avoid glitching on the stick travel
@ -710,6 +727,40 @@ void AP_OSD_ParamScreen::draw(void)
draw_parameter(SAVE_PARAM, save_x, save_y); draw_parameter(SAVE_PARAM, save_x, save_y);
} }
// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
bool AP_OSD::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const
{
// currently in the OSD menu, do not allow arming
if (!is_readonly_screen()) {
hal.util->snprintf(failure_msg, failure_msg_len, "In OSD menu");
return false;
}
// if we got this far everything must be ok
return true;
}
#endif // OSD_ENABLED
// save all of the parameters
void AP_OSD_ParamScreen::save_parameters()
{
if (!_requires_save) {
return;
}
for (uint8_t i = 0; i < NUM_PARAMS; i++) {
if (params[i].enabled && (_requires_save & (1 << i))) {
AP_Param* p = params[i]._param;
if (p != nullptr) {
p->save();
}
params[i].save_as_new();
}
}
_requires_save = 0;
}
// handle OSD configuration messages // handle OSD configuration messages
void AP_OSD_ParamScreen::handle_write_msg(const mavlink_osd_param_config_t& packet, const GCS_MAVLINK& link) void AP_OSD_ParamScreen::handle_write_msg(const mavlink_osd_param_config_t& packet, const GCS_MAVLINK& link)
{ {
@ -751,16 +802,3 @@ void AP_OSD_ParamScreen::handle_read_msg(const mavlink_osd_param_show_config_t&
} }
#endif // OSD_PARAM_ENABLED #endif // OSD_PARAM_ENABLED
// pre_arm_check - returns true if all pre-takeoff checks have completed successfully
bool AP_OSD::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const
{
// currently in the OSD menu, do not allow arming
if (!is_readonly_screen()) {
hal.util->snprintf(failure_msg, failure_msg_len, "In OSD menu");
return false;
}
// if we got this far everything must be ok
return true;
}

View File

@ -24,6 +24,7 @@
#include "AP_OSD.h" #include "AP_OSD.h"
#include <AP_Vehicle/AP_Vehicle_Type.h> #include <AP_Vehicle/AP_Vehicle_Type.h>
#include <SRV_Channel/SRV_Channel.h> #include <SRV_Channel/SRV_Channel.h>
#include <ctype.h>
#if OSD_PARAM_ENABLED #if OSD_PARAM_ENABLED
@ -366,7 +367,7 @@ void AP_OSD_ParamSetting::guess_ranges(bool force)
} }
incr = MAX(1, powf(10, digits - 2)); incr = MAX(1, powf(10, digits - 2));
max = powf(10, digits + 1); max = powf(10, digits + 1);
debug("Guessing range for value %d as %f -> %f, %f\n", p->get(), min, max, incr); debug("Guessing range for value %d as %f -> %f, %f\n", int(p->get()), min, max, incr);
break; break;
} }
case AP_PARAM_FLOAT: { case AP_PARAM_FLOAT: {
@ -415,6 +416,23 @@ void AP_OSD_ParamSetting::guess_ranges(bool force)
} }
} }
// copy the name converting FOO_BAR_BAZ to FooBarBaz
void AP_OSD_ParamSetting::copy_name_camel_case(char* name, size_t len)
{
char buf[17];
_param->copy_name_token(_current_token, buf, 17);
buf[16] = 0;
name[0] = buf[0];
for (uint8_t i = 1, n = 1; i < len; i++, n++) {
if (buf[i] == '_') {
name[n] = buf[i+1];
i++;
} else {
name[n] = tolower(buf[i]);
}
}
}
bool AP_OSD_ParamSetting::set_from_metadata() bool AP_OSD_ParamSetting::set_from_metadata()
{ {
// check for statically configured setting metadata // check for statically configured setting metadata

View File

@ -22,6 +22,7 @@
#include "AP_OSD.h" #include "AP_OSD.h"
#include "AP_OSD_Backend.h" #include "AP_OSD_Backend.h"
#if OSD_ENABLED
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_HAL/Util.h> #include <AP_HAL/Util.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
@ -1809,3 +1810,4 @@ void AP_OSD_Screen::draw(void)
DRAW_SETTING(eff); DRAW_SETTING(eff);
} }
#endif #endif
#endif // OSD_ENABLED