Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: added virtual getters for waypoint info

Browse Source
This commit is contained in:
yaapu 2020-12-05 19:44:48 +01:00 committed by Peter Barker
parent 497dbd1414
commit 141010ac39
2 changed files with 46 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
ArduPlane/ArduPlane.cpp
View File

@ -105,9 +105,6 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75),
#endif
#if OSD_ENABLED == ENABLED
SCHED_TASK(publish_osd_info, 1, 10),
#endif
#if LANDING_GEAR_ENABLED == ENABLED
SCHED_TASK(landing_gear_update, 5, 50),
#endif
@ -601,17 +598,49 @@ float Plane::tecs_hgt_afe(void)
return hgt_afe;
}
#if OSD_ENABLED == ENABLED
void Plane::publish_osd_info()
// vehicle specific waypoint info helpers
bool Plane::get_wp_distance_m(float &distance) const
{
AP_OSD::NavInfo nav_info;
nav_info.wp_distance = auto_state.wp_distance;
nav_info.wp_bearing = nav_controller->target_bearing_cd();
nav_info.wp_xtrack_error = nav_controller->crosstrack_error();
nav_info.wp_number = mission.get_current_nav_index();
osd.set_nav_info(nav_info);
// see GCS_MAVLINK_Plane::send_nav_controller_output()
if (control_mode == &mode_manual) {
return false;
}
if (quadplane.in_vtol_mode()) {
distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination() : 0;
} else {
distance = auto_state.wp_distance;
}
return true;
}
#endif
bool Plane::get_wp_bearing_deg(float &bearing) const
{
// see GCS_MAVLINK_Plane::send_nav_controller_output()
if (control_mode == &mode_manual) {
return false;
}
if (quadplane.in_vtol_mode()) {
bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0;
} else {
bearing = nav_controller->target_bearing_cd() * 0.01;
}
return true;
}
bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
{
// see GCS_MAVLINK_Plane::send_nav_controller_output()
if (control_mode == &mode_manual) {
return false;
}
if (quadplane.in_vtol_mode()) {
xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error() : 0;
} else {
xtrack_error = nav_controller->crosstrack_error();
}
return true;
}
// set target location (for use by scripting)
bool Plane::set_target_location(const Location& target_loc)

8
ArduPlane/Plane.h
View File

@ -977,9 +977,6 @@ private:
void update_control_mode(void);
void update_flight_stage();
void set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs);
#if OSD_ENABLED || OSD_PARAM_ENABLED
void publish_osd_info();
#endif
// navigation.cpp
void set_nav_controller(void);
@ -1082,6 +1079,11 @@ private:
void update_soaring();
#endif
// 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;
// reverse_thrust.cpp
bool reversed_throttle;
bool have_reverse_throttle_rc_option;