From 6e66cf43cbe0e8739dcd83ad4b108e5adb25a30d Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sun, 13 Jul 2014 00:48:30 -0700 Subject: [PATCH] AC_AttitudeControl: Use bias-corrected angular rates instead of raw gyro measurements --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 18 +++++++++--------- .../AC_AttitudeControl/AC_AttitudeControl.h | 3 --- .../AC_AttitudeControl_Heli.cpp | 4 ++-- .../AC_AttitudeControl_Heli.h | 3 +-- .../AC_AttitudeControl_test.pde | 2 +- 5 files changed, 13 insertions(+), 17 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 07a01c11fc..f2f3ca0e49 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -80,7 +80,7 @@ void AC_AttitudeControl::set_dt(float delta_sec) void AC_AttitudeControl::relax_bf_rate_controller() { // ensure zero error in body frame rate controllers - const Vector3f& gyro = _ins.get_gyro(); + const Vector3f& gyro = _ahrs.get_gyro(); _rate_bf_target = gyro * AC_ATTITUDE_CONTROL_DEGX100; } @@ -488,19 +488,19 @@ void AC_AttitudeControl::update_ef_yaw_angle_and_error(float yaw_rate_ef, Vector void AC_AttitudeControl::integrate_bf_rate_error_to_angle_errors() { // roll - calculate body-frame angle error by integrating body-frame rate error - _angle_bf_error.x += (_rate_bf_desired.x - (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; + _angle_bf_error.x += (_rate_bf_desired.x - (_ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; // roll - limit maximum error _angle_bf_error.x = constrain_float(_angle_bf_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); // pitch - calculate body-frame angle error by integrating body-frame rate error - _angle_bf_error.y += (_rate_bf_desired.y - (_ins.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; + _angle_bf_error.y += (_rate_bf_desired.y - (_ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; // pitch - limit maximum error _angle_bf_error.y = constrain_float(_angle_bf_error.y, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); // yaw - calculate body-frame angle error by integrating body-frame rate error - _angle_bf_error.z += (_rate_bf_desired.z - (_ins.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; + _angle_bf_error.z += (_rate_bf_desired.z - (_ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100)) * _dt; // yaw - limit maximum error _angle_bf_error.z = constrain_float(_angle_bf_error.z, -AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); @@ -533,8 +533,8 @@ void AC_AttitudeControl::update_rate_bf_targets() _rate_bf_target.z = constrain_float(_rate_bf_target.z,-_angle_rate_y_max,_angle_rate_y_max); } - _rate_bf_target.x += -_angle_bf_error.y * _ins.get_gyro().z; - _rate_bf_target.y += _angle_bf_error.x * _ins.get_gyro().z; + _rate_bf_target.x += -_angle_bf_error.y * _ahrs.get_gyro().z; + _rate_bf_target.y += _angle_bf_error.x * _ahrs.get_gyro().z; } // @@ -550,7 +550,7 @@ float AC_AttitudeControl::rate_bf_to_motor_roll(float rate_target_cds) // get current rate // To-Do: make getting gyro rates more efficient? - current_rate = (_ins.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100); + current_rate = (_ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100); // calculate error and call pid controller rate_error = rate_target_cds - current_rate; @@ -582,7 +582,7 @@ float AC_AttitudeControl::rate_bf_to_motor_pitch(float rate_target_cds) // get current rate // To-Do: make getting gyro rates more efficient? - current_rate = (_ins.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100); + current_rate = (_ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100); // calculate error and call pid controller rate_error = rate_target_cds - current_rate; @@ -614,7 +614,7 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_cds) // get current rate // To-Do: make getting gyro rates more efficient? - current_rate = (_ins.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100); + current_rate = (_ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100); // calculate error and call pid controller rate_error = rate_target_cds - current_rate; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 0c12412b0c..14c03123cd 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -45,14 +45,12 @@ class AC_AttitudeControl { public: AC_AttitudeControl( AP_AHRS &ahrs, - AP_InertialSensor& ins, const AP_Vehicle::MultiCopter &aparm, AP_Motors& motors, AC_P& pi_angle_roll, AC_P& pi_angle_pitch, AC_P& pi_angle_yaw, AC_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw ) : _ahrs(ahrs), - _ins(ins), _aparm(aparm), _motors(motors), _p_angle_roll(pi_angle_roll), @@ -223,7 +221,6 @@ protected: // references to external libraries const AP_AHRS& _ahrs; - const AP_InertialSensor& _ins; const AP_Vehicle::MultiCopter &_aparm; AP_Motors& _motors; AC_P& _p_angle_roll; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index ae5c3dd869..f9ab49a534 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -90,7 +90,7 @@ void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target float pitch_pd, pitch_i, pitch_ff; // used to capture pid values float rate_roll_error, rate_pitch_error; // simply target_rate - current_rate float roll_out, pitch_out; - const Vector3f& gyro = _ins.get_gyro(); // get current rates + const Vector3f& gyro = _ahrs.get_gyro(); // get current rates // calculate error rate_roll_error = rate_roll_target_cds - gyro.x * AC_ATTITUDE_CONTROL_DEGX100; @@ -251,7 +251,7 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_cds) // get current rate // To-Do: make getting gyro rates more efficient? - current_rate = (_ins.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100); + current_rate = (_ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100); // calculate error and call pid controller rate_error = rate_target_cds - current_rate; diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h index b41c93a60d..4fe6aa4a28 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h @@ -19,13 +19,12 @@ class AC_AttitudeControl_Heli : public AC_AttitudeControl { public: AC_AttitudeControl_Heli( AP_AHRS &ahrs, - AP_InertialSensor& ins, const AP_Vehicle::MultiCopter &aparm, AP_MotorsHeli& motors, AC_P& p_angle_roll, AC_P& p_angle_pitch, AC_P& p_angle_yaw, AC_HELI_PID& pid_rate_roll, AC_HELI_PID& pid_rate_pitch, AC_HELI_PID& pid_rate_yaw ) : - AC_AttitudeControl(ahrs, ins, aparm, motors, + AC_AttitudeControl(ahrs, aparm, motors, p_angle_roll, p_angle_pitch, p_angle_yaw, pid_rate_roll, pid_rate_pitch, pid_rate_yaw) { diff --git a/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde b/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde index 05f82ddc41..fc9f59df49 100644 --- a/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde +++ b/libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde @@ -80,7 +80,7 @@ AP_MotorsQuad motors(rc_roll, rc_pitch, rc_throttle, rc_yaw); int16_t motor_roll, motor_pitch, motor_yaw, motor_throttle; // Attitude Control -AC_AttitudeControl ac_control(ahrs, ins, aparm, motors, p_angle_roll, p_angle_pitch, p_angle_yaw, pid_rate_roll, pid_rate_pitch, pid_rate_yaw); +AC_AttitudeControl ac_control(ahrs, aparm, motors, p_angle_roll, p_angle_pitch, p_angle_yaw, pid_rate_roll, pid_rate_pitch, pid_rate_yaw); /// Position Control AC_PosControl pos_control(ahrs, inertial_nav, motors, ac_control, p_alt_pos, pid_alt_rate, pid_alt_accel, p_pos_xy, pid_rate_lat, pid_rate_lon);