Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: publish nav data to OSD

Browse Source
This commit is contained in:
Henry Wurzburg 2019-09-21 14:49:31 -05:00 committed by Andrew Tridgell
parent 0f0ceb6499
commit fb66b8b961
2 changed files with 26 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
APMrover2/Rover.cpp
View File

@ -105,6 +105,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
SCHED_TASK(afs_fs_check, 10, 200),
#endif
SCHED_TASK(read_airspeed, 10, 100),
#if OSD_ENABLED == ENABLED
SCHED_TASK(publish_osd_info, 1, 10),
#endif
};
constexpr int8_t Rover::_failsafe_priorities[7];
@ -324,6 +327,24 @@ void Rover::update_mission(void)
}
}
#if OSD_ENABLED == ENABLED
void Rover::publish_osd_info()
{
AP_OSD::NavInfo nav_info {};
if (control_mode == &mode_loiter) {
nav_info.wp_xtrack_error = control_mode->get_distance_to_destination();
} else {
nav_info.wp_xtrack_error = control_mode->crosstrack_error();
}
nav_info.wp_distance = control_mode->get_distance_to_destination();
nav_info.wp_bearing = control_mode->wp_bearing() * 100.0f;
if (control_mode == &mode_auto) {
nav_info.wp_number = mode_auto.mission.get_current_nav_index();
}
osd.set_nav_info(nav_info);
}
#endif
Rover rover;
AP_HAL_MAIN_CALLBACKS(&rover);

4
APMrover2/Rover.h
View File

@ -449,6 +449,10 @@ private:
bool should_log(uint32_t mask);
bool is_boat() const;
#if OSD_ENABLED == ENABLED
void publish_osd_info();
#endif
enum Failsafe_Action {
Failsafe_Action_None = 0,
Failsafe_Action_RTL = 1,