Line data    Source code

       1             : #include <AP_AHRS/AP_AHRS.h>
       2             : 
       3             : #include <AP_Compass/AP_Compass.h>
       4             : 
       5             : #include "Compass_learn.h"
       6             : #include <GCS_MAVLink/GCS.h>
       7             : #include <AP_Vehicle/AP_Vehicle.h>
       8             : #include <AP_NavEKF/EKFGSF_yaw.h>
       9             : 
      10             : #include <AP_Logger/AP_Logger.h>
      11             : 
      12             : #if COMPASS_LEARN_ENABLED
      13             : 
      14             : extern const AP_HAL::HAL &hal;
      15             : 
      16             : // constructor
      17           2 : CompassLearn::CompassLearn(Compass &_compass) :
      18           2 :     compass(_compass)
      19             : {
      20           2 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CompassLearn: Initialised");
      21           2 : }
      22             : 
      23             : // accuracy threshold applied for GSF yaw estimate
      24             : #define YAW_ACCURACY_THRESHOLD_DEG 5.0
      25             : 
      26             : /*
      27             :   update when new compass sample available
      28             :  */
      29         493 : void CompassLearn::update(void)
      30             : {
      31         493 :     const AP_Vehicle *vehicle = AP::vehicle();
      32         986 :     if (compass.get_learn_type() != Compass::LearnType::INFLIGHT ||
      33         977 :         !hal.util->get_soft_armed() ||
      34         484 :         vehicle->get_time_flying_ms() < 3000) {
      35             :         // only learn when flying and with enough time to be clear of
      36             :         // the ground
      37         492 :         return;
      38             :     }
      39             : 
      40         479 :     const auto &ahrs = AP::ahrs();
      41         479 :     const auto *gsf = ahrs.get_yaw_estimator();
      42         479 :     if (gsf == nullptr) {
      43             :         // no GSF available
      44             :         return;
      45             :     }
      46         479 :     if (fabsf(ahrs.get_pitch_deg()) > 50) {
      47             :         // we don't want to be too close to nose up, or yaw gets
      48             :         // problematic. Tailsitters need to wait till they are in
      49             :         // forward flight
      50             :         return;
      51             :     }
      52             : 
      53         479 :     AP_Notify::flags.compass_cal_running = true;
      54             : 
      55             :     ftype yaw_rad, yaw_variance;
      56             :     uint8_t n_clips;
      57         958 :     if (!gsf->getYawData(yaw_rad, yaw_variance, &n_clips) ||
      58         479 :         !is_positive(yaw_variance) ||
      59         958 :         n_clips > 1 ||
      60         479 :         yaw_variance >= sq(radians(YAW_ACCURACY_THRESHOLD_DEG))) {
      61             :         // not converged
      62         478 :         return;
      63             :     }
      64             : 
      65           1 :     const bool result = compass.mag_cal_fixed_yaw(degrees(yaw_rad), (1U<<HAL_COMPASS_MAX_SENSORS)-1, 0, 0, true);
      66           1 :     if (result) {
      67           1 :         AP_Notify::flags.compass_cal_running = false;
      68           1 :         compass.set_learn_type(Compass::LearnType::NONE, true);
      69           1 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "CompassLearn: Finished");
      70             :     }
      71             : }
      72             : 
      73             : #endif  // COMPASS_LEARN_ENABLED
      74             : 
      75             : #if AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
      76             : /*
      77             :   true if the field published for this instance reaches us in the body
      78             :   frame.  rotate_field() applies, in order, MAG_BOARD_ORIENTATION, the
      79             :   board-specific per-instance rotation, and then either the board
      80             :   orientation (internal compasses) or COMPASS_ORIENT (external ones).
      81             :   Any of those leaves the reading in a frame a body-frame offset does
      82             :   not belong in - checking only the board orientation would accept a
      83             :   compass carrying a COMPASS_ORIENT and save an offset in the wrong
      84             :   frame, and would reject every instance on a board which merely
      85             :   defines MAG_BOARD_ORIENTATION.
      86             :  */
      87           3 : bool Compass::instance_is_unrotated(uint8_t i) const
      88             : {
      89           3 :     if (i >= COMPASS_MAX_INSTANCES) {
      90             :         return false;
      91             :     }
      92             :     if (MAG_BOARD_ORIENTATION != ROTATION_NONE) {
      93             :         return false;
      94             :     }
      95           3 :     const mag_state &state = _get_state(Priority(i));
      96           3 :     if (state.rotation != ROTATION_NONE) {
      97             :         return false;
      98             :     }
      99           3 :     if (!state.external) {
     100           2 :         return _board_orientation == ROTATION_NONE;
     101             :     }
     102           1 :     return (enum Rotation)state.orientation.get() == ROTATION_NONE;
     103             : }
     104             : 
     105           3 : uint32_t Compass::get_dev_id(uint8_t i) const
     106             : {
     107           3 :     if (i >= COMPASS_MAX_INSTANCES) {
     108             :         return 0;
     109             :     }
     110           3 :     return uint32_t(_get_state(Priority(i)).dev_id.get());
     111             : }
     112             : 
     113             : /*
     114             :   save any compass offsets the EKF has learned.  Called on disarm.
     115             :  */
     116        1103 : void Compass::save_ekf_learned_offsets()
     117             : {
     118        1103 :     if (get_learn_type() != LearnType::COPY_FROM_EKF) {
     119             :         return;
     120             :     }
     121             : 
     122           4 :     auto &ahrs = AP::ahrs();
     123           4 :     if (!ahrs.healthy()) {
     124             :         // Note that this is a deliberate tightening rather than part of
     125             :         // moving the vehicles' code here: Copter, Sub and Blimp each
     126             :         // asked getMagOffsets() directly, with no health precondition.
     127             :         // getMagOffsets() judges whether an individual estimate is
     128             :         // usable, but it cannot tell that the estimator as a whole is
     129             :         // unhealthy or that the active backend is no longer the
     130             :         // configured one - and offsets are set_and_save()d, so a bad
     131             :         // set persists across a reboot and has to be recalibrated out
     132             :         // by hand.  Declining to copy costs a learning opportunity; the
     133             :         // vehicle keeps the offsets it already had.
     134             :         return;
     135             :     }
     136           4 :     if (!ahrs.use_compass()) {
     137             :         return;
     138             :     }
     139             : 
     140             :     // note that this loop is not redundant even though a single EKF
     141             :     // core will only ever return offsets for its own selected
     142             :     // compass; with EK3_AFFINITY compass affinity enabled each core
     143             :     // takes a different compass (AP_NavEKF3_Measurements.cpp
     144             :     // update_mag_selection) and the frontend asks every core for each
     145             :     // instance in turn (AP_NavEKF3.cpp getMagOffsets).
     146             :     uint8_t saved_count = 0;
     147          16 :     for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
     148          12 :         Vector3f magOffsets;
     149          12 :         if (ahrs.getMagOffsets(i, magOffsets)) {
     150           8 :             set_and_save_offsets(i, magOffsets);
     151           8 :             saved_count++;
     152             :         }
     153             :     }
     154             : 
     155             : #if HAL_LOGGING_ENABLED
     156           4 :     if (saved_count != 0) {
     157             :         // the EKF frequently has nothing to offer here, so log the fact
     158             :         // that we did save something; otherwise a failure to learn is
     159             :         // indistinguishable from the feature not being enabled
     160           4 :         AP::logger().Write_Event(LogEvent::EKF_MAG_OFFSETS_SAVED);
     161             :     }
     162             : #endif  // HAL_LOGGING_ENABLED
     163             : }
     164             : #endif  // AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED