Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: added virtual getters for waypoint info

Browse Source
This commit is contained in:
yaapu 2020-12-05 19:45:11 +01:00 committed by Peter Barker
parent 141010ac39
commit 8ea896e138
2 changed files with 34 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
Rover/Rover.cpp
View File

@ -107,9 +107,6 @@ 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
}; };
@ -384,23 +381,39 @@ void Rover::update_mission(void)
} }
} }
#if OSD_ENABLED == ENABLED // vehicle specific waypoint info helpers
void Rover::publish_osd_info() bool Rover::get_wp_distance_m(float &distance) const
{ {
AP_OSD::NavInfo nav_info {0}; // see GCS_MAVLINK_Rover::send_nav_controller_output()
if (control_mode == &mode_loiter) { if (!rover.control_mode->is_autopilot_mode()) {
nav_info.wp_xtrack_error = control_mode->get_distance_to_destination(); return false;
} else {
nav_info.wp_xtrack_error = control_mode->crosstrack_error();
} }
nav_info.wp_distance = control_mode->get_distance_to_destination(); distance = control_mode->get_distance_to_destination();
nav_info.wp_bearing = control_mode->wp_bearing() * 100.0f; return true;
if (control_mode == &mode_auto) {
nav_info.wp_number = mode_auto.mission.get_current_nav_index();
}
osd.set_nav_info(nav_info);
} }
#endif
// vehicle specific waypoint info helpers
bool Rover::get_wp_bearing_deg(float &bearing) const
{
// see GCS_MAVLINK_Rover::send_nav_controller_output()
if (!rover.control_mode->is_autopilot_mode()) {
return false;
}
bearing = control_mode->wp_bearing();
return true;
}
// vehicle specific waypoint info helpers
bool Rover::get_wp_crosstrack_error_m(float &xtrack_error) const
{
// see GCS_MAVLINK_Rover::send_nav_controller_output()
if (!rover.control_mode->is_autopilot_mode()) {
return false;
}
xtrack_error = control_mode->crosstrack_error();
return true;
}
Rover rover; Rover rover;
AP_Vehicle& vehicle = rover; AP_Vehicle& vehicle = rover;

7
Rover/Rover.h
View File

@ -390,9 +390,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 || OSD_PARAM_ENABLED // vehicle specific waypoint info helpers
void publish_osd_info(); bool get_wp_distance_m(float &distance) const override;
#endif bool get_wp_bearing_deg(float &bearing) const override;
bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
enum Failsafe_Action { enum Failsafe_Action {
Failsafe_Action_None = 0, Failsafe_Action_None = 0,