From 27fc6eea105d7d4452c2accd36bc9d3faa2c70ee Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 1 Nov 2011 21:18:47 -0700 Subject: [PATCH] Z dampener Integrated Aurelio R. Ramos' Z dampening code. --- ArduCopter/APM_Config.h | 4 +- ArduCopter/ArduCopter.pde | 13 +--- ArduCopter/Attitude.pde | 128 +++++++++++++++++++++++++++++--------- ArduCopter/motors.pde | 6 ++ 4 files changed, 108 insertions(+), 43 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index d97a104670..2fbfc1b4da 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -39,9 +39,7 @@ CH7_SAVE_WP */ -#define ACCEL_ALT_HOLD 0 // disabled by default, work in progress -#define ACCEL_ALT_HOLD_GAIN 20.0 -// ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode +#define ACCEL_ALT_HOLD 1 // disabled by default, work in progress // See the config.h and defines.h files for how to set this up! // diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 113dbe5a4a..42dbee5d9b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -246,14 +246,6 @@ static const char* flight_mode_strings[] = { 8 TBD */ -// test -#if ACCEL_ALT_HOLD == 1 -Vector3f accels_rot; -static int accels_rot_count; -static float accels_rot_sum; -static float alt_hold_gain = ACCEL_ALT_HOLD_GAIN; -#endif - // temp static int y_actual_speed; static int y_rate_error; @@ -1070,13 +1062,14 @@ void update_throttle_mode(void) g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle); #else angle_boost = get_angle_boost(g.throttle_cruise); + if(manual_boost != 0){ //remove alt_hold_velocity when implemented - g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + alt_hold_velocity(); + g.rc_3.servo_out = g.throttle_cruise + angle_boost + manual_boost + get_z_damping(); // reset next_WP.alt next_WP.alt = max(current_loc.alt, 100); }else{ - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + alt_hold_velocity(); + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping(); } #endif diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 030d6c36aa..2d222dd159 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -176,36 +176,6 @@ get_nav_yaw_offset(int yaw_input, int reset) } } -static int alt_hold_velocity() -{ - #if ACCEL_ALT_HOLD == 1 - Vector3f accel_filt; - float error; - - // subtract filtered Accel - error = abs(next_WP.alt - current_loc.alt) - 25; - error = min(error, 50.0); - error = max(error, 0.0); - error = 1 - (error/ 50.0); - - accel_filt = imu.get_accel_filtered(); - accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); - int output = (accels_rot.z + 9.81) * alt_hold_gain * error; // alt_hold_gain = 12 - - //Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output); - return constrain(output, -70, 70); - -// fast rise -//s: -17.6241, g:0.0000, e:1.0000, o:0 -//s: -18.4990, g:0.0000, e:1.0000, o:0 -//s: -19.3193, g:0.0000, e:1.0000, o:0 -//s: -13.1310, g:47.8700, e:1.0000, o:-158 - - #else - return 0; - #endif -} - static int get_angle_boost(int value) { float temp = cos_pitch_x * cos_roll_x; @@ -213,3 +183,101 @@ static int get_angle_boost(int value) return (int)(temp * value); } +// Accelerometer Z dampening by Aurelio R. Ramos +// --------------------------------------------- + +#if ACCEL_ALT_HOLD == 1 + +// contains G and any other DC offset +static float estimatedAccelOffset = 0; + +// state +static float synVelo = 0; +static float synPos = 0; +static float synPosFiltered = 0; +static float posError = 0; +static float prevSensedPos = 0; + +// tuning for dead reckoning +static float synPosP = 25 * G_Dt; +static float synPosI = 100 * G_Dt; +static float synVeloP = 1.5 * G_Dt; +static float maxVeloCorrection = 5 * G_Dt; +static float maxSensedVelo = 1; +static float synPosFilter = 0.13; // lower to filter more. should approximate sensor filtering + +#define NUM_G_SAMPLES 200 + +// Z damping term. +static float fullDampP = 0.200; + +float get_world_Z_accel() +{ + Vector3f accels_rot = dcm.get_dcm_matrix() * imu.get_accel(); + return accels_rot.z; +} + + +static void init_z_damper() +{ + estimatedAccelOffset = 0; + for (int i = 0; i < NUM_G_SAMPLES; i++){ + estimatedAccelOffset += get_world_Z_accel(); + } + estimatedAccelOffset /= (float)NUM_G_SAMPLES; +} + +float dead_reckon_Z(float sensedPos, float sensedAccel) +{ + // the following algorithm synthesizes position and velocity from + // a noisy altitude and accelerometer data. + + // synthesize uncorrected velocity by integrating acceleration + synVelo += (sensedAccel - estimatedAccelOffset) * G_Dt; + + // synthesize uncorrected position by integrating uncorrected velocity + synPos += synVelo * G_Dt; + + // filter synPos, the better this filter matches the filtering and dead time + // of the sensed position, the less the position estimate will lag. + synPosFiltered = synPosFiltered * (1 - synPosFilter) + synPos * synPosFilter; + + // calculate error against sensor position + posError = sensedPos - synPosFiltered; + + // correct altitude + synPos += synPosP * posError; + + // correct integrated velocity by posError + synVelo = synVelo + constrain(posError, -maxVeloCorrection, maxVeloCorrection) * synPosI; + + // correct integrated velocity by the sensed position delta in a small proportion + // (i.e., the low frequency of the delta) + float sensedVelo = (sensedPos - prevSensedPos) / G_Dt; + synVelo += constrain(sensedVelo - synVelo, -maxSensedVelo, maxSensedVelo) * synVeloP; + + prevSensedPos = sensedPos; + return synVelo; +} + +static int get_z_damping() +{ + float sensedAccel = get_world_Z_accel(); + float sensedPos = current_loc.alt / 100.0; + + float synVelo = dead_reckon_Z(sensedPos, sensedAccel); + return constrain(fullDampP * synVelo * (-1), -300, 300); +} + +#else + +static int get_z_damping() +{ + return 0; +} + +#endif + + + + diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 36eff7d7af..0005c98148 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -53,6 +53,12 @@ static void arm_motors() // -------------------- init_simple_bearing(); + // init the Z damopener + // -------------------- + #if ACCEL_ALT_HOLD == 1 + init_z_damper(); + #endif + // Reset home position // ---------------------- if(home_is_set)