Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

added ki_Yaw private variable and accessors. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2778 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-07-08 03:57:12 +00:00
parent 0d31d9be10
commit 5e163c6052
2 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_DCM/AP_DCM.cpp
View File

@ -27,7 +27,7 @@
//#define Ki_ROLLPITCH 0.0 // 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain //#define Ki_ROLLPITCH 0.0 // 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain
//#define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain //#define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain
#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain //#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain
#define SPEEDFILT 300 // centimeters/second #define SPEEDFILT 300 // centimeters/second
#define ADC_CONSTRAINT 900 #define ADC_CONSTRAINT 900
@ -302,7 +302,7 @@ AP_DCM::drift_correction(void)
_error_yaw = _dcm_matrix.c * error_course; // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. _error_yaw = _dcm_matrix.c * error_course; // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
_omega_P += _error_yaw * _kp_yaw; // Adding yaw correction to proportional correction vector. _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. _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 half the saturation limit of the gyros
integrator_magnitude = _omega_I.length(); integrator_magnitude = _omega_I.length();

8
libraries/AP_DCM/AP_DCM.h
View File

@ -33,7 +33,8 @@ public:
_health(1.), _health(1.),
_kp_roll_pitch(0.05967), _kp_roll_pitch(0.05967),
_ki_roll_pitch(0.00001278), _ki_roll_pitch(0.00001278),
_kp_yaw(0.8), _kp_yaw(0.8), // .8
_ki_yaw(0.00004), // 0.00004
_toggle(0) _toggle(0)
{} {}
@ -73,6 +74,9 @@ public:
float kp_yaw() { return _kp_yaw; } float kp_yaw() { return _kp_yaw; }
void kp_yaw(float v) { _kp_yaw = v; } void kp_yaw(float v) { _kp_yaw = v; }
float ki_yaw() { return _ki_yaw; }
void ki_yaw(float v) { _ki_yaw = v; }
static const float kDCM_kp_rp_high = 0.15; static const float kDCM_kp_rp_high = 0.15;
static const float kDCM_kp_rp_medium = 0.05967; static const float kDCM_kp_rp_medium = 0.05967;
static const float kDCM_kp_rp_low = 0.01; static const float kDCM_kp_rp_low = 0.01;
@ -82,6 +86,8 @@ private:
float _kp_roll_pitch; float _kp_roll_pitch;
float _ki_roll_pitch; float _ki_roll_pitch;
float _kp_yaw; float _kp_yaw;
float _ki_yaw;
// Methods // Methods
void read_adc_raw(void); void read_adc_raw(void);
void accel_adjust(void); void accel_adjust(void);