Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: use altitude_error for quadplane climb rate assistance

This commit is contained in:
Andrew Tridgell 2016-01-20 17:21:54 +11:00
parent 69b2421563
commit 0223218c39
1 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduPlane/quadplane.cpp
View File

@ -691,13 +691,16 @@ void QuadPlane::set_armed(bool armed)
*/
float QuadPlane::assist_climb_rate_cms(void)
{
float climb_rate;
if (plane.auto_throttle_mode) {
// ask TECS for its desired climb rate
return plane.TECS_controller.get_height_rate_demand()*100;
// use altitude_error_cm, spread over 10s interval
climb_rate = plane.altitude_error_cm / 10;
} else {
// otherwise estimate from pilot input
climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd);
climb_rate *= plane.channel_throttle->control_in;
}
// otherwise estimate from pilot input
float climb_rate = plane.g.flybywire_climb_rate * (plane.nav_pitch_cd/(float)plane.aparm.pitch_limit_max_cd);
climb_rate *= plane.channel_throttle->control_in;
climb_rate = constrain_float(climb_rate, -wp_nav->get_speed_down(), wp_nav->get_speed_up());
return climb_rate;
}
@ -1068,10 +1071,12 @@ void QuadPlane::control_auto(const Location &loc)
switch (plane.mission.get_current_nav_cmd().id) {
case MAV_CMD_NAV_VTOL_LAND:
if (land_state > QLAND_POSITION && land_state < QLAND_FINAL) {
pos_control->set_alt_target_from_climb_rate(-wp_nav->get_speed_down(), plane.G_Dt, true);
if (land_state == QLAND_POSITION) {
pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false);
} else if (land_state > QLAND_POSITION && land_state < QLAND_FINAL) {
pos_control->set_alt_target_from_climb_rate(-wp_nav->get_speed_down(), plane.G_Dt, true);
} else {
pos_control->set_alt_target_from_climb_rate(-land_speed_cms, plane.G_Dt, true);
pos_control->set_alt_target_from_climb_rate(-land_speed_cms, plane.G_Dt, true);
}
break;
case MAV_CMD_NAV_VTOL_TAKEOFF:
@ -1145,6 +1150,7 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
target.y = diff2d.y * 100;
target.z = plane.next_WP_loc.alt - origin.alt;
wp_nav->set_wp_origin_and_destination(inertial_nav.get_position(), target);
pos_control->set_alt_target(inertial_nav.get_altitude());
// also update nav_controller for status output
plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);