Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Plane: takeoff to xtrack to virtual wp ahead instead of heading-hold #14679

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+21 −4
Open

Plane: takeoff to xtrack to virtual wp ahead instead of heading-hold #14679

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 21 additions & 4 deletions ArduPlane/commands_logic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -558,17 +558,34 @@ bool Plane::verify_takeoff()
// course. This keeps wings level until we are ready to
// rotate, and also allows us to cope with arbitrary
// compass errors for auto takeoff
float takeoff_course = wrap_PI(radians(gps.ground_course())) - steer_state.locked_course_err;
takeoff_course = wrap_PI(takeoff_course);
steer_state.hold_course_cd = wrap_360_cd(degrees(takeoff_course)*100);
const float ground_course_rad = wrap_PI(ahrs.groundspeed_vector().angle());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should have parameters to choose between current scheme, next WP lat/lon and this scheme

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_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;
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

An improvement would be to start off as true and then flip to false one we gain a little altitude, like 10m. That would solve any ground-steering vehicle issues and not induce any funny low-altitude behavior.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This relies on starting with a good initial velocity vector. I had to look at this for a fast plane that had a longer takeoff run a while ago and ended up adding an option to use the takeoff waypoint location to set the desired ground track - see priseborough@a6d2b2b


// 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)",
(int)steer_state.hold_course_cd,
(double)gps.ground_speed(),
(double)degrees(steer_state.locked_course_err));
}
}

if (steer_state.hold_course_cd != -1) {
if (!steer_state.locked_course && !prev_WP_loc.same_latlon_as(next_WP_loc)) {
magicrub marked this conversation as resolved.
Show resolved Hide resolved
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should instead check auto_state.crosstrack

// 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
nav_controller->update_heading_hold(steer_state.hold_course_cd);
} else {
Expand Down