Line data    Source code

       1             : /*
       2             :   A wrapper around the AP_InertialNav class which uses the NavEKF
       3             :   filter if available, and falls back to the AP_InertialNav filter
       4             :   when EKF is not available
       5             :  */
       6             : #pragma once
       7             : 
       8             : #include <AP_NavEKF/AP_Nav_Common.h>              // definitions shared by inertial and ekf nav filters
       9             : 
      10             : class AP_InertialNav
      11             : {
      12             : public:
      13             :     // Constructor
      14         692 :     AP_InertialNav(class AP_AHRS &ahrs) :
      15         692 :         _ahrs_ekf(ahrs)
      16         692 :         {}
      17             : 
      18             :     /**
      19             :        update internal state
      20             :     */
      21             :     void        update(bool high_vibes = false);
      22             : 
      23             :     /**
      24             :      * get_position_neu_cm - returns the current position relative to the EKF origin in cm.
      25             :      *
      26             :      * @return
      27             :      */
      28             :     const Vector3f&    get_position_neu_cm() const;
      29             : 
      30             :     /**
      31             :      * get_position_xy_cm - returns the current x-y position relative to the EKF origin in cm.
      32             :      *
      33             :      * @return
      34             :      */
      35             :     const Vector2f&    get_position_xy_cm() const;
      36             : 
      37             :     /**
      38             :      * get_position_z_up_cm - returns the current z position relative to the EKF origin, frame z up, in cm.
      39             :      * @return
      40             :      */
      41             :     float              get_position_z_up_cm() const;
      42             : 
      43             :     /**
      44             :      * get_velocity_neu_cms - returns the current velocity in cm/s
      45             :      *
      46             :      * @return velocity vector:
      47             :      *                  .x : latitude  velocity in cm/s
      48             :      *                          .y : longitude velocity in cm/s
      49             :      *                          .z : vertical  velocity in cm/s
      50             :      */
      51             :     const Vector3f&    get_velocity_neu_cms() const;
      52             : 
      53             :     /**
      54             :      * get_velocity_xy_cms - returns the current x-y velocity relative to the EKF origin in cm.
      55             :      *
      56             :      * @return
      57             :      */
      58             :     const Vector2f& get_velocity_xy_cms() const;
      59             : 
      60             :     /**
      61             :      * get_speed_xy_cms - returns the current horizontal speed in cm/s
      62             :      *
      63             :      * @returns the current horizontal speed in cm/s
      64             :      */
      65             :     float        get_speed_xy_cms() const;
      66             : 
      67             :     /**
      68             :      * get_velocity_z_up_cms - returns the current z-axis velocity, frame z-axis up, in cm/s
      69             :      *
      70             :      * @return z-axis velocity, frame z-axis up, in cm/s
      71             :      */
      72             :     float       get_velocity_z_up_cms() const;
      73             : 
      74             : private:
      75             :     Vector3f _relpos_cm;   // NEU
      76             :     Vector3f _velocity_cm; // NEU
      77             :     AP_AHRS &_ahrs_ekf;
      78             : };