ArduCopter: added get_throttle_althold_with_slew to allow slower altitude target changes
Improved surface tracking by using slewed althold controller
Reduced sonar mode filter to just 3 elements to reduce lag but at the possible consequence of allowing sonar noise to creep through for people with margin sonar set-ups.
set_land_complete(false); // mark landing as incomplete
land_detector = 0; // A counter that goes up if our climb rate stalls out.
set_new_altitude(0); // Set a new target altitude
throttle_initialised = true;
break;
case THROTTLE_SURFACE_TRACKING:
if( g.sonar_enabled ) {
set_new_altitude(current_loc.alt); // by default hold the current altitude
if ( throttle_mode < THROTTLE_HOLD ) { // reset the alt hold I terms if previous throttle mode was manual
reset_throttle_I();
}
throttle_initialised = true;
controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude
// Set target altitude to LAND_START_ALT if we are high, below this altitude the get_throttle_rate_stabilized will take care of setting the next_WP.alt
if (current_loc.alt >= LAND_START_ALT) {
set_new_altitude(LAND_START_ALT);
}
// To-Do: handle the case where the sonar is not enabled
get_throttle_althold(target_alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller
get_throttle_althold(controller_desired_alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller
}
// get_throttle_land - high level landing logic
// sends the desired acceleration in the accel based throttle controller
// called at 50hz
#define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent
#define LAND_DETECTOR_TRIGGER 50 // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete.
static void
get_throttle_land()
{
// if we are above 10m and the sonar does not sense anything perform regular alt hold descent
if (current_loc.alt >= LAND_START_ALT && !(g.sonar_enabled && sonar_alt_ok)) {
get_throttle_althold(next_WP.alt, -g.pilot_velocity_z_max-250, g.pilot_velocity_z_max+250); // 250 is added to give head room to alt hold controller
get_throttle_althold_with_slew(controller_desired_alt, target_rate-sonar_induced_slew_rate, target_rate+sonar_induced_slew_rate); // VELZ_MAX limits how quickly we react
rmackay9
12 years ago
committed by
Andrew Tridgell