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

Added dynamic setting of kp_rollpitch, ki_rollpitch, kp_yaw.

Browse Source 
Added three constants for kp_rollpitch, (high, med -default, low)
Functionally equivalent to prior version.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2550 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-06-12 23:49:01 +00:00
parent 73004e45dc
commit 595266152b
2 changed files with 83 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_DCM/AP_DCM.cpp
View File

@ -22,9 +22,11 @@
#define ToRad(x) (x*0.01745329252) // *pi/180 #define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi #define ToDeg(x) (x*57.2957795131) // *180/pi
#define Kp_ROLLPITCH 0.05967 // .0014 * 418/9.81 Pitch&Roll Drift Correction Proportional Gain //#define Kp_ROLLPITCH 0.05967 // .0014 * 418/9.81 Pitch&Roll Drift Correction Proportional Gain
#define Ki_ROLLPITCH 0.00001278 // 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain //#define Ki_ROLLPITCH 0.00001278 // 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain
#define Kp_YAW 0.8 // Yaw Drift Correction Porportional 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 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
@ -239,8 +241,8 @@ AP_DCM::drift_correction(void)
_error_roll_pitch.y = constrain(_error_roll_pitch.y, -1.17f, 1.17f); _error_roll_pitch.y = constrain(_error_roll_pitch.y, -1.17f, 1.17f);
_error_roll_pitch.z = constrain(_error_roll_pitch.z, -1.17f, 1.17f); _error_roll_pitch.z = constrain(_error_roll_pitch.z, -1.17f, 1.17f);
_omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight); _omega_P = _error_roll_pitch * (_kp_roll_pitch * accel_weight);
_omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight); _omega_I += _error_roll_pitch * (_ki_roll_pitch * accel_weight);
//*****YAW*************** //*****YAW***************
@ -253,6 +255,7 @@ AP_DCM::drift_correction(void)
// Use GPS Ground course to correct yaw gyro drift // Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= SPEEDFILT) { if (_gps->ground_speed >= SPEEDFILT) {
_course_over_ground_x = cos(ToRad(_gps->ground_course/100.0)); _course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
_course_over_ground_y = sin(ToRad(_gps->ground_course/100.0)); _course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
if(in_motion) { if(in_motion) {
@ -280,8 +283,8 @@ AP_DCM::drift_correction(void)
_dcm_matrix = rot_mat * _dcm_matrix; _dcm_matrix = rot_mat * _dcm_matrix;
in_motion = true; in_motion = true;
error_course = 0; error_course = 0;
} }
} else { } else {
error_course = 0; error_course = 0;
in_motion = false; in_motion = false;
@ -290,7 +293,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

22
libraries/AP_DCM/AP_DCM.h
View File

@ -30,7 +30,10 @@ public:
0, 0, 1), 0, 0, 1),
_course_over_ground_x(0), _course_over_ground_x(0),
_course_over_ground_y(1), _course_over_ground_y(1),
_health(1.) _health(1.),
_kp_roll_pitch(0.05967),
_ki_roll_pitch(0.00001278),
_kp_yaw(0.8)
{} {}
// Accessors // Accessors
@ -60,7 +63,24 @@ public:
uint8_t renorm_sqrt_count; uint8_t renorm_sqrt_count;
uint8_t renorm_blowup_count; uint8_t renorm_blowup_count;
float kp_roll_pitch() { return _kp_roll_pitch; }
void kp_roll_pitch(float v) { _kp_roll_pitch = v; }
float ki_roll_pitch() { return _ki_roll_pitch; }
void ki_roll_pitch(float v) { _ki_roll_pitch = v; }
float kp_yaw() { return _kp_yaw; }
void kp_yaw(float v) { _kp_yaw = v; }
static const float kDCM_kp_rp_high = 0.15;
static const float kDCM_kp_rp_medium = 0.05967;
static const float kDCM_kp_rp_low = 0.01;
private: private:
float _kp_roll_pitch;
float _ki_roll_pitch;
float _kp_yaw;
// Methods // Methods
void read_adc_raw(void); void read_adc_raw(void);
void accel_adjust(void); void accel_adjust(void);