aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: Use bias-corrected angular rates instead of raw gyro measurements

Browse Source
This commit is contained in:
Jonathan Challinger 2014-07-13 00:48:30 -07:00 committed by Randy Mackay
parent a09f7b9198
commit 6e66cf43cb
5 changed files with 13 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -80,7 +80,7 @@ void AC_AttitudeControl::set_dt(float delta_sec)
void AC_AttitudeControl::relax_bf_rate_controller() void AC_AttitudeControl::relax_bf_rate_controller()
{ {
// ensure zero error in body frame rate controllers // 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; _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() void AC_AttitudeControl::integrate_bf_rate_error_to_angle_errors()
{ {
// roll - calculate body-frame angle error by integrating body-frame rate error // 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 // 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); _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 // 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 // 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); _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 // 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 // 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); _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.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.x += -_angle_bf_error.y * _ahrs.get_gyro().z;
_rate_bf_target.y += _angle_bf_error.x * _ins.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 // get current rate
// To-Do: make getting gyro rates more efficient? // 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 // calculate error and call pid controller
rate_error = rate_target_cds - current_rate; 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 // get current rate
// To-Do: make getting gyro rates more efficient? // 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 // calculate error and call pid controller
rate_error = rate_target_cds - current_rate; 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 // get current rate
// To-Do: make getting gyro rates more efficient? // 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 // calculate error and call pid controller
rate_error = rate_target_cds - current_rate; rate_error = rate_target_cds - current_rate;

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -45,14 +45,12 @@
class AC_AttitudeControl { class AC_AttitudeControl {
public: public:
AC_AttitudeControl( AP_AHRS &ahrs, AC_AttitudeControl( AP_AHRS &ahrs,
AP_InertialSensor& ins,
const AP_Vehicle::MultiCopter &aparm, const AP_Vehicle::MultiCopter &aparm,
AP_Motors& motors, AP_Motors& motors,
AC_P& pi_angle_roll, AC_P& pi_angle_pitch, AC_P& pi_angle_yaw, 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 AC_PID& pid_rate_roll, AC_PID& pid_rate_pitch, AC_PID& pid_rate_yaw
) : ) :
_ahrs(ahrs), _ahrs(ahrs),
_ins(ins),
_aparm(aparm), _aparm(aparm),
_motors(motors), _motors(motors),
_p_angle_roll(pi_angle_roll), _p_angle_roll(pi_angle_roll),
@ -223,7 +221,6 @@ protected:
// references to external libraries // references to external libraries
const AP_AHRS& _ahrs; const AP_AHRS& _ahrs;
const AP_InertialSensor& _ins;
const AP_Vehicle::MultiCopter &_aparm; const AP_Vehicle::MultiCopter &_aparm;
AP_Motors& _motors; AP_Motors& _motors;
AC_P& _p_angle_roll; AC_P& _p_angle_roll;

4
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -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 pitch_pd, pitch_i, pitch_ff; // used to capture pid values
float rate_roll_error, rate_pitch_error; // simply target_rate - current_rate float rate_roll_error, rate_pitch_error; // simply target_rate - current_rate
float roll_out, pitch_out; 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 // calculate error
rate_roll_error = rate_roll_target_cds - gyro.x * AC_ATTITUDE_CONTROL_DEGX100; 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 // get current rate
// To-Do: make getting gyro rates more efficient? // 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 // calculate error and call pid controller
rate_error = rate_target_cds - current_rate; rate_error = rate_target_cds - current_rate;

3
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -19,13 +19,12 @@
class AC_AttitudeControl_Heli : public AC_AttitudeControl { class AC_AttitudeControl_Heli : public AC_AttitudeControl {
public: public:
AC_AttitudeControl_Heli( AP_AHRS &ahrs, AC_AttitudeControl_Heli( AP_AHRS &ahrs,
AP_InertialSensor& ins,
const AP_Vehicle::MultiCopter &aparm, const AP_Vehicle::MultiCopter &aparm,
AP_MotorsHeli& motors, AP_MotorsHeli& motors,
AC_P& p_angle_roll, AC_P& p_angle_pitch, AC_P& p_angle_yaw, 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_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, p_angle_roll, p_angle_pitch, p_angle_yaw,
pid_rate_roll, pid_rate_pitch, pid_rate_yaw) pid_rate_roll, pid_rate_pitch, pid_rate_yaw)
{ {

2
libraries/AC_AttitudeControl/examples/AC_AttitudeControl_test/AC_AttitudeControl_test.pde
View File

@ -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; int16_t motor_roll, motor_pitch, motor_yaw, motor_throttle;
// Attitude Control // 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 /// 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); 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);