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

AP_OSD: add waypoint and xtrack_error items

This commit is contained in:
Alexander Malishev 2018-07-26 03:47:06 +04:00 committed by Andrew Tridgell
parent aae1054a94
commit eec34fc47d
3 changed files with 52 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_OSD/AP_OSD.cpp
View File

@ -278,3 +278,10 @@ void AP_OSD::next_screen()
} while (t != current_screen && !screen[t].enabled);
current_screen = t;
}
// set navigation information for display
void AP_OSD::set_nav_info(NavInfo &navinfo)
{
// do this without a lock for now
nav_info = navinfo;
}

15
libraries/AP_OSD/AP_OSD.h
View File

@ -102,6 +102,8 @@ private:
AP_OSD_Setting pitch_angle{false, 0, 0};
AP_OSD_Setting temp{false, 0, 0};
AP_OSD_Setting hdop{false, 0, 0};
AP_OSD_Setting waypoint{false, 0, 0};
AP_OSD_Setting xtrack_error{false, 0, 0};
bool check_option(uint32_t option);
@ -152,10 +154,13 @@ private:
void draw_pitch_angle(uint8_t x, uint8_t y);
void draw_temp(uint8_t x, uint8_t y);
void draw_hdop(uint8_t x, uint8_t y);
void draw_waypoint(uint8_t x, uint8_t y);
void draw_xtrack_error(uint8_t x, uint8_t y);
};
class AP_OSD {
public:
friend class AP_OSD_Screen;
//constructor
AP_OSD();
@ -213,6 +218,15 @@ public:
AP_OSD_Screen screen[AP_OSD_NUM_SCREENS];
struct NavInfo {
float wp_distance;
int32_t wp_bearing;
float wp_xtrack_error;
uint16_t wp_number;
};
void set_nav_info(NavInfo &nav_info);
private:
void osd_thread();
void update_osd();
@ -226,4 +240,5 @@ private:
uint16_t previous_channel_value;
bool switch_debouncer;
uint32_t last_switch_ms;
struct NavInfo nav_info;
};

30
libraries/AP_OSD/AP_OSD_Screen.cpp
View File

@ -161,6 +161,14 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
// @Path: AP_OSD_Setting.cpp
AP_SUBGROUPINFO(hdop, "HDOP", 29, AP_OSD_Screen, AP_OSD_Setting),
// @Group: WAYPOINT
// @Path: AP_OSD_Setting.cpp
AP_SUBGROUPINFO(waypoint, "WAYPOINT", 30, AP_OSD_Screen, AP_OSD_Setting),
// @Group: XTRACK
// @Path: AP_OSD_Setting.cpp
AP_SUBGROUPINFO(xtrack_error, "XTRACK", 31, AP_OSD_Screen, AP_OSD_Setting),
AP_GROUPEND
};
@ -795,6 +803,25 @@ void AP_OSD_Screen::draw_hdop(uint8_t x, uint8_t y)
backend->write(x, y, false, "%c%c%3.2f", SYM_HDOP_L, SYM_HDOP_R, hdp);
}
void AP_OSD_Screen::draw_waypoint(uint8_t x, uint8_t y)
{
AP_AHRS &ahrs = AP::ahrs();
int32_t angle = wrap_360_cd(osd->nav_info.wp_bearing - ahrs.yaw_sensor);
int32_t interval = 36000 / SYM_ARROW_COUNT;
if (osd->nav_info.wp_distance < 2.0f) {
//avoid fast rotating arrow at small distances
angle = 0;
}
char arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT;
backend->write(x,y, false, "%c%2u%c",SYM_WPNO, osd->nav_info.wp_number, arrow);
draw_distance(x+4, y, osd->nav_info.wp_distance);
}
void AP_OSD_Screen::draw_xtrack_error(uint8_t x, uint8_t y)
{
backend->write(x, y, false, "%c%4d", SYM_XERR, (int)osd->nav_info.wp_xtrack_error);
}
#define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
void AP_OSD_Screen::draw(void)
@ -810,6 +837,8 @@ void AP_OSD_Screen::draw(void)
DRAW_SETTING(horizon);
DRAW_SETTING(compass);
DRAW_SETTING(altitude);
DRAW_SETTING(waypoint);
DRAW_SETTING(xtrack_error);
DRAW_SETTING(bat_volt);
DRAW_SETTING(rssi);
DRAW_SETTING(current);
@ -837,3 +866,4 @@ void AP_OSD_Screen::draw(void)
DRAW_SETTING(gps_latitude);
DRAW_SETTING(gps_longitude);
}