Line data    Source code

       1             : #include "AP_AHRS_config.h"
       2             : 
       3             : #if AP_AHRS_NAVEKF3_ENABLED
       4             : 
       5             : #include "AP_AHRS_NavEKF3.h"
       6             : #include <AP_AHRS/AP_AHRS.h>
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <AP_Logger/AP_Logger.h>
       9             : #include <GCS_MAVLink/GCS.h>
      10             : 
      11             : extern const AP_HAL::HAL& hal;
      12             : 
      13             : NavEKF3 AP_AHRS_NavEKF3::EKF3;
      14             : 
      15     2389898 : bool AP_AHRS_NavEKF3::start()
      16             : {
      17     2389898 :     const auto now_ms = AP_HAL::millis();
      18             : 
      19     2389898 :     if (start_time_ms == 0) {
      20        2086 :         start_time_ms = now_ms;
      21             :     }
      22             : 
      23             : #if HAL_LOGGING_ENABLED
      24             :     // if we're doing Replay logging then don't allow any data
      25             :     // into the EKF yet.  Don't allow it to block us for long.
      26     2389898 :     if (!hal.util->was_watchdog_reset()) {
      27     2389416 :         if (now_ms - start_time_ms < 5000) {
      28     1184019 :             if (!AP::logger().allow_start_ekf()) {
      29          43 :                 return false;
      30             :             }
      31             :         }
      32             :     }
      33             : #endif
      34             : 
      35             :     // wait 1 second for DCM to output a valid tilt error estimate
      36             :     // FIXME: work out whether this is still required!
      37     2389855 :     if (now_ms - start_time_ms <= 1000) {
      38      530414 :         return false;
      39             :     }
      40             : 
      41             :     // try to start the filter:
      42     1859441 :     return EKF3.InitialiseFilter();
      43             : }
      44             : 
      45    41686716 : void AP_AHRS_NavEKF3::update()
      46             : {
      47    41686716 :     if (!started) {
      48     2389898 :         started = start();
      49             :     }
      50    41686716 :     if (!started) {
      51     2388344 :         return;
      52             :     }
      53    39298372 :     EKF3.UpdateFilter();
      54             : 
      55             :     // check the current primary core; if it has changed then assume
      56             :     // our attitude is reset:
      57    39298372 :     const int8_t primary_core = EKF3.getPrimaryCoreIndex();
      58    39298372 :     if (attitude_reset_tracker.update(primary_core)) {
      59          68 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(primary_core));
      60          68 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 primary changed:%d", (unsigned)primary_core);
      61             :     }
      62             : 
      63    39298372 :     yaw_reset_tracker.update(EKF3.getYawResetCount());
      64             : 
      65    39298372 :     position_NE_reset_tracker.update(EKF3.getPosNorthEastResetCount());
      66             : 
      67    39298372 :     position_D_reset_tracker.update(EKF3.getPosDownResetCount());
      68             : }
      69             : 
      70    41686716 : void AP_AHRS_NavEKF3::get_results(AP_AHRS_Backend::Estimates &results)
      71             : {
      72    41686716 :     const auto now_ms = AP_HAL::millis();
      73             : 
      74             :     // initialisation complete some time after ekf has started
      75    41686716 :     results.initialised = (started && (now_ms - start_time_ms > 20000));
      76             : 
      77    41686716 :     results.healthy = started && EKF3.healthy();
      78             : 
      79    41686716 :     const AP_InertialSensor &_ins = AP::ins();
      80             : 
      81             :     /*
      82             :      * rotational rate estimates:
      83             :      */
      84             :     // The backend should tell us what gyro it is using right now.
      85             :     // There's no API for that at the moment, so we assume the
      86             :     // gyro corresponds to the IMU index.
      87    41686716 :     const int8_t ek3_primary_gyro = EKF3.getPrimaryCoreIMUIndex();
      88    41686716 :     if (ek3_primary_gyro == -1) {
      89     1633697 :         results.primary_gyro = _ins.get_first_usable_gyro();
      90             :     } else {
      91    40053019 :         results.primary_gyro = ek3_primary_gyro;
      92             :     }
      93             : 
      94             :     // get gyro bias for primary EKF and change sign to give gyro drift
      95             :     // Note sign convention used by EKF is bias = measurement - truth
      96    41686716 :     Vector3f drift;
      97    41686716 :     EKF3.getGyroBias(-1, drift);
      98    41686716 :     results.gyro_drift = -drift;
      99             : 
     100             :     // use the same IMU as the primary EKF and correct for gyro drift
     101    41686716 :     results.gyro_estimate = _ins.get_gyro(results.primary_gyro) + results.gyro_drift;
     102             : 
     103             :     /*
     104             :      * attitude estimates:
     105             :      */
     106    41686716 :     EKF3.getRotationBodyToNED(results.dcm_matrix);
     107             : 
     108    41686716 :     Vector3f eulers;
     109    41686716 :     EKF3.getEulerAngles(eulers);
     110    41686716 :     results.roll_rad  = eulers.x;
     111    41686716 :     results.pitch_rad = eulers.y;
     112    41686716 :     results.yaw_rad   = eulers.z;
     113             : 
     114    41686716 :     EKF3.getQuaternion(results.quaternion);
     115    41686716 :     results.quaternion.rotate(-AP::ahrs().get_trim());
     116             : 
     117    41686716 :     results.attitude_valid = started;
     118             : 
     119    41686716 :     results.attitude_reset_count = attitude_reset_tracker.count();
     120             : 
     121    41686716 :     results.yaw_reset_count = yaw_reset_tracker.count();
     122             : 
     123    41686716 :     results.is_vibration_affected = EKF3.isVibrationAffected();
     124             : 
     125             :     /*
     126             :      * acceleration estimates
     127             :      */
     128             : 
     129             :     // The backend should tell us what accelerometer it is using
     130             :     // right now.  There's no API for that at the moment, so we assume
     131             :     // the accelerometer corresponds to the IMU index.
     132    41686716 :     const int8_t ek3_primary_accel = EKF3.getPrimaryCoreIMUIndex();
     133    41686716 :     if (ek3_primary_accel == -1) {
     134     1633697 :         results.primary_accel = _ins.get_first_usable_accel();
     135             :     } else {
     136    40053019 :         results.primary_accel = ek3_primary_accel;
     137             :     }
     138             : 
     139             :     // get 3-axis accel bias estimates for active EKF (this is usually
     140             :     // for the primary IMU)
     141    41686716 :     EKF3.getAccelBias(-1, results.accel_bias);
     142             : 
     143             :     // use the primary IMU for accel earth frame
     144    41686716 :     Vector3f accel = _ins.get_accel(results.primary_accel);
     145    41686716 :     accel -= results.accel_bias;
     146    41686716 :     results.accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
     147             : 
     148             :     /*
     149             :      * velocity estimates
     150             :      */
     151    41686716 :     EKF3.getVelNED(results.velocity_NED);
     152    41686716 :     results.velocity_NED_valid = true;
     153    41686716 :     results.vert_pos_rate_D = EKF3.getPosDownDerivative();
     154    41686716 :     results.vert_pos_rate_D_valid = true;
     155             : 
     156             :     // ground velocity estimate in meters/second, in North/East order
     157    41686716 :     results.velocity_NE = results.velocity_NED.xy();
     158             : 
     159             :     /*
     160             :      * position estimates
     161             :      */
     162    41686716 :     results.location_valid = EKF3.getLLH(results.location);
     163             : 
     164             :     // origin-relative functions
     165    41686716 :     results.provides_common_origin = true;
     166             : 
     167             :     // origin-relative position:
     168    41686716 :     results.position_NE_valid = EKF3.getPosNE(results.position_NE);
     169    41686716 :     results.position_NE_reset_count = position_NE_reset_tracker.count();
     170             : 
     171    41686716 :     results.position_D_valid = EKF3.getPosD(results.position_D);
     172    41686716 :     results.position_D_reset_count = position_D_reset_tracker.count();
     173             : 
     174    41686716 :     results.hagl_valid = EKF3.getHAGL(results.hagl);
     175             : 
     176             :     /*
     177             :      * air data estimates
     178             :      */
     179    41686716 :     results.wind_valid = EKF3.getWind(results.wind);
     180             : 
     181             :     /*
     182             :      * Sensor-related information
     183             :      */
     184             :     // true if the estimator will use GPS data in creating its
     185             :     // estimate when the data is good:
     186    41686716 :     results.configured_to_use_gps = EKF3.using_gps();
     187             :     // true if GPS is configured as the horizontal position source
     188             :     // for this estimator.  Used to decide whether GPS will set
     189             :     // the navigation origin:
     190    41686716 :     results.configured_to_use_gps_for_pos_XY = EKF3.configuredToUseGPSForPosXY();
     191             : 
     192             :     // are we consuming yaw from an external (e.g. vision-based) source?
     193    41686716 :     results.using_extnav_for_yaw = EKF3.using_extnav_for_yaw();
     194             : 
     195             :     // are we consuming yaw from a source which is *not* a compass
     196             :     // (e.g. the GSF)
     197    41686716 :     results.using_noncompass_for_yaw = EKF3.using_noncompass_for_yaw();
     198             : 
     199             : #if AP_AHRS_GET_MAG_DATA_ENABLED
     200             :     // estimators can provide their predicted magnetic fields:
     201    41686716 :     EKF3.getMagNED(results.mag_field_NED);
     202    41686716 :     results.mag_field_NED_valid = true;
     203    41686716 :     EKF3.getMagXYZ(results.mag_field_corrections);
     204    41686716 :     results.mag_field_corrections_valid = true;
     205             : #endif  // AP_AHRS_GET_MAG_DATA_ENABLED
     206             : 
     207             :     /*
     208             :      * filter status and estimates quality values:
     209             :      */
     210    41686716 :     EKF3.getFilterStatus(results.filter_status);
     211    41686716 :     results.filter_status_valid = true;
     212             : 
     213    41686716 :     EKF3.getFilterFaults(results.filter_faults);
     214             : 
     215             :     // provides the innovations normalised between 0 and 1:
     216    41686716 :     Vector2f offset;
     217    41686716 :     results.variances_valid = EKF3.getVariances(results.velVar, results.posVar, results.hgtVar, results.magVar, results.tasVar, offset);
     218             : 
     219    41686716 :     results.terrain_alt_variance_valid = EKF3.getTerrainAltVariance(results.terrain_alt_variance);
     220             : 
     221    41686716 :     EKF3.getEkfControlLimits(results.control_ground_speed_limit_ms, results.control_gain_scaler_XY);
     222    41686716 :     results.control_gain_scaler_Z = 1;
     223             : 
     224    41686716 :     results.control_height_limit_valid = EKF3.getHeightControlLimit(results.control_height_limit_m);
     225    41686716 : }
     226             : 
     227       31446 : bool AP_AHRS_NavEKF3::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
     228             : {
     229       31446 :     if (!started) {
     230           0 :         hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 not started");
     231           0 :         return false;
     232             :     }
     233       31446 :     return EKF3.pre_arm_check(requires_position, failure_msg, failure_msg_len);
     234             : }
     235             : 
     236             : #endif  // AP_AHRS_NAVEKF3_ENABLED