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

Rover: publish nav data to OSD

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

23
APMrover2/Rover.cpp
View File

@ -22,7 +22,7 @@
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Jean-Louis Naudin, Grant Morphett
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier
APMrover alpha version tester: Franco Borasio, Daniel Chapelat...
@ -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,