From bdb47c449aa8e83932a3d559bfdcb353b352bdfa Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sun, 25 Nov 2012 18:07:33 +0900 Subject: [PATCH] ArduCopter: pass linear altitude error to throttle rate controllers. Previously the requested rate used the square root of the altitude error. Scale pilot's desired vertical rate and acceleration based on ACCELERATION_MAX_Z and VELOCITY_MAX_Z #defines --- ArduCopter/Attitude.pde | 25 +++++++------------------ ArduCopter/config.h | 6 +++++- 2 files changed, 12 insertions(+), 19 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 619fba578c..44481b79a4 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -855,10 +855,10 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) // check throttle is above, below or in the deadband if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) { // below the deadband - desired_rate = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + desired_rate = (int32_t)VELOCITY_MAX_Z * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); }else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) { // above the deadband - desired_rate = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + desired_rate = (int32_t)VELOCITY_MAX_Z * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); }else{ // must be in the deadband desired_rate = 0; @@ -888,10 +888,10 @@ static int16_t get_pilot_desired_acceleration(int16_t throttle_control) // check throttle is above, below or in the deadband if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) { // below the deadband - desired_accel = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + desired_accel = (int32_t)ACCELERATION_MAX_Z * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); }else if(throttle_control > THROTTLE_IN_DEADBAND_TOP) { // above the deadband - desired_accel = (int32_t)THROTTLE_IN_MIDDLE * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); + desired_accel = (int32_t)ACCELERATION_MAX_Z * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - THROTTLE_IN_DEADBAND); }else{ // must be in the deadband desired_accel = 0; @@ -999,14 +999,9 @@ get_throttle_rate_stabilized(int16_t target_rate) altitude_error = constrain(alt_desired - current_loc.alt, -alt_error_max, alt_error_max); alt_desired = current_loc.alt + altitude_error; - if(altitude_error > 0){ - desired_rate = g.pi_alt_hold.get_p(10*sqrt(altitude_error)); - }else if(altitude_error < 0){ - desired_rate = g.pi_alt_hold.get_p(-10*sqrt(abs(altitude_error))); - }else{ - desired_rate = g.pi_alt_hold.get_p(0); - } + desired_rate = g.pi_alt_hold.get_p(altitude_error); desired_rate = constrain(desired_rate, -200, 200) + target_rate; + desired_rate = constrain(desired_rate, -VELOCITY_MAX_Z, VELOCITY_MAX_Z); // TO-DO: replace constrains with ALTHOLD_MIN_CLIMB_RATE and ALTHOLD_MAX_CLIMB_RATE? // call rate based throttle controller which will update accel based throttle controller targets get_throttle_rate(desired_rate); @@ -1022,13 +1017,7 @@ get_throttle_althold(int32_t target_alt, int16_t max_climb_rate) int16_t desired_rate; altitude_error = target_alt - current_loc.alt; - if(altitude_error > 0){ - desired_rate = g.pi_alt_hold.get_p(10*sqrt(altitude_error)); - }else if(altitude_error < 0){ - desired_rate = g.pi_alt_hold.get_p(-10*sqrt(abs(altitude_error))); - }else{ - desired_rate = g.pi_alt_hold.get_p(0); - } + desired_rate = g.pi_alt_hold.get_p(altitude_error); desired_rate = constrain(desired_rate, ALTHOLD_MIN_CLIMB_RATE, max_climb_rate); // call rate based throttle controller which will update accel based throttle controller targets diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 99cefd9f9c..378fe35259 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -895,12 +895,16 @@ // minimum and maximum climb rates while in alt hold mode #ifndef ALTHOLD_MAX_CLIMB_RATE - # define ALTHOLD_MAX_CLIMB_RATE 500 + # define ALTHOLD_MAX_CLIMB_RATE 250 #endif #ifndef ALTHOLD_MIN_CLIMB_RATE # define ALTHOLD_MIN_CLIMB_RATE -ALTHOLD_MAX_CLIMB_RATE #endif +// max allowed acceleration +#define VELOCITY_MAX_Z 250 // maximum vertical velocity in cm/s +#define ACCELERATION_MAX_Z 750 // maximum veritcal acceleration in cm/s/s + // Throttle Accel control #ifndef THROTTLE_ACCEL_P # define THROTTLE_ACCEL_P 0.5