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

Plane: rapid climb in QRTL if below target altitude

Browse Source
This commit is contained in:
Andrew Tridgell 2016-04-29 15:50:45 +10:00
parent 4666b25258
commit 66d3668ac4
2 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduPlane/quadplane.cpp
View File

@ -1208,12 +1208,16 @@ void QuadPlane::land_controller(void)
case QLAND_POSITION2:
if (plane.control_mode == QRTL) {
plane.ahrs.get_position(plane.current_loc);
plane.auto_state.wp_proportion = location_path_proportion(plane.current_loc,
plane.prev_WP_loc, plane.next_WP_loc);
float target_altitude = linear_interpolate(plane.prev_WP_loc.alt,
plane.next_WP_loc.alt,
plane.auto_state.wp_proportion,
0, 1);
float target_altitude = plane.next_WP_loc.alt;
if (land.slow_descent) {
// gradually descend as we approach target
plane.auto_state.wp_proportion = location_path_proportion(plane.current_loc,
plane.prev_WP_loc, plane.next_WP_loc);
target_altitude = linear_interpolate(plane.prev_WP_loc.alt,
plane.next_WP_loc.alt,
plane.auto_state.wp_proportion,
0, 1);
}
pos_control->set_alt_target(target_altitude - plane.home.alt);
} else {
pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false);
@ -1370,6 +1374,7 @@ void QuadPlane::init_qrtl(void)
// use do_RTL() to setup next_WP_loc
plane.do_RTL(plane.home.alt + qrtl_alt*100UL);
plane.prev_WP_loc = plane.current_loc;
land.slow_descent = (plane.current_loc.alt > plane.next_WP_loc.alt);
}
/*

1
ArduPlane/quadplane.h
View File

@ -257,6 +257,7 @@ private:
Vector2f target_velocity;
float max_speed;
Vector3f target;
bool slow_descent:1;
} land;
enum frame_class {