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             :  *  SITL-based AHRS (Attitude Heading Reference System) interface for
      20             :  *  ArduPilot
      21             :  *
      22             :  */
      23             : 
      24             : #include "AP_AHRS_config.h"
      25             : 
      26             : #if AP_AHRS_SIM_ENABLED
      27             : 
      28             : #include "AP_AHRS_Backend.h"
      29             : 
      30             : #include <GCS_MAVLink/GCS.h>
      31             : #include <SITL/SITL.h>
      32             : 
      33             : #if HAL_NAVEKF3_AVAILABLE
      34             : #include <AP_NavEKF3/AP_NavEKF3.h>
      35             : #endif
      36             : 
      37             : class AP_AHRS_SIM : public AP_AHRS_Backend {
      38             : public:
      39             : 
      40             : #if HAL_NAVEKF3_AVAILABLE
      41        2097 :     AP_AHRS_SIM(NavEKF3 &_EKF3) :
      42             :         AP_AHRS_Backend(),
      43        2097 :         EKF3(_EKF3)
      44        2097 :         { }
      45         457 :     ~AP_AHRS_SIM() {}
      46             : #else
      47             :     // a version of the constructor which doesn't take a non-existant
      48             :     // NavEKF3 class instance as a parameter.
      49             :     AP_AHRS_SIM() : AP_AHRS_Backend() { }
      50             : #endif
      51             : 
      52             :     CLASS_NO_COPY(AP_AHRS_SIM);
      53             : 
      54          33 :     const char *shortname() const override { return "SIM"; }
      55             : 
      56             :     // reset the current gyro drift estimate
      57             :     //  should be called if gyro offsets are recalculated
      58           7 :     void reset_gyro_drift() override {};
      59             : 
      60             :     // Methods
      61    41686716 :     void            update() override { }
      62             :     void            get_results(Estimates &results) override;
      63        2115 :     void            reset() override { return; }
      64             : 
      65             :     // return an airspeed estimate if available. return true
      66             :     // if we have an estimate
      67             :     bool airspeed_EAS(float &airspeed_ret) const override;
      68             : 
      69             :     // return an airspeed estimate if available. return true
      70             :     // if we have an estimate from a specific sensor index
      71             :     bool airspeed_EAS(uint8_t airspeed_index, float &airspeed_ret) const override;
      72             : 
      73        3481 :     bool            use_compass() override { return true; }
      74             : 
      75             : #if AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
      76             :     // return the ideal compass offsets for a mag instance; these are
      77             :     // simply the offsets the simulation is applying, so the SIM
      78             :     // backend behaves like a perfectly-converged estimator
      79             :     bool get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const override;
      80             : #endif  // AP_COMPASS_LEARN_COPY_FROM_EKF_ENABLED
      81             : 
      82             :     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
      83             :     // requires_position should be true if horizontal position configuration should be checked (not used)
      84        3917 :     bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override { return true; }
      85             : 
      86             :     // relative-origin functions for fallback in AP_InertialNav
      87             :     bool get_origin(Location &ret) const override;
      88             : 
      89             :     bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override;
      90             : 
      91             : private:
      92             : 
      93             :     // get_filter_status - returns filter status as a series of flags
      94             :     bool get_filter_status(nav_filter_status &status) const;
      95             : 
      96             :     // dead-reckoning support
      97             :     bool get_location(Location &loc) const;
      98             : 
      99             : #if HAL_NAVEKF3_AVAILABLE
     100             :     // a reference to the EKF3 backend that we can use to send in
     101             :     // body-frame-odometry data into the EKF.  Rightfully there should
     102             :     // be something over in the SITL directory doing this.
     103             :     NavEKF3 &EKF3;
     104             : #endif
     105             : 
     106             :     class SITL::SIM *_sitl;
     107             :     uint32_t _last_body_odm_update_ms;
     108             : };
     109             : 
     110             : #endif  // AP_AHRS_SIM_ENABLED