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             :  *  AHRS (Attitude Heading Reference System) interface for ArduPilot
      20             :  *
      21             :  */
      22             : 
      23             : #include <AP_Math/AP_Math.h>
      24             : #include <inttypes.h>
      25             : #include <AP_Airspeed/AP_Airspeed.h>
      26             : #include <AP_InertialSensor/AP_InertialSensor.h>
      27             : #include <AP_Common/Location.h>
      28             : #include <AP_NavEKF/AP_NavEKF_Source.h>
      29             : #include <AP_NavEKF/AP_Nav_Common.h>
      30             : #include "AP_AHRS_config.h"
      31             : #include <AP_Compass/AP_Compass_config.h>
      32             : 
      33             : #define AP_AHRS_TRIM_LIMIT 10.0f        // maximum trim angle in degrees
      34             : #define AP_AHRS_RP_P_MIN   0.05f        // minimum value for AHRS_RP_P parameter
      35             : #define AP_AHRS_YAW_P_MIN  0.05f        // minimum value for AHRS_YAW_P parameter
      36             : 
      37             : enum class GPSUse : uint8_t {
      38             :     Disable = 0,
      39             :     Enable  = 1,
      40             :     EnableWithHeight = 2,
      41             : };
      42             : 
      43             : class AP_AHRS_Backend
      44             : {
      45             : public:
      46             : 
      47             :     // Constructor
      48       10494 :     AP_AHRS_Backend() {}
      49             : 
      50             :     // empty virtual destructor
      51        2294 :     virtual ~AP_AHRS_Backend() {}
      52             : 
      53             :     CLASS_NO_COPY(AP_AHRS_Backend);
      54             : 
      55             :     virtual const char *shortname() const = 0;
      56             : 
      57             :     // structure to retrieve results from backends:
      58             :     struct Estimates {
      59             :         // allow backends to set the private members:
      60             :         friend class AP_AHRS_DCM;
      61             :         friend class AP_AHRS_SIM;
      62             :         friend class AP_AHRS_External;
      63             :         friend class AP_AHRS_NavEKF2;
      64             :         friend class AP_AHRS_NavEKF3;
      65             : 
      66             :         // is the AHRS subsystem healthy?
      67             :         bool healthy;
      68             : 
      69             :         // true if the AHRS has completed initialisation
      70             :         bool initialised;
      71             : 
      72             :         // filter fault status bitmask (see NavFilterFaultBit); non-zero
      73             :         // if the backend is reporting one or more filter faults.
      74             :         // Backends with no concept of filter faults leave this zero.
      75             :         uint16_t filter_faults;
      76             : 
      77             :         // inertial sensor information
      78             :         uint8_t primary_gyro;
      79             : 
      80             :         // if attitude_valid is true then all of the
      81             :         // eulers/quaternion/matrix must be valid:
      82             :         bool attitude_valid;
      83             :         float roll_rad;
      84             :         float pitch_rad;
      85             :         float yaw_rad;
      86             :         Matrix3f dcm_matrix;
      87             :         Quaternion quaternion;
      88             :         uint16_t attitude_reset_count;  // counter incremented each time a sudden shift happens in attitude
      89             :         uint16_t yaw_reset_count;  // incremented when a sudden shift happens in yaw
      90             : 
      91             :         // true when the state estimates are significantly degraded by vibration:
      92             :         bool is_vibration_affected;
      93             : 
      94             :         // backends must always return the result in the vehicle body
      95             :         // frame.  A backend using the autopilot sensors will need to
      96             :         // rotate according to the TRIM parameters.  An ExternalAHRS
      97             :         // won't!
      98    41686716 :         bool get_quaternion(Quaternion &quat) const {
      99    41686716 :             quat = quaternion;
     100    41686716 :             return attitude_valid;
     101             :         }
     102             : 
     103             :         Vector3f gyro_estimate;
     104             :         Vector3f gyro_drift;
     105             : 
     106             :         uint8_t primary_accel;
     107             : 
     108             :         Vector3f accel_ef;
     109             :         Vector3f accel_bias;
     110             : 
     111             :         // a ground velocity in meters/second, North/East/Down
     112             :         Vector3f velocity_NED;
     113             :         bool velocity_NED_valid;
     114             :         // return a ground velocity in meters/second, North/East/Down
     115    41686716 :         bool get_velocity_NED(Vector3f &vel) const WARN_IF_UNUSED {
     116    41686716 :             if (!velocity_NED_valid) {
     117     1258309 :                 return false;
     118             :             }
     119    40428407 :             vel = velocity_NED;
     120    40428407 :             return true;
     121             :         };
     122             :         // ground velocity estimate in meters/second, in North/East order
     123             :         Vector2f velocity_NE;
     124             : 
     125             :         // a derivative of the vertical position in m/s which is
     126             :         // kinematically consistent with the vertical position is
     127             :         // required by some control loops.
     128             :         // This is different to the vertical velocity from the EKF
     129             :         // which is not always consistent with the vertical position
     130             :         // due to the various errors that are being corrected for.
     131             :         float vert_pos_rate_D;
     132             :         bool vert_pos_rate_D_valid;
     133             :         // Get a derivative of the vertical position in m/s which is
     134             :         // kinematically consistent with the vertical position is
     135             :         // required by some control loops.
     136             :         // This is different to the vertical velocity from the EKF
     137             :         // which is not always consistent with the vertical position
     138             :         // due to the various errors that are being corrected for.
     139     2879287 :         bool get_vert_pos_rate_D(float &velocity) const {
     140     2879287 :             if (!vert_pos_rate_D_valid) {
     141       10653 :                 return false;
     142             :             }
     143     2868634 :             velocity = vert_pos_rate_D;
     144     2868634 :             return true;
     145             :         }
     146             : 
     147             :         /*
     148             :          * position estimates
     149             :          */
     150             :         Location location;
     151             :         bool location_valid;
     152             : 
     153    42168335 :         bool get_location(Location &loc) const {
     154    42168335 :             loc = location;
     155    42168335 :             return location_valid;
     156             :         };
     157             : 
     158             :         /*
     159             :          * origin-relative functions
     160             :          */
     161             :         bool provides_common_origin;
     162             : 
     163             :         // origin-relative position:
     164             :         Vector2p position_NE;
     165             :         bool position_NE_valid;  // true if position_NE is valid
     166             :         uint16_t position_NE_reset_count;  // incremented when a sudden shift happens in position
     167             : 
     168             :         postype_t position_D;
     169             :         bool position_D_valid;   // true if position_D is valid
     170             :         uint16_t position_D_reset_count;  // incremented when a sudden shift happens in position (down)
     171             : 
     172     2473248 :         bool get_hagl(float &height) const WARN_IF_UNUSED {
     173     2473248 :             height = hagl;
     174     2473248 :             return hagl_valid;
     175             :         }
     176             : 
     177             :         /*
     178             :          * air data estimates
     179             :          */
     180             :         // estimated wind in m/s, NED frame
     181             :         Vector3f wind;
     182             :         bool wind_valid;
     183             : 
     184             :         /*
     185             :          * Sensor-related information
     186             :          */
     187             : 
     188             :         // configured_to_use_gps will be true if the estimator will
     189             :         // use GPS data in creating its estimate when the data is good
     190             :         bool configured_to_use_gps;
     191             :         // configured_to_use_gps_for_pos_XY will be true if GPS is
     192             :         // configured as the horizontal position source for this
     193             :         // estimator.  Used to decide whether GPS will set the
     194             :         // navigation origin.
     195             :         bool configured_to_use_gps_for_pos_XY;
     196             : 
     197             :         // if true, "external navigation" (as provided by MAVLink
     198             :         // messages) is providing the yaw estimate.  e.g. a T265
     199             :         // vision-position-estimate camera tracking yaw via image
     200             :         // recognition
     201             :         bool using_extnav_for_yaw;
     202             : 
     203             :         // if true then however the yaw in these estimates was derived
     204             :         // a compass was not involved:
     205             :         bool using_noncompass_for_yaw;
     206             : 
     207             : #if AP_AHRS_GET_MAG_DATA_ENABLED
     208             :         // estimators can provide their predicted magnetic fields:
     209             :         Vector3f mag_field_NED;
     210             :         bool mag_field_NED_valid;
     211             :         Vector3f mag_field_corrections;
     212             :         bool mag_field_corrections_valid;
     213             : #endif // AP_AHRS_GET_MAG_DATA_ENABLED
     214             : 
     215             :         /*
     216             :          * filter status and estimates quality values:
     217             :          */
     218             :         nav_filter_status filter_status;
     219             :         bool filter_status_valid;
     220             : 
     221             :         // the innovations normalised using the innovation variance
     222             :         // where a value of 0 indicates perfect consistency between
     223             :         // the measurement and the EKF solution and a value of 1 is
     224             :         // the maximum inconsistency that will be accepted by the
     225             :         // filter:
     226             :         float velVar, posVar, hgtVar, tasVar;
     227             :         Vector3f magVar;
     228             :         bool variances_valid;
     229             : 
     230             :         float terrain_alt_variance;
     231             :         bool terrain_alt_variance_valid;
     232             : 
     233             :         // impositions on control placed on it by the estimator.  So,
     234             :         // for example, if Optical flow is in play then perhaps we
     235             :         // can't travel as fast:
     236             :         float control_ground_speed_limit_ms;
     237             :         // or we want to scale down the control magnitudes:
     238             :         float control_gain_scaler_XY;
     239             :         float control_gain_scaler_Z;
     240             : 
     241             :         // hgt_ctrl_limit - get maximum height to be observed by the control loops in metres and a validity flag
     242             :         // this is used to limit height during optical flow navigation
     243             :         float control_height_limit_m;
     244             :         bool control_height_limit_valid; // false when no limiting is required
     245             : 
     246             :     private:
     247             :         bool hagl_valid;
     248             :         float hagl;
     249             :     };
     250             : 
     251             :     // Methods
     252             :     virtual void update() = 0;
     253             : 
     254             :     virtual void get_results(Estimates &results) = 0;
     255             : 
     256             :     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
     257             :     // requires_position should be true if horizontal position configuration should be checked
     258             :     virtual bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const = 0;
     259             : 
     260             :     // see if EKF lane switching is possible to avoid EKF failsafe
     261           7 :     virtual void check_lane_switch(void) {}
     262             : 
     263             :     // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
     264           0 :     virtual void request_yaw_reset(void) {}
     265             : 
     266             :     // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
     267           0 :     virtual void set_posvelyaw_source_set(AP_NavEKF_Source::SourceSetSelection source_set_idx) {}
     268             : 
     269             :     // reset the current gyro drift estimate
     270             :     //  should be called if gyro offsets are recalculated
     271             :     virtual void reset_gyro_drift(void) = 0;
     272             : 
     273             :     // reset the current attitude, used on new IMU calibration
     274             :     virtual void reset() = 0;
     275             : 
     276             :     // return an airspeed estimate if available. return true
     277             :     // if we have an estimate
     278           0 :     virtual bool airspeed_EAS(float &airspeed_ret) const WARN_IF_UNUSED { return false; }
     279           0 :     virtual bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const { return false; }
     280             : 
     281             :     // return a true airspeed estimate (navigation airspeed) if
     282             :     // available. return true if we have an estimate
     283    43673294 :     bool airspeed_TAS(float &airspeed_ret) const WARN_IF_UNUSED {
     284    43673294 :         if (!airspeed_EAS(airspeed_ret)) {
     285    34286394 :             return false;
     286             :         }
     287     9386900 :         airspeed_ret *= get_EAS2TAS();
     288     9386900 :         return true;
     289             :     }
     290             : 
     291             :     // get apparent to true airspeed ratio
     292             :     static float get_EAS2TAS(void);
     293    34447442 :     static float get_TAS2EAS(void) { return 1.0/get_EAS2TAS(); }
     294             : 
     295             :     // return true if airspeed comes from an airspeed sensor, as
     296             :     // opposed to an IMU estimate
     297      769857 :     static bool airspeed_sensor_enabled(void) {
     298             :     #if AP_AIRSPEED_ENABLED
     299      769857 :         const AP_Airspeed *_airspeed = AP::airspeed();
     300      769857 :         return _airspeed != nullptr && _airspeed->use() && _airspeed->healthy();
     301             :     #else
     302             :         return false;
     303             :     #endif
     304             :     }
     305             : 
     306             :     // return true if airspeed comes from a specific airspeed sensor, as
     307             :     // opposed to an IMU estimate
     308   126587281 :     static bool airspeed_sensor_enabled(uint8_t airspeed_index) {
     309             :     #if AP_AIRSPEED_ENABLED
     310   126587281 :         const AP_Airspeed *_airspeed = AP::airspeed();
     311   126587281 :         return _airspeed != nullptr && _airspeed->use(airspeed_index) && _airspeed->healthy(airspeed_index);
     312             :     #else
     313             :         return false;
     314             :     #endif
     315             :     }
     316             : 
     317        1790 :     virtual bool set_origin(const Location &loc) {
     318        1790 :         return false;
     319             :     }
     320             :     virtual bool get_origin(Location &ret) const = 0;
     321             : 
     322             :     // return true if we will use compass for yaw
     323             :     virtual bool use_compass(void) = 0;
     324             : 
     325             : #if AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
     326             :     // return the compass offsets this backend has estimated for a
     327             :     // compass instance, in body frame, milligauss; returns true if the
     328             :     // offsets are valid.  Backends which do not estimate compass
     329             :     // offsets need not override this.
     330           0 :     virtual bool get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const {
     331           0 :         return false;
     332             :     }
     333             : #endif  // AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
     334             : 
     335             :     // Resets the baro so that it reads zero at the current height
     336             :     // Resets the EKF height to zero
     337             :     // Adjusts the EKf origin height so that the EKF height + origin height is the same as before
     338        9363 :     virtual void resetHeightDatum(void) { }
     339             : 
     340             :     // return the innovations for the specified instance
     341             :     // An out of range instance (eg -1) returns data for the primary instance
     342         403 :     virtual bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const {
     343         403 :         return false;
     344             :     }
     345             : };
     346             : 
     347             : // Converts an upstream "something changed" key (an EKF reset
     348             : // count or timestamp, or another counter's count) into a monotonic
     349             : // local count.  KeyT is the upstream key type.
     350             : template <typename KeyT>
     351             : class AP_AHRS_ResetCounter {
     352             : public:
     353             :     // bump count if the upstream key changed.  Returns true if a
     354             :     // reset was recorded.
     355   332361800 :     bool update(KeyT new_key) {
     356   332361800 :         if (new_key == last_key) {
     357   332333339 :             return false;
     358             :         }
     359       28461 :         fill(new_key);
     360       28461 :         return true;
     361             :     }
     362             : 
     363             :     // unconditionally record a reset, re-seating the key so the next
     364             :     // update() doesn't double-count.  Used when the active estimator
     365             :     // changes.
     366       34581 :     void fill(KeyT new_key) {
     367       34581 :         last_key = new_key;
     368       34581 :         reset_count++;
     369       34581 :     }
     370             : 
     371   468373736 :     uint16_t count() const { return reset_count; }
     372             : 
     373             : private:
     374             :     KeyT last_key;
     375             :     uint16_t reset_count;
     376             : };