AHRS: adapt the quaternion library to AHRS

libraries/AP_AHRS/AP_AHRS_Quaternion.cpp

 /*
-	AP_Quaternion code, based on quaternion code from Jeb Madgwick
+	AP_AHRS_Quaternion code, based on quaternion code from Jeb Madgwick
 
 	See http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
 
@@ -13,24 +13,7 @@
 	version.
 */
 #include <FastSerial.h>
-#include <AP_Quaternion.h>
-
-#define ToRad(x) (x*0.01745329252)	// *pi/180
-#define ToDeg(x) (x*57.2957795131)	// *180/pi
-
-// this is the speed in cm/s above which we first get a yaw lock with
-// the GPS
-#define GPS_SPEED_MIN 300
-
-// this is the speed in cm/s at which we stop using drift correction
-// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
-#define GPS_SPEED_RESET 100
-
-void
-AP_Quaternion::set_compass(Compass *compass)
-{
-	_compass = compass;
-}
+#include <AP_AHRS.h>
 
 // to keep the code as close to the original as possible, we use these
 // macros for quaternion access
@@ -40,7 +23,7 @@ AP_Quaternion::set_compass(Compass *compass)
 #define SEq_4 q.q4
 
 // Function to compute one quaternion iteration without magnetometer
-void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
+void AP_AHRS_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
 {
     // Local system variables
     float norm;                                                            // vector norm
@@ -134,7 +117,7 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel)
 
 
 // Function to compute one quaternion iteration including magnetometer
-void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag)
+void AP_AHRS_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag)
 {
     // local system variables
     float norm;                                                             //                vector norm
@@ -253,7 +236,7 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
 
     // don't allow the drift rate to be exceeded. This prevents a
     // sudden drift change coming from a outage in the compass
-    float max_change = gyroMeasDrift * deltat;
+    float max_change = _gyro_drift_limit * deltat;
     drift_delta.x = constrain(drift_delta.x, -max_change, max_change);
     drift_delta.y = constrain(drift_delta.y, -max_change, max_change);
     drift_delta.z = constrain(drift_delta.z, -max_change, max_change);
@@ -308,161 +291,8 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V
 }
 
 
-// Function to update our gyro_bias drift estimate using the accelerometer
-// and magnetometer. This is a cut-down version of update_MARG(), but
-// without the quaternion update.
-void AP_Quaternion::update_drift(float deltat, Vector3f &mag)
-{
-    // local system variables
-    float norm;                                                             //                vector norm
-    float f_4, f_5, f_6;                                     //                objective function elements
-    float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33,               //                objective function Jacobian elements
-    J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; //
-    float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4;               //                estimated direction of the gyroscope error
-
-    // computed flux in the earth frame
-    Vector3f flux;
-
-    // estimated direction of the gyroscope error (radians)
-    Vector3f w_err;
-
-    // normalise the magnetometer measurement
-    mag.normalize();
-    if (mag.is_inf()) {
-	    // discard this data point
-	    renorm_range_count++;
-	    return;
-    }
-
-    // auxiliary variables to avoid repeated calculations
-    float twoSEq_1 = 2.0f * SEq_1;
-    float twoSEq_2 = 2.0f * SEq_2;
-    float twoSEq_3 = 2.0f * SEq_3;
-    float twoSEq_4 = 2.0f * SEq_4;
-    float twob_x = 2.0f * b_x;
-    float twob_z = 2.0f * b_z;
-    float twob_xSEq_1 = 2.0f * b_x * SEq_1;
-    float twob_xSEq_2 = 2.0f * b_x * SEq_2;
-    float twob_xSEq_3 = 2.0f * b_x * SEq_3;
-    float twob_xSEq_4 = 2.0f * b_x * SEq_4;
-    float twob_zSEq_1 = 2.0f * b_z * SEq_1;
-    float twob_zSEq_2 = 2.0f * b_z * SEq_2;
-    float twob_zSEq_3 = 2.0f * b_z * SEq_3;
-    float twob_zSEq_4 = 2.0f * b_z * SEq_4;
-    float SEq_1SEq_2;
-    float SEq_1SEq_3 = SEq_1 * SEq_3;
-    float SEq_1SEq_4;
-    float SEq_2SEq_3;
-    float SEq_2SEq_4 = SEq_2 * SEq_4;
-    float SEq_3SEq_4;
-    Vector3f twom = mag * 2.0;
-
-    // compute the objective function and Jacobian
-    f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - mag.x;
-    f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - mag.y;
-    f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - mag.z;
-    J_11or24 = twoSEq_3;                                  // J_11 negated in matrix multiplication
-    J_12or23 = 2.0f * SEq_4;
-    J_13or22 = twoSEq_1;                                  // J_12 negated in matrix multiplication
-    J_14or21 = twoSEq_2;
-    J_32 = 2.0f * J_14or21;                               // negated in matrix multiplication
-    J_33 = 2.0f * J_11or24;                               // negated in matrix multiplication
-    J_41 = twob_zSEq_3;                                   // negated in matrix multiplication
-    J_42 = twob_zSEq_4;
-    J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1;              // negated in matrix multiplication
-    J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2;              // negated in matrix multiplication
-    J_51 = twob_xSEq_4 - twob_zSEq_2;                     // negated in matrix multiplication
-    J_52 = twob_xSEq_3 + twob_zSEq_1;
-    J_53 = twob_xSEq_2 + twob_zSEq_4;
-    J_54 = twob_xSEq_1 - twob_zSEq_3;                     // negated in matrix multiplication
-    J_61 = twob_xSEq_3;
-    J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2;
-    J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3;
-    J_64 = twob_xSEq_2;
-
-    // compute the gradient (matrix multiplication)
-    SEqHatDot_1 = - J_41 * f_4 - J_51 * f_5 + J_61 * f_6;
-    SEqHatDot_2 = + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
-    SEqHatDot_3 = - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
-    SEqHatDot_4 = - J_44 * f_4 - J_54 * f_5 + J_64 * f_6;
-
-    // normalise the gradient to estimate direction of the gyroscope error
-    norm = 1.0 / safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
-    if (isinf(norm)) {
-	    // discard this data point
-	    renorm_range_count++;
-	    return;
-    }
-    SEqHatDot_1 *= norm;
-    SEqHatDot_2 *= norm;
-    SEqHatDot_3 *= norm;
-    SEqHatDot_4 *= norm;
-
-    // compute angular estimated direction of the gyroscope error
-    w_err.x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3;
-    w_err.y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2;
-    w_err.z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;
-
-    // keep track of the error rates
-    _error_rp_sum += 0.5*(fabs(w_err.x) + fabs(w_err.y));
-    _error_yaw_sum += fabs(w_err.z);
-    _error_rp_count++;
-    _error_yaw_count++;
-
-    // compute the gyroscope bias delta
-    Vector3f drift_delta = w_err * (deltat * zeta);
-
-    // don't allow the drift rate to be exceeded. This prevents a
-    // sudden drift change coming from a outage in the compass
-    float max_change = gyroMeasDrift * deltat;
-    drift_delta.x = constrain(drift_delta.x, -max_change, max_change);
-    drift_delta.y = constrain(drift_delta.y, -max_change, max_change);
-    drift_delta.z = constrain(drift_delta.z, -max_change, max_change);
-    gyro_bias += drift_delta;
-
-    // compute then integrate the estimated quaternion rate
-    SEq_1 -= (beta * SEqHatDot_1) * deltat;
-    SEq_2 -= (beta * SEqHatDot_2) * deltat;
-    SEq_3 -= (beta * SEqHatDot_3) * deltat;
-    SEq_4 -= (beta * SEqHatDot_4) * deltat;
-
-    // normalise quaternion
-    norm = 1.0/safe_sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
-    if (isinf(norm)) {
-	    // our quaternion is bad! Reset based on roll/pitch/yaw
-	    // and hope for the best ...
-	    renorm_blowup_count++;
-	    _compass->null_offsets_disable();
-	    q.from_euler(roll, pitch, yaw);
-	    _compass->null_offsets_disable();
-	    return;
-    }
-    SEq_1 *= norm;
-    SEq_2 *= norm;
-    SEq_3 *= norm;
-    SEq_4 *= norm;
-
-    // compute flux in the earth frame
-    // recompute axulirary variables
-    SEq_1SEq_2 = SEq_1 * SEq_2;
-    SEq_1SEq_3 = SEq_1 * SEq_3;
-    SEq_1SEq_4 = SEq_1 * SEq_4;
-    SEq_3SEq_4 = SEq_3 * SEq_4;
-    SEq_2SEq_3 = SEq_2 * SEq_3;
-    SEq_2SEq_4 = SEq_2 * SEq_4;
-    flux.x = twom.x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom.y * (SEq_2SEq_3 - SEq_1SEq_4) + twom.z * (SEq_2SEq_4 + SEq_1SEq_3);
-    flux.y = twom.x * (SEq_2SEq_3 + SEq_1SEq_4) + twom.y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom.z * (SEq_3SEq_4 - SEq_1SEq_2);
-    flux.z = twom.x * (SEq_2SEq_4 - SEq_1SEq_3) + twom.y * (SEq_3SEq_4 + SEq_1SEq_2) + twom.z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3);
-
-    // normalise the flux vector to have only components in the x and z
-    b_x = sqrt((flux.x * flux.x) + (flux.y * flux.y));
-    b_z = flux.z;
-}
-
-
-
 // Function to compute one quaternion iteration
-void AP_Quaternion::update(void)
+void AP_AHRS_Quaternion::update(void)
 {
 	Vector3f gyro, accel;
 	float deltat;
@@ -518,42 +348,15 @@ void AP_Quaternion::update(void)
 		accel.z += (gyro.y - gyro_bias.y) * veloc;
 	}
 
-#define SEPARATE_DRIFT 0
-#if SEPARATE_DRIFT
-	/*
-	  The full Madgwick quaternion 'MARG' system assumes you get
-	  gyro, accel and magnetometer updates at the same rate. On
-	  APM we get them at very different rates, and re-calculating
-	  our drift due to the magnetometer in the fast loop is very
-	  wasteful of CPU.
-
-	  Instead, we only update the gyro_bias vector when we get a
-	  new magnetometer point, and use the much simpler
-	  update_IMU() as the main quaternion update function.
-	 */
-	_gyro_sum += gyro;
-	_accel_sum += accel;
-	_sum_count++;
-
-	if (_compass != NULL && _compass->use_for_yaw() &&
-	    _compass->last_update != _compass_last_update &&
-	    _sum_count != 0) {
-		// use new compass sample for drift correction
-		float mag_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
+	if (_compass != NULL && _compass->use_for_yaw()) {
 		Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
-		_sum_count = 0;
-		update_drift(mag_deltat, mag);
-		_compass_last_update = _compass->last_update;
+		update_MARG(deltat, gyro, accel, mag);
+	} else {
+		// step the quaternion solution using just gyros and accels
+		gyro -= gyro_bias;
+		update_IMU(deltat, gyro, accel);
 	}
 
-	// step the quaternion solution using just gyros and accels
-	gyro -= gyro_bias;
-	update_IMU(deltat, gyro, accel);
-#else
-	Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
-	update_MARG(deltat, gyro, accel, mag);
-#endif
-
 #ifdef DESKTOP_BUILD
 	if (q.is_nan()) {
 		SITL_debug("QUAT NAN: deltat=%f roll=%f pitch=%f yaw=%f q=[%f %f %f %f] a=[%f %f %f] g=(%f %f %f)\n",
@@ -587,7 +390,7 @@ void AP_Quaternion::update(void)
 }
 
 // average error in roll/pitch since last call
-float AP_Quaternion::get_error_rp(void)
+float AP_AHRS_Quaternion::get_error_rp(void)
 {
 	float ret;
 	if (_error_rp_count == 0) {
@@ -600,7 +403,7 @@ float AP_Quaternion::get_error_rp(void)
 }
 
 // average error in yaw since last call
-float AP_Quaternion::get_error_yaw(void)
+float AP_AHRS_Quaternion::get_error_yaw(void)
 {
 	float ret;
 	if (_error_yaw_count == 0) {
@@ -611,3 +414,18 @@ float AP_Quaternion::get_error_yaw(void)
 	_error_yaw_count = 0;
 	return ret;
 }
+
+// reset attitude system
+void AP_AHRS_Quaternion::reset(bool recover_eulers)
+{
+	if (recover_eulers) {
+		q.from_euler(roll, pitch, yaw);
+	} else {
+		q(1, 0, 0, 0);
+	}
+	gyro_bias.zero();
+
+	// reference direction of flux in earth frame
+	b_x = 0;
+	b_z = -1;
+}

libraries/AP_AHRS/AP_AHRS_Quaternion.h

@@ -13,66 +13,41 @@
 	#include "WProgram.h"
 #endif
 
-class AP_Quaternion
+class AP_AHRS_Quaternion : public AP_AHRS
 {
 public:
 	// Constructor
-	AP_Quaternion(IMU *imu, GPS *&gps) :
-		_imu(imu),
-		_gps(gps)
+	AP_AHRS_Quaternion(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
 	{
-		// reference direction of flux in earth frame
-		b_x = 0;
-		b_z = -1;
-
-		// limit the drift to the drift rate reported by the
-		// sensor driver
-		gyroMeasDrift = imu->get_gyro_drift_rate();
-
 		// scaled gyro drift limits
 		beta = sqrt(3.0f / 4.0f) * gyroMeasError;
-		zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift;
-	}
+		zeta = sqrt(3.0f / 4.0f) * _gyro_drift_limit;
 
-	// Accessors
-	void		set_centripetal(bool b) {_centripetal = b;}
-	bool		get_centripetal(void) {return _centripetal;}
-	void		set_compass(Compass *compass);
+		// reset attitude
+		reset();
+	}
 
 	// Methods
 	void 		update(void);
+	void 		reset(bool recover_eulers=false);
 
-	// Euler angles (radians)
-	float		roll;
-	float		pitch;
-	float		yaw;
-
-	// integer Euler angles (Degrees * 100)
-	int32_t		roll_sensor;
-	int32_t		pitch_sensor;
-	int32_t		yaw_sensor;
-
-
-	// compatibility methods with DCM
-	void update_DCM(void) { update(); }
-	void update_DCM_fast(void) { update(); }
+	// get corrected gyro vector
 	Vector3f get_gyro(void) {
 		// notice the sign reversals here
 		return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z);
 	}
+
 	Vector3f get_gyro_drift(void) {
-		// notice the sign reversals here
-		return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z);
+		// notice the sign reversals here. The quaternion
+		// system uses a -ve gyro bias, DCM uses a +ve
+		return Vector3f(gyro_bias.x, gyro_bias.y, -gyro_bias.z);
         }
-	float get_accel_weight(void) { return 0; }
-	float get_renorm_val(void) { return 0; }
-	float get_health(void) { return 0; }
-	void matrix_reset(void) { }
-	uint8_t gyro_sat_count;
-	uint8_t renorm_range_count;
-	uint8_t renorm_blowup_count;
+
 	float get_error_rp(void);
 	float get_error_yaw(void);
+
+	// convert quaternion to a DCM matrix, used by compass
+	// null offsets code
 	Matrix3f get_dcm_matrix(void) {
 		Matrix3f ret;
 		q.rotation_matrix(ret);
@@ -82,31 +57,16 @@ public:
 private:
 	void 		update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
 	void 		update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
-	void 		update_drift(float deltat, Vector3f &mag);
 
 	bool		_have_initial_yaw;
 
 	// Methods
 	void 		accel_adjust(void);
 
-	// members
-	Compass 	* _compass;
-	// time in microseconds of last compass update
-	uint32_t        _compass_last_update;
-
-	// note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
-	//       IMU under us without our noticing.
-	GPS 		*&_gps;
-	IMU 		*_imu;
-
-	// true if we are doing centripetal acceleration correction
-	bool		_centripetal;
-
 	// maximum gyroscope measurement error in rad/s (set to 7 degrees/second)
 	static const float gyroMeasError = 20.0 * (M_PI/180.0);
 
-	float gyroMeasDrift;
-
+	// scaled tuning constants
 	float beta;
 	float zeta;
 
@@ -124,11 +84,6 @@ private:
 	// the current corrected gyro vector
 	Vector3f _gyro_corrected;
 
-	// accel and gyro accumulators for drift correction
-	Vector3f _gyro_sum;
-	Vector3f _accel_sum;
-	uint32_t _sum_count;
-
 	// estimate of error
 	float		_error_rp_sum;
 	uint16_t	_error_rp_count;