Line data Source code
1 : /*
2 : * APM_AHRS_DCM.cpp
3 : *
4 : * AHRS system using DCM matrices
5 : *
6 : * Based on DCM code by Doug Weibel, Jordi Munoz and Jose Julio. DIYDrones.com
7 : *
8 : * Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
9 :
10 : This program is free software: you can redistribute it and/or modify
11 : it under the terms of the GNU General Public License as published by
12 : the Free Software Foundation, either version 3 of the License, or
13 : (at your option) any later version.
14 :
15 : This program is distributed in the hope that it will be useful,
16 : but WITHOUT ANY WARRANTY; without even the implied warranty of
17 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 : GNU General Public License for more details.
19 :
20 : You should have received a copy of the GNU General Public License
21 : along with this program. If not, see <http://www.gnu.org/licenses/>.
22 : */
23 :
24 : #include "AP_AHRS_config.h"
25 :
26 : #if AP_AHRS_DCM_ENABLED
27 :
28 : #include "AP_AHRS.h"
29 : #include <AP_HAL/AP_HAL.h>
30 : #include <GCS_MAVLink/GCS.h>
31 : #include <AP_GPS/AP_GPS.h>
32 : #include <AP_Baro/AP_Baro.h>
33 : #include <AP_Compass/AP_Compass.h>
34 : #include <AP_Logger/AP_Logger.h>
35 : #include <AP_Vehicle/AP_Vehicle_Type.h>
36 :
37 : extern const AP_HAL::HAL& hal;
38 :
39 : // this is the speed in m/s above which we first get a yaw lock with
40 : // the GPS
41 : #define GPS_SPEED_MIN 3
42 :
43 : // the limit (in degrees/second) beyond which we stop integrating
44 : // omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
45 : // which results in false gyro drift. See
46 : // http://gentlenav.googlecode.com/files/fastRotations.pdf
47 : #define SPIN_RATE_LIMIT 20
48 :
49 : // reset the current gyro drift estimate
50 : // should be called if gyro offsets are recalculated
51 : void
52 7 : AP_AHRS_DCM::reset_gyro_drift(void)
53 : {
54 7 : _omega_I.zero();
55 7 : _omega_I_sum.zero();
56 7 : _omega_I_sum_time = 0;
57 7 : }
58 :
59 : // run a full DCM update round
60 : void
61 41686716 : AP_AHRS_DCM::update()
62 : {
63 41686716 : AP_InertialSensor &_ins = AP::ins();
64 :
65 : // ask the IMU how much time this sensor reading represents
66 41686716 : const float delta_t = _ins.get_delta_time();
67 :
68 : // if the update call took more than 0.2 seconds then discard it,
69 : // otherwise we may move too far. This happens when arming motors
70 : // in ArduCopter
71 41686716 : if (delta_t > 0.2f) {
72 0 : memset((void *)&_ra_sum[0], 0, sizeof(_ra_sum));
73 0 : _ra_deltat = 0;
74 0 : return;
75 : }
76 :
77 : // Integrate the DCM matrix using gyro inputs
78 41686716 : matrix_update();
79 :
80 : // Normalize the DCM matrix
81 41686716 : normalize();
82 :
83 : // Perform drift correction
84 41686716 : drift_correction(delta_t);
85 :
86 : // paranoid check for bad values in the DCM matrix
87 41686716 : check_matrix();
88 :
89 : // calculate the euler angles and DCM matrix which will be used
90 : // for high level navigation control. Apply trim such that a
91 : // positive trim value results in a positive vehicle rotation
92 : // about that axis (ie a negative offset)
93 41686716 : _body_dcm_matrix = _dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
94 41686716 : _body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
95 :
96 : // pre-calculate some trig for CPU purposes:
97 41686716 : _cos_yaw = cosf(yaw);
98 41686716 : _sin_yaw = sinf(yaw);
99 :
100 41686716 : backup_attitude();
101 :
102 : // remember the last origin for fallback support
103 41686716 : IGNORE_RETURN(AP::ahrs().get_origin(last_origin));
104 :
105 : // update our wind estimate when a new GPS sample arrives and we
106 : // have a 3D fix; the GPS velocity is what feeds the wind triangle.
107 : // estimate_wind itself is rate-limited.
108 41686716 : const AP_GPS &gps = AP::gps();
109 41686716 : if (gps.status() >= AP_GPS_FixType::FIX_3D) {
110 38125678 : const uint32_t last_gps_ms = gps.last_message_time_ms();
111 38125678 : if (last_gps_ms != _last_wind_gps_ms) {
112 696272 : _last_wind_gps_ms = last_gps_ms;
113 696272 : estimate_wind();
114 : }
115 : }
116 :
117 : #if HAL_LOGGING_ENABLED
118 41686716 : const uint32_t now_ms = AP_HAL::millis();
119 41686716 : if (now_ms - last_log_ms >= 100) {
120 : // log DCM at 10Hz
121 1501471 : last_log_ms = now_ms;
122 :
123 : // @LoggerMessage: DCM
124 : // @Description: DCM Estimator Data
125 : // @Field: TimeUS: Time since system startup
126 : // @Field: Roll: estimated roll
127 : // @Field: Pitch: estimated pitch
128 : // @Field: Yaw: estimated yaw
129 : // @Field: ErrRP: lowest estimated gyro drift error
130 : // @Field: ErrYaw: difference between measured yaw and DCM yaw estimate
131 : // @Field: VWN: wind velocity, to-the-North component
132 : // @Field: VWE: wind velocity, to-the-East component
133 : // @Field: VWD: wind velocity, Up-to-Down component
134 : // @Field: SAs: synthetic (equivalent) airspeed
135 3002942 : AP::logger().WriteStreaming(
136 : "DCM",
137 : "TimeUS," "Roll," "Pitch," "Yaw," "ErrRP," "ErrYaw," "VWN," "VWE," "VWD," "SAs",
138 : "s" "d" "d" "d" "d" "h" "n" "n" "n" "n",
139 : "F" "0" "0" "0" "0" "0" "0" "0" "0" "0",
140 : "Q" "f" "f" "f" "f" "f" "f" "f" "f" "f",
141 : AP_HAL::micros64(),
142 1501471 : degrees(roll),
143 1501471 : degrees(pitch),
144 1501471 : wrap_360(degrees(yaw)),
145 1501471 : get_error_rp(),
146 1501471 : get_error_yaw(),
147 1501471 : _wind.x,
148 1501471 : _wind.y,
149 1501471 : _wind.z,
150 1501471 : _last_airspeed_TAS / get_EAS2TAS()
151 : );
152 : }
153 : #endif // HAL_LOGGING_ENABLED
154 : }
155 :
156 : // note that contrary to most code we leave commented code in here as
157 : // an exhaustive list of all of the data that *can* be returned in the
158 : // Estimates:
159 41686716 : void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results)
160 : {
161 41686716 : const auto now_ms = AP_HAL::millis();
162 :
163 : // always initialised
164 41686716 : results.initialised = true;
165 :
166 : // consider ourselves healthy if there have been no failures for 5 seconds
167 41686716 : results.healthy = (_last_failure_ms == 0 || now_ms - _last_failure_ms > 5000);
168 :
169 : // not using a specific sensor:
170 41686716 : results.primary_gyro = AP::ins().get_first_usable_gyro();
171 41686716 : results.primary_accel = AP::ins().get_first_usable_accel();
172 :
173 41686716 : results.roll_rad = roll;
174 41686716 : results.pitch_rad = pitch;
175 41686716 : results.yaw_rad = yaw;
176 :
177 41686716 : results.dcm_matrix = _body_dcm_matrix;
178 :
179 : // quaternion is derived from transformation matrix:
180 41686716 : results.quaternion.from_rotation_matrix(_dcm_matrix);
181 41686716 : results.quaternion.rotate(-AP::ahrs().get_trim());
182 :
183 41686716 : results.attitude_valid = true;
184 :
185 41686716 : results.gyro_estimate = _omega;
186 41686716 : results.gyro_drift = _omega_I;
187 :
188 : // true when the state estimates are significantly degraded by vibration:
189 : // results.is_vibration_affected = false;
190 :
191 : /*
192 : * acceleration estimates
193 : */
194 : // results.accel_bias = {} - DCM does not estimate accel bias
195 41686716 : results.accel_ef = _accel_ef;
196 :
197 41686716 : results.velocity_NED_valid = get_velocity_NED(results.velocity_NED);
198 :
199 : // ground velocity estimate in meters/second, in North/East order
200 : // note: velocity_NE is significantly different to results.velocity_NED.xy()!
201 41686716 : results.velocity_NE = groundspeed_vector();
202 :
203 41686716 : results.vert_pos_rate_D_valid = get_vert_pos_rate_D(results.vert_pos_rate_D);
204 :
205 : /*
206 : * position estimates
207 : */
208 41686716 : results.location_valid = get_location(results.location);
209 :
210 : // origin-relative functions
211 : // results.provides_common_origin = false;
212 :
213 : // origin-relative position:
214 : {
215 : // DCM calculates in global co-ordinates and here converts
216 : // back to relative-to-origin by subtracting the current
217 : // location from the origin:
218 41686716 : Location origin;
219 41686716 : if (get_origin(origin) && results.location_valid) {
220 36648032 : const Vector3p posNED = origin.get_distance_NED_postype(results.location);
221 36648032 : results.position_NE = posNED.xy();
222 36648032 : results.position_NE_valid = true;
223 36648032 : results.position_D = posNED.z;
224 36648032 : results.position_D_valid = true;
225 : }
226 : }
227 : // hagl is not supplied:
228 : // results.hagl_valid = false;
229 : // results.hagl = 0;
230 :
231 : /*
232 : * air data estimates
233 : */
234 41686716 : results.wind = _wind;
235 41686716 : results.wind_valid = true;
236 :
237 : /*
238 : * Sensor-related information
239 : */
240 : // true if the estimator will use GPS data in creating its
241 : // estimate when the data is good:
242 41686716 : results.configured_to_use_gps = _gps_use != GPSUse::Disable;
243 : // true if GPS is configured as the horizontal position source
244 : // for this estimator. Used to decide whether GPS will set
245 : // the navigation origin.
246 41686716 : results.configured_to_use_gps_for_pos_XY = _gps_use != GPSUse::Disable;
247 :
248 : // are we consuming yaw from an external (e.g. vision-based) source?
249 : // results.using_extnav_for_yaw = false;
250 :
251 : // are we consuming yaw from a source which is *not* a compass
252 : // results.using_noncompass_for_yaw = false;
253 :
254 : #if AP_AHRS_GET_MAG_DATA_ENABLED
255 : // estimators can provide their predicted magnetic fields:
256 : // ... but DCM does not:
257 : // results.mag_field_NED = {};
258 : // results.mag_field_NED_valid = false;
259 : // results.mag_field_corrections = {};
260 : // results.mag_field_corrections_valid = false;
261 : #endif // AP_AHRS_GET_MAG_DATA_ENABLED
262 :
263 : /*
264 : * filter status and estimates quality values:
265 : */
266 : // getFilterStatus(results.filter_status);
267 : // results.filter_status_valid = false;
268 :
269 : // provides the innovations normalised between 0 and 1:
270 : // results.variances_valid = false;
271 :
272 : // terrain_alt_variance_valid = false;
273 :
274 : // lower gains in VTOL controllers when flying on DCM
275 41686716 : results.control_ground_speed_limit_ms = 50.0;
276 41686716 : results.control_gain_scaler_XY = 0.5;
277 41686716 : results.control_gain_scaler_Z = 0.25;
278 : // control height is never limited:
279 : // results.control_height_limit_valid = false;
280 : // results.control_height_limit_m = 0;
281 41686716 : }
282 :
283 : /*
284 : backup attitude to persistent_data for use in watchdog reset
285 : */
286 41686716 : void AP_AHRS_DCM::backup_attitude(void)
287 : {
288 41686716 : AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
289 41686716 : pd.roll_rad = roll;
290 41686716 : pd.pitch_rad = pitch;
291 41686716 : pd.yaw_rad = yaw;
292 41686716 : }
293 :
294 : // update the DCM matrix using only the gyros
295 41686716 : void AP_AHRS_DCM::matrix_update(void)
296 : {
297 : // use only the primary gyro so our bias estimate is valid, allowing us to return the right filtered gyro
298 : // for rate controllers
299 41686716 : const auto &_ins = AP::ins();
300 41686716 : Vector3f delta_angle;
301 : float dangle_dt;
302 41686716 : if (_ins.get_delta_angle(delta_angle, dangle_dt) && dangle_dt > 0) {
303 41686716 : _omega = delta_angle / dangle_dt;
304 41686716 : _omega += _omega_I;
305 41686716 : _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * dangle_dt);
306 : }
307 :
308 : // now update _omega from the filtered value from the primary IMU. We need to use
309 : // the primary IMU as the notch filters will only be running on one IMU
310 :
311 : // note that we do not include the P terms in _omega. This is
312 : // because the spin_rate is calculated from _omega.length(),
313 : // and including the P terms would give positive feedback into
314 : // the _P_gain() calculation, which can lead to a very large P
315 : // value
316 41686716 : _omega = _ins.get_gyro() + _omega_I;
317 41686716 : }
318 :
319 :
320 : /*
321 : * reset the DCM matrix and omega. Used on ground start, and on
322 : * extreme errors in the matrix
323 : */
324 : void
325 2115 : AP_AHRS_DCM::reset(bool recover_eulers)
326 : {
327 : // reset the integration terms
328 2115 : _omega_I.zero();
329 2115 : _omega_P.zero();
330 2115 : _omega_yaw_P.zero();
331 2115 : _omega.zero();
332 :
333 : // if the caller wants us to try to recover to the current
334 : // attitude then calculate the dcm matrix from the current
335 : // roll/pitch/yaw values
336 2115 : if (hal.util->was_watchdog_reset() && AP_HAL::millis() < 10000) {
337 1 : const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
338 1 : roll = pd.roll_rad;
339 1 : pitch = pd.pitch_rad;
340 1 : yaw = pd.yaw_rad;
341 1 : _dcm_matrix.from_euler(roll, pitch, yaw);
342 1 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Restored watchdog attitude %.0f %.0f %.0f",
343 : degrees(roll), degrees(pitch), degrees(yaw));
344 2114 : } else if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) {
345 0 : _dcm_matrix.from_euler(roll, pitch, yaw);
346 : } else {
347 :
348 : // Use the measured accel due to gravity to calculate an initial
349 : // roll and pitch estimate
350 :
351 2114 : AP_InertialSensor &_ins = AP::ins();
352 :
353 : // Get body frame accel vector
354 2114 : Vector3f initAccVec = _ins.get_accel();
355 2114 : uint8_t counter = 0;
356 :
357 : // the first vector may be invalid as the filter starts up
358 2846 : while ((initAccVec.length() < 9.0f || initAccVec.length() > 11) && counter++ < 20) {
359 732 : _ins.wait_for_sample();
360 732 : _ins.update();
361 732 : initAccVec = _ins.get_accel();
362 : }
363 :
364 : // normalise the acceleration vector
365 2114 : if (initAccVec.length() > 5.0f) {
366 : // calculate initial pitch angle
367 2114 : pitch = atan2f(initAccVec.x, norm(initAccVec.y, initAccVec.z));
368 : // calculate initial roll angle
369 2114 : roll = atan2f(-initAccVec.y, -initAccVec.z);
370 : } else {
371 : // If we can't use the accel vector, then align flat
372 0 : roll = 0.0f;
373 0 : pitch = 0.0f;
374 : }
375 2114 : _dcm_matrix.from_euler(roll, pitch, 0);
376 :
377 : }
378 :
379 : // pre-calculate some trig for CPU purposes:
380 2115 : _cos_yaw = cosf(yaw);
381 2115 : _sin_yaw = sinf(yaw);
382 :
383 2115 : _last_startup_ms = AP_HAL::millis();
384 2115 : }
385 :
386 : /*
387 : * check the DCM matrix for pathological values
388 : */
389 : void
390 41686716 : AP_AHRS_DCM::check_matrix(void)
391 : {
392 41686716 : if (_dcm_matrix.is_nan()) {
393 : //Serial.printf("ERROR: DCM matrix NAN\n");
394 0 : AP_AHRS_DCM::reset(true);
395 0 : return;
396 : }
397 : // some DCM matrix values can lead to an out of range error in
398 : // the pitch calculation via asin(). These NaN values can
399 : // feed back into the rest of the DCM matrix via the
400 : // error_course value.
401 41686716 : if (!(_dcm_matrix.c.x < 1.0f &&
402 41682169 : _dcm_matrix.c.x > -1.0f)) {
403 : // We have an invalid matrix. Force a normalisation.
404 12220 : normalize();
405 :
406 24440 : if (_dcm_matrix.is_nan() ||
407 12220 : fabsf(_dcm_matrix.c.x) > 10.0) {
408 : // See Issue #20284: regarding the selection of 10.0 for DCM reset
409 : // This won't be lowered without evidence of an issue or mathematical proof & testing of a lower bound
410 :
411 : // normalisation didn't fix the problem! We're
412 : // in real trouble. All we can do is reset
413 : //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
414 : // _dcm_matrix.c.x);
415 0 : AP_AHRS_DCM::reset(true);
416 : }
417 : }
418 : }
419 :
420 : // renormalise one vector component of the DCM matrix
421 : // this will return false if renormalization fails
422 : bool
423 125096808 : AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
424 : {
425 : // numerical errors will slowly build up over time in DCM,
426 : // causing inaccuracies. We can keep ahead of those errors
427 : // using the renormalization technique from the DCM IMU paper
428 : // (see equations 18 to 21).
429 :
430 : // For APM we don't bother with the taylor expansion
431 : // optimisation from the paper as on our 2560 CPU the cost of
432 : // the sqrt() is 44 microseconds, and the small time saving of
433 : // the taylor expansion is not worth the potential of
434 : // additional error buildup.
435 :
436 : // Note that we can get significant renormalisation values
437 : // when we have a larger delta_t due to a glitch elsewhere in
438 : // APM, such as a I2c timeout or a set of EEPROM writes. While
439 : // we would like to avoid these if possible, if it does happen
440 : // we don't want to compound the error by making DCM less
441 : // accurate.
442 :
443 125096808 : const float renorm_val = 1.0f / a.length();
444 :
445 : // keep the average for reporting
446 125096808 : _renorm_val_sum += renorm_val;
447 125096808 : _renorm_val_count++;
448 :
449 125096808 : if (!(renorm_val < 2.0f && renorm_val > 0.5f)) {
450 : // this is larger than it should get - log it as a warning
451 0 : if (!(renorm_val < 1.0e6f && renorm_val > 1.0e-6f)) {
452 : // we are getting values which are way out of
453 : // range, we will reset the matrix and hope we
454 : // can recover our attitude using drift
455 : // correction before we hit the ground!
456 : //Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
457 : // renorm_val);
458 0 : return false;
459 : }
460 : }
461 :
462 125096808 : result = a * renorm_val;
463 125096808 : return true;
464 : }
465 :
466 : /*************************************************
467 : * Direction Cosine Matrix IMU: Theory
468 : * William Premerlani and Paul Bizard
469 : *
470 : * Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
471 : * to approximations rather than identities. In effect, the axes in the two frames of reference no
472 : * longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
473 : * simple matter to stay ahead of it.
474 : * We call the process of enforcing the orthogonality conditions: renormalization.
475 : */
476 : void
477 41698936 : AP_AHRS_DCM::normalize(void)
478 : {
479 41698936 : const float error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
480 :
481 41698936 : const Vector3f t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19
482 41698936 : const Vector3f t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19
483 41698936 : const Vector3f t2 = t0 % t1; // c= a x b // eq.20
484 :
485 41698936 : if (!renorm(t0, _dcm_matrix.a) ||
486 83397872 : !renorm(t1, _dcm_matrix.b) ||
487 41698936 : !renorm(t2, _dcm_matrix.c)) {
488 : // Our solution is blowing up and we will force back
489 : // to last euler angles
490 0 : _last_failure_ms = AP_HAL::millis();
491 0 : AP_AHRS_DCM::reset(true);
492 : }
493 41698936 : }
494 :
495 :
496 : // produce a yaw error value. The returned value is proportional
497 : // to sin() of the current heading error in earth frame
498 : float
499 1488533 : AP_AHRS_DCM::yaw_error_compass(Compass &compass)
500 : {
501 1488533 : const Vector3f &mag = compass.get_field();
502 : // get the mag vector in the earth frame
503 1488533 : Vector2f rb = _dcm_matrix.mulXY(mag);
504 :
505 1488533 : if (rb.length() < FLT_EPSILON) {
506 0 : return 0.0f;
507 : }
508 :
509 1488533 : rb.normalize();
510 1488533 : if (rb.is_inf()) {
511 : // not a valid vector
512 0 : return 0.0f;
513 : }
514 :
515 : // update vector holding earths magnetic field (if required)
516 1488533 : if( !is_equal(_last_declination, compass.get_declination()) ) {
517 1100 : _last_declination = compass.get_declination();
518 1100 : _mag_earth.x = cosf(_last_declination);
519 1100 : _mag_earth.y = sinf(_last_declination);
520 : }
521 :
522 : // calculate the error term in earth frame
523 : // calculate the Z component of the cross product of rb and _mag_earth
524 1488533 : return rb % _mag_earth;
525 : }
526 :
527 : // the _P_gain raises the gain of the PI controller
528 : // when we are spinning fast. See the fastRotations
529 : // paper from Bill.
530 : float
531 2247777 : AP_AHRS_DCM::_P_gain(float spin_rate)
532 : {
533 2247777 : if (spin_rate < radians(50)) {
534 2197184 : return 1.0f;
535 : }
536 50593 : if (spin_rate > radians(500)) {
537 2869 : return 10.0f;
538 : }
539 47724 : return spin_rate/radians(50);
540 : }
541 :
542 : // _yaw_gain reduces the gain of the PI controller applied to heading errors
543 : // when observability from change of velocity is good (eg changing speed or turning)
544 : // This reduces unwanted roll and pitch coupling due to compass errors for planes.
545 : // High levels of noise on _accel_ef will cause the gain to drop and could lead to
546 : // increased heading drift during straight and level flight, however some gain is
547 : // always available. TODO check the necessity of adding adjustable acc threshold
548 : // and/or filtering accelerations before getting magnitude
549 : float
550 1488962 : AP_AHRS_DCM::_yaw_gain(void) const
551 : {
552 1488962 : const float VdotEFmag = _accel_ef.xy().length();
553 :
554 1488962 : if (VdotEFmag <= 4.0f) {
555 1369568 : return 0.2f*(4.5f - VdotEFmag);
556 : }
557 119394 : return 0.1f;
558 : }
559 :
560 :
561 : // return true if we have and should use GPS
562 48876285 : bool AP_AHRS_DCM::have_gps(void) const
563 : {
564 48876285 : if (_gps_use == GPSUse::Disable || AP::gps().status() <= AP_GPS_FixType::NONE) {
565 4263268 : return false;
566 : }
567 44613017 : return true;
568 : }
569 :
570 : /*
571 : when we are getting the initial attitude we want faster gains so
572 : that if the board starts upside down we quickly approach the right
573 : attitude.
574 : We don't want to keep those high gains for too long though as high P
575 : gains cause slow gyro offset learning. So we keep the high gains for
576 : a maximum of 20 seconds
577 : */
578 2247777 : bool AP_AHRS_DCM::use_fast_gains(void) const
579 : {
580 2247777 : return !hal.util->get_soft_armed() && (AP_HAL::millis() - _last_startup_ms) < 20000U;
581 : }
582 :
583 :
584 : // return true if we should use the compass for yaw correction
585 41963735 : bool AP_AHRS_DCM::use_compass(void)
586 : {
587 41963735 : const Compass &compass = AP::compass();
588 :
589 41963735 : if (!compass.use_for_yaw()) {
590 : // no compass available
591 109007 : return false;
592 : }
593 41854728 : if (!AP::ahrs().get_fly_forward() || !have_gps()) {
594 : // we don't have any alternative to the compass
595 36872081 : return true;
596 : }
597 4982647 : if (AP::gps().ground_speed() < GPS_SPEED_MIN) {
598 : // we are not going fast enough to use the GPS
599 1255781 : return true;
600 : }
601 :
602 : // if the current yaw differs from the GPS yaw by more than 45
603 : // degrees and the estimated wind speed is less than 80% of the
604 : // ground speed, then switch to GPS navigation. This will help
605 : // prevent flyaways with very bad compass offsets
606 3726866 : const float error = fabsf(wrap_180(degrees(yaw) - AP::gps().ground_course()));
607 3726866 : if (error > 45 && _wind.xy().length() < AP::gps().ground_speed()*0.8f) {
608 10396 : if (AP_HAL::millis() - _last_consistent_heading > 2000) {
609 : // start using the GPS for heading if the compass has been
610 : // inconsistent with the GPS for 2 seconds
611 1023 : return false;
612 : }
613 : } else {
614 3716470 : _last_consistent_heading = AP_HAL::millis();
615 : }
616 :
617 : // use the compass
618 3725843 : return true;
619 : }
620 :
621 : // yaw drift correction using the compass or GPS
622 : // this function produces the _omega_yaw_P vector, and also
623 : // contributes to the _omega_I.z long term yaw drift estimate
624 : void
625 41686716 : AP_AHRS_DCM::drift_correction_yaw(void)
626 : {
627 41686716 : bool new_value = false;
628 : float yaw_error;
629 41686716 : float yaw_deltat = 0.0f;
630 :
631 41686716 : const AP_GPS &_gps = AP::gps();
632 :
633 41686716 : Compass &compass = AP::compass();
634 :
635 : #if COMPASS_CAL_ENABLED
636 41686716 : if (compass.is_calibrating()) {
637 : // don't do any yaw correction while calibrating
638 565279 : return;
639 : }
640 : #endif
641 :
642 41121437 : if (AP_AHRS_DCM::use_compass()) {
643 : /*
644 : we are using compass for yaw
645 : */
646 41013052 : if (compass.last_update_usec() != _compass_last_update) {
647 1488533 : yaw_deltat = (compass.last_update_usec() - _compass_last_update) * 1.0e-6f;
648 1488533 : _compass_last_update = compass.last_update_usec();
649 : // we force an additional compass read()
650 : // here. This has the effect of throwing away
651 : // the first compass value, which can be bad
652 1488533 : if (!have_initial_yaw && compass.read()) {
653 2082 : const float heading = compass.calculate_heading(_dcm_matrix);
654 2082 : _dcm_matrix.from_euler(roll, pitch, heading);
655 2082 : _omega_yaw_P.zero();
656 2082 : have_initial_yaw = true;
657 : }
658 1488533 : new_value = true;
659 1488533 : yaw_error = yaw_error_compass(compass);
660 :
661 : // also update the _gps_last_update, so if we later
662 : // disable the compass due to significant yaw error we
663 : // don't suddenly change yaw with a reset
664 1488533 : _gps_last_update = _gps.last_fix_time_ms();
665 : }
666 108385 : } else if (AP::ahrs().get_fly_forward() && have_gps()) {
667 : /*
668 : we are using GPS for yaw
669 : */
670 15537 : if (_gps.last_fix_time_ms() != _gps_last_update &&
671 5309 : _gps.ground_speed() >= GPS_SPEED_MIN) {
672 429 : yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f;
673 429 : _gps_last_update = _gps.last_fix_time_ms();
674 429 : new_value = true;
675 429 : const float gps_course_rad = radians(_gps.ground_course());
676 429 : const float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
677 429 : yaw_error = sinf(yaw_error_rad);
678 :
679 : /* reset yaw to match GPS heading under any of the
680 : following 3 conditions:
681 :
682 : 1) if we have reached GPS_SPEED_MIN and have never had
683 : yaw information before
684 :
685 : 2) if the last time we got yaw information from the GPS
686 : is more than 20 seconds ago, which means we may have
687 : suffered from considerable gyro drift
688 :
689 : 3) if we are over 3*GPS_SPEED_MIN (which means 9m/s)
690 : and our yaw error is over 60 degrees, which means very
691 : poor yaw. This can happen on bungee launch when the
692 : operator pulls back the plane rapidly enough then on
693 : release the GPS heading changes very rapidly
694 : */
695 427 : if (!have_initial_yaw ||
696 1246 : yaw_deltat > 20 ||
697 817 : (_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
698 : // reset DCM matrix based on current yaw
699 37 : _dcm_matrix.from_euler(roll, pitch, gps_course_rad);
700 37 : _omega_yaw_P.zero();
701 37 : have_initial_yaw = true;
702 37 : yaw_error = 0;
703 : }
704 : }
705 : }
706 :
707 41121437 : if (!new_value) {
708 : // we don't have any new yaw information
709 : // slowly decay _omega_yaw_P to cope with loss
710 : // of our yaw source
711 39632475 : _omega_yaw_P *= 0.97f;
712 39632475 : return;
713 : }
714 :
715 : // convert the error vector to body frame
716 1488962 : const float error_z = _dcm_matrix.c.z * yaw_error;
717 :
718 : // the spin rate changes the P gain, and disables the
719 : // integration at higher rates
720 1488962 : const float spin_rate = _omega.length();
721 :
722 : // sanity check _kp_yaw
723 1488962 : if (_kp_yaw < AP_AHRS_YAW_P_MIN) {
724 0 : _kp_yaw.set(AP_AHRS_YAW_P_MIN);
725 : }
726 :
727 : // update the proportional control to drag the
728 : // yaw back to the right value. We use a gain
729 : // that depends on the spin rate. See the fastRotations.pdf
730 : // paper from Bill Premerlani
731 : // We also adjust the gain depending on the rate of change of horizontal velocity which
732 : // is proportional to how observable the heading is from the accelerations and GPS velocity
733 : // The acceleration derived heading will be more reliable in turns than compass or GPS
734 :
735 1488962 : _omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw * _yaw_gain();
736 1488962 : if (use_fast_gains()) {
737 214454 : _omega_yaw_P.z *= 8;
738 : }
739 :
740 : // don't update the drift term if we lost the yaw reference
741 : // for more than 2 seconds
742 1488962 : if (yaw_deltat < 2.0f && spin_rate < radians(SPIN_RATE_LIMIT)) {
743 : // also add to the I term
744 1368043 : _omega_I_sum.z += error_z * _ki_yaw * yaw_deltat;
745 : }
746 :
747 1488962 : _error_yaw = 0.8f * _error_yaw + 0.2f * fabsf(yaw_error);
748 : }
749 :
750 :
751 : /**
752 : return an accel vector delayed by AHRS_ACCEL_DELAY samples for a
753 : specific accelerometer instance
754 : */
755 1204792 : Vector3f AP_AHRS_DCM::ra_delayed(uint8_t instance, const Vector3f &ra)
756 : {
757 : // get the old element, and then fill it with the new element
758 1204792 : const Vector3f ret = _ra_delay_buffer[instance];
759 1204792 : _ra_delay_buffer[instance] = ra;
760 1204792 : if (ret.is_zero()) {
761 : // use the current vector if the previous vector is exactly
762 : // zero. This prevents an error on initialisation
763 2426 : return ra;
764 : }
765 1202366 : return ret;
766 : }
767 :
768 : /* returns true if attitude should be corrected from GPS-derived
769 : * velocity-deltas. We turn this off for Copter and other similar
770 : * vehicles while the vehicle is disarmed to avoid the HUD bobbing
771 : * around while the vehicle is disarmed.
772 : */
773 758815 : bool AP_AHRS_DCM::should_correct_centrifugal() const
774 : {
775 : #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduSub) || APM_BUILD_TYPE(APM_BUILD_Blimp)
776 406873 : return hal.util->get_soft_armed();
777 : #endif
778 :
779 351942 : return true;
780 : }
781 :
782 : // perform drift correction. This function aims to update _omega_P and
783 : // _omega_I with our best estimate of the short term and long term
784 : // gyro error. The _omega_P value is what pulls our attitude solution
785 : // back towards the reference vector quickly. The _omega_I term is an
786 : // attempt to learn the long term drift rate of the gyros.
787 : //
788 : // This drift correction implementation is based on a paper
789 : // by Bill Premerlani from here:
790 : // http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
791 : void
792 41686716 : AP_AHRS_DCM::drift_correction(float deltat)
793 : {
794 41686716 : Vector3f velocity;
795 : uint32_t last_correction_time;
796 :
797 : // perform yaw drift correction if we have a new yaw reference
798 : // vector
799 41686716 : drift_correction_yaw();
800 :
801 41686716 : const AP_InertialSensor &_ins = AP::ins();
802 :
803 : // rotate accelerometer values into the earth frame
804 125136564 : for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
805 83449848 : if (_ins.use_accel(i)) {
806 : /*
807 : by using get_delta_velocity() instead of get_accel() the
808 : accel value is sampled over the right time delta for
809 : each sensor, which prevents an aliasing effect
810 : */
811 83449848 : Vector3f delta_velocity;
812 : float delta_velocity_dt;
813 83449848 : _ins.get_delta_velocity(i, delta_velocity, delta_velocity_dt);
814 83449848 : if (delta_velocity_dt > 0) {
815 83449846 : Vector3f accel_ef = _dcm_matrix * (delta_velocity / delta_velocity_dt);
816 : // integrate the accel vector in the earth frame between GPS readings
817 83449846 : _ra_sum[i] += accel_ef * deltat;
818 : }
819 : }
820 : }
821 :
822 : // set _accel_ef_blended based on filtered accel
823 41686716 : _accel_ef = _dcm_matrix * _ins.get_accel();
824 :
825 : // keep a sum of the deltat values, so we know how much time
826 : // we have integrated over
827 41686716 : _ra_deltat += deltat;
828 :
829 41686716 : const AP_GPS &_gps = AP::gps();
830 41686716 : const bool fly_forward = AP::ahrs().get_fly_forward();
831 :
832 79812329 : if (!have_gps() ||
833 79812329 : _gps.status() < AP_GPS_FixType::FIX_3D ||
834 38125373 : _gps.num_sats() < _gps_minsats) {
835 : // no GPS, or not a good lock. From experience we need at
836 : // least 6 satellites to get a really reliable velocity number
837 : // from the GPS.
838 : //
839 : // As a fallback we use the fixed wing acceleration correction
840 : // if we have an airspeed estimate (which we only have if
841 : // _fly_forward is set), otherwise no correction
842 3579164 : if (_ra_deltat < 0.2f) {
843 : // not enough time has accumulated
844 40927901 : return;
845 : }
846 :
847 65188 : float airspeed_TAS = _last_airspeed_TAS;
848 : #if AP_AIRSPEED_ENABLED
849 65188 : if (airspeed_sensor_enabled()) {
850 14638 : airspeed_TAS = AP::airspeed()->get_airspeed() * get_EAS2TAS();
851 : }
852 : #endif
853 :
854 : // use airspeed to estimate our ground velocity in
855 : // earth frame by subtracting the wind
856 65188 : velocity = _dcm_matrix.colx() * airspeed_TAS;
857 :
858 : // add in wind estimate
859 65188 : velocity += _wind;
860 :
861 65188 : last_correction_time = AP_HAL::millis();
862 65188 : _have_gps_lock = false;
863 : } else {
864 38107552 : if (_gps.last_fix_time_ms() == _ra_sum_start) {
865 : // we don't have a new GPS fix - nothing more to do
866 37411837 : return;
867 : }
868 695715 : velocity = _gps.velocity();
869 695715 : last_correction_time = _gps.last_fix_time_ms();
870 695715 : if (_have_gps_lock == false) {
871 : // if we didn't have GPS lock in the last drift
872 : // correction interval then set the velocities equal
873 1134 : _last_velocity = velocity;
874 : }
875 695715 : _have_gps_lock = true;
876 :
877 : // keep last airspeed estimate for dead-reckoning purposes
878 695715 : Vector3f airspeed = velocity - _wind;
879 :
880 : // rotate vector to body frame
881 695715 : airspeed = _body_dcm_matrix.mul_transpose(airspeed);
882 :
883 : // take positive component in X direction. This mimics a pitot
884 : // tube
885 695715 : _last_airspeed_TAS = MAX(airspeed.x, 0);
886 : }
887 :
888 760903 : if (have_gps()) {
889 : // use GPS for positioning with any fix, even a 2D fix
890 696160 : _last_lat = _gps.location().lat;
891 696160 : _last_lng = _gps.location().lng;
892 696160 : _last_pos_ms = AP_HAL::millis();
893 696160 : _position_offset_north = 0;
894 696160 : _position_offset_east = 0;
895 :
896 : // once we have a single GPS lock, we can update using
897 : // dead-reckoning from then on
898 696160 : _have_position = true;
899 : } else {
900 : // update dead-reckoning position estimate
901 64743 : _position_offset_north += velocity.x * _ra_deltat;
902 64743 : _position_offset_east += velocity.y * _ra_deltat;
903 : }
904 :
905 : // see if this is our first time through - in which case we
906 : // just setup the start times and return
907 760903 : if (_ra_sum_start == 0) {
908 2088 : _ra_sum_start = last_correction_time;
909 2088 : _last_velocity = velocity;
910 2088 : return;
911 : }
912 :
913 : // equation 9: get the corrected acceleration vector in earth frame. Units
914 : // are m/s/s
915 758815 : Vector3f GA_e(0.0f, 0.0f, -1.0f);
916 :
917 758815 : if (_ra_deltat <= 0) {
918 : // waiting for more data
919 0 : return;
920 : }
921 :
922 758815 : bool using_gps_corrections = false;
923 758815 : float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS);
924 :
925 758815 : if (should_correct_centrifugal() && (_have_gps_lock || fly_forward)) {
926 599345 : const float v_scale = gps_gain.get() * ra_scale;
927 599345 : const Vector3f vdelta = (velocity - _last_velocity) * v_scale;
928 599345 : GA_e += vdelta;
929 599345 : GA_e.normalize();
930 599345 : if (GA_e.is_inf()) {
931 : // wait for some non-zero acceleration information
932 0 : _last_failure_ms = AP_HAL::millis();
933 0 : return;
934 : }
935 599345 : using_gps_corrections = true;
936 : }
937 :
938 : // calculate the error term in earth frame.
939 : // we do this for each available accelerometer then pick the
940 : // accelerometer that leads to the smallest error term. This takes
941 : // advantage of the different sample rates on different
942 : // accelerometers to dramatically reduce the impact of aliasing
943 : // due to harmonics of vibrations that match closely the sampling
944 : // rate of our accelerometers. On the Pixhawk we have the LSM303D
945 : // running at 800Hz and the MPU6000 running at 1kHz, by combining
946 : // the two the effects of aliasing are greatly reduced.
947 758815 : Vector3f error[INS_MAX_INSTANCES];
948 : float error_dirn[INS_MAX_INSTANCES];
949 758815 : Vector3f GA_b[INS_MAX_INSTANCES];
950 758815 : int8_t besti = -1;
951 758815 : float best_error = 0;
952 2282990 : for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
953 1524175 : if (!_ins.get_accel_health(i)) {
954 : // only use healthy sensors
955 0 : continue;
956 : }
957 1524175 : _ra_sum[i] *= ra_scale;
958 :
959 : // get the delayed ra_sum to match the GPS lag
960 1524175 : if (using_gps_corrections) {
961 1204792 : GA_b[i] = ra_delayed(i, _ra_sum[i]);
962 : } else {
963 319383 : GA_b[i] = _ra_sum[i];
964 : }
965 1524175 : if (GA_b[i].is_zero()) {
966 : // wait for some non-zero acceleration information
967 0 : continue;
968 : }
969 1524175 : GA_b[i].normalize();
970 1524175 : if (GA_b[i].is_inf()) {
971 : // wait for some non-zero acceleration information
972 0 : continue;
973 : }
974 1524175 : error[i] = GA_b[i] % GA_e;
975 : // Take dot product to catch case vectors are opposite sign and parallel
976 1524175 : error_dirn[i] = GA_b[i] * GA_e;
977 1524175 : const float error_length = error[i].length();
978 1524175 : if (besti == -1 || error_length < best_error) {
979 1128359 : besti = i;
980 1128359 : best_error = error_length;
981 : }
982 : // Catch case where orientation is 180 degrees out
983 1524175 : if (error_dirn[besti] < 0.0f) {
984 4470 : best_error = 1.0f;
985 : }
986 :
987 : }
988 :
989 758815 : if (besti == -1) {
990 : // no healthy accelerometers!
991 0 : _last_failure_ms = AP_HAL::millis();
992 0 : return;
993 : }
994 :
995 758815 : _active_accel_instance = besti;
996 :
997 : #define YAW_INDEPENDENT_DRIFT_CORRECTION 0
998 : #if YAW_INDEPENDENT_DRIFT_CORRECTION
999 : // step 2 calculate earth_error_Z
1000 : const float earth_error_Z = error.z;
1001 :
1002 : // equation 10
1003 : const float tilt = GA_e.xy().length();
1004 :
1005 : // equation 11
1006 : const float theta = atan2f(GA_b[besti].y, GA_b[besti].x);
1007 :
1008 : // equation 12
1009 : const Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);
1010 :
1011 : // step 6
1012 : error = GA_b[besti] % GA_e2;
1013 : error.z = earth_error_Z;
1014 : #endif // YAW_INDEPENDENT_DRIFT_CORRECTION
1015 :
1016 : // to reduce the impact of two competing yaw controllers, we
1017 : // reduce the impact of the gps/accelerometers on yaw when we are
1018 : // flat, but still allow for yaw correction using the
1019 : // accelerometers at high roll angles as long as we have a GPS
1020 758815 : if (AP_AHRS_DCM::use_compass()) {
1021 757170 : if (have_gps() && is_equal(gps_gain.get(), 1.0f)) {
1022 694957 : error[besti].z *= sinf(fabsf(roll));
1023 : } else {
1024 62213 : error[besti].z = 0;
1025 : }
1026 : }
1027 :
1028 : // if ins is unhealthy then stop attitude drift correction and
1029 : // hope the gyros are OK for a while. Just slowly reduce _omega_P
1030 : // to prevent previous bad accels from throwing us off
1031 758815 : if (!_ins.healthy()) {
1032 0 : error[besti].zero();
1033 : } else {
1034 : // convert the error term to body frame
1035 758815 : error[besti] = _dcm_matrix.mul_transpose(error[besti]);
1036 : }
1037 :
1038 758815 : if (error[besti].is_nan() || error[besti].is_inf()) {
1039 : // don't allow bad values
1040 0 : check_matrix();
1041 0 : _last_failure_ms = AP_HAL::millis();
1042 0 : return;
1043 : }
1044 :
1045 758815 : _error_rp = 0.8f * _error_rp + 0.2f * best_error;
1046 :
1047 : // base the P gain on the spin rate
1048 758815 : const float spin_rate = _omega.length();
1049 :
1050 : // sanity check _kp value
1051 758815 : if (_kp < AP_AHRS_RP_P_MIN) {
1052 0 : _kp.set(AP_AHRS_RP_P_MIN);
1053 : }
1054 :
1055 : // we now want to calculate _omega_P and _omega_I. The
1056 : // _omega_P value is what drags us quickly to the
1057 : // accelerometer reading.
1058 758815 : _omega_P = error[besti] * _P_gain(spin_rate) * _kp;
1059 758815 : if (use_fast_gains()) {
1060 109693 : _omega_P *= 8;
1061 : }
1062 :
1063 456328 : if (fly_forward && _gps.status() >= AP_GPS_FixType::FIX_2D &&
1064 220180 : _gps.ground_speed() < GPS_SPEED_MIN &&
1065 59989 : _ins.get_accel().x >= 7 &&
1066 995234 : pitch > radians(-30) && pitch < radians(30)) {
1067 : // assume we are in a launch acceleration, and reduce the
1068 : // rp gain by 50% to reduce the impact of GPS lag on
1069 : // takeoff attitude when using a catapult
1070 122 : _omega_P *= 0.5f;
1071 : }
1072 :
1073 : // accumulate some integrator error
1074 758815 : if (spin_rate < radians(SPIN_RATE_LIMIT)) {
1075 694098 : _omega_I_sum += error[besti] * _ki * _ra_deltat;
1076 694098 : _omega_I_sum_time += _ra_deltat;
1077 : }
1078 :
1079 758815 : if (_omega_I_sum_time >= 5) {
1080 : // limit the rate of change of omega_I to the hardware
1081 : // reported maximum gyro drift rate. This ensures that
1082 : // short term errors don't cause a buildup of omega_I
1083 : // beyond the physical limits of the device
1084 25788 : const float change_limit = AP::ins().get_gyro_drift_rate() * _omega_I_sum_time;
1085 25788 : _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
1086 25788 : _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
1087 25788 : _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
1088 25788 : _omega_I += _omega_I_sum;
1089 25788 : _omega_I_sum.zero();
1090 25788 : _omega_I_sum_time = 0;
1091 : }
1092 :
1093 : // zero our accumulator ready for the next GPS step
1094 758815 : memset((void *)&_ra_sum[0], 0, sizeof(_ra_sum));
1095 758815 : _ra_deltat = 0;
1096 758815 : _ra_sum_start = last_correction_time;
1097 :
1098 : // remember the velocity for next time
1099 758815 : _last_velocity = velocity;
1100 : }
1101 :
1102 :
1103 : // update our wind speed estimate
1104 696272 : void AP_AHRS_DCM::estimate_wind(void)
1105 : {
1106 696272 : if (!AP::ahrs().get_wind_estimation_enabled()) {
1107 458447 : return;
1108 : }
1109 :
1110 : // the wind-triangle filters below blend a fixed fraction of the new
1111 : // estimate on each call, and the straight-flight branch does not
1112 : // update _last_wind_time, so the effective filter time constant is
1113 : // set by the call rate. Now that this is driven on each new GPS
1114 : // sample rather than by a fixed-rate vehicle task, limit to 10Hz
1115 : // here so the time constant stays sane:
1116 264533 : const uint32_t now = AP_HAL::millis();
1117 264533 : if (now - _last_wind_estimate_ms < 100) {
1118 5890 : return;
1119 : }
1120 258643 : _last_wind_estimate_ms = now;
1121 :
1122 258643 : const Vector3f &velocity = _last_velocity;
1123 :
1124 : // this is based on the wind speed estimation code from MatrixPilot by
1125 : // Bill Premerlani. Adaption for ArduPilot by Jon Challinger
1126 : // See http://gentlenav.googlecode.com/files/WindEstimation.pdf
1127 : // use the trim-corrected (vehicle body) forward axis, not the raw board
1128 : // axis _dcm_matrix.colx(): a non-zero AHRS_TRIM otherwise feeds the wind
1129 : // triangle the sensor-board direction rather than the fuselage direction.
1130 258643 : const Vector3f fuselageDirection = _body_dcm_matrix.colx();
1131 258643 : const Vector3f fuselageDirectionDiff = fuselageDirection - _last_fuse;
1132 :
1133 : // scrap our data and start over if we're taking too long to get a direction change
1134 258643 : if (now - _last_wind_time > 10000) {
1135 2803 : _last_wind_time = now;
1136 2803 : _last_fuse = fuselageDirection;
1137 2803 : _last_vel = velocity;
1138 2803 : return;
1139 : }
1140 :
1141 255840 : float diff_length = fuselageDirectionDiff.length();
1142 255840 : if (diff_length > 0.2f) {
1143 : // when turning, use the attitude response to estimate
1144 : // wind speed
1145 18015 : const Vector3f velocityDiff = velocity - _last_vel;
1146 :
1147 : // estimate airspeed it using equation 6
1148 18015 : const float V = velocityDiff.length() / diff_length;
1149 :
1150 18015 : const Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse;
1151 18015 : const Vector3f velocitySum = velocity + _last_vel;
1152 :
1153 18015 : _last_fuse = fuselageDirection;
1154 18015 : _last_vel = velocity;
1155 :
1156 18015 : const float theta = atan2f(velocityDiff.y, velocityDiff.x) - atan2f(fuselageDirectionDiff.y, fuselageDirectionDiff.x);
1157 18015 : const float sintheta = sinf(theta);
1158 18015 : const float costheta = cosf(theta);
1159 :
1160 : Vector3f wind{
1161 18015 : velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y),
1162 18015 : velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y),
1163 18015 : velocitySum.z - V * fuselageDirectionSum.z
1164 18015 : };
1165 18015 : wind *= 0.5f;
1166 :
1167 18015 : if (wind.length() < _wind.length() + 20) {
1168 16356 : _wind = _wind * 0.95f + wind * 0.05f;
1169 : }
1170 :
1171 18015 : _last_wind_time = now;
1172 :
1173 18015 : return;
1174 : }
1175 :
1176 : #if AP_AIRSPEED_ENABLED
1177 237825 : if (now - _last_wind_time > 2000 && airspeed_sensor_enabled()) {
1178 : // when flying straight use airspeed to get wind estimate if available
1179 137503 : const Vector3f airspeed = fuselageDirection * AP::airspeed()->get_airspeed();
1180 137503 : const Vector3f wind = velocity - (airspeed * get_EAS2TAS());
1181 137503 : _wind = _wind * 0.92f + wind * 0.08f;
1182 : }
1183 : #endif
1184 : }
1185 :
1186 : #if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
1187 14 : void AP_AHRS_DCM::set_external_wind_estimate(float speed, float direction) {
1188 14 : _wind.x = -cosf(radians(direction)) * speed;
1189 14 : _wind.y = -sinf(radians(direction)) * speed;
1190 14 : _wind.z = 0;
1191 14 : }
1192 : #endif
1193 :
1194 : // return our current position estimate using
1195 : // dead-reckoning or GPS
1196 41686716 : bool AP_AHRS_DCM::get_location(Location &loc) const
1197 : {
1198 41686716 : if (_last_lat == 0 && _last_lng == 0) {
1199 : // we have never had a position
1200 3211625 : return false;
1201 : }
1202 38475091 : loc.lat = _last_lat;
1203 38475091 : loc.lng = _last_lng;
1204 38475091 : const auto &baro = AP::baro();
1205 38475091 : const auto &gps = AP::gps();
1206 : int32_t alt_cm;
1207 38475091 : if (_gps_use == GPSUse::EnableWithHeight &&
1208 0 : gps.status() >= AP_GPS_FixType::FIX_3D) {
1209 0 : alt_cm = gps.location().alt;
1210 : } else {
1211 38475091 : alt_cm = baro.get_altitude() * 100 + AP::ahrs().get_home().alt;
1212 : }
1213 38475091 : loc.set_alt_cm(alt_cm, Location::AltFrame::ABSOLUTE);
1214 38475091 : loc.offset(_position_offset_north, _position_offset_east);
1215 38475091 : if (_have_position) {
1216 38475091 : const uint32_t now = AP_HAL::millis();
1217 38475091 : float dt = 0;
1218 38475091 : gps.get_lag(dt);
1219 38475091 : dt += constrain_float((now - _last_pos_ms) * 0.001, 0, 0.5);
1220 38475091 : Vector2f dpos = _last_velocity.xy() * dt;
1221 38475091 : loc.offset(dpos.x, dpos.y);
1222 : }
1223 38475091 : return _have_position;
1224 : }
1225 :
1226 43673294 : bool AP_AHRS_DCM::airspeed_EAS(float &airspeed_ret) const
1227 : {
1228 : #if AP_AIRSPEED_ENABLED
1229 43673294 : const auto *airspeed = AP::airspeed();
1230 43673294 : if (airspeed != nullptr) {
1231 43581483 : return airspeed_EAS(airspeed->get_primary(), airspeed_ret);
1232 : }
1233 : #endif
1234 : // airspeed_estimate will also make this nullptr check and act
1235 : // appropriately when we call it with a dummy sensor ID.
1236 91811 : return airspeed_EAS(0, airspeed_ret);
1237 : }
1238 :
1239 : // return an (equivalent) airspeed estimate:
1240 : // - from a real sensor if available
1241 : // - otherwise from a GPS-derived wind-triangle estimate (if GPS available)
1242 : // - otherwise from a cached wind-triangle estimate value (but returning false)
1243 43744280 : bool AP_AHRS_DCM::airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const
1244 : {
1245 : // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order:
1246 : // airspeed as filled-in by an enabled airspeed sensor
1247 : // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
1248 : // Or if none of the above, fills-in using the previous airspeed estimate
1249 : // Return false: if we are using the previous airspeed estimate
1250 43744280 : if (!get_unconstrained_airspeed_EAS(airspeed_index, airspeed_ret)) {
1251 34344030 : return false;
1252 : }
1253 :
1254 9400250 : const float _wind_max = AP::ahrs().get_max_wind();
1255 9400250 : if (_wind_max > 0 && AP::gps().status() >= AP_GPS_FixType::FIX_2D) {
1256 : // constrain the airspeed by the ground speed
1257 : // and AHRS_WIND_MAX
1258 0 : const float gnd_speed = AP::gps().ground_speed();
1259 0 : float true_airspeed = airspeed_ret * get_EAS2TAS();
1260 0 : true_airspeed = constrain_float(true_airspeed,
1261 : gnd_speed - _wind_max,
1262 : gnd_speed + _wind_max);
1263 0 : airspeed_ret = true_airspeed / get_EAS2TAS();
1264 : }
1265 :
1266 9400250 : return true;
1267 : }
1268 :
1269 : // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order:
1270 : // airspeed as filled-in by an enabled airspeed sensor
1271 : // if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
1272 : // Or if none of the above, fills-in using the previous airspeed estimate
1273 : // Return false: if we are using the previous airspeed estimate
1274 43744280 : bool AP_AHRS_DCM::get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const
1275 : {
1276 : #if AP_AIRSPEED_ENABLED
1277 43744280 : if (airspeed_sensor_enabled(airspeed_index)) {
1278 9296838 : airspeed_ret = AP::airspeed()->get_airspeed(airspeed_index);
1279 9296838 : return true;
1280 : }
1281 : #endif
1282 :
1283 34447442 : if (AP::ahrs().get_wind_estimation_enabled() && have_gps()) {
1284 : // estimated via GPS speed and wind
1285 103412 : airspeed_ret = _last_airspeed_TAS * get_TAS2EAS();
1286 103412 : return true;
1287 : }
1288 :
1289 : // Else give the last estimate, but return false.
1290 : // This is used by the dead-reckoning code
1291 34344030 : airspeed_ret = _last_airspeed_TAS * get_TAS2EAS();
1292 34344030 : return false;
1293 : }
1294 :
1295 : /*
1296 : return NED velocity if we have GPS lock
1297 : */
1298 83373432 : bool AP_AHRS_DCM::get_velocity_NED(Vector3f &vec) const
1299 : {
1300 83373432 : const AP_GPS &_gps = AP::gps();
1301 83373432 : if (_gps.status() < AP_GPS_FixType::FIX_3D) {
1302 7122076 : return false;
1303 : }
1304 76251356 : vec = _gps.velocity();
1305 76251356 : return true;
1306 : }
1307 :
1308 : // return a ground speed estimate in m/s
1309 41686716 : Vector2f AP_AHRS_DCM::groundspeed_vector(void)
1310 : {
1311 : // Generate estimate of ground speed vector using air data system
1312 41686716 : Vector2f gndVelADS;
1313 41686716 : Vector2f gndVelGPS;
1314 41686716 : float airspeed = 0;
1315 41686716 : const bool gotAirspeed = airspeed_TAS(airspeed);
1316 41686716 : const bool gotGPS = (AP::gps().status() >= AP_GPS_FixType::FIX_2D);
1317 41686716 : if (gotAirspeed) {
1318 8316926 : const Vector2f airspeed_vector{_cos_yaw * airspeed, _sin_yaw * airspeed};
1319 8316926 : gndVelADS = airspeed_vector + _wind.xy();
1320 : }
1321 :
1322 : // Generate estimate of ground speed vector using GPS
1323 41686716 : if (gotGPS) {
1324 38125918 : const float cog = radians(AP::gps().ground_course());
1325 38125918 : gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed();
1326 : }
1327 : // If both ADS and GPS data is available, apply a complementary filter
1328 41686716 : if (gotAirspeed && gotGPS) {
1329 : // The LPF is applied to the GPS and the HPF is applied to the air data estimate
1330 : // before the two are summed
1331 : //Define filter coefficients
1332 : // alpha and beta must sum to one
1333 : // beta = dt/Tau, where
1334 : // dt = filter time step (0.1 sec if called by nav loop)
1335 : // Tau = cross-over time constant (nominal 2 seconds)
1336 : // More lag on GPS requires Tau to be bigger, less lag allows it to be smaller
1337 : // To-Do - set Tau as a function of GPS lag.
1338 7944122 : const float alpha = 1.0f - beta;
1339 : // Run LP filters
1340 7944122 : _lp = gndVelGPS * beta + _lp * alpha;
1341 : // Run HP filters
1342 7944122 : _hp = (gndVelADS - _lastGndVelADS) + _hp * alpha;
1343 : // Save the current ADS ground vector for the next time step
1344 7944122 : _lastGndVelADS = gndVelADS;
1345 : // Sum the HP and LP filter outputs
1346 7944122 : return _hp + _lp;
1347 : }
1348 : // Only ADS data is available return ADS estimate
1349 33742594 : if (gotAirspeed && !gotGPS) {
1350 372804 : return gndVelADS;
1351 : }
1352 : // Only GPS data is available so return GPS estimate
1353 33369790 : if (!gotAirspeed && gotGPS) {
1354 30181796 : return gndVelGPS;
1355 : }
1356 :
1357 3187994 : if (airspeed > 0) {
1358 : // we have a rough airspeed, and we have a yaw. For
1359 : // dead-reckoning purposes we can create a estimated
1360 : // groundspeed vector
1361 102808 : Vector2f ret{_cos_yaw, _sin_yaw};
1362 102808 : ret *= airspeed;
1363 : // adjust for estimated wind
1364 102808 : ret += _wind.xy();
1365 102808 : return ret;
1366 : }
1367 :
1368 3085186 : return Vector2f(0.0f, 0.0f);
1369 : }
1370 :
1371 : // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
1372 : // This is different to the vertical velocity from the EKF which is not always consistent with the vertical position due to the various errors that are being corrected for.
1373 41686716 : bool AP_AHRS_DCM::get_vert_pos_rate_D(float &velocity) const
1374 : {
1375 41686716 : Vector3f velned;
1376 41686716 : if (get_velocity_NED(velned)) {
1377 38125678 : velocity = velned.z;
1378 38125678 : return true;
1379 3561038 : } else if (AP::baro().healthy()) {
1380 3546448 : velocity = -AP::baro().get_climb_rate();
1381 3546448 : return true;
1382 : }
1383 14590 : return false;
1384 : }
1385 :
1386 : /*
1387 : relative-origin functions for fallback in AP_InertialNav
1388 : */
1389 41913062 : bool AP_AHRS_DCM::get_origin(Location &ret) const
1390 : {
1391 41913062 : ret = last_origin;
1392 41913062 : if (ret.is_zero()) {
1393 : // use home if we never have had an origin
1394 5110769 : ret = AP::ahrs().get_home();
1395 : }
1396 41913062 : return !ret.is_zero();
1397 : }
1398 :
1399 : // return true if DCM has a yaw source available
1400 8462025 : bool AP_AHRS_DCM::yaw_source_available(void) const
1401 : {
1402 8462025 : return AP::compass().use_for_yaw();
1403 : }
1404 :
1405 : #endif // AP_AHRS_DCM_ENABLED
|
