Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <AP_DCM.h>
00020
00021 #define OUTPUTMODE 1 // This is just used for debugging, remove later
00022 #define ToRad(x) (x*0.01745329252) // *pi/180
00023 #define ToDeg(x) (x*57.2957795131) // *180/pi
00024
00025 #define Kp_ROLLPITCH 0.05967 // .0014 * 418/9.81 Pitch&Roll Drift Correction Proportional Gain
00026 #define Ki_ROLLPITCH 0.00001278 // 0.0000003 * 418/9.81 Pitch&Roll Drift Correction Integrator Gain
00027 #define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain
00028 #define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain
00029
00030 #define SPEEDFILT 300 // centimeters/second
00031 #define ADC_CONSTRAINT 900
00032
00033
00034 void
00035 AP_DCM::set_compass(Compass *compass)
00036 {
00037 _compass = compass;
00038 }
00039
00040
00041 void
00042 AP_DCM::update_DCM(float _G_Dt)
00043 {
00044 _gyro_vector = _imu->get_gyro();
00045 _accel_vector = _imu->get_accel();
00046
00047 matrix_update(_G_Dt);
00048 normalize();
00049 drift_correction();
00050
00051 euler_angles();
00052 }
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 void
00072 AP_DCM::matrix_update(float _G_Dt)
00073 {
00074 Matrix3f update_matrix;
00075 Matrix3f temp_matrix;
00076
00077
00078 if((fabs(_gyro_vector.x) >= radians(300)) ||
00079 (fabs(_gyro_vector.y) >= radians(300)) ||
00080 (fabs(_gyro_vector.z) >= radians(300))){
00081 gyro_sat_count++;
00082 }
00083
00084 _omega_integ_corr = _gyro_vector + _omega_I;
00085 _omega = _omega_integ_corr + _omega_P;
00086
00087 if(_centripetal){
00088 accel_adjust();
00089 }
00090
00091 #if OUTPUTMODE == 1
00092 update_matrix.a.x = 0;
00093 update_matrix.a.y = -_G_Dt * _omega.z;
00094 update_matrix.a.z = _G_Dt * _omega.y;
00095 update_matrix.b.x = _G_Dt * _omega.z;
00096 update_matrix.b.y = 0;
00097 update_matrix.b.z = -_G_Dt * _omega.x;
00098 update_matrix.c.x = -_G_Dt * _omega.y;
00099 update_matrix.c.y = _G_Dt * _omega.x;
00100 update_matrix.c.z = 0;
00101 #else // Uncorrected data (no drift correction)
00102 update_matrix.a.x = 0;
00103 update_matrix.a.y = -_G_Dt * _gyro_vector.z;
00104 update_matrix.a.z = _G_Dt * _gyro_vector.y;
00105 update_matrix.b.x = _G_Dt * _gyro_vector.z;
00106 update_matrix.b.y = 0;
00107 update_matrix.b.z = -_G_Dt * _gyro_vector.x;
00108 update_matrix.c.x = -_G_Dt * _gyro_vector.y;
00109 update_matrix.c.y = _G_Dt * _gyro_vector.x;
00110 update_matrix.c.z = 0;
00111 #endif
00112
00113 temp_matrix = _dcm_matrix * update_matrix;
00114 _dcm_matrix = _dcm_matrix + temp_matrix;
00115
00116 }
00117
00118
00119
00120 void
00121 AP_DCM::accel_adjust(void)
00122 {
00123 Vector3f veloc, temp;
00124 float vel;
00125
00126 veloc.x = _gps->ground_speed / 100;
00127
00128
00129
00130
00131 temp.x = 0;
00132 temp.y = _omega_integ_corr.z * veloc.x;
00133 temp.z = -1.0f * _omega_integ_corr.y * veloc.x;
00134
00135 _accel_vector -= temp;
00136 }
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149 void
00150 AP_DCM::normalize(void)
00151 {
00152 float error = 0;
00153 Vector3f temporary[3];
00154
00155 int problem = 0;
00156
00157 error = _dcm_matrix.a * _dcm_matrix.b;
00158
00159 temporary[0] = _dcm_matrix.b;
00160 temporary[1] = _dcm_matrix.a;
00161 temporary[0] = _dcm_matrix.a - (temporary[0] * (0.5f * error));
00162 temporary[1] = _dcm_matrix.b - (temporary[1] * (0.5f * error));
00163
00164 temporary[2] = temporary[0] % temporary[1];
00165
00166 _dcm_matrix.a = renorm(temporary[0], problem);
00167 _dcm_matrix.b = renorm(temporary[1], problem);
00168 _dcm_matrix.c = renorm(temporary[2], problem);
00169
00170 if (problem == 1) {
00171 _dcm_matrix.a.x = 1.0f;
00172 _dcm_matrix.a.y = 0.0f;
00173 _dcm_matrix.a.z = 0.0f;
00174 _dcm_matrix.b.x = 0.0f;
00175 _dcm_matrix.b.y = 1.0f;
00176 _dcm_matrix.b.z = 0.0f;
00177 _dcm_matrix.c.x = 0.0f;
00178 _dcm_matrix.c.y = 0.0f;
00179 _dcm_matrix.c.z = 1.0f;
00180 }
00181 }
00182
00183
00184 Vector3f
00185 AP_DCM::renorm(Vector3f const &a, int &problem)
00186 {
00187 float renorm;
00188
00189 renorm = a * a;
00190
00191 if (renorm < 1.5625f && renorm > 0.64f) {
00192 renorm = 0.5 * (3 - renorm);
00193 } else if (renorm < 100.0f && renorm > 0.01f) {
00194 renorm = 1.0 / sqrt(renorm);
00195 renorm_sqrt_count++;
00196 } else {
00197 problem = 1;
00198 renorm_blowup_count++;
00199 }
00200
00201 return(a * renorm);
00202 }
00203
00204
00205 void
00206 AP_DCM::drift_correction(void)
00207 {
00208
00209
00210
00211 float error_course;
00212 float accel_magnitude;
00213 float accel_weight;
00214 float integrator_magnitude;
00215
00216
00217 static bool in_motion = false;
00218 Matrix3f rot_mat;
00219
00220
00221
00222
00223 accel_magnitude = _accel_vector.length() / 9.80665f;
00224
00225
00226
00227 accel_weight = constrain(1 - 2 * fabs(1 - accel_magnitude), 0, 1);
00228
00229
00230 _health += constrain((0.02 * (accel_weight - .5)), 0, 1);
00231
00232
00233 _error_roll_pitch = _dcm_matrix.c % _accel_vector;
00234
00235
00236
00237 _error_roll_pitch.x = constrain(_error_roll_pitch.x, -1.17f, 1.17f);
00238 _error_roll_pitch.y = constrain(_error_roll_pitch.y, -1.17f, 1.17f);
00239 _error_roll_pitch.z = constrain(_error_roll_pitch.z, -1.17f, 1.17f);
00240
00241 _omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight);
00242 _omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight);
00243
00244
00245
00246
00247 if (_compass) {
00248
00249 error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);
00250
00251 } else {
00252
00253
00254 if (_gps->ground_speed >= SPEEDFILT) {
00255 _course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
00256 _course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
00257 if(in_motion) {
00258 error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x);
00259 } else {
00260 float cos_psi_err, sin_psi_err;
00261
00262
00263 yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
00264 cos_psi_err = cos(ToRad(_gps->ground_course/100.0) - yaw);
00265 sin_psi_err = sin(ToRad(_gps->ground_course/100.0) - yaw);
00266
00267
00268
00269 rot_mat.a.x = cos_psi_err;
00270 rot_mat.a.y = -sin_psi_err;
00271 rot_mat.b.x = sin_psi_err;
00272 rot_mat.b.y = cos_psi_err;
00273 rot_mat.a.z = 0;
00274 rot_mat.b.z = 0;
00275 rot_mat.c.x = 0;
00276 rot_mat.c.y = 0;
00277 rot_mat.c.z = 1.0;
00278
00279 _dcm_matrix = rot_mat * _dcm_matrix;
00280 in_motion = true;
00281 error_course = 0;
00282
00283 }
00284 } else {
00285 error_course = 0;
00286 in_motion = false;
00287 }
00288 }
00289
00290 _error_yaw = _dcm_matrix.c * error_course;
00291
00292 _omega_P += _error_yaw * Kp_YAW;
00293 _omega_I += _error_yaw * Ki_YAW;
00294
00295
00296 integrator_magnitude = _omega_I.length();
00297 if (integrator_magnitude > radians(300)) {
00298 _omega_I *= (0.5f * radians(300) / integrator_magnitude);
00299 }
00300
00301 }
00302
00303
00304
00305 void
00306 AP_DCM::euler_angles(void)
00307 {
00308 #if (OUTPUTMODE == 2) // Only accelerometer info (debugging purposes)
00309 roll = atan2(_accel_vector.y, -_accel_vector.z);
00310 pitch = asin((_accel_vector.x) / (double)9.81);
00311 yaw = 0;
00312 #else
00313 pitch = -asin(_dcm_matrix.c.x);
00314 roll = atan2(_dcm_matrix.c.y, _dcm_matrix.c.z);
00315 yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
00316 #endif
00317
00318 roll_sensor = degrees(roll) * 100;
00319 pitch_sensor = degrees(pitch) * 100;
00320 yaw_sensor = degrees(yaw) * 100;
00321
00322 if (yaw_sensor < 0)
00323 yaw_sensor += 36000;
00324 }
00325
00326
00327
00328 float
00329 AP_DCM::get_health(void)
00330 {
00331 return _health;
00332 }
00333
00334