From 21b6c78d12eec79a1050a68198ffa73ab2214e49 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Tue, 8 Jan 2013 16:41:07 +0900 Subject: [PATCH] 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. --- ArduCopter/ArduCopter.pde | 34 ++++++++-------- ArduCopter/Attitude.pde | 85 +++++++++++++++++++++++++-------------- ArduCopter/config.h | 19 ++++++++- ArduCopter/sensors.pde | 8 ++-- 4 files changed, 94 insertions(+), 52 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c965009ed3..3c1865ff63 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -315,9 +315,8 @@ GCS_MAVLINK gcs3; // SONAR selection //////////////////////////////////////////////////////////////////////////////// // - +ModeFilterInt16_Size3 sonar_mode_filter(1); #if CONFIG_SONAR == ENABLED -ModeFilterInt16_Size5 sonar_mode_filter(2); AP_HAL::AnalogSource *sonar_analog_source; AP_RangeFinder_MaxsonarXL *sonar; #endif @@ -640,6 +639,8 @@ static float current_total1; //////////////////////////////////////////////////////////////////////////////// // Altitude //////////////////////////////////////////////////////////////////////////////// +// The (throttle) controller desired altitude in cm +static float controller_desired_alt; // The cm we are off in altitude from next_WP.alt – Positive value means we are below the WP static int32_t altitude_error; // The cm/s we are moving up or down based on sensor data - Positive = UP @@ -650,7 +651,7 @@ static int16_t climb_rate_error; static int16_t climb_rate; // The altitude as reported by Sonar in cm – Values are 20 to 700 generally. static int16_t sonar_alt; -static bool sonar_alt_ok; // true if we can trust the altitude from the sonar +static uint8_t sonar_alt_health; // true if we can trust the altitude from the sonar // The climb_rate as reported by sonar in cm/s static int16_t sonar_rate; // The altitude as reported by Baro in cm – Values can be quite high @@ -1786,13 +1787,17 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) altitude_error = 0; // clear altitude error reported to GCS throttle_initialised = true; break; + case THROTTLE_STABILIZED_RATE: case THROTTLE_DIRECT_ALT: + controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude throttle_initialised = true; break; case THROTTLE_HOLD: case THROTTLE_AUTO: + case THROTTLE_SURFACE_TRACKING: + controller_desired_alt = current_loc.alt; // reset controller desired altitude to current altitude 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(); @@ -1803,19 +1808,12 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) case THROTTLE_LAND: 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 + throttle_initialised = true; break; default: @@ -1954,7 +1952,7 @@ void update_throttle_mode(void) altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS }else{ int32_t desired_alt = get_pilot_desired_direct_alt(g.rc_3.control_in); - get_throttle_althold(desired_alt, g.auto_velocity_z_min, g.auto_velocity_z_max); + get_throttle_althold_with_slew(desired_alt, g.auto_velocity_z_min, g.auto_velocity_z_max); } break; @@ -1967,7 +1965,7 @@ void update_throttle_mode(void) case THROTTLE_AUTO: // auto pilot altitude controller with target altitude held in next_WP.alt if(motors.auto_armed() == true) { - get_throttle_althold(next_WP.alt, g.auto_velocity_z_min, g.auto_velocity_z_max); + get_throttle_althold_with_slew(next_WP.alt, g.auto_velocity_z_min, g.auto_velocity_z_max); } break; @@ -1979,7 +1977,7 @@ void update_throttle_mode(void) case THROTTLE_SURFACE_TRACKING: // surface tracking with sonar or other rangefinder plus pilot input of climb rate pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); - if( sonar_alt_ok ) { + if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) { // if sonar is ok, use surface tracking get_throttle_surface_tracking(pilot_climb_rate); }else{ diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 8b31422d98..9c11508dd5 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -787,15 +787,24 @@ static int16_t get_throttle_accel(int16_t z_target_accel) { static float z_accel_error = 0; // The acceleration error in cm. + static uint32_t last_call_ms = 0; // the last time this controller was called int32_t p,i,d; // used to capture pid values for logging int16_t output; float z_accel_meas; + uint32_t now = millis(); // Calculate Earth Frame Z acceleration z_accel_meas = -(ahrs.get_accel_ef().z + GRAVITY_MSS) * 100; - // calculate accel error and Filter with fc = 2 Hz - z_accel_error = z_accel_error + 0.11164 * (constrain(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error); + // reset target altitude if this controller has just been engaged + if( now - last_call_ms > 100 ) { + // Reset Filter + z_accel_error = 0; + } else { + // calculate accel error and Filter with fc = 2 Hz + z_accel_error = z_accel_error + 0.11164 * (constrain(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error); + } + last_call_ms = now; // separately calculate p, i, d values for logging p = g.pid_throttle_accel.get_p(z_accel_error); @@ -916,12 +925,21 @@ static int32_t get_pilot_desired_direct_alt(int16_t throttle_control) static void get_throttle_rate(int16_t z_target_speed) { + static uint32_t last_call_ms = 0; static float z_rate_error = 0; // The velocity error in cm. int32_t p,i,d; // used to capture pid values for logging int16_t output; // the target acceleration if the accel based throttle is enabled, otherwise the output to be sent to the motors + uint32_t now = millis(); - // calculate rate error and filter with cut off frequency of 2 Hz - z_rate_error = z_rate_error + 0.20085 * ((z_target_speed - climb_rate) - z_rate_error); + // reset target altitude if this controller has just been engaged + if( now - last_call_ms > 100 ) { + // Reset Filter + z_rate_error = 0; + } else { + // calculate rate error and filter with cut off frequency of 2 Hz + z_rate_error = z_rate_error + 0.20085 * ((z_target_speed - climb_rate) - z_rate_error); + } + last_call_ms = now; // separately calculate p, i, d values for logging p = g.pid_throttle.get_p(z_rate_error); @@ -980,9 +998,9 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli if( g.pi_alt_hold.kP() != 0 ) { linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP()); if( alt_error > 2*linear_distance ) { - desired_rate = sqrt(2*250*(alt_error-linear_distance)); + desired_rate = safe_sqrt(2*250*(alt_error-linear_distance)); }else if( alt_error < -2*linear_distance ) { - desired_rate = -sqrt(2*250*(-alt_error-linear_distance)); + desired_rate = -safe_sqrt(2*250*(-alt_error-linear_distance)); }else{ desired_rate = g.pi_alt_hold.get_p(alt_error); } @@ -1001,44 +1019,45 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli // TO-DO: enabled PID logging for this controller } +// get_throttle_althold_with_slew - altitude controller with slew to avoid step changes in altitude target +// calls normal althold controller which updates accel based throttle controller targets +static void +get_throttle_althold_with_slew(int16_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate) +{ + // limit target altitude change + controller_desired_alt += constrain(target_alt-controller_desired_alt, min_climb_rate*0.02, max_climb_rate*0.02); + + // do not let target altitude get too far from current altitude + controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750); + + get_throttle_althold(controller_desired_alt, min_climb_rate-250, max_climb_rate+250); // 250 is added to give head room to alt hold controller +} + // get_throttle_rate_stabilized - rate controller with additional 'stabilizer' // 'stabilizer' ensure desired rate is being met // calls normal throttle rate controller which updates accel based throttle controller targets static void get_throttle_rate_stabilized(int16_t target_rate) { - static float target_alt = 0; // The desired altitude in cm. - static uint32_t last_call_ms = 0; - - uint32_t now = millis(); - - // reset target altitude if this controller has just been engaged - if( now - last_call_ms > 1000 ) { - target_alt = current_loc.alt; - } - last_call_ms = millis(); - - target_alt += target_rate * 0.02; + controller_desired_alt += target_rate * 0.02; // do not let target altitude get too far from current altitude - target_alt = constrain(target_alt,current_loc.alt-750,current_loc.alt+750); + controller_desired_alt = constrain(controller_desired_alt,current_loc.alt-750,current_loc.alt+750); - set_new_altitude(target_alt); + set_new_altitude(controller_desired_alt); - 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(LAND_START_ALT, g.auto_velocity_z_min, -abs(g.land_speed)); + if (current_loc.alt >= LAND_START_ALT && !(g.sonar_enabled && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { + get_throttle_althold_with_slew(LAND_START_ALT, g.auto_velocity_z_min, -abs(g.land_speed)); }else{ get_throttle_rate_stabilized(-abs(g.land_speed)); @@ -1069,23 +1088,29 @@ get_throttle_surface_tracking(int16_t target_rate) { static float target_sonar_alt = 0; // The desired altitude in cm above the ground static uint32_t last_call_ms = 0; + float distance_error; + float sonar_induced_slew_rate; uint32_t now = millis(); // reset target altitude if this controller has just been engaged - if( now - last_call_ms > 1000 ) { - target_sonar_alt = sonar_alt + next_WP.alt - current_loc.alt; + if( now - last_call_ms > 200 ) { + target_sonar_alt = sonar_alt + controller_desired_alt - current_loc.alt; } - last_call_ms = millis(); + last_call_ms = now; target_sonar_alt += target_rate * 0.02; + distance_error = (target_sonar_alt-sonar_alt); + sonar_induced_slew_rate = constrain(fabs(THR_SURFACE_TRACKING_P * distance_error),0,THR_SURFACE_TRACKING_VELZ_MAX); + // do not let target altitude get too far from current altitude above ground // Note: the 750cm limit is perhaps too wide but is consistent with the regular althold limits and helps ensure a smooth transition target_sonar_alt = constrain(target_sonar_alt,sonar_alt-750,sonar_alt+750); - set_new_altitude(current_loc.alt+(target_sonar_alt-sonar_alt)); + controller_desired_alt = current_loc.alt+(target_sonar_alt-sonar_alt); + set_new_altitude(controller_desired_alt); - 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 } /* diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3c8041a91a..d67a18f017 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -311,6 +311,18 @@ # define CONFIG_SONAR ENABLED #endif +#ifndef SONAR_ALT_HEALTH_MAX + # define SONAR_ALT_HEALTH_MAX 3 // number of good reads that indicates a healthy sonar +#endif + +#ifndef THR_SURFACE_TRACKING_P + # define THR_SURFACE_TRACKING_P 0.2 // gain for controlling how quickly sonar range adjusts target altitude (lower means slower reaction) +#endif + +#ifndef THR_SURFACE_TRACKING_VELZ_MAX + # define THR_SURFACE_TRACKING_VELZ_MAX 30 // max speed number of good reads that indicates a healthy sonar +#endif + ////////////////////////////////////////////////////////////////////////////// // Channel 7 default option // @@ -504,7 +516,12 @@ #ifndef LAND_SPEED # define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s #endif - +#ifndef LAND_START_ALT + # define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent +#endif +#ifndef LAND_DETECTOR_TRIGGER + # define LAND_DETECTOR_TRIGGER 50 // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete. +#endif ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 9ffffc56c4..8addff15f5 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -37,10 +37,12 @@ static int16_t read_sonar(void) #if CONFIG_SONAR == ENABLED int16_t temp_alt = sonar->read(); - if(temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * 0.70) { - sonar_alt_ok = true; + if (temp_alt >= sonar->min_distance && temp_alt <= sonar->max_distance * 0.70) { + if ( sonar_alt_health < SONAR_ALT_HEALTH_MAX ) { + sonar_alt_health++; + } }else{ - sonar_alt_ok = false; + sonar_alt_health = 0; } #if SONAR_TILT_CORRECTION == 1