From c7077fa9e80c7010de40278b0651cc9738c6d9d2 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 5 Nov 2011 08:59:16 -0600 Subject: [PATCH 1/9] In case of DCM renorm "blow-up" reset the drift correction integrators This should aid in recovery if we loose the AHRS solution and have to force to initial orientation. Also removed some old commented out code moved to the IMU object --- libraries/AP_DCM/AP_DCM.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 4cbcdfeb02..c0ed1f2f00 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -125,14 +125,6 @@ AP_DCM::matrix_update(float _G_Dt) Matrix3f update_matrix; Matrix3f temp_matrix; - //Record when you saturate any of the gyros. - /* - if( (fabs(_gyro_vector.x) >= radians(300)) || - (fabs(_gyro_vector.y) >= radians(300)) || - (fabs(_gyro_vector.z) >= radians(300))){ - gyro_sat_count++; - }*/ - _omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega) _omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms @@ -232,6 +224,9 @@ AP_DCM::normalize(void) _dcm_matrix.c.x = 0.0f; _dcm_matrix.c.y = 0.0f; _dcm_matrix.c.z = 1.0f; + _omega_I.x = 0.0f; + _omega_I.y = 0.0f; + _omega_I.z = 0.0f; } } From 947950398c330303a9a2a9d3738ecd47e9b28782 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 5 Nov 2011 09:01:20 -0600 Subject: [PATCH 2/9] Changed the DCM drift correction integrator limit to a vector magnitude of 30 degrees/second. The drift correction integrator limit previously was near/at the gyro saturation limit. If we have that much drift there is a serious hardware problem. 30 degrees/second is arbitrary but should handle all temperature variation, etc. --- libraries/AP_DCM/AP_DCM.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index c0ed1f2f00..2d881e5866 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -343,10 +343,10 @@ AP_DCM::drift_correction(void) _omega_P += _error_yaw * _kp_yaw; // Adding yaw correction to proportional correction vector. _omega_I += _error_yaw * _ki_yaw; // adding yaw correction to integrator correction vector. - // Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros + // Here we will place a limit on the integrator so that the integrator cannot ever exceed ~30 degrees/second integrator_magnitude = _omega_I.length(); - if (integrator_magnitude > radians(300)) { - _omega_I *= (0.5f * radians(300) / integrator_magnitude); // Why do we have this discontinuous? EG, why the 0.5? + if (integrator_magnitude > radians(30)) { + _omega_I *= (radians(30) / integrator_magnitude); } //Serial.print("*"); } From e3ab1b03534adc008fd944d8c8c0e336143e49f5 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 5 Nov 2011 09:02:23 -0600 Subject: [PATCH 3/9] Add an accessor for the DCM drift correction integrator to monitor performance Also made the get_health accessor an inline function. --- libraries/AP_DCM/AP_DCM.cpp | 8 -------- libraries/AP_DCM/AP_DCM.h | 6 +++--- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 2d881e5866..751caa00b2 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -393,12 +393,4 @@ AP_DCM::euler_yaw(void) yaw_sensor += 36000; } -/**************************************************/ - -float -AP_DCM::get_health(void) -{ - return _health; -} - diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index 503f2ab1fa..afec0fcd45 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -43,7 +43,9 @@ public: Vector3f get_accel(void) { return _accel_vector; } Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();} - + Vector3f get_integrator(void) {return _omega_I; } // We return the current drift correction integrator values + + float get_health(void) {return _health;} void set_centripetal(bool b) {_centripetal = b;} bool get_centripetal(void) {return _centripetal;} void set_compass(Compass *compass); @@ -52,8 +54,6 @@ public: void update_DCM(void); void update_DCM_fast(void); - float get_health(void); - long roll_sensor; // Degrees * 100 long pitch_sensor; // Degrees * 100 long yaw_sensor; // Degrees * 100 From f1e626e3c14f9b4a822480e84940abf78be749c8 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 6 Nov 2011 17:37:59 +0800 Subject: [PATCH 4/9] TradHeli - changed deadzones for throttle to zero (and also reduced for yaw) --- ArduCopter/radio.pde | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index cf5abebca0..f0b3d11d7a 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -8,8 +8,13 @@ static void default_dead_zones() { g.rc_1.set_dead_zone(60); g.rc_2.set_dead_zone(60); - g.rc_3.set_dead_zone(60); - g.rc_4.set_dead_zone(200); + #if FRAME_CONFIG == HELI_FRAME + g.rc_3.set_dead_zone(0); + g.rc_4.set_dead_zone(60); + #else + g.rc_3.set_dead_zone(60); + g.rc_4.set_dead_zone(200); + #endif } static void init_rc_in() From 156b496babd17dfad170f534c70a3d7489393657 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 6 Nov 2011 17:40:17 +0800 Subject: [PATCH 5/9] TradHeli - small change to make throttle_cruise in terms of radio input instead of servo output --- ArduCopter/system.pde | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 8c4e333f45..ad738089c9 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -527,11 +527,7 @@ init_throttle_cruise() if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ g.pi_throttle.reset_I(); g.pi_alt_hold.reset_I(); - #if FRAME_CONFIG == HELI_FRAME - g.throttle_cruise.set_and_save(heli_get_scaled_throttle(g.rc_3.control_in)); - #else - g.throttle_cruise.set_and_save(g.rc_3.control_in); - #endif + g.throttle_cruise.set_and_save(g.rc_3.control_in); } } From 755cb4b201775e99280ec559ea118e475dd8a069 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 6 Nov 2011 17:47:24 +0800 Subject: [PATCH 6/9] Altitude hold bug fix - changed g.throttle_cruise to be updated to steal I term from correct controller --- ArduCopter/ArduCopter.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ac612127f5..bc98f41e50 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1280,14 +1280,14 @@ adjust_altitude() // we remove 0 to 100 PWM from hover manual_boost = g.rc_3.control_in - 180; manual_boost = max(-120, manual_boost); - g.throttle_cruise += g.pi_alt_hold.get_integrator(); + g.throttle_cruise += g.pi_throttle.get_integrator(); g.pi_alt_hold.reset_I(); g.pi_throttle.reset_I(); }else if (g.rc_3.control_in >= 650){ // we add 0 to 100 PWM to hover manual_boost = g.rc_3.control_in - 650; - g.throttle_cruise += g.pi_alt_hold.get_integrator(); + g.throttle_cruise += g.pi_throttle.get_integrator(); g.pi_alt_hold.reset_I(); g.pi_throttle.reset_I(); From 32057fb3c9c49a68fa8591d5c726c4819e093955 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 6 Nov 2011 18:25:13 +0800 Subject: [PATCH 7/9] AltHold - made TradHeli code also use manual_boost --- ArduCopter/ArduCopter.pde | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index bc98f41e50..f2ad56f397 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1066,21 +1066,25 @@ void update_throttle_mode(void) // clear the new data flag invalid_throttle = false; } - #if FRAME_CONFIG == HELI_FRAME - g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle); - #else - angle_boost = get_angle_boost(g.throttle_cruise); + angle_boost = get_angle_boost(g.throttle_cruise); - if(manual_boost != 0){ - //remove alt_hold_velocity when implemented + if(manual_boost != 0){ + //remove alt_hold_velocity when implemented + #if FRAME_CONFIG == HELI_FRAME + g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.throttle_cruise + nav_throttle + manual_boost + get_z_damping())); + #else 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{ + #endif + // reset next_WP.alt + next_WP.alt = max(current_loc.alt, 100); + }else{ + #if FRAME_CONFIG == HELI_FRAME + //g.rc_3.servo_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping()); + g.rc_3.servo_out = heli_get_angle_boost(heli_get_scaled_throttle(g.throttle_cruise + nav_throttle + get_z_damping())); + #else g.rc_3.servo_out = g.throttle_cruise + nav_throttle + angle_boost + get_z_damping(); - } - - #endif + #endif + } break; } } From 9c59fdfdd68ca4e3a316cb530a40440c1196897e Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 6 Nov 2011 19:06:13 +0800 Subject: [PATCH 8/9] AltHold - another correction to the manual boost. Hopefully properly transferring building up I terms to g.throttle_cruise --- ArduCopter/ArduCopter.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f2ad56f397..c3c7225769 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1284,14 +1284,14 @@ adjust_altitude() // we remove 0 to 100 PWM from hover manual_boost = g.rc_3.control_in - 180; manual_boost = max(-120, manual_boost); - g.throttle_cruise += g.pi_throttle.get_integrator(); + g.throttle_cruise += (g.pi_alt_hold.get_integrator() * g.pi_throttle.kP() + g.pi_throttle.get_integrator()); g.pi_alt_hold.reset_I(); g.pi_throttle.reset_I(); }else if (g.rc_3.control_in >= 650){ // we add 0 to 100 PWM to hover manual_boost = g.rc_3.control_in - 650; - g.throttle_cruise += g.pi_throttle.get_integrator(); + g.throttle_cruise += (g.pi_alt_hold.get_integrator() * g.pi_throttle.kP() + g.pi_throttle.get_integrator()); g.pi_alt_hold.reset_I(); g.pi_throttle.reset_I(); From 251cf7d87c298333f06dfddd354ca5c5c81d734d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 7 Nov 2011 13:33:10 +1100 Subject: [PATCH 9/9] force LANG=C for MacOS awk --- libraries/AP_Common/Arduino.mk | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index 9850b11475..f245c32c6c 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -36,6 +36,9 @@ # SYSTYPE := $(shell uname) +# force LANG to C so awk works sanely on MacOS +export LANG=C + # # Locate the sketch sources based on the initial Makefile's path #