Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :    This program is free software: you can redistribute it and/or modify
       5             :    it under the terms of the GNU General Public License as published by
       6             :    the Free Software Foundation, either version 3 of the License, or
       7             :    (at your option) any later version.
       8             : 
       9             :    This program is distributed in the hope that it will be useful,
      10             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
      11             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      12             :    GNU General Public License for more details.
      13             : 
      14             :    You should have received a copy of the GNU General Public License
      15             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      16             :  */
      17             : 
      18             : /*
      19             :  *  DCM based AHRS (Attitude Heading Reference System) interface for
      20             :  *  ArduPilot
      21             :  *
      22             :  */
      23             : 
      24             : #include <AP_InertialSensor/AP_InertialSensor.h>
      25             : #include "AP_AHRS_config.h"
      26             : 
      27             : #if AP_AHRS_DCM_ENABLED
      28             : 
      29             : #include "AP_AHRS_Backend.h"
      30             : 
      31             : class AP_AHRS_DCM : public AP_AHRS_Backend {
      32             : public:
      33             : 
      34        2100 :     AP_AHRS_DCM(AP_Float &kp_yaw,
      35             :                 AP_Float &kp,
      36             :                 AP_Float &_gps_gain,
      37             :                 AP_Float &_beta,
      38             :                 AP_Enum<GPSUse> &gps_use,
      39             :                 AP_Int8 &gps_minsats)
      40        2100 :         : AP_AHRS_Backend(),
      41        2100 :           _kp_yaw(kp_yaw),
      42        2100 :           _kp(kp),
      43        2100 :           gps_gain(_gps_gain),
      44        2100 :           beta(_beta),
      45        2100 :           _gps_minsats(gps_minsats),
      46        2100 :           _gps_use(gps_use)
      47             :     {
      48        2100 :         _dcm_matrix.identity();
      49        2100 :     }
      50             : 
      51             :     /* Do not allow copies */
      52             :     CLASS_NO_COPY(AP_AHRS_DCM);
      53             : 
      54          58 :     const char *shortname() const override { return "DCM"; }
      55             : 
      56             :     // reset the current gyro drift estimate
      57             :     //  should be called if gyro offsets are recalculated
      58             :     void reset_gyro_drift() override;
      59             : 
      60             :     // Methods
      61             :     void            update() override;
      62             :     void            get_results(Estimates &results) override;
      63        2115 :     void            reset() override { reset(false); }
      64             : 
      65             :     // return true if yaw has been initialised
      66        5070 :     bool yaw_initialised(void) const {
      67        5070 :         return have_initial_yaw;
      68             :     }
      69             : 
      70             :     // status reporting
      71     2619228 :     float           get_error_rp() const {
      72     2619228 :         return _error_rp;
      73             :     }
      74     2619228 :     float           get_error_yaw() const {
      75     2619228 :         return _error_yaw;
      76             :     }
      77             : 
      78             : #if AP_AHRS_EXTERNAL_WIND_ESTIMATE_ENABLED
      79             :     void set_external_wind_estimate(float speed, float direction);
      80             : #endif
      81             : 
      82             :     // return an airspeed estimate if available. return true
      83             :     // if we have an estimate
      84             :     bool airspeed_EAS(float &airspeed_ret) const override;
      85             : 
      86             :     // return an airspeed estimate if available. return true
      87             :     // if we have an estimate from a specific sensor index
      88             :     bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override;
      89             : 
      90             :     bool            use_compass() override;
      91             : 
      92             :     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
      93             :     // requires_position should be true if horizontal position configuration should be checked (not used)
      94          30 :     bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override {
      95          30 :         return true;
      96             :     }
      97             : 
      98             :     // relative-origin functions for fallback in AP_InertialNav
      99             :     bool get_origin(Location &ret) const override;
     100             : 
     101             :     // return true if DCM has a yaw source
     102             :     bool yaw_source_available(void) const;
     103             : 
     104             : private:
     105             : 
     106             :     // Get a derivative of the vertical position in m/s which is kinematically consistent with the vertical position is required by some control loops.
     107             :     // 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.
     108             :     bool get_vert_pos_rate_D(float &velocity) const;
     109             : 
     110             :     bool get_velocity_NED(Vector3f &vec) const;
     111             :     // return a ground velocity estimate in meters/second, in North/East order
     112             :     Vector2f groundspeed_vector();
     113             : 
     114             :     // dead-reckoning support
     115             :     bool get_location(Location &loc) const;
     116             : 
     117             :     // settable parameters
     118             :     AP_Float &_kp_yaw;
     119             :     AP_Float &_kp;
     120             :     AP_Float &gps_gain;
     121             : 
     122             :     AP_Float &beta;
     123             : 
     124             :     AP_Int8 &_gps_minsats;
     125             : 
     126             :     AP_Enum<GPSUse> &_gps_use;
     127             : 
     128             :     // these are experimentally derived from the simulator
     129             :     // with large drift levels
     130             :     static constexpr float _ki = 0.0087f;
     131             :     static constexpr float _ki_yaw = 0.01f;
     132             : 
     133             :     // accelerometer values in the earth frame in m/s/s
     134             :     Vector3f        _accel_ef;
     135             : 
     136             :     // Methods
     137             :     void            matrix_update(void);
     138             :     void            normalize(void);
     139             :     void            check_matrix(void);
     140             :     bool            renorm(Vector3f const &a, Vector3f &result);
     141             :     void            drift_correction(float deltat);
     142             :     void            drift_correction_yaw(void);
     143             :     float           yaw_error_compass(class Compass &compass);
     144             :     bool            have_gps(void) const;
     145             :     bool            use_fast_gains(void) const;
     146             :     void            backup_attitude(void);
     147             : 
     148             :     // internal reset function.  Called externally, we never reset the
     149             :     // DCM matrix from the eulers.  Called internally we may.
     150             :     void            reset(bool recover_eulers);
     151             : 
     152             :     // update our wind estimate from the latest GPS velocity and attitude:
     153             :     void estimate_wind(void);
     154             : 
     155             :     // airspeed_ret: will always be filled-in by get_unconstrained_airspeed_EAS which fills in airspeed_ret in this order:
     156             :     //               airspeed as filled-in by an enabled airspeed sensor
     157             :     //               if no airspeed sensor: airspeed estimated using the GPS speed & wind_speed_estimation
     158             :     //               Or if none of the above, fills-in using the previous airspeed estimate
     159             :     // Return false: if we are using the previous airspeed estimate
     160             :     bool get_unconstrained_airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const;
     161             : 
     162             :     // primary representation of attitude of board used for all inertial calculations
     163             :     Matrix3f _dcm_matrix;
     164             : 
     165             :     // primary representation of attitude of flight vehicle body
     166             :     Matrix3f _body_dcm_matrix;
     167             : 
     168             :     // euler angles - used for recovering if the DCM
     169             :     // matrix becomes ill-conditioned and watchdog storage
     170             :     float roll;
     171             :     float pitch;
     172             :     float yaw;
     173             : 
     174             :     Vector3f _omega_P;                          // accel Omega proportional correction
     175             :     Vector3f _omega_yaw_P;                      // proportional yaw correction
     176             :     Vector3f _omega_I;                          // Omega Integrator correction
     177             :     Vector3f _omega_I_sum;
     178             :     float _omega_I_sum_time;
     179             :     Vector3f _omega;                            // Corrected Gyro_Vector data
     180             : 
     181             :     bool have_initial_yaw; // true if the yaw value has been initialised with a reference
     182             : 
     183             :     // variables to cope with delaying the GA sum to match GPS lag
     184             :     Vector3f ra_delayed(uint8_t instance, const Vector3f &ra);
     185             :     Vector3f _ra_delay_buffer[INS_MAX_INSTANCES];
     186             : 
     187             :     // P term gain based on spin rate
     188             :     float           _P_gain(float spin_rate);
     189             : 
     190             :     // P term yaw gain based on rate of change of horiz velocity
     191             :     float           _yaw_gain(void) const;
     192             : 
     193             :     /* returns true if attitude should be corrected from GPS-derived
     194             :      * velocity-deltas.  We turn this off for Copter and other similar
     195             :      * vehicles while the vehicle is disarmed to avoid the HUD bobbing
     196             :      * around while the vehicle is disarmed.
     197             :      */
     198             :     bool should_correct_centrifugal() const;
     199             : 
     200             :     // state to support status reporting
     201             :     float _renorm_val_sum;
     202             :     uint16_t _renorm_val_count;
     203             :     float _error_rp{1.0f};
     204             :     float _error_yaw{1.0f};
     205             : 
     206             :     // time in microseconds of last compass update
     207             :     uint32_t _compass_last_update;
     208             : 
     209             :     // time in millis when we last got a GPS heading
     210             :     uint32_t _gps_last_update;
     211             : 
     212             :     // GPS message time of the sample last fed to the wind estimator:
     213             :     uint32_t _last_wind_gps_ms;
     214             : 
     215             :     // state of accel drift correction
     216             :     Vector3f _ra_sum[INS_MAX_INSTANCES];
     217             :     Vector3f _last_velocity;
     218             :     float _ra_deltat;
     219             :     uint32_t _ra_sum_start;
     220             : 
     221             :     // which accelerometer instance is active
     222             :     uint8_t _active_accel_instance;
     223             : 
     224             :     // the earths magnetic field
     225             :     float _last_declination;
     226             :     Vector2f _mag_earth{1, 0};
     227             : 
     228             :     // whether we have GPS lock
     229             :     bool _have_gps_lock;
     230             : 
     231             :     // the lat/lng where we last had GPS lock
     232             :     int32_t _last_lat;
     233             :     int32_t _last_lng;
     234             :     uint32_t _last_pos_ms;
     235             : 
     236             :     // position offset from last GPS lock
     237             :     float _position_offset_north;
     238             :     float _position_offset_east;
     239             : 
     240             :     // whether we have a position estimate
     241             :     bool _have_position;
     242             : 
     243             :     // support for wind estimation
     244             :     Vector3f _last_fuse;
     245             :     Vector3f _last_vel;
     246             :     uint32_t _last_wind_time;
     247             :     float _last_airspeed_TAS;
     248             : 
     249             :     // time of last wind estimate update, used to rate-limit estimation:
     250             :     uint32_t _last_wind_estimate_ms;
     251             :     uint32_t _last_consistent_heading;
     252             : 
     253             :     // estimated wind in m/s
     254             :     Vector3f _wind;
     255             : 
     256             :     // last time AHRS failed in milliseconds
     257             :     uint32_t _last_failure_ms;
     258             : 
     259             :     // time when DCM was last reset
     260             :     uint32_t _last_startup_ms;
     261             : 
     262             :     // last origin we returned, for DCM fallback from EKF
     263             :     Location last_origin;
     264             : 
     265             :     // Declare filter states for HPF and LPF used by complementary
     266             :     // filter in AP_AHRS::groundspeed_vector
     267             :     Vector2f _lp; // ground vector low-pass filter
     268             :     Vector2f _hp; // ground vector high-pass filter
     269             :     Vector2f _lastGndVelADS; // previous HPF input
     270             : 
     271             :     // pre-calculated trig cache:
     272             :     float _sin_yaw;
     273             :     float _cos_yaw;
     274             : 
     275             :     uint32_t last_log_ms;
     276             : };
     277             : 
     278             : #endif  // AP_AHRS_DCM_ENABLED