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 4d6637389a
commit b07c32acd0
2 changed files with 83 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

121
libraries/AP_DCM/AP_DCM.cpp
View File

@ -3,7 +3,7 @@
Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
This library works with the ArduPilot Mega and "Oilpan" This library works with the ArduPilot Mega and "Oilpan"
This library is free software; you can redistribute it and/or This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
@ -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
@ -44,11 +46,11 @@ AP_DCM::update_DCM(float _G_Dt)
_imu->update(); _imu->update();
_gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors _gyro_vector = _imu->get_gyro(); // Get current values for IMU sensors
_accel_vector = _imu->get_accel(); // Get current values for IMU sensors _accel_vector = _imu->get_accel(); // Get current values for IMU sensors
matrix_update(_G_Dt); // Integrate the DCM matrix matrix_update(_G_Dt); // Integrate the DCM matrix
normalize(); // Normalize the DCM matrix normalize(); // Normalize the DCM matrix
drift_correction(); // Perform drift correction drift_correction(); // Perform drift correction
euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); // Calculate pitch, roll, yaw for stabilization and navigation
} }
@ -61,35 +63,35 @@ printm(const char *l, Matrix3f &m)
{ Serial.println(" "); Serial.println(l); { Serial.println(" "); Serial.println(l);
Serial.print(m.a.x, 12); Serial.print(" "); Serial.print(m.a.y, 12); Serial.print(" "); Serial.println(m.a.z, 12); Serial.print(m.a.x, 12); Serial.print(" "); Serial.print(m.a.y, 12); Serial.print(" "); Serial.println(m.a.z, 12);
Serial.print(m.b.x, 12); Serial.print(" "); Serial.print(m.b.y, 12); Serial.print(" "); Serial.println(m.b.z, 12); Serial.print(m.b.x, 12); Serial.print(" "); Serial.print(m.b.y, 12); Serial.print(" "); Serial.println(m.b.z, 12);
Serial.print(m.c.x, 12); Serial.print(" "); Serial.print(m.c.y, 12); Serial.print(" "); Serial.println(m.c.z, 12); Serial.print(m.c.x, 12); Serial.print(" "); Serial.print(m.c.y, 12); Serial.print(" "); Serial.println(m.c.z, 12);
Serial.print(*(uint32_t *)&(m.a.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.a.y), HEX); Serial.print(" "); Serial.println(*(uint32_t *)&(m.a.z), HEX); Serial.print(*(uint32_t *)&(m.a.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.a.y), HEX); Serial.print(" "); Serial.println(*(uint32_t *)&(m.a.z), HEX);
Serial.print(*(uint32_t *)&(m.b.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.b.y), HEX); Serial.print(" "); Serial.println(*(uint32_t *)&(m.b.z), HEX); Serial.print(*(uint32_t *)&(m.b.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.b.y), HEX); Serial.print(" "); Serial.println(*(uint32_t *)&(m.b.z), HEX);
Serial.print(*(uint32_t *)&(m.c.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.c.y), HEX); Serial.print(" "); Serial.println(*(uint32_t *)&(m.c.z), HEX); Serial.print(*(uint32_t *)&(m.c.x), HEX); Serial.print(" "); Serial.print(*(uint32_t *)&(m.c.y), HEX); Serial.print(" "); Serial.println(*(uint32_t *)&(m.c.z), HEX);
} }
*/ */
/**************************************************/ /**************************************************/
void void
AP_DCM::matrix_update(float _G_Dt) AP_DCM::matrix_update(float _G_Dt)
{ {
Matrix3f update_matrix; Matrix3f update_matrix;
Matrix3f temp_matrix; Matrix3f temp_matrix;
//Record when you saturate any of the gyros. //Record when you saturate any of the gyros.
if((fabs(_gyro_vector.x) >= radians(300)) || if((fabs(_gyro_vector.x) >= radians(300)) ||
(fabs(_gyro_vector.y) >= radians(300)) || (fabs(_gyro_vector.y) >= radians(300)) ||
(fabs(_gyro_vector.z) >= radians(300))){ (fabs(_gyro_vector.z) >= radians(300))){
gyro_sat_count++; gyro_sat_count++;
} }
_omega_integ_corr = _gyro_vector + _omega_I; // Used for _centripetal correction (theoretically better than _omega) _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 _omega = _omega_integ_corr + _omega_P; // Equation 16, adding proportional and integral correction terms
if(_centripetal){ if(_centripetal){
accel_adjust(); // Remove _centripetal acceleration. accel_adjust(); // Remove _centripetal acceleration.
} }
#if OUTPUTMODE == 1 #if OUTPUTMODE == 1
update_matrix.a.x = 0; update_matrix.a.x = 0;
update_matrix.a.y = -_G_Dt * _omega.z; // -delta Theta z update_matrix.a.y = -_G_Dt * _omega.z; // -delta Theta z
update_matrix.a.z = _G_Dt * _omega.y; // delta Theta y update_matrix.a.z = _G_Dt * _omega.y; // delta Theta y
@ -99,39 +101,39 @@ AP_DCM::matrix_update(float _G_Dt)
update_matrix.c.x = -_G_Dt * _omega.y; // -delta Theta y update_matrix.c.x = -_G_Dt * _omega.y; // -delta Theta y
update_matrix.c.y = _G_Dt * _omega.x; // delta Theta x update_matrix.c.y = _G_Dt * _omega.x; // delta Theta x
update_matrix.c.z = 0; update_matrix.c.z = 0;
#else // Uncorrected data (no drift correction) #else // Uncorrected data (no drift correction)
update_matrix.a.x = 0; update_matrix.a.x = 0;
update_matrix.a.y = -_G_Dt * _gyro_vector.z; update_matrix.a.y = -_G_Dt * _gyro_vector.z;
update_matrix.a.z = _G_Dt * _gyro_vector.y; update_matrix.a.z = _G_Dt * _gyro_vector.y;
update_matrix.b.x = _G_Dt * _gyro_vector.z; update_matrix.b.x = _G_Dt * _gyro_vector.z;
update_matrix.b.y = 0; update_matrix.b.y = 0;
update_matrix.b.z = -_G_Dt * _gyro_vector.x; update_matrix.b.z = -_G_Dt * _gyro_vector.x;
update_matrix.c.x = -_G_Dt * _gyro_vector.y; update_matrix.c.x = -_G_Dt * _gyro_vector.y;
update_matrix.c.y = _G_Dt * _gyro_vector.x; update_matrix.c.y = _G_Dt * _gyro_vector.x;
update_matrix.c.z = 0; update_matrix.c.z = 0;
#endif #endif
temp_matrix = _dcm_matrix * update_matrix; temp_matrix = _dcm_matrix * update_matrix;
_dcm_matrix = _dcm_matrix + temp_matrix; // Equation 17 _dcm_matrix = _dcm_matrix + temp_matrix; // Equation 17
} }
/**************************************************/ /**************************************************/
void void
AP_DCM::accel_adjust(void) AP_DCM::accel_adjust(void)
{ {
Vector3f veloc, temp; Vector3f veloc, temp;
float vel; float vel;
veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units veloc.x = _gps->ground_speed / 100; // We are working with acceleration in m/s^2 units
// We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive // We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive
//_accel_vector -= _omega_integ_corr % _veloc; // Equation 26 This line is giving the compiler a problem so we break it up below //_accel_vector -= _omega_integ_corr % _veloc; // Equation 26 This line is giving the compiler a problem so we break it up below
temp.x = 0; temp.x = 0;
temp.y = _omega_integ_corr.z * veloc.x; // only computing the non-zero terms temp.y = _omega_integ_corr.z * veloc.x; // only computing the non-zero terms
temp.z = -1.0f * _omega_integ_corr.y * veloc.x; // After looking at the compiler issue lets remove _veloc and simlify temp.z = -1.0f * _omega_integ_corr.y * veloc.x; // After looking at the compiler issue lets remove _veloc and simlify
_accel_vector -= temp; _accel_vector -= temp;
} }
@ -141,24 +143,24 @@ AP_DCM::accel_adjust(void)
Direction Cosine Matrix IMU: Theory Direction Cosine Matrix IMU: Theory
William Premerlani and Paul Bizard William Premerlani and Paul Bizard
Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5 Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
to approximations rather than identities. In effect, the axes in the two frames of reference no to approximations rather than identities. In effect, the axes in the two frames of reference no
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
simple matter to stay ahead of it. simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ. We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
*/ */
void void
AP_DCM::normalize(void) AP_DCM::normalize(void)
{ {
float error = 0; float error = 0;
Vector3f temporary[3]; Vector3f temporary[3];
int problem = 0; int problem = 0;
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18 error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
temporary[0] = _dcm_matrix.b; temporary[0] = _dcm_matrix.b;
temporary[1] = _dcm_matrix.a; temporary[1] = _dcm_matrix.a;
temporary[0] = _dcm_matrix.a - (temporary[0] * (0.5f * error)); // eq.19 temporary[0] = _dcm_matrix.a - (temporary[0] * (0.5f * error)); // eq.19
temporary[1] = _dcm_matrix.b - (temporary[1] * (0.5f * error)); // eq.19 temporary[1] = _dcm_matrix.b - (temporary[1] * (0.5f * error)); // eq.19
@ -167,7 +169,7 @@ AP_DCM::normalize(void)
_dcm_matrix.a = renorm(temporary[0], problem); _dcm_matrix.a = renorm(temporary[0], problem);
_dcm_matrix.b = renorm(temporary[1], problem); _dcm_matrix.b = renorm(temporary[1], problem);
_dcm_matrix.c = renorm(temporary[2], problem); _dcm_matrix.c = renorm(temporary[2], problem);
if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down! if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
_dcm_matrix.a.x = 1.0f; _dcm_matrix.a.x = 1.0f;
_dcm_matrix.a.y = 0.0f; _dcm_matrix.a.y = 0.0f;
@ -188,7 +190,7 @@ AP_DCM::renorm(Vector3f const &a, int &problem)
float renorm; float renorm;
renorm = a * a; renorm = a * a;
if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK to use Taylor expansion if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK to use Taylor expansion
renorm = 0.5 * (3 - renorm); // eq.21 renorm = 0.5 * (3 - renorm); // eq.21
} else if (renorm < 100.0f && renorm > 0.01f) { } else if (renorm < 100.0f && renorm > 0.01f) {
@ -203,10 +205,10 @@ AP_DCM::renorm(Vector3f const &a, int &problem)
} }
/**************************************************/ /**************************************************/
void void
AP_DCM::drift_correction(void) AP_DCM::drift_correction(void)
{ {
//Compensation the Roll, Pitch and Yaw drift. //Compensation the Roll, Pitch and Yaw drift.
//float mag_heading_x; //float mag_heading_x;
//float mag_heading_y; //float mag_heading_y;
float error_course; float error_course;
@ -217,7 +219,7 @@ AP_DCM::drift_correction(void)
//static float scaled_omega_I[3]; //static float scaled_omega_I[3];
static bool in_motion = false; static bool in_motion = false;
Matrix3f rot_mat; Matrix3f rot_mat;
//*****Roll and Pitch*************** //*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector // Calculate the magnitude of the accelerometer vector
@ -225,12 +227,12 @@ AP_DCM::drift_correction(void)
// Dynamic weighting of accelerometer info (reliability filter) // Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
accel_weight = constrain(1 - 2 * fabs(1 - accel_magnitude), 0, 1); // accel_weight = constrain(1 - 2 * fabs(1 - accel_magnitude), 0, 1); //
// We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting // We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting
_health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1); _health = constrain(_health+(0.02 * (accel_weight - .5)), 0, 1);
// adjust the ground of reference // adjust the ground of reference
_error_roll_pitch = _dcm_matrix.c % _accel_vector; // Equation 27 *** sign changed from prev implementation??? _error_roll_pitch = _dcm_matrix.c % _accel_vector; // Equation 27 *** sign changed from prev implementation???
// error_roll_pitch are in Accel m/s^2 units // error_roll_pitch are in Accel m/s^2 units
@ -239,20 +241,21 @@ 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***************
if (_compass) { if (_compass) {
// We make the gyro YAW drift correction based on compass magnetic heading // We make the gyro YAW drift correction based on compass magnetic heading
error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error
} else { } else {
// 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) {
@ -276,22 +279,22 @@ AP_DCM::drift_correction(void)
rot_mat.c.x = 0; rot_mat.c.x = 0;
rot_mat.c.y = 0; rot_mat.c.y = 0;
rot_mat.c.z = 1.0; rot_mat.c.z = 1.0;
_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;
} }
} }
_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();
@ -303,7 +306,7 @@ AP_DCM::drift_correction(void)
/**************************************************/ /**************************************************/
void void
AP_DCM::euler_angles(void) AP_DCM::euler_angles(void)
{ {
#if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes) #if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes)
@ -315,7 +318,7 @@ AP_DCM::euler_angles(void)
roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z); roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x); yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
#endif #endif
roll_sensor = degrees(roll) * 100; roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100; pitch_sensor = degrees(pitch) * 100;
yaw_sensor = degrees(yaw) * 100; yaw_sensor = degrees(yaw) * 100;

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);