Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -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 Randy Mackay
parent f592806184
commit 9ac79b1698
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 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... 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), SCHED_TASK(afs_fs_check, 10, 200),
#endif #endif
SCHED_TASK(read_airspeed, 10, 100), 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]; 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; Rover rover;
AP_HAL_MAIN_CALLBACKS(&rover); AP_HAL_MAIN_CALLBACKS(&rover);

4
APMrover2/Rover.h
View File

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