From 595266152b8424611cac0012335e0fddc61618e4 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 12 Jun 2011 23:49:01 +0000 Subject: [PATCH] Added dynamic setting of kp_rollpitch, ki_rollpitch, kp_yaw. 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 --- libraries/AP_DCM/AP_DCM.cpp | 121 ++++++++++++++++++------------------ libraries/AP_DCM/AP_DCM.h | 22 ++++++- 2 files changed, 83 insertions(+), 60 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 8c370d05d7..6e415dfcf6 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -3,7 +3,7 @@ Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com This library works with the ArduPilot Mega and "Oilpan" - + This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either @@ -22,9 +22,11 @@ #define ToRad(x) (x*0.01745329252) // *pi/180 #define ToDeg(x) (x*57.2957795131) // *180/pi -#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 Kp_YAW 0.8 // Yaw Drift Correction Porportional 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.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 SPEEDFILT 300 // centimeters/second @@ -44,11 +46,11 @@ AP_DCM::update_DCM(float _G_Dt) _imu->update(); _gyro_vector = _imu->get_gyro(); // 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 normalize(); // Normalize the DCM matrix drift_correction(); // Perform drift correction - + 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.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.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.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); } */ - + /**************************************************/ -void +void AP_DCM::matrix_update(float _G_Dt) { Matrix3f update_matrix; Matrix3f temp_matrix; - + //Record when you saturate any of the gyros. - if((fabs(_gyro_vector.x) >= radians(300)) || - (fabs(_gyro_vector.y) >= radians(300)) || + if((fabs(_gyro_vector.x) >= radians(300)) || + (fabs(_gyro_vector.y) >= radians(300)) || (fabs(_gyro_vector.z) >= radians(300))){ gyro_sat_count++; } _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){ accel_adjust(); // Remove _centripetal acceleration. } - - #if OUTPUTMODE == 1 + + #if OUTPUTMODE == 1 update_matrix.a.x = 0; update_matrix.a.y = -_G_Dt * _omega.z; // -delta Theta z 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.y = _G_Dt * _omega.x; // delta Theta x 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.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.b.x = _G_Dt * _gyro_vector.z; + update_matrix.b.x = _G_Dt * _gyro_vector.z; update_matrix.b.y = 0; - update_matrix.b.z = -_G_Dt * _gyro_vector.x; - update_matrix.c.x = -_G_Dt * _gyro_vector.y; - update_matrix.c.y = _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.y = _G_Dt * _gyro_vector.x; update_matrix.c.z = 0; #endif temp_matrix = _dcm_matrix * update_matrix; _dcm_matrix = _dcm_matrix + temp_matrix; // Equation 17 - + } /**************************************************/ -void +void AP_DCM::accel_adjust(void) { Vector3f veloc, temp; float vel; - + 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 //_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.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; } @@ -141,24 +143,24 @@ AP_DCM::accel_adjust(void) Direction Cosine Matrix IMU: Theory William Premerlani and Paul Bizard -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 -longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a +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 +longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a simple matter to stay ahead of it. We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ. */ -void +void AP_DCM::normalize(void) { float error = 0; Vector3f temporary[3]; - + int problem = 0; - + error = _dcm_matrix.a * _dcm_matrix.b; // eq.18 - temporary[0] = _dcm_matrix.b; - temporary[1] = _dcm_matrix.a; + temporary[0] = _dcm_matrix.b; + temporary[1] = _dcm_matrix.a; temporary[0] = _dcm_matrix.a - (temporary[0] * (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.b = renorm(temporary[1], 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! _dcm_matrix.a.x = 1.0f; _dcm_matrix.a.y = 0.0f; @@ -188,7 +190,7 @@ AP_DCM::renorm(Vector3f const &a, int &problem) float renorm; renorm = a * a; - + if (renorm < 1.5625f && renorm > 0.64f) { // Check if we are OK to use Taylor expansion renorm = 0.5 * (3 - renorm); // eq.21 } 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) { - //Compensation the Roll, Pitch and Yaw drift. + //Compensation the Roll, Pitch and Yaw drift. //float mag_heading_x; //float mag_heading_y; float error_course; @@ -217,7 +219,7 @@ AP_DCM::drift_correction(void) //static float scaled_omega_I[3]; static bool in_motion = false; Matrix3f rot_mat; - + //*****Roll and Pitch*************** // Calculate the magnitude of the accelerometer vector @@ -225,12 +227,12 @@ AP_DCM::drift_correction(void) // Dynamic weighting of accelerometer info (reliability filter) // 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 _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 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.z = constrain(_error_roll_pitch.z, -1.17f, 1.17f); - _omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight); - _omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight); + _omega_P = _error_roll_pitch * (_kp_roll_pitch * accel_weight); + _omega_I += _error_roll_pitch * (_ki_roll_pitch * accel_weight); + - //*****YAW*************** - + if (_compass) { // 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 { - + // Use GPS Ground course to correct yaw gyro drift if (_gps->ground_speed >= SPEEDFILT) { + _course_over_ground_x = cos(ToRad(_gps->ground_course/100.0)); _course_over_ground_y = sin(ToRad(_gps->ground_course/100.0)); if(in_motion) { @@ -276,22 +279,22 @@ AP_DCM::drift_correction(void) rot_mat.c.x = 0; rot_mat.c.y = 0; rot_mat.c.z = 1.0; - + _dcm_matrix = rot_mat * _dcm_matrix; in_motion = true; error_course = 0; - - } + } + } else { error_course = 0; 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. - - _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_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. // 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(); @@ -303,7 +306,7 @@ AP_DCM::drift_correction(void) /**************************************************/ -void +void AP_DCM::euler_angles(void) { #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); yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x); #endif - + roll_sensor = degrees(roll) * 100; pitch_sensor = degrees(pitch) * 100; yaw_sensor = degrees(yaw) * 100; diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index e9640d7c81..306d1e1dba 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -30,7 +30,10 @@ public: 0, 0, 1), _course_over_ground_x(0), _course_over_ground_y(1), - _health(1.) + _health(1.), + _kp_roll_pitch(0.05967), + _ki_roll_pitch(0.00001278), + _kp_yaw(0.8) {} // Accessors @@ -60,7 +63,24 @@ public: uint8_t renorm_sqrt_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: + float _kp_roll_pitch; + float _ki_roll_pitch; + float _kp_yaw; // Methods void read_adc_raw(void); void accel_adjust(void);