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             :  *  shim AP_NavEKF2 into AP_AHRS_Backend
      20             :  */
      21             : 
      22             : #include "AP_AHRS_config.h"
      23             : 
      24             : #if AP_AHRS_NAVEKF2_ENABLED
      25             : 
      26             : #include <AP_NavEKF2/AP_NavEKF2.h>
      27             : #include "AP_AHRS_Backend.h"
      28             : 
      29             : class AP_AHRS_NavEKF2 : public AP_AHRS_Backend {
      30             : public:
      31             : 
      32             :     /* Do not allow copies */
      33             :     CLASS_NO_COPY(AP_AHRS_NavEKF2);
      34        2097 :     AP_AHRS_NavEKF2() {}
      35             : 
      36          12 :     const char *shortname() const override { return "EKF2"; }
      37             : 
      38           7 :     void reset_gyro_drift() override { EKF2.resetGyroBias(); }
      39             : 
      40             :     void update() override;
      41             : 
      42             :     void get_results(Estimates &results) override;
      43        2115 :     void reset() override {
      44        2115 :         if (!started) {
      45        2093 :             return;
      46             :         }
      47          22 :         started = EKF2.InitialiseFilter();
      48             :     }
      49             : 
      50     6779639 :     bool get_origin(Location &ret) const override {
      51     6779639 :         return EKF2.getOriginLLH(ret);
      52             :     }
      53         750 :     bool set_origin(const Location &loc) override {
      54         750 :         return EKF2.setOriginLLH(loc);
      55             :     }
      56             : 
      57         129 :     bool use_compass() override {
      58         129 :         return EKF2.use_compass();
      59             :     }
      60             : 
      61             : #if AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
      62           0 :     bool get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const override {
      63           0 :         return EKF2.getMagOffsets(mag_idx, magOffsets);
      64             :     }
      65             : #endif  // AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
      66             : 
      67        3121 :     void resetHeightDatum(void) override {
      68        3121 :         EKF2.resetHeightDatum();
      69        3121 :     }
      70             : 
      71             :     bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override;
      72             : 
      73             :     // // return the innovations for the specified instance
      74             :     // // An out of range instance (eg -1) returns data for the primary instance
      75        3439 :     bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override {
      76        3439 :         return EKF2.getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
      77             :     }
      78             : 
      79           2 :     void request_yaw_reset(void) override {
      80           2 :         EKF2.requestYawReset();
      81           2 :     }
      82           1 :     void check_lane_switch() override {
      83           1 :         EKF2.checkLaneSwitch();
      84           1 :     }
      85             : 
      86             :     // this is out here so parameters can be poked into it
      87             :     static NavEKF2 EKF2;
      88             : 
      89             :     bool start();
      90             :     bool started;
      91             :     uint32_t start_time_ms;  // timer used to delay starting the filter
      92             : 
      93             :     // a counter which is incremented each time the primary core changes:
      94             :     AP_AHRS_ResetCounter<int8_t> attitude_reset_tracker;
      95             : 
      96             :     AP_AHRS_ResetCounter<uint16_t> yaw_reset_tracker;
      97             :     AP_AHRS_ResetCounter<uint16_t> position_NE_reset_tracker;
      98             :     AP_AHRS_ResetCounter<uint16_t> position_D_reset_tracker;
      99             : };
     100             : 
     101             : #endif  // AP_AHRS_NAVEKF2_ENABLED