AP_InertialNav: implement InertialNav in terms of AHRS when available

libraries/AP_InertialNav/AP_InertialNav.h

@@ -34,7 +34,7 @@ class AP_InertialNav
 public:
 
     // Constructor
-    AP_InertialNav( const AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
+    AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
         _ahrs(ahrs),
         _baro(baro),
         _gps(gps),
@@ -52,7 +52,7 @@ public:
     /**
      * initializes the object.
      */
-    void        init();
+    virtual void init();
 
     /**
      * update - updates velocity and position estimates using latest info from accelerometers
@@ -60,7 +60,7 @@ public:
      *
      * @param dt : time since last update in seconds
      */
-    void        update(float dt);
+    virtual void update(float dt);
 
     //
     // XY Axis specific methods
@@ -80,7 +80,7 @@ public:
      * position_ok - true if inertial based altitude and position can be trusted
      * @return
      */
-    bool        position_ok() const;
+    virtual bool position_ok() const;
 
     /**
      * check_gps - checks if new gps readings have arrived and calls correct_with_gps to
@@ -103,33 +103,33 @@ public:
      *
      * @return
      */
-    const Vector3f&    get_position() const { return _position; }
+    virtual const Vector3f&    get_position() const { return _position; }
 
     /**
      * get_latitude - returns the latitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
      * @return
      */
-    int32_t     get_latitude() const;
+    virtual int32_t     get_latitude() const;
 
     /**
      * get_longitude - returns the longitude of the current position estimation in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
      * @return
      */
-    int32_t     get_longitude() const;
+    virtual int32_t     get_longitude() const;
 
     /**
      * get_latitude_diff - returns the current latitude difference from the home location.
      *
      * @return difference in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
      */
-    float       get_latitude_diff() const;
+    virtual float       get_latitude_diff() const;
 
     /**
      * get_longitude_diff - returns the current longitude difference from the home location.
      *
      * @return difference in 100 nano degrees (i.e. degree value multiplied by 10,000,000)
      */
-    float       get_longitude_diff() const;
+    virtual float       get_longitude_diff() const;
 
     /**
      * get_velocity - returns the current velocity in cm/s
@@ -139,14 +139,14 @@ public:
      * 				.y : longitude velocity in cm/s
      * 				.z : vertical  velocity in cm/s
      */
-    const Vector3f&    get_velocity() const { return _velocity; }
+    virtual const Vector3f&    get_velocity() const { return _velocity; }
 
     /**
      * get_velocity_xy - returns the current horizontal velocity in cm/s
      *
      * @returns the current horizontal velocity in cm/s
      */
-    float        get_velocity_xy();
+    virtual float        get_velocity_xy();
 
     /**
      * set_velocity_xy - overwrites the current horizontal velocity in cm/s
@@ -179,7 +179,7 @@ public:
      * altitude_ok - returns true if inertial based altitude and position can be trusted
      * @return
      */
-    bool        altitude_ok() const { return true; }
+    virtual bool        altitude_ok() const { return true; }
 
     /**
      * check_baro - checks if new baro readings have arrived and calls correct_with_baro to
@@ -198,10 +198,11 @@ public:
     void        correct_with_baro(float baro_alt, float dt);
 
     /**
-     * get_altitude - get latest altitude estimate in cm
+     * get_altitude - get latest altitude estimate in cm above the
+     * reference position
      * @return
      */
-    float       get_altitude() const { return _position.z; }
+    virtual float       get_altitude() const { return _position.z; }
 
     /**
      * set_altitude - overwrites the current altitude value.
@@ -215,9 +216,9 @@ public:
      *
      * @see get_velocity().z
      *
-     * @return climbrate in cm/s
+     * @return climbrate in cm/s (positive up)
      */
-    float       get_velocity_z() const { return _velocity.z; }
+    virtual float       get_velocity_z() const { return _velocity.z; }
 
     /**
      * set_velocity_z - overwrites the current climbrate.
@@ -229,7 +230,7 @@ public:
     /**
      * error_count - returns number of missed updates from GPS
      */
-    uint8_t     error_count() const { return _error_count; }
+    virtual uint8_t     error_count() const { return _error_count; }
 
     /**
      * ignore_next_error - the next error (if it occurs immediately) will not be added to the error count
@@ -270,7 +271,7 @@ protected:
         uint8_t ignore_error        : 3;                // the number of iterations for which we should ignore errors
     } _flags;
 
-    const AP_AHRS          &_ahrs;                      // reference to ahrs object
+    AP_AHRS                &_ahrs;                      // reference to ahrs object
     AP_Baro                &_baro;                      // reference to barometer
     GPS*&                   _gps;                       // pointer to gps
 

libraries/AP_InertialNav/AP_InertialNav_NavEKF.cpp

@@ -1,7 +1,10 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
+#include <AP_HAL.h>
 #include "AP_InertialNav.h"
 
+extern const AP_HAL::HAL& hal;
+
 #if AP_AHRS_NAVEKF_AVAILABLE
 
 /*
@@ -18,12 +21,30 @@ void AP_InertialNav_NavEKF::init()
     AP_InertialNav::init();
 }
 
+/**
+   update internal state
+*/
+void AP_InertialNav_NavEKF::update(float dt)
+{
+    AP_InertialNav::update(dt);
+    _relpos_cm = _ahrs.get_relative_position_NED() * 100; 
+    _haveabspos = _ahrs.get_position(_abspos);
+    _velocity_cm = _ahrs.get_velocity_NED() * 100;
+
+    // InertialNav is NEU
+    _relpos_cm.z = - _relpos_cm.z;
+    _velocity_cm.z = -_velocity_cm.z;
+}
+
 /**
  * position_ok - true if inertial based altitude and position can be trusted
  * @return
  */
 bool AP_InertialNav_NavEKF::position_ok() const
 {
+    if (_ahrs.have_inertial_nav() && _haveabspos) {
+        return true;
+    }
     return AP_InertialNav::position_ok();
 }
 
@@ -36,6 +57,9 @@ bool AP_InertialNav_NavEKF::position_ok() const
  */
 const Vector3f &AP_InertialNav_NavEKF::get_position(void) const 
 {
+    if (_ahrs.have_inertial_nav()) {
+        return _relpos_cm;
+    }
     return AP_InertialNav::get_position();
 }
 
@@ -44,6 +68,9 @@ const Vector3f &AP_InertialNav_NavEKF::get_position(void) const
  */
 int32_t AP_InertialNav_NavEKF::get_latitude() const
 {
+    if (_ahrs.have_inertial_nav() && _haveabspos) {
+        return _abspos.lat;
+    }
     return AP_InertialNav::get_latitude();
 }
 
@@ -53,6 +80,9 @@ int32_t AP_InertialNav_NavEKF::get_latitude() const
  */
 int32_t AP_InertialNav_NavEKF::get_longitude() const
 {
+    if (_ahrs.have_inertial_nav() && _haveabspos) {
+        return _abspos.lng;
+    }
     return AP_InertialNav::get_longitude();
 }
 
@@ -63,6 +93,9 @@ int32_t AP_InertialNav_NavEKF::get_longitude() const
  */
 float AP_InertialNav_NavEKF::get_latitude_diff() const
 {
+    if (_ahrs.have_inertial_nav()) {
+        return _relpos_cm.x / LATLON_TO_CM;
+    }
     return AP_InertialNav::get_latitude_diff();
 }
 
@@ -73,6 +106,9 @@ float AP_InertialNav_NavEKF::get_latitude_diff() const
  */
 float AP_InertialNav_NavEKF::get_longitude_diff() const
 {
+    if (_ahrs.have_inertial_nav()) {
+        return _relpos_cm.y / _lon_to_cm_scaling;
+    }
     return AP_InertialNav::get_longitude_diff();
 }
 
@@ -86,6 +122,9 @@ float AP_InertialNav_NavEKF::get_longitude_diff() const
  */
 const Vector3f &AP_InertialNav_NavEKF::get_velocity() const
 {
+    if (_ahrs.have_inertial_nav()) {
+        return _velocity_cm;
+    }
     return AP_InertialNav::get_velocity();
 }
 
@@ -96,6 +135,9 @@ const Vector3f &AP_InertialNav_NavEKF::get_velocity() const
  */
 float AP_InertialNav_NavEKF::get_velocity_xy()
 {
+    if (_ahrs.have_inertial_nav()) {
+        return pythagorous2(_velocity_cm.x, _velocity_cm.y);
+    }
     return AP_InertialNav::get_velocity_xy();
 }
 
@@ -105,6 +147,9 @@ float AP_InertialNav_NavEKF::get_velocity_xy()
  */
 bool AP_InertialNav_NavEKF::altitude_ok() const
 {
+    if (_ahrs.have_inertial_nav() && _haveabspos) {
+        return true;
+    }
     return AP_InertialNav::altitude_ok();
 }
 
@@ -114,6 +159,9 @@ bool AP_InertialNav_NavEKF::altitude_ok() const
  */
 float AP_InertialNav_NavEKF::get_altitude() const
 {
+    if (_ahrs.have_inertial_nav()) {
+        return _relpos_cm.z;
+    }
     return AP_InertialNav::get_altitude();
 }
 
@@ -126,6 +174,9 @@ float AP_InertialNav_NavEKF::get_altitude() const
  */
 float AP_InertialNav_NavEKF::get_velocity_z() const
 {
+    if (_ahrs.have_inertial_nav()) {
+        return _velocity_cm.z;
+    }
     return AP_InertialNav::get_velocity_z();
 }
 

libraries/AP_InertialNav/AP_InertialNav_NavEKF.h

@@ -14,7 +14,7 @@ class AP_InertialNav_NavEKF : public AP_InertialNav
 {
 public:
     // Constructor
-    AP_InertialNav_NavEKF(const AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
+    AP_InertialNav_NavEKF(AP_AHRS &ahrs, AP_Baro &baro, GPS*& gps, GPS_Glitch& gps_glitch ) :
         AP_InertialNav(ahrs, baro, gps, gps_glitch)
         {
         }
@@ -24,6 +24,11 @@ public:
      */
     void        init();
 
+    /**
+       update internal state
+    */
+    void        update(float dt);
+
     /**
      * position_ok - true if inertial based altitude and position can be trusted
      * @return
@@ -101,6 +106,12 @@ public:
      * @return climbrate in cm/s
      */
     float       get_velocity_z() const;
+
+private:
+    Vector3f _relpos_cm;   // NEU
+    Vector3f _velocity_cm; // NEU
+    struct Location _abspos;
+    bool _haveabspos;
 };
 
 #endif // __AP_INERTIALNAV_NAVEKF_H__