Line data    Source code

       1             : #include "AP_AHRS_config.h"
       2             : 
       3             : #if AP_AHRS_NAVEKF2_ENABLED
       4             : 
       5             : #include "AP_AHRS_NavEKF2.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             : NavEKF2 AP_AHRS_NavEKF2::EKF2;
      14             : 
      15    39580103 : bool AP_AHRS_NavEKF2::start()
      16             : {
      17    39580103 :     const auto now_ms = AP_HAL::millis();
      18             : 
      19    39580103 :     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    39580103 :     if (!hal.util->was_watchdog_reset()) {
      27    39579661 :         if (now_ms - start_time_ms < 5000) {
      28     2171934 :             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    39580060 :     if (now_ms - start_time_ms <= 1000) {
      38      530414 :         return false;
      39             :     }
      40             : 
      41             :     // try to start the filter:
      42    39049646 :     return EKF2.InitialiseFilter();
      43             : }
      44             : 
      45    41686716 : void AP_AHRS_NavEKF2::update()
      46             : {
      47    41686716 :     if (!started) {
      48    39580103 :         started = start();
      49             :     }
      50    41686716 :     if (!started) {
      51    39579824 :         return;
      52             :     }
      53     2106892 :     EKF2.UpdateFilter();
      54             : 
      55             :     // check the current primary core; if it has changed then assume
      56             :     // our attitude is reset:
      57     2106892 :     const int8_t primary_core = EKF2.getPrimaryCoreIndex();
      58     2106892 :     if (attitude_reset_tracker.update(primary_core)) {
      59           6 :         LOGGER_WRITE_ERROR(LogErrorSubsystem::EKF_PRIMARY, LogErrorCode(primary_core));
      60           6 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 primary changed:%d", (unsigned)primary_core);
      61             :     }
      62             : 
      63     2106892 :     yaw_reset_tracker.update(EKF2.getYawResetCount());
      64             : 
      65     2106892 :     position_NE_reset_tracker.update(EKF2.getPosNorthEastResetCount());
      66             : 
      67     2106892 :     position_D_reset_tracker.update(EKF2.getPosDownResetCount());
      68             : }
      69             : 
      70    41686716 : void AP_AHRS_NavEKF2::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 && EKF2.healthy();
      78             : 
      79    41686716 :     const AP_InertialSensor &_ins = AP::ins();
      80             : 
      81             :     /*
      82             :      * rotational rate estimates:
      83             :      */
      84             : 
      85             :     // The backend should tell us what gyro it is using right now.
      86             :     // There's no API for that at the moment, so we assume the
      87             :     // gyro corresponds to the IMU index.
      88    41686716 :     const int8_t ek2_primary_gyro = EKF2.getPrimaryCoreIMUIndex();
      89    41686716 :     if (ek2_primary_gyro == -1) {
      90    39476267 :         results.primary_gyro = _ins.get_first_usable_gyro();
      91             :     } else {
      92     2210449 :         results.primary_gyro = ek2_primary_gyro;
      93             :     }
      94             : 
      95             :     // get gyro bias for primary EKF and change sign to give gyro drift
      96             :     // Note sign convention used by EKF is bias = measurement - truth
      97    41686716 :     Vector3f drift;
      98    41686716 :     EKF2.getGyroBias(drift);
      99    41686716 :     results.gyro_drift = -drift;
     100             : 
     101             :     // use the same IMU as the primary EKF and correct for gyro drift
     102    41686716 :     results.gyro_estimate = _ins.get_gyro(results.primary_gyro) + results.gyro_drift;
     103             : 
     104             :     /*
     105             :      * attitude estimates:
     106             :      */
     107    41686716 :     EKF2.getRotationBodyToNED(results.dcm_matrix);
     108    41686716 :     Vector3f eulers;
     109    41686716 :     EKF2.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 :     EKF2.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             :     /*
     124             :      * acceleration estimates
     125             :      */
     126             : 
     127             :     // The backend should tell us what accelerometer it is using
     128             :     // right now.  There's no API for that at the moment, so we assume
     129             :     // the accelerometer corresponds to the IMU index.
     130    41686716 :     const int8_t ek2_primary_accel = EKF2.getPrimaryCoreIMUIndex();
     131    41686716 :     if (ek2_primary_accel == -1) {
     132    39476267 :         results.primary_accel = _ins.get_first_usable_accel();
     133             :     } else {
     134     2210449 :         results.primary_accel = ek2_primary_accel;
     135             :     }
     136             : 
     137             :     // get z accel bias estimate from active EKF (this is usually for the primary IMU)
     138    41686716 :     EKF2.getAccelZBias(results.accel_bias.z);
     139             : 
     140             :     // This EKF is currently using primary_imu, and a bias applies to only that IMU
     141    41686716 :     Vector3f accel = _ins.get_accel(results.primary_accel);
     142    41686716 :     accel.z -= results.accel_bias.z;
     143    41686716 :     results.accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel;
     144             : 
     145             :     /*
     146             :      * velocity estimates
     147             :      */
     148    41686716 :     EKF2.getVelNED(results.velocity_NED);
     149    41686716 :     results.velocity_NED_valid = true;
     150    41686716 :     results.vert_pos_rate_D = EKF2.getPosDownDerivative();
     151    41686716 :     results.vert_pos_rate_D_valid = true;
     152             : 
     153             :     // ground velocity estimate in meters/second, in North/East order
     154    41686716 :     results.velocity_NE = results.velocity_NED.xy();
     155             : 
     156             :     /*
     157             :      * position estimates
     158             :      */
     159    41686716 :     results.location_valid = EKF2.getLLH(results.location);
     160             : 
     161             :     // origin-relative functions
     162    41686716 :     results.provides_common_origin = true;
     163             : 
     164             :     // origin-relative position:
     165    41686716 :     results.position_NE_valid = EKF2.getPosNE(results.position_NE);
     166    41686716 :     results.position_NE_reset_count = position_NE_reset_tracker.count();
     167             : 
     168    41686716 :     results.position_D_valid = EKF2.getPosD(results.position_D);
     169    41686716 :     results.position_D_reset_count = position_D_reset_tracker.count();
     170             : 
     171    41686716 :     results.hagl_valid = EKF2.getHAGL(results.hagl);
     172             : 
     173             :     /*
     174             :      * air data estimates
     175             :      */
     176    41686716 :     EKF2.getWind(results.wind);
     177    41686716 :     results.wind_valid = true;
     178             : 
     179             :     /*
     180             :      * Sensor-related information
     181             :      */
     182             :     // true if the estimator will use GPS data in creating its
     183             :     // estimate when the data is good:
     184    41686716 :     results.configured_to_use_gps = EKF2.using_gps();
     185             :     // true if GPS is configured as the horizontal position source for
     186             :     // this estimator.  Used to decide whether GPS will set the
     187             :     // navigation origin:
     188    41686716 :     results.configured_to_use_gps_for_pos_XY = EKF2.configuredToUseGPSForPosXY();
     189             : 
     190             :     // are we consuming yaw from an external (e.g. vision-based) source?
     191    41686716 :     results.using_extnav_for_yaw = EKF2.isExtNavUsedForYaw();
     192             : 
     193             :     // are we consuming yaw from a source which is *not* a compass
     194    41686716 :     results.using_noncompass_for_yaw = EKF2.isExtNavUsedForYaw();
     195             : 
     196             : #if AP_AHRS_GET_MAG_DATA_ENABLED
     197             :     // estimators can provide their predicted magnetic fields:
     198    41686716 :     EKF2.getMagNED(results.mag_field_NED);
     199    41686716 :     results.mag_field_NED_valid = true;
     200    41686716 :     EKF2.getMagXYZ(results.mag_field_corrections);
     201    41686716 :     results.mag_field_corrections_valid = true;
     202             : #endif  // AP_AHRS_GET_MAG_DATA_ENABLED
     203             : 
     204             :     /*
     205             :      * filter status and estimates quality values:
     206             :      */
     207    41686716 :     EKF2.getFilterStatus(results.filter_status);
     208    41686716 :     results.filter_status_valid = true;
     209             : 
     210    41686716 :     EKF2.getFilterFaults(results.filter_faults);
     211             : 
     212             :     // provides the innovations normalised between 0 and 1:
     213    41686716 :     Vector2f offset;
     214    41686716 :     results.variances_valid = EKF2.getVariances(results.velVar, results.posVar, results.hgtVar, results.magVar, results.tasVar, offset);
     215             : 
     216    41686716 :     results.terrain_alt_variance_valid = EKF2.getTerrainAltVariance(results.terrain_alt_variance);
     217             : 
     218    41686716 :     EKF2.getEkfControlLimits(results.control_ground_speed_limit_ms, results.control_gain_scaler_XY);
     219    41686716 :     results.control_gain_scaler_Z = 1;
     220             : 
     221    41686716 :     results.control_height_limit_valid = EKF2.getHeightControlLimit(results.control_height_limit_m);
     222    41686716 : }
     223             : 
     224          80 : bool AP_AHRS_NavEKF2::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
     225             : {
     226          80 :     if (!started) {
     227           0 :         hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 not started");
     228           0 :         return false;
     229             :     }
     230          80 :     return EKF2.pre_arm_check(failure_msg, failure_msg_len);
     231             : }
     232             : 
     233             : #endif  // AP_AHRS_NAVEKF2_ENABLED