From af4071894e91bf88c97f3538a37ad7ec0d8ab82b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 11 Aug 2012 12:00:31 +1000
Subject: [PATCH] AHRS: added long-term dead-reckoning

this uses airspeed (if available) or last GPS ground speed to update
our position estimate in AHRS
---
 libraries/AP_AHRS/AP_AHRS.h       | 13 +++++++-
 libraries/AP_AHRS/AP_AHRS_DCM.cpp | 52 ++++++++++++++++++++++++++++---
 libraries/AP_AHRS/AP_AHRS_DCM.h   | 14 +++++++++
 3 files changed, 74 insertions(+), 5 deletions(-)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index fc75f9e9b0..004e93416b 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -89,7 +89,18 @@ public:
 	// attitude
 	virtual Matrix3f get_dcm_matrix(void) = 0;
 
-	//static const struct AP_Param::GroupInfo var_info[];
+	// 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	
+	bool get_position(struct Location *loc) {
+		if (!_gps || _gps->status() != GPS::GPS_OK) {
+			return false;
+		}
+		loc->lat = _gps->latitude;
+		loc->lng = _gps->longitude;
+		return true;
+	}
 
 protected:
 	// pointer to compass object, if available
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 62a2f9fa60..cf1a90c185 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -402,14 +402,34 @@ AP_AHRS_DCM::drift_correction(float deltat)
         // no GPS, or no lock. We assume zero velocity. This at
         // least means we can cope with gyro drift while sitting
         // on a bench with no GPS lock
-        if (_ra_deltat < 0.1) {
+        if (_ra_deltat < 0.2) {
             // not enough time has accumulated
             return;
         }
-        velocity.zero();
-	_last_velocity.zero();
-        last_correction_time = millis();
+	float speed_estimate;
+	if (_airspeed && _airspeed->use()) {
+		// use airspeed to estimate our ground velocity in
+		// earth frame. This isn't exactly right, but is
+		// better than nothing
+		speed_estimate = _airspeed->get_airspeed() * cos(pitch);
+	} else if (_gps != NULL) {
+		// use the last gps speed for our speed estimate. This 
+		// will work for short GPS outages, or where we don't
+		// change speed a lot
+		speed_estimate = _gps->last_ground_speed();
+	} else {
+		speed_estimate = 0;
+	}
+	// calculate our ground speed vector using our speed estimate
+	velocity.x = speed_estimate * cos(yaw);
+	velocity.y = speed_estimate * sin(yaw);
+	velocity.z = 0;
+	last_correction_time = millis();
 	_have_gps_lock = false;
+
+	// update position delta for get_position()
+	_position_offset_north += velocity.x * _ra_deltat;
+	_position_offset_east  += velocity.y * _ra_deltat;
     } else {
         if (_gps->last_fix_time == _ra_sum_start) {
             // we don't have a new GPS fix - nothing more to do
@@ -423,6 +443,16 @@ AP_AHRS_DCM::drift_correction(float deltat)
 		_last_velocity = velocity;
 	}
 	_have_gps_lock = true;
+
+	// remember position for get_position()
+	_last_lat = _gps->latitude;
+	_last_lng = _gps->longitude;
+	_position_offset_north = 0;
+	_position_offset_east = 0;
+
+	// once we have a single GPS lock, we update using
+	// dead-reckoning from then on
+	_have_position = true;
     }
 
 #define USE_BAROMETER_FOR_VERTICAL_VELOCITY 1
@@ -589,3 +619,17 @@ float AP_AHRS_DCM::get_error_yaw(void)
 	_error_yaw_count = 0;
 	return _error_yaw_last;
 }
+
+// return our current position estimate using 
+// dead-reckoning or GPS
+bool AP_AHRS_DCM::get_position(struct Location *loc) 
+{
+	if (!_have_position) {
+		return false;
+	}
+	loc->lat = _last_lat;
+	loc->lng = _last_lng;
+	location_offset(loc, _position_offset_north, _position_offset_east);
+	return true;
+}
+
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index e833fe9976..61c367c846 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -38,6 +38,9 @@ public:
 	void 		update(void);
 	void 		reset(bool recover_eulers = false);
 
+	// dead-reckoning support
+	bool get_position(struct Location *loc);
+
 	// status reporting
 	float		get_error_rp(void);
 	float		get_error_yaw(void);
@@ -106,6 +109,17 @@ private:
 
 	// whether we have GPS lock
 	bool		_have_gps_lock;
+
+	// the lat/lng where we last had GPS lock
+	int32_t         _last_lat;
+	int32_t         _last_lng;
+
+	// position offset from last GPS lock
+	float		_position_offset_north;
+	float		_position_offset_east;
+
+	// whether we have a position estimate
+	bool		_have_position;
 };
 
 #endif // AP_AHRS_DCM_H