Line data    Source code

       1             : #include "AP_InertialNav.h"
       2             : 
       3             : #include <AP_AHRS/AP_AHRS.h>
       4             : 
       5             : /*
       6             :   A wrapper around the AP_InertialNav class which uses the NavEKF
       7             :   filter if available, and falls back to the AP_InertialNav filter
       8             :   when EKF is not available
       9             :  */
      10             : 
      11             : /**
      12             :    update internal state
      13             : */
      14     8377547 : void AP_InertialNav::update(bool high_vibes)
      15             : {
      16             :     // get the NE position relative to the local earth frame origin
      17     8377547 :     Vector2f posNE;
      18     8377547 :     if (_ahrs_ekf.get_relative_position_NE_origin_float(posNE)) {
      19     7942616 :         _relpos_cm.x = posNE.x * 100; // convert from m to cm
      20     7942616 :         _relpos_cm.y = posNE.y * 100; // convert from m to cm
      21             :     }
      22             : 
      23             :     // get the D position relative to the local earth frame origin
      24             :     float posD;
      25     8377547 :     if (_ahrs_ekf.get_relative_position_D_origin_float(posD)) {
      26     7946078 :         _relpos_cm.z = - posD * 100; // convert from m in NED to cm in NEU
      27             :     }
      28             : 
      29             :     // get the velocity relative to the local earth frame
      30     8377547 :     Vector3f velNED;
      31             :     
      32     8377547 :     const bool velned_ok = _ahrs_ekf.get_velocity_NED(velNED);
      33     8377547 :     if (velned_ok) {
      34     7975664 :         _velocity_cm = velNED * 100; // convert to cm/s
      35     7975664 :         _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
      36             :     }
      37             :     //  During high vibration events, or failure of get_velocity_NED, use the
      38             :     //  fallback vertical velocity estimate. For get_velocity_NED failure, freeze
      39             :     //  the horizontal velocity at the last good value.
      40     8377547 :     if (!velned_ok || high_vibes) {
      41             :         float rate_z;
      42      401883 :         if (_ahrs_ekf.get_vert_pos_rate_D(rate_z)) {
      43      401810 :             _velocity_cm.z = -rate_z * 100; // convert from m/s in NED to cm/s in NEU
      44             :         }
      45             :     }
      46     8377547 : }
      47             : 
      48             : /**
      49             :  * get_position_neu_cm - returns the current position relative to the EKF origin in cm.
      50             :  *
      51             :  * @return
      52             :  */
      53       19634 : const Vector3f &AP_InertialNav::get_position_neu_cm(void) const 
      54             : {
      55       19634 :     return _relpos_cm;
      56             : }
      57             : 
      58             : /**
      59             :  * get_position_xy_cm - returns the current x-y position relative to the EKF origin in cm.
      60             :  *
      61             :  * @return
      62             :  */
      63       24800 : const Vector2f &AP_InertialNav::get_position_xy_cm() const
      64             : {
      65       24800 :     return _relpos_cm.xy();
      66             : }
      67             : 
      68             : /**
      69             :  * get_position_z_up_cm - returns the current z position relative to the EKF origin, frame z-axis up, in cm.
      70             :  * @return
      71             :  */
      72      259602 : float AP_InertialNav::get_position_z_up_cm() const
      73             : {
      74      259602 :     return _relpos_cm.z;
      75             : }
      76             : 
      77             : /**
      78             :  * get_velocity_neu_cms - returns the current velocity in cm/s
      79             :  *
      80             :  * @return velocity vector:
      81             :  *                      .x : latitude  velocity in cm/s
      82             :  *                              .y : longitude velocity in cm/s
      83             :  *                              .z : vertical  velocity in cm/s
      84             :  */
      85           0 : const Vector3f &AP_InertialNav::get_velocity_neu_cms() const
      86             : {
      87           0 :     return _velocity_cm;
      88             : }
      89             : 
      90             : /**
      91             :  * get_velocity_xy_cms - returns the current x-y velocity relative to the EKF origin in cm.
      92             :  *
      93             :  * @return
      94             :  */
      95           0 : const Vector2f &AP_InertialNav::get_velocity_xy_cms() const
      96             : {
      97           0 :     return _velocity_cm.xy();
      98             : }
      99             : 
     100             : /**
     101             :  * get_speed_xy_cms - returns the current horizontal speed in cm/s
     102             :  *
     103             :  * @returns the current horizontal speed in cm/s
     104             :  */
     105           0 : float AP_InertialNav::get_speed_xy_cms() const
     106             : {
     107           0 :     return _velocity_cm.xy().length();
     108             : }
     109             : 
     110             : /**
     111             :  * get_velocity_z_up_cms - returns the current z-axis velocity, frame z-axis up, in cm/s
     112             :  *
     113             :  * @return z-axis velocity, frame z-axis up, in cm/s
     114             :  */
     115      305321 : float AP_InertialNav::get_velocity_z_up_cms() const
     116             : {
     117      305321 :     return _velocity_cm.z;
     118             : }