/* * APM_AHRS_DCM.cpp * * AHRS system using DCM matrices * * Based on DCM code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com * * Adapted for the general ArduPilot AHRS interface by Andrew Tridgell * * 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 version 2.1 * of the License, or (at your option) any later version. */ #include #include // this is the speed in cm/s above which we first get a yaw lock with // the GPS #define GPS_SPEED_MIN 300 // this is the speed in cm/s at which we stop using drift correction // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN #define GPS_SPEED_RESET 100 // the limit (in degrees/second) beyond which we stop integrating // omega_I. At larger spin rates the DCM PI controller can get 'dizzy' // which results in false gyro drift. See // http://gentlenav.googlecode.com/files/fastRotations.pdf #define SPIN_RATE_LIMIT 20 // run a full DCM update round void AP_AHRS_DCM::update(void) { float delta_t; // tell the IMU to grab some data _imu->update(); // ask the IMU how much time this sensor reading represents delta_t = _imu->get_delta_time(); // Get current values for gyros _gyro_vector = _imu->get_gyro(); _accel_vector = _imu->get_accel(); // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); // Normalize the DCM matrix normalize(); // Perform drift correction drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); // Calculate pitch, roll, yaw for stabilization and navigation euler_angles(); } // update the DCM matrix using only the gyros void AP_AHRS_DCM::matrix_update(float _G_Dt) { // note that we do not include the P terms in _omega. This is // because the spin_rate is calculated from _omega.length(), // and including the P terms would give positive feedback into // the _P_gain() calculation, which can lead to a very large P // value _omega = _gyro_vector + _omega_I; _dcm_matrix.rotate((_omega + _omega_P + _omega_yaw_P) * _G_Dt); } /* * reset the DCM matrix and omega. Used on ground start, and on * extreme errors in the matrix */ void AP_AHRS_DCM::reset(bool recover_eulers) { // reset the integration terms _omega_I.zero(); _omega_P.zero(); _omega_yaw_P.zero(); _omega.zero(); // if the caller wants us to try to recover to the current // attitude then calculate the dcm matrix from the current // roll/pitch/yaw values if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) { _dcm_matrix.from_euler(roll, pitch, yaw); } else { // otherwise make it flat _dcm_matrix.from_euler(0, 0, 0); } } /* * check the DCM matrix for pathological values */ void AP_AHRS_DCM::check_matrix(void) { if (_dcm_matrix.is_nan()) { //Serial.printf("ERROR: DCM matrix NAN\n"); SITL_debug("ERROR: DCM matrix NAN\n"); renorm_blowup_count++; reset(true); return; } // some DCM matrix values can lead to an out of range error in // the pitch calculation via asin(). These NaN values can // feed back into the rest of the DCM matrix via the // error_course value. if (!(_dcm_matrix.c.x < 1.0 && _dcm_matrix.c.x > -1.0)) { // We have an invalid matrix. Force a normalisation. renorm_range_count++; normalize(); if (_dcm_matrix.is_nan() || fabs(_dcm_matrix.c.x) > 10) { // normalisation didn't fix the problem! We're // in real trouble. All we can do is reset //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", // _dcm_matrix.c.x); SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", _dcm_matrix.c.x); renorm_blowup_count++; reset(true); } } } // renormalise one vector component of the DCM matrix // this will return false if renormalization fails bool AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result) { float renorm_val; // numerical errors will slowly build up over time in DCM, // causing inaccuracies. We can keep ahead of those errors // using the renormalization technique from the DCM IMU paper // (see equations 18 to 21). // For APM we don't bother with the taylor expansion // optimisation from the paper as on our 2560 CPU the cost of // the sqrt() is 44 microseconds, and the small time saving of // the taylor expansion is not worth the potential of // additional error buildup. // Note that we can get significant renormalisation values // when we have a larger delta_t due to a glitch eleswhere in // APM, such as a I2c timeout or a set of EEPROM writes. While // we would like to avoid these if possible, if it does happen // we don't want to compound the error by making DCM less // accurate. renorm_val = 1.0 / a.length(); // keep the average for reporting _renorm_val_sum += renorm_val; _renorm_val_count++; if (!(renorm_val < 2.0 && renorm_val > 0.5)) { // this is larger than it should get - log it as a warning renorm_range_count++; if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) { // we are getting values which are way out of // range, we will reset the matrix and hope we // can recover our attitude using drift // correction before we hit the ground! //Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n", // renorm_val); SITL_debug("ERROR: DCM renormalisation error. renorm_val=%f\n", renorm_val); renorm_blowup_count++; return false; } } result = a * renorm_val; return true; } /************************************************* * 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 * simple matter to stay ahead of it. * We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ. */ void AP_AHRS_DCM::normalize(void) { float error; Vector3f t0, t1, t2; error = _dcm_matrix.a * _dcm_matrix.b; // eq.18 t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19 t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19 t2 = t0 % t1; // c= a x b // eq.20 if (!renorm(t0, _dcm_matrix.a) || !renorm(t1, _dcm_matrix.b) || !renorm(t2, _dcm_matrix.c)) { // Our solution is blowing up and we will force back // to last euler angles reset(true); } } // produce a yaw error value. The returned value is proportional // to sin() of the current heading error in earth frame float AP_AHRS_DCM::yaw_error_compass(void) { Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z); // get the mag vector in the earth frame Vector3f rb = _dcm_matrix * mag; rb.normalize(); if (rb.is_inf()) { // not a valid vector return 0.0; } // get the earths magnetic field (only X and Y components needed) Vector3f mag_earth = Vector3f(cos(_compass->get_declination()), sin(_compass->get_declination()), 0); // calculate the error term in earth frame Vector3f error = rb % mag_earth; return error.z; } // produce a yaw error value using the GPS. The returned value is proportional // to sin() of the current heading error in earth frame float AP_AHRS_DCM::yaw_error_gps(void) { return sin(ToRad(_gps->ground_course * 0.01) - yaw); } // the _P_gain raises the gain of the PI controller // when we are spinning fast. See the fastRotations // paper from Bill. float AP_AHRS_DCM::_P_gain(float spin_rate) { if (spin_rate < ToDeg(50)) { return 1.0; } if (spin_rate > ToDeg(500)) { return 10.0; } return spin_rate/ToDeg(50); } // return true if we have and should use GPS bool AP_AHRS_DCM::have_gps(void) { if (!_gps || _gps->status() != GPS::GPS_OK || !_gps_use) { return false; } return true; } // yaw drift correction using the compass or GPS // this function prodoces the _omega_yaw_P vector, and also // contributes to the _omega_I.z long term yaw drift estimate void AP_AHRS_DCM::drift_correction_yaw(void) { bool new_value = false; float yaw_error; float yaw_deltat; if (_compass && _compass->use_for_yaw()) { if (_compass->last_update != _compass_last_update) { yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6; _compass_last_update = _compass->last_update; // we force an additional compass read() // here. This has the effect of throwing away // the first compass value, which can be bad if (!_have_initial_yaw && _compass->read()) { float heading = _compass->calculate_heading(_dcm_matrix); _dcm_matrix.from_euler(roll, pitch, heading); _omega_yaw_P.zero(); _have_initial_yaw = true; } new_value = true; yaw_error = yaw_error_compass(); } } else if (_fly_forward && have_gps()) { if (_gps->last_fix_time != _gps_last_update && _gps->ground_speed >= GPS_SPEED_MIN) { yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3; _gps_last_update = _gps->last_fix_time; if (!_have_initial_yaw) { _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01)); _omega_yaw_P.zero(); _have_initial_yaw = true; } new_value = true; yaw_error = yaw_error_gps(); } } if (!new_value) { // we don't have any new yaw information // slowly decay _omega_yaw_P to cope with loss // of our yaw source _omega_yaw_P *= 0.97; return; } // the yaw error is a vector in earth frame Vector3f error = Vector3f(0,0, yaw_error); // convert the error vector to body frame error = _dcm_matrix.mul_transpose(error); // the spin rate changes the P gain, and disables the // integration at higher rates float spin_rate = _omega.length(); // update the proportional control to drag the // yaw back to the right value. We use a gain // that depends on the spin rate. See the fastRotations.pdf // paper from Bill Premerlani _omega_yaw_P.z = error.z * _P_gain(spin_rate) * _kp_yaw; if (_fast_ground_gains) { _omega_yaw_P.z *= 8; } // don't update the drift term if we lost the yaw reference // for more than 2 seconds if (yaw_deltat < 2.0 && spin_rate < ToRad(SPIN_RATE_LIMIT)) { // also add to the I term _omega_I_sum.z += error.z * _ki_yaw * yaw_deltat; } _error_yaw_sum += fabs(yaw_error); _error_yaw_count++; } // perform drift correction. This function aims to update _omega_P and // _omega_I with our best estimate of the short term and long term // gyro error. The _omega_P value is what pulls our attitude solution // back towards the reference vector quickly. The _omega_I term is an // attempt to learn the long term drift rate of the gyros. // // This drift correction implementation is based on a paper // by Bill Premerlani from here: // http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf void AP_AHRS_DCM::drift_correction(float deltat) { Vector3f velocity; uint32_t last_correction_time; // perform yaw drift correction if we have a new yaw reference // vector drift_correction_yaw(); // integrate the accel vector in the earth frame between GPS readings _ra_sum += _dcm_matrix * (_accel_vector * deltat); // keep a sum of the deltat values, so we know how much time // we have integrated over _ra_deltat += deltat; if (!have_gps()) { // no GPS, or no lock. We assume zero velocity. This at // least means we can cope with gyro drift while sitting // on a bench with no GPS lock if (_ra_deltat < 0.2) { // not enough time has accumulated return; } float airspeed; if (_airspeed && _airspeed->use()) { airspeed = _airspeed->get_airspeed(); } else { airspeed = _last_airspeed; } // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind velocity = _dcm_matrix.colx() * airspeed; // add in wind estimate velocity += _wind; last_correction_time = millis(); _have_gps_lock = false; // update position delta for get_position() _position_offset_north += velocity.x * _ra_deltat; _position_offset_east += velocity.y * _ra_deltat; } else { if (_gps->last_fix_time == _ra_sum_start) { // we don't have a new GPS fix - nothing more to do return; } velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), 0); last_correction_time = _gps->last_fix_time; if (_have_gps_lock == false) { // if we didn't have GPS lock in the last drift // correction interval then set the velocities equal _last_velocity = velocity; } _have_gps_lock = true; // remember position for get_position() _last_lat = _gps->latitude; _last_lng = _gps->longitude; _position_offset_north = 0; _position_offset_east = 0; // once we have a single GPS lock, we update using // dead-reckoning from then on _have_position = true; // keep last airspeed estimate for dead-reckoning purposes Vector3f airspeed = velocity - _wind; airspeed.z = 0; _last_airspeed = airspeed.length(); } /* * The barometer for vertical velocity is only enabled if we got * at least 5 pressure samples for the reading. This ensures we * don't use very noisy climb rate data */ if (_baro_use && _barometer != NULL && _barometer->get_pressure_samples() >= 5) { // Z velocity is down velocity.z = -_barometer->get_climb_rate(); } // see if this is our first time through - in which case we // just setup the start times and return if (_ra_sum_start == 0) { _ra_sum_start = last_correction_time; _last_velocity = velocity; return; } // equation 9: get the corrected acceleration vector in earth frame. Units // are m/s/s Vector3f GA_e; float v_scale = gps_gain.get()/(_ra_deltat*_gravity); Vector3f vdelta = (velocity - _last_velocity) * v_scale; // limit vertical acceleration correction to 0.5 gravities. The // barometer sometimes gives crazy acceleration changes. vdelta.z = constrain(vdelta.z, -0.5, 0.5); GA_e = Vector3f(0, 0, -1.0) + vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information return; } // calculate the error term in earth frame. Vector3f GA_b = _ra_sum / (_ra_deltat * _gravity); float length = GA_b.length(); if (length > 1.0) { GA_b /= length; if (GA_b.is_inf()) { // wait for some non-zero acceleration information return; } } Vector3f error = GA_b % GA_e; #define YAW_INDEPENDENT_DRIFT_CORRECTION 0 #if YAW_INDEPENDENT_DRIFT_CORRECTION // step 2 calculate earth_error_Z float earth_error_Z = error.z; // equation 10 float tilt = sqrt(sq(GA_e.x) + sq(GA_e.y)); // equation 11 float theta = atan2(GA_b.y, GA_b.x); // equation 12 Vector3f GA_e2 = Vector3f(cos(theta)*tilt, sin(theta)*tilt, GA_e.z); // step 6 error = GA_b % GA_e2; error.z = earth_error_Z; #endif // YAW_INDEPENDENT_DRIFT_CORRECTION // to reduce the impact of two competing yaw controllers, we // reduce the impact of the gps/accelerometers on yaw when we are // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS if (_compass && _compass->use_for_yaw()) { if (have_gps() && gps_gain == 1.0) { error.z *= sin(fabs(roll)); } else { error.z = 0; } } // convert the error term to body frame error = _dcm_matrix.mul_transpose(error); if (error.is_nan() || error.is_inf()) { // don't allow bad values check_matrix(); return; } _error_rp_sum += error.length(); _error_rp_count++; // base the P gain on the spin rate float spin_rate = _omega.length(); // we now want to calculate _omega_P and _omega_I. The // _omega_P value is what drags us quickly to the // accelerometer reading. _omega_P = error * _P_gain(spin_rate) * _kp; if (_fast_ground_gains) { _omega_P *= 8; } // accumulate some integrator error if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { _omega_I_sum += error * _ki * _ra_deltat; _omega_I_sum_time += _ra_deltat; } if (_omega_I_sum_time >= 5) { // limit the rate of change of omega_I to the hardware // reported maximum gyro drift rate. This ensures that // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device float change_limit = _gyro_drift_limit * _omega_I_sum_time; _omega_I_sum.x = constrain(_omega_I_sum.x, -change_limit, change_limit); _omega_I_sum.y = constrain(_omega_I_sum.y, -change_limit, change_limit); _omega_I_sum.z = constrain(_omega_I_sum.z, -change_limit, change_limit); _omega_I += _omega_I_sum; _omega_I_sum.zero(); _omega_I_sum_time = 0; } // zero our accumulator ready for the next GPS step _ra_sum.zero(); _ra_deltat = 0; _ra_sum_start = last_correction_time; // remember the velocity for next time _last_velocity = velocity; if (_have_gps_lock && _fly_forward) { // update wind estimate estimate_wind(velocity); } } // update our wind speed estimate void AP_AHRS_DCM::estimate_wind(Vector3f &velocity) { // this is based on the wind speed estimation code from MatrixPilot by // Bill Premerlani. Adaption for ArduPilot by Jon Challinger // See http://gentlenav.googlecode.com/files/WindEstimation.pdf Vector3f fuselageDirection = _dcm_matrix.colx(); Vector3f fuselageDirectionDiff = fuselageDirection - _last_fuse; uint32_t now = millis(); // scrap our data and start over if we're taking too long to get a direction change if (now - _last_wind_time > 10000) { _last_wind_time = now; _last_fuse = fuselageDirection; _last_vel = velocity; return; } float diff_length = fuselageDirectionDiff.length(); if (diff_length > 0.2) { // when turning, use the attitude response to estimate // wind speed float V; Vector3f velocityDiff = velocity - _last_vel; // estimate airspeed it using equation 6 V = velocityDiff.length() / diff_length; _last_fuse = fuselageDirection; _last_vel = velocity; Vector3f fuselageDirectionSum = fuselageDirection + _last_fuse; Vector3f velocitySum = velocity + _last_vel; float theta = atan2(velocityDiff.y, velocityDiff.x) - atan2(fuselageDirectionDiff.y, fuselageDirectionDiff.x); float sintheta = sin(theta); float costheta = cos(theta); Vector3f wind = Vector3f(); wind.x = velocitySum.x - V * (costheta * fuselageDirectionSum.x - sintheta * fuselageDirectionSum.y); wind.y = velocitySum.y - V * (sintheta * fuselageDirectionSum.x + costheta * fuselageDirectionSum.y); wind.z = velocitySum.z - V * fuselageDirectionSum.z; wind *= 0.5; if (wind.length() < _wind.length() + 20) { _wind = _wind * 0.95 + wind * 0.05; } _last_wind_time = now; } else if (now - _last_wind_time > 2000 && _airspeed && _airspeed->use()) { // when flying straight use airspeed to get wind estimate if available Vector3f airspeed = _dcm_matrix.colx() * _airspeed->get_airspeed(); Vector3f wind = velocity - airspeed; _wind = _wind * 0.92 + wind * 0.08; } } // calculate the euler angles which will be used for high level // navigation control void AP_AHRS_DCM::euler_angles(void) { _dcm_matrix.to_euler(&roll, &pitch, &yaw); roll_sensor = degrees(roll) * 100; pitch_sensor = degrees(pitch) * 100; yaw_sensor = degrees(yaw) * 100; if (yaw_sensor < 0) yaw_sensor += 36000; } /* reporting of DCM state for MAVLink */ // average error_roll_pitch since last call float AP_AHRS_DCM::get_error_rp(void) { if (_error_rp_count == 0) { // this happens when telemetry is setup on two // serial ports return _error_rp_last; } _error_rp_last = _error_rp_sum / _error_rp_count; _error_rp_sum = 0; _error_rp_count = 0; return _error_rp_last; } // average error_yaw since last call float AP_AHRS_DCM::get_error_yaw(void) { if (_error_yaw_count == 0) { // this happens when telemetry is setup on two // serial ports return _error_yaw_last; } _error_yaw_last = _error_yaw_sum / _error_yaw_count; _error_yaw_sum = 0; _error_yaw_count = 0; return _error_yaw_last; } // return our current position estimate using // dead-reckoning or GPS bool AP_AHRS_DCM::get_position(struct Location *loc) { if (!_have_position) { return false; } loc->lat = _last_lat; loc->lng = _last_lng; location_offset(loc, _position_offset_north, _position_offset_east); return true; } // return an airspeed estimate if available bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret) { bool ret = false; if (_airspeed && _airspeed->use()) { *airspeed_ret = _airspeed->get_airspeed(); ret = true; } // estimate it via GPS speed and wind if (have_gps()) { *airspeed_ret = _last_airspeed; ret = true; } if (ret && _wind_max > 0 && _gps && _gps->status() == GPS::GPS_OK) { // constrain the airspeed by the ground speed // and AHRS_WIND_MAX *airspeed_ret = constrain(*airspeed_ret, _gps->ground_speed*0.01 - _wind_max, _gps->ground_speed*0.01 + _wind_max); } return ret; }