Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: after takeoff, xtrack to a heading-hold generated virtual wp 10k ahead

Browse Source
This commit is contained in:
Tom Pittenger 2021-05-14 17:37:03 -07:00 committed by Tom Pittenger
parent a57e6455ab
commit 3dd51e9345
1 changed files with 21 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
ArduPlane/commands_logic.cpp
View File

@ -558,9 +558,23 @@ bool Plane::verify_takeoff()
// course. This keeps wings level until we are ready to // course. This keeps wings level until we are ready to
// rotate, and also allows us to cope with arbitrary // rotate, and also allows us to cope with arbitrary
// compass errors for auto takeoff // compass errors for auto takeoff
float takeoff_course = wrap_PI(radians(gps.ground_course())) - steer_state.locked_course_err; const float ground_course_rad = wrap_PI(ahrs.groundspeed_vector().angle());
takeoff_course = wrap_PI(takeoff_course); const float takeoff_course_rad = wrap_PI(ground_course_rad - steer_state.locked_course_err);
steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100); steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course_rad)*100);
// when true, we will follow the recorded takeoff heading.
// when false, we xtrack to a virtual waypoint 10km ahead of us on this takeoff heading.
// We start false (heading hold) until we gain a little altitude then we switch to xtrack
steer_state.locked_course = false;
// grab current location and heading to calcule next_WP_loc 10km ahead.
// Save this for when we gain a little altitude
const float ground_course = wrap_360(degrees(ground_course_rad));
next_WP_loc = current_loc;
next_WP_loc.offset_bearing(ground_course, 10000); // push it out 10km
set_next_WP(next_WP_loc);
auto_state.crosstrack = true; // For when we start calling update_waypoint() soon
gcs().send_text(MAV_SEVERITY_INFO, "Holding course %d at %.1fm/s (%.1f)", gcs().send_text(MAV_SEVERITY_INFO, "Holding course %d at %.1fm/s (%.1f)",
(int)steer_state.hold_course_cd, (int)steer_state.hold_course_cd,
(double)gps.ground_speed(), (double)gps.ground_speed(),
@ -568,7 +582,10 @@ bool Plane::verify_takeoff()
} }
} }
if (steer_state.hold_course_cd != -1) { if (!steer_state.locked_course && !prev_WP_loc.same_latlon_as(next_WP_loc)) {
// heading is not locked and we have valid waypoints to follow
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
} else if (steer_state.hold_course_cd != -1) {
// call navigation controller for heading hold // call navigation controller for heading hold
nav_controller->update_heading_hold(steer_state.hold_course_cd); nav_controller->update_heading_hold(steer_state.hold_course_cd);
} else { } else {