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

Copter: Fix LAND overshoot issue

Browse Source
This commit is contained in:
Jonathan Challinger 2014-12-17 20:11:28 -08:00 committed by Randy Mackay
parent 3afde0061f
commit 91bff63267
3 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/control_auto.pde
View File

@ -323,10 +323,14 @@ static void auto_land_run()
// run loiter controller
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
float cmb_rate = get_land_descent_speed();
// call z-axis position controller
pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt, true);
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
}

14
ArduCopter/control_land.pde
View File

@ -108,9 +108,11 @@ static void land_gps_run()
cmb_rate = 0;
} else {
land_pause = false;
cmb_rate = get_throttle_land();
cmb_rate = get_land_descent_speed();
}
desired_climb_rate = cmb_rate;
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
@ -166,18 +168,20 @@ static void land_nogps_run()
cmb_rate = 0;
} else {
land_pause = false;
cmb_rate = get_throttle_land();
cmb_rate = get_land_descent_speed();
}
desired_climb_rate = cmb_rate;
// call position controller
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
}
// get_throttle_land - high level landing logic
// get_land_descent_speed - high level landing logic
// returns climb rate (in cm/s) which should be passed to the position controller
// should be called at 100hz or higher
static float get_throttle_land()
static float get_land_descent_speed()
{
#if CONFIG_SONAR == ENABLED
bool sonar_ok = sonar_enabled && sonar.healthy();
@ -185,7 +189,7 @@ static float get_throttle_land()
bool sonar_ok = false;
#endif
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
if (current_loc.alt >= LAND_START_ALT && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
if (pos_control.get_pos_target().z >= LAND_START_ALT && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) {
return pos_control.get_speed_down();
}else{
return -abs(g.land_speed);

4
ArduCopter/control_rtl.pde
View File

@ -376,10 +376,12 @@ static void rtl_land_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
float cmb_rate = get_throttle_land();
float cmb_rate = get_land_descent_speed();
pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true);
pos_control.update_z_controller();
desired_climb_rate = cmb_rate;
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);