From 0a101162c2111e533dc9c13b584fbf6d4dfd9cee Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sat, 5 Jan 2013 10:55:18 +0900 Subject: [PATCH] ArduCopter: update altitude_error reported to GCS --- ArduCopter/ArduCopter.pde | 8 +++++++- ArduCopter/Attitude.pde | 17 ++++++++++------- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 9969a8c4ef..04faf00001 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1767,16 +1767,21 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) case THROTTLE_MANUAL: case THROTTLE_MANUAL_TILT_COMPENSATED: throttle_accel_deactivate(); // this controller does not use accel based throttle controller + altitude_error = 0; // clear altitude error reported to GCS throttle_initialised = true; break; case THROTTLE_ACCELERATION: // pilot inputs the desired acceleration if( g.throttle_accel_enabled ) { // this throttle mode requires use of the accel based throttle controller + altitude_error = 0; // clear altitude error reported to GCS throttle_initialised = true; } break; case THROTTLE_RATE: + altitude_error = 0; // clear altitude error reported to GCS + throttle_initialised = true; + break; case THROTTLE_STABILIZED_RATE: case THROTTLE_DIRECT_ALT: throttle_initialised = true; @@ -1930,6 +1935,7 @@ void update_throttle_mode(void) if(g.rc_3.control_in <= 0){ set_throttle_out(0, false); throttle_accel_deactivate(); // do not allow the accel based throttle to override our command + altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS }else{ pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in); get_throttle_rate_stabilized(pilot_climb_rate); @@ -1941,8 +1947,8 @@ void update_throttle_mode(void) if(g.rc_3.control_in <= 0){ set_throttle_out(0, false); throttle_accel_deactivate(); // do not allow the accel based throttle to override our command + altitude_error = 0; // clear altitude error reported to GCS - normally underlying alt hold controller updates altitude error reported to GCS }else{ - // To-Do: this should update the global desired altitude variable next_WP.alt 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); } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 2baeb89685..7d70bcfb2c 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -988,22 +988,22 @@ get_throttle_rate(int16_t z_target_speed) static void get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_climb_rate) { - int32_t altitude_error; + int32_t alt_error; int16_t desired_rate; int32_t linear_distance; // the distace we swap between linear and sqrt. // calculate altitude error - altitude_error = target_alt - current_loc.alt; + alt_error = target_alt - current_loc.alt; // check kP to avoid division by zero if( g.pi_alt_hold.kP() != 0 ) { linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP()); - if( altitude_error > 2*linear_distance ) { - desired_rate = sqrt(2*250*(altitude_error-linear_distance)); - }else if( altitude_error < -2*linear_distance ) { - desired_rate = -sqrt(2*250*(-altitude_error-linear_distance)); + if( alt_error > 2*linear_distance ) { + desired_rate = sqrt(2*250*(alt_error-linear_distance)); + }else if( alt_error < -2*linear_distance ) { + desired_rate = -sqrt(2*250*(-alt_error-linear_distance)); }else{ - desired_rate = g.pi_alt_hold.get_p(altitude_error); + desired_rate = g.pi_alt_hold.get_p(alt_error); } }else{ desired_rate = 0; @@ -1014,6 +1014,9 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli // call rate based throttle controller which will update accel based throttle controller targets get_throttle_rate(desired_rate); + // update altitude error reported to GCS + altitude_error = alt_error; + // TO-DO: enabled PID logging for this controller }