AP_AHRS: make AHRS handle altitude

AHRS now holds the home position

libraries/AP_AHRS/AP_AHRS.cpp

@@ -228,26 +228,6 @@ Vector2f AP_AHRS::groundspeed_vector(void)
     return Vector2f(0.0f, 0.0f);
 }
 
-/*
-  get position projected by groundspeed and heading
- */
-bool AP_AHRS::get_projected_position(struct Location &loc)
-{
-        if (!get_position(loc)) {
-		return false;
-        }
-        location_update(loc, degrees(yaw), _gps->ground_speed_cm * 0.01 * _gps->get_lag());
-        return true;
-}
-
-/*
-  get the GPS lag in seconds
- */
-float AP_AHRS::get_position_lag(void) const
-{
-    return _gps->get_lag();
-}
-
 // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
 //      should be called after _dcm_matrix is updated
 void AP_AHRS::update_trig(void)

libraries/AP_AHRS/AP_AHRS.h

@@ -61,6 +61,14 @@ public:
 
         // enable centrifugal correction by default
         _flags.correct_centrifugal = true;
+
+        // initialise _home
+        _home.id         = MAV_CMD_NAV_WAYPOINT;
+        _home.options    = 0;
+        _home.p1         = 0;
+        _home.alt        = 0;
+        _home.lng        = 0;
+        _home.lat        = 0;
     }
 
     // init sets up INS board orientation
@@ -143,12 +151,6 @@ public:
     // reset the current attitude, used on new IMU calibration
     virtual void reset_attitude(const float &roll, const float &pitch, const float &yaw) = 0;
 
-    // how often our attitude representation has gone out of range
-    uint8_t renorm_range_count;
-
-    // how often our attitude representation has blown up completely
-    uint8_t renorm_blowup_count;
-
     // return the average size of the roll/pitch error estimate
     // since last call
     virtual float get_error_rp(void) = 0;
@@ -161,30 +163,12 @@ public:
     // attitude
     virtual const Matrix3f &get_dcm_matrix(void) const = 0;
 
-    // get our current position, either from GPS or via
-    // dead-reckoning. Return true if a position is available,
-    // otherwise false. This only updates the lat and lng fields
-    // of the Location
-    virtual bool get_position(struct Location &loc) {
-        if (!_gps || _gps->status() <= GPS::NO_FIX) {
-            return false;
-        }
-        loc.lat = _gps->latitude;
-        loc.lng = _gps->longitude;
-        return true;
-    }
-
-    // get our projected position, based on our GPS position plus
-    // heading and ground speed
-    bool get_projected_position(struct Location &loc);
-
-    // return the estimated lag in our position due to GPS lag
-    float get_position_lag(void) const;
+    // get our current position estimate. Return true if a position is available,
+    // otherwise false. This call fills in lat, lng and alt
+    virtual bool get_position(struct Location &loc) = 0;
 
     // return a wind estimation vector, in m/s
-    virtual Vector3f wind_estimate(void) {
-        return Vector3f(0,0,0);
-    }
+    virtual Vector3f wind_estimate(void) = 0;
 
     // return an airspeed estimate if available. return true
     // if we have an estimate
@@ -275,6 +259,15 @@ public:
     // return secondary position solution if available
     virtual bool get_secondary_position(struct Location &loc) { return false; }
 
+    // get the home location. This is const to prevent any changes to
+    // home without telling AHRS about the change    
+    const struct Location &get_home(void) const { return _home; }
+
+    // set the home location in 10e7 degrees. This should be called
+    // when the vehicle is at this position. It is assumed that the
+    // current barometer and GPS altitudes correspond to this altitude
+    virtual void set_home(int32_t lat, int32_t lon, int32_t alt_cm) = 0;
+
 protected:
     // settable parameters
     AP_Float beta;
@@ -329,6 +322,9 @@ protected:
 	Vector2f _hp; // ground vector high-pass filter
     Vector2f _lastGndVelADS; // previous HPF input		
 
+    // reference position for NED positions
+    struct Location _home;
+
     // helper trig variables
     float _cos_roll, _cos_pitch, _cos_yaw;
     float _sin_roll, _sin_pitch, _sin_yaw;

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -134,7 +134,6 @@ AP_AHRS_DCM::check_matrix(void)
 {
     if (_dcm_matrix.is_nan()) {
         //Serial.printf("ERROR: DCM matrix NAN\n");
-        renorm_blowup_count++;
         reset(true);
         return;
     }
@@ -145,7 +144,6 @@ AP_AHRS_DCM::check_matrix(void)
     if (!(_dcm_matrix.c.x < 1.0f &&
           _dcm_matrix.c.x > -1.0f)) {
         // We have an invalid matrix. Force a normalisation.
-        renorm_range_count++;
         normalize();
 
         if (_dcm_matrix.is_nan() ||
@@ -154,7 +152,6 @@ AP_AHRS_DCM::check_matrix(void)
             // in real trouble. All we can do is reset
             //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
             //	   _dcm_matrix.c.x);
-            renorm_blowup_count++;
             reset(true);
         }
     }
@@ -193,7 +190,6 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
 
     if (!(renorm_val < 2.0f && renorm_val > 0.5f)) {
         // this is larger than it should get - log it as a warning
-        renorm_range_count++;
         if (!(renorm_val < 1.0e6f && renorm_val > 1.0e-6f)) {
             // we are getting values which are way out of
             // range, we will reset the matrix and hope we
@@ -201,7 +197,6 @@ AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
             // correction before we hit the ground!
             //Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n",
             //	   renorm_val);
-            renorm_blowup_count++;
             return false;
         }
     }
@@ -854,7 +849,11 @@ bool AP_AHRS_DCM::get_position(struct Location &loc)
     }
     loc.lat = _last_lat;
     loc.lng = _last_lng;
+    loc.alt = _baro.get_altitude() * 100 + _home.alt;
     location_offset(loc, _position_offset_north, _position_offset_east);
+    if (_flags.fly_forward) {
+        location_update(loc, degrees(yaw), _gps->ground_speed_cm * 0.01 * _gps->get_lag());
+    }
     return true;
 }
 
@@ -889,3 +888,10 @@ bool AP_AHRS_DCM::airspeed_estimate(float *airspeed_ret)
 	}
 	return ret;
 }
+
+void AP_AHRS_DCM::set_home(int32_t lat, int32_t lng, int32_t alt_cm)
+{
+    _home.lat = lat;
+    _home.lng = lng;
+    _home.alt = alt_cm;
+}

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -78,6 +78,8 @@ public:
 
     bool            use_compass(void);
 
+    void set_home(int32_t lat, int32_t lng, int32_t alt_cm);
+
 private:
     float _ki;
     float _ki_yaw;

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -187,4 +187,12 @@ Vector2f AP_AHRS_NavEKF::groundspeed_vector(void)
     return Vector2f(vec.x, vec.y);
 }
 
+void AP_AHRS_NavEKF::set_home(int32_t lat, int32_t lng, int32_t alt_cm)
+{
+    AP_AHRS_DCM::set_home(lat, lng, alt_cm);
+    if (ekf_started) {
+        EKF.InitialiseFilter();
+    }
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -82,6 +82,9 @@ public:
     // EKF has a better ground speed vector estimate
     Vector2f groundspeed_vector(void);
 
+    // set home location
+    void set_home(int32_t lat, int32_t lng, int32_t alt_cm);
+
 private:
     bool using_EKF(void) { return ekf_started && _ekf_use && EKF.healthy(); }