diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 58c195549c..751d410859 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1,22 +1,19 @@ /* - APM_DCM.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega - Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com + APM_AHRS_DCM.cpp - This library works with the ArduPilot Mega and "Oilpan" + AHRS system using DCM matrices - 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. + Based on DCM code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com - Methods: - update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data - get_gyro() : Returns gyro vector corrected for bias - get_accel() : Returns accelerometer vector - get_dcm_matrix() : Returns dcm matrix + 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 +#include // this is the speed in cm/s above which we first get a yaw lock with // the GPS @@ -26,21 +23,11 @@ // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN #define GPS_SPEED_RESET 100 -void -AP_DCM::set_compass(Compass *compass) -{ - _compass = compass; -} - - // run a full DCM update round -// the drift_correction_frequency is how many steps of the algorithm -// to run before doing an accelerometer and yaw drift correction step void -AP_DCM::update_DCM(uint8_t drift_correction_frequency) +AP_AHRS_DCM::update(void) { float delta_t; - Vector3f accel; // tell the IMU to grab some data _imu->update(); @@ -50,43 +37,16 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency) // Get current values for gyros _gyro_vector = _imu->get_gyro(); - - // accumulate some more accelerometer data - accel = _imu->get_accel(); - _accel_sum += accel; - _drift_correction_time += delta_t; + _accel_vector = _imu->get_accel(); // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); - // add up the omega vector so we pass a value to the drift - // correction averaged over the same time period as the - // accelerometers - _omega_sum += _omega_integ_corr; - // Normalize the DCM matrix normalize(); - // see if we will perform drift correction on this call - _drift_correction_count++; - - if (_drift_correction_count >= drift_correction_frequency) { - // calculate the average accelerometer vector over - // since the last drift correction call - float scale = 1.0 / _drift_correction_count; - _accel_vector = _accel_sum * scale; - _accel_sum.zero(); - - // calculate the average omega value over this time - _omega_smoothed = _omega_sum * scale; - _omega_sum.zero(); - - // Perform drift correction - drift_correction(_drift_correction_time); - - _drift_correction_time = 0; - _drift_correction_count = 0; - } + // Perform drift correction + drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); @@ -97,7 +57,7 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency) // update the DCM matrix using only the gyros void -AP_DCM::matrix_update(float _G_Dt) +AP_AHRS_DCM::matrix_update(float _G_Dt) { // _omega_integ_corr is used for _centripetal correction // (theoretically better than _omega) @@ -132,7 +92,7 @@ AP_DCM::matrix_update(float _G_Dt) // adjust an accelerometer vector for known acceleration forces void -AP_DCM::accel_adjust(Vector3f &accel) +AP_AHRS_DCM::accel_adjust(Vector3f &accel) { float veloc; // compensate for linear acceleration. This makes a @@ -149,8 +109,8 @@ AP_DCM::accel_adjust(Vector3f &accel) // direction as positive // Equation 26 broken up into separate pieces - accel.y -= _omega_smoothed.z * veloc; - accel.z += _omega_smoothed.y * veloc; + accel.y -= _omega_integ_corr.z * veloc; + accel.z += _omega_integ_corr.y * veloc; } /* @@ -158,7 +118,7 @@ AP_DCM::accel_adjust(Vector3f &accel) extreme errors in the matrix */ void -AP_DCM::matrix_reset(bool recover_eulers) +AP_AHRS_DCM::reset(bool recover_eulers) { if (_compass != NULL) { _compass->null_offsets_disable(); @@ -169,7 +129,6 @@ AP_DCM::matrix_reset(bool recover_eulers) _omega_P.zero(); _omega_yaw_P.zero(); _omega_integ_corr.zero(); - _omega_smoothed.zero(); _omega.zero(); // if the caller wants us to try to recover to the current @@ -193,13 +152,13 @@ AP_DCM::matrix_reset(bool recover_eulers) check the DCM matrix for pathological values */ void -AP_DCM::check_matrix(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++; - matrix_reset(true); + reset(true); return; } // some DCM matrix values can lead to an out of range error in @@ -221,7 +180,7 @@ AP_DCM::check_matrix(void) SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", _dcm_matrix.c.x); renorm_blowup_count++; - matrix_reset(true); + reset(true); } } } @@ -229,7 +188,7 @@ AP_DCM::check_matrix(void) // renormalise one vector component of the DCM matrix // this will return false if renormalization fails bool -AP_DCM::renorm(Vector3f const &a, Vector3f &result) +AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result) { float renorm_val; @@ -289,7 +248,7 @@ simple matter to stay ahead of it. We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ. */ void -AP_DCM::normalize(void) +AP_AHRS_DCM::normalize(void) { float error; Vector3f t0, t1, t2; @@ -305,7 +264,7 @@ AP_DCM::normalize(void) !renorm(t2, _dcm_matrix.c)) { // Our solution is blowing up and we will force back // to last euler angles - matrix_reset(true); + reset(true); } } @@ -319,7 +278,7 @@ AP_DCM::normalize(void) // This function also updates _omega_yaw_P with a yaw correction term // from our yaw reference vector void -AP_DCM::drift_correction(float deltat) +AP_AHRS_DCM::drift_correction(float deltat) { float error_course = 0; Vector3f accel; @@ -530,7 +489,7 @@ AP_DCM::drift_correction(float deltat) // calculate the euler angles which will be used for high level // navigation control void -AP_DCM::euler_angles(void) +AP_AHRS_DCM::euler_angles(void) { _dcm_matrix.to_euler(&roll, &pitch, &yaw); @@ -544,27 +503,8 @@ AP_DCM::euler_angles(void) /* reporting of DCM state for MAVLink */ -// average accel_weight since last call -float AP_DCM::get_accel_weight(void) -{ - return 1.0; -} - -// average renorm_val since last call -float AP_DCM::get_renorm_val(void) -{ - float ret; - if (_renorm_val_count == 0) { - return 0; - } - ret = _renorm_val_sum / _renorm_val_count; - _renorm_val_sum = 0; - _renorm_val_count = 0; - return ret; -} - // average error_roll_pitch since last call -float AP_DCM::get_error_rp(void) +float AP_AHRS_DCM::get_error_rp(void) { float ret; if (_error_rp_count == 0) { @@ -577,7 +517,7 @@ float AP_DCM::get_error_rp(void) } // average error_yaw since last call -float AP_DCM::get_error_yaw(void) +float AP_AHRS_DCM::get_error_yaw(void) { float ret; if (_error_yaw_count == 0) { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 2a0bfa2157..ee5b3403ac 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -1,85 +1,44 @@ -#ifndef AP_DCM_h -#define AP_DCM_h - -// temporarily include all other classes here -// since this naming is a bit off from the -// convention and the AP_DCM should be the top -// header file -#include "AP_DCM_HIL.h" - -#include "../FastSerial/FastSerial.h" -#include "../AP_Math/AP_Math.h" -#include -#include "../AP_Compass/AP_Compass.h" -#include "../AP_ADC/AP_ADC.h" -#include "../AP_GPS/AP_GPS.h" -#include "../AP_IMU/AP_IMU.h" -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - -class AP_DCM +#ifndef AP_AHRS_DCM_H +#define AP_AHRS_DCM_H +/* + DCM based AHRS (Attitude Heading Reference System) interface for + ArduPilot + + 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. +*/ + +class AP_AHRS_DCM : public AP_AHRS { public: // Constructors - AP_DCM(IMU *imu, GPS *&gps) : - _kp_roll_pitch(0.13), - _kp_yaw(0.4), - _gps(gps), - _imu(imu), - _dcm_matrix(1, 0, 0, - 0, 1, 0, - 0, 0, 1), - _health(1.), - _toggle(0) + AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps) { - // base the ki values by the sensors maximum drift - // rate. The APM2 has gyros which are much less drift - // prone than the APM1, so we should have a lower ki, - // which will make us less prone to increasing omegaI - // incorrectly due to sensor noise - _gyro_drift_limit = imu->get_gyro_drift_rate(); + _kp_roll_pitch = 0.13; + _kp_yaw = 0.4; + _dcm_matrix(Vector3f(1, 0, 0), + Vector3f(0, 1, 0), + Vector3f(0, 0, 1)); + + // base the ki values on the sensors drift rate _ki_roll_pitch = _gyro_drift_limit * 5; _ki_yaw = _gyro_drift_limit * 8; } - // Accessors - // return the smoothed gyro vector corrected for drift - Vector3f get_gyro(void) {return _omega_smoothed; } + Vector3f get_gyro(void) {return _omega; } Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } - Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();} // return the current drift correction integrator value Vector3f get_gyro_drift(void) {return _omega_I; } - float get_health(void) {return _health;} - void set_centripetal(bool b) {_centripetal = b;} - bool get_centripetal(void) {return _centripetal;} - void set_compass(Compass *compass); - // Methods - void update_DCM(uint8_t drift_correction_frequency=1); - void update(void) { update_DCM(); } - void matrix_reset(bool recover_eulers = false); - - long roll_sensor; // Degrees * 100 - long pitch_sensor; // Degrees * 100 - long yaw_sensor; // Degrees * 100 - - float roll; // Radians - float pitch; // Radians - float yaw; // Radians - - uint8_t gyro_sat_count; - uint8_t renorm_range_count; - uint8_t renorm_blowup_count; + void update(void); + void reset(bool recover_eulers = false); // status reporting - float get_accel_weight(void); - float get_renorm_val(void); float get_error_rp(void); float get_error_yaw(void); @@ -88,13 +47,10 @@ private: float _ki_roll_pitch; float _kp_yaw; float _ki_yaw; - float _gyro_drift_limit; // radians/s/s bool _have_initial_yaw; // Methods - void read_adc_raw(void); void accel_adjust(Vector3f &accel); - float read_adc(int select); void matrix_update(float _G_Dt); void normalize(void); void check_matrix(void); @@ -102,34 +58,17 @@ private: void drift_correction(float deltat); void euler_angles(void); - // members - Compass * _compass; - - // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing - // IMU under us without our noticing. - GPS *&_gps; // note: this is a reference to a pointer owned by the caller - - IMU *_imu; - + // primary representation of attitude Matrix3f _dcm_matrix; - // sum of accel vectors between drift_correction() calls - // this allows the drift correction to run at a different rate - // to the main DCM update code - Vector3f _accel_vector; - Vector3f _accel_sum; - Vector3f _gyro_vector; // Store the gyros turn rate in a vector + Vector3f _accel_vector; // current accel vector + Vector3f _omega_P; // accel Omega Proportional correction Vector3f _omega_yaw_P; // yaw Omega Proportional correction Vector3f _omega_I; // Omega Integrator correction Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction Vector3f _omega; // Corrected Gyro_Vector data - Vector3f _omega_sum; - Vector3f _omega_smoothed; - float _health; - bool _centripetal; - uint8_t _toggle; // state to support status reporting float _renorm_val_sum; @@ -139,16 +78,8 @@ private: float _error_yaw_sum; uint16_t _error_yaw_count; - // time in micros when we last got a compass fix - uint32_t _compass_last_update; - // time in millis when we last got a GPS heading uint32_t _gps_last_update; - - // counter of calls to update_DCM() without drift correction - uint8_t _drift_correction_count; - float _drift_correction_time; - }; -#endif +#endif // AP_AHRS_DCM_H