aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduSub: added virtual getters for waypoint info

Browse Source
This commit is contained in:
yaapu 2020-12-06 13:38:12 +01:00 committed by Peter Barker
parent 8ea896e138
commit 2e69ba1091
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduSub/ArduSub.cpp
View File

@ -326,4 +326,28 @@ bool Sub::control_check_barometer()
return true;
}
// vehicle specific waypoint info helpers
bool Sub::get_wp_distance_m(float &distance) const
{
// see GCS_MAVLINK_Sub::send_nav_controller_output()
distance = sub.wp_nav.get_wp_distance_to_destination() * 0.01;
return true;
}
// vehicle specific waypoint info helpers
bool Sub::get_wp_bearing_deg(float &bearing) const
{
// see GCS_MAVLINK_Sub::send_nav_controller_output()
bearing = sub.wp_nav.get_wp_bearing_to_destination() * 0.01;
return true;
}
// vehicle specific waypoint info helpers
bool Sub::get_wp_crosstrack_error_m(float &xtrack_error) const
{
// no crosstrack error reported, see GCS_MAVLINK_Sub::send_nav_controller_output()
xtrack_error = 0;
return true;
}
AP_HAL_MAIN_CALLBACKS(&sub);

5
ArduSub/Sub.h
View File

@ -639,6 +639,11 @@ private:
bool control_check_barometer();
// vehicle specific waypoint info helpers
bool get_wp_distance_m(float &distance) const override;
bool get_wp_bearing_deg(float &bearing) const override;
bool get_wp_crosstrack_error_m(float &xtrack_error) const override;
enum Failsafe_Action {
Failsafe_Action_None = 0,
Failsafe_Action_Warn = 1,