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