AHRS: adapt the DCM library to the AHRS framework

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -1,22 +1,19 @@
 /*
-	APM_DCM.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega
-		Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
+	APM_AHRS_DCM.cpp
 
-	This library works with the ArduPilot Mega and "Oilpan"
+	AHRS system using DCM matrices
 
-	This library is free software; you can redistribute it and/or
-    modify it under the terms of the GNU Lesser General Public
-    License as published by the Free Software Foundation; either
-    version 2.1 of the License, or (at your option) any later version.
+	Based on DCM code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
 
-        Methods:
-				update_DCM()	: Updates the AHRS by integrating the rotation matrix over time using the IMU object data
-				get_gyro()			: Returns gyro vector corrected for bias
-				get_accel()		: Returns accelerometer vector
-				get_dcm_matrix()	: Returns dcm matrix
+	Adapted for the general ArduPilot AHRS interface by Andrew Tridgell
 
+	This library is free software; you can redistribute it and/or
+	modify it under the terms of the GNU Lesser General Public License
+	as published by the Free Software Foundation; either version 2.1
+	of the License, or (at your option) any later version.
 */
-#include <AP_DCM.h>
+#include <FastSerial.h>
+#include <AP_AHRS.h>
 
 // this is the speed in cm/s above which we first get a yaw lock with
 // the GPS
@@ -26,21 +23,11 @@
 // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
 #define GPS_SPEED_RESET 100
 
-void
-AP_DCM::set_compass(Compass *compass)
-{
-	_compass = compass;
-}
-
-
 // run a full DCM update round
-// the drift_correction_frequency is how many steps of the algorithm
-// to run before doing an accelerometer and yaw drift correction step
 void
-AP_DCM::update_DCM(uint8_t drift_correction_frequency)
+AP_AHRS_DCM::update(void)
 {
 	float delta_t;
-	Vector3f accel;
 
 	// tell the IMU to grab some data
 	_imu->update();
@@ -50,43 +37,16 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency)
 
 	// Get current values for gyros
 	_gyro_vector  = _imu->get_gyro();
-
-	// accumulate some more accelerometer data
-	accel = _imu->get_accel();
-	_accel_sum += accel;
-	_drift_correction_time += delta_t;
+	_accel_vector = _imu->get_accel();
 
 	// Integrate the DCM matrix using gyro inputs
 	matrix_update(delta_t);
 
-	// add up the omega vector so we pass a value to the drift
-	// correction averaged over the same time period as the
-	// accelerometers
-	_omega_sum += _omega_integ_corr;
-
 	// Normalize the DCM matrix
 	normalize();
 
-	// see if we will perform drift correction on this call
-	_drift_correction_count++;
-
-	if (_drift_correction_count >= drift_correction_frequency) {
-		// calculate the average accelerometer vector over
-		// since the last drift correction call
-		float scale = 1.0 / _drift_correction_count;
-		_accel_vector = _accel_sum * scale;
-		_accel_sum.zero();
-
-		// calculate the average omega value over this time
-		_omega_smoothed = _omega_sum * scale;
-		_omega_sum.zero();
-
-		// Perform drift correction
-		drift_correction(_drift_correction_time);
-
-		_drift_correction_time = 0;
-		_drift_correction_count = 0;
-	}
+	// Perform drift correction
+	drift_correction(delta_t);
 
 	// paranoid check for bad values in the DCM matrix
 	check_matrix();
@@ -97,7 +57,7 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency)
 
 // update the DCM matrix using only the gyros
 void
-AP_DCM::matrix_update(float _G_Dt)
+AP_AHRS_DCM::matrix_update(float _G_Dt)
 {
 	// _omega_integ_corr is used for _centripetal correction
 	// (theoretically better than _omega)
@@ -132,7 +92,7 @@ AP_DCM::matrix_update(float _G_Dt)
 
 // adjust an accelerometer vector for known acceleration forces
 void
-AP_DCM::accel_adjust(Vector3f &accel)
+AP_AHRS_DCM::accel_adjust(Vector3f &accel)
 {
 	float veloc;
 	// compensate for linear acceleration. This makes a
@@ -149,8 +109,8 @@ AP_DCM::accel_adjust(Vector3f &accel)
 	// direction as positive
 
 	// Equation 26 broken up into separate pieces
-	accel.y -= _omega_smoothed.z * veloc;
-	accel.z += _omega_smoothed.y * veloc;
+	accel.y -= _omega_integ_corr.z * veloc;
+	accel.z += _omega_integ_corr.y * veloc;
 }
 
 /*
@@ -158,7 +118,7 @@ AP_DCM::accel_adjust(Vector3f &accel)
   extreme errors in the matrix
  */
 void
-AP_DCM::matrix_reset(bool recover_eulers)
+AP_AHRS_DCM::reset(bool recover_eulers)
 {
 	if (_compass != NULL) {
 		_compass->null_offsets_disable();
@@ -169,7 +129,6 @@ AP_DCM::matrix_reset(bool recover_eulers)
 	_omega_P.zero();
 	_omega_yaw_P.zero();
 	_omega_integ_corr.zero();
-	_omega_smoothed.zero();
 	_omega.zero();
 
 	// if the caller wants us to try to recover to the current
@@ -193,13 +152,13 @@ AP_DCM::matrix_reset(bool recover_eulers)
   check the DCM matrix for pathological values
  */
 void
-AP_DCM::check_matrix(void)
+AP_AHRS_DCM::check_matrix(void)
 {
 	if (_dcm_matrix.is_nan()) {
 		//Serial.printf("ERROR: DCM matrix NAN\n");
 		SITL_debug("ERROR: DCM matrix NAN\n");
 		renorm_blowup_count++;
-		matrix_reset(true);
+		reset(true);
 		return;
 	}
 	// some DCM matrix values can lead to an out of range error in
@@ -221,7 +180,7 @@ AP_DCM::check_matrix(void)
 			SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n",
 				   _dcm_matrix.c.x);
 			renorm_blowup_count++;
-			matrix_reset(true);
+			reset(true);
 		}
 	}
 }
@@ -229,7 +188,7 @@ AP_DCM::check_matrix(void)
 // renormalise one vector component of the DCM matrix
 // this will return false if renormalization fails
 bool
-AP_DCM::renorm(Vector3f const &a, Vector3f &result)
+AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result)
 {
 	float	renorm_val;
 
@@ -289,7 +248,7 @@ simple matter to stay ahead of it.
 We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
 */
 void
-AP_DCM::normalize(void)
+AP_AHRS_DCM::normalize(void)
 {
 	float error;
 	Vector3f t0, t1, t2;
@@ -305,7 +264,7 @@ AP_DCM::normalize(void)
 	    !renorm(t2, _dcm_matrix.c)) {
 		// Our solution is blowing up and we will force back
 		// to last euler angles
-		matrix_reset(true);
+		reset(true);
 	}
 }
 
@@ -319,7 +278,7 @@ AP_DCM::normalize(void)
 // This function also updates _omega_yaw_P with a yaw correction term
 // from our yaw reference vector
 void
-AP_DCM::drift_correction(float deltat)
+AP_AHRS_DCM::drift_correction(float deltat)
 {
 	float error_course = 0;
 	Vector3f accel;
@@ -530,7 +489,7 @@ AP_DCM::drift_correction(float deltat)
 // calculate the euler angles which will be used for high level
 // navigation control
 void
-AP_DCM::euler_angles(void)
+AP_AHRS_DCM::euler_angles(void)
 {
 	_dcm_matrix.to_euler(&roll, &pitch, &yaw);
 
@@ -544,27 +503,8 @@ AP_DCM::euler_angles(void)
 
 /* reporting of DCM state for MAVLink */
 
-// average accel_weight since last call
-float AP_DCM::get_accel_weight(void)
-{
-	return 1.0;
-}
-
-// average renorm_val since last call
-float AP_DCM::get_renorm_val(void)
-{
-	float ret;
-	if (_renorm_val_count == 0) {
-		return 0;
-	}
-	ret = _renorm_val_sum / _renorm_val_count;
-	_renorm_val_sum = 0;
-	_renorm_val_count = 0;
-	return ret;
-}
-
 // average error_roll_pitch since last call
-float AP_DCM::get_error_rp(void)
+float AP_AHRS_DCM::get_error_rp(void)
 {
 	float ret;
 	if (_error_rp_count == 0) {
@@ -577,7 +517,7 @@ float AP_DCM::get_error_rp(void)
 }
 
 // average error_yaw since last call
-float AP_DCM::get_error_yaw(void)
+float AP_AHRS_DCM::get_error_yaw(void)
 {
 	float ret;
 	if (_error_yaw_count == 0) {

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -1,85 +1,44 @@
-#ifndef AP_DCM_h
-#define AP_DCM_h
-
-// temporarily include all other classes here
-// since this naming is a bit off from the
-// convention and the AP_DCM should be the top
-// header file
-#include "AP_DCM_HIL.h"
-
-#include "../FastSerial/FastSerial.h"
-#include "../AP_Math/AP_Math.h"
-#include <inttypes.h>
-#include "../AP_Compass/AP_Compass.h"
-#include "../AP_ADC/AP_ADC.h"
-#include "../AP_GPS/AP_GPS.h"
-#include "../AP_IMU/AP_IMU.h"
-#if defined(ARDUINO) && ARDUINO >= 100
-	#include "Arduino.h"
-#else
-	#include "WProgram.h"
-#endif
-
-class AP_DCM
+#ifndef AP_AHRS_DCM_H
+#define AP_AHRS_DCM_H
+/*
+  DCM based AHRS (Attitude Heading Reference System) interface for
+  ArduPilot
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+*/
+
+class AP_AHRS_DCM : public AP_AHRS
 {
 public:
 	// Constructors
-	AP_DCM(IMU *imu, GPS *&gps) :
-		_kp_roll_pitch(0.13),
-		_kp_yaw(0.4),
-		_gps(gps),
-		_imu(imu),
-		_dcm_matrix(1, 0, 0,
-			    0, 1, 0,
-			    0, 0, 1),
-		_health(1.),
-		_toggle(0)
+	AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
 	{
-		// base the ki values by the sensors maximum drift
-		// rate. The APM2 has gyros which are much less drift
-		// prone than the APM1, so we should have a lower ki,
-		// which will make us less prone to increasing omegaI
-		// incorrectly due to sensor noise
-		_gyro_drift_limit = imu->get_gyro_drift_rate();
+		_kp_roll_pitch = 0.13;
+		_kp_yaw        = 0.4;
+		_dcm_matrix(Vector3f(1, 0, 0),
+			    Vector3f(0, 1, 0),
+			    Vector3f(0, 0, 1));
+
+		// base the ki values on the sensors drift rate
 		_ki_roll_pitch = _gyro_drift_limit * 5;
 		_ki_yaw        = _gyro_drift_limit * 8;
 	}
 
-	// Accessors
-
 	// return the smoothed gyro vector corrected for drift
-	Vector3f	get_gyro(void) {return _omega_smoothed; }
+	Vector3f	get_gyro(void) {return _omega; }
 	Matrix3f	get_dcm_matrix(void) {return _dcm_matrix; }
-	Matrix3f	get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix;  return temp.transpose();}
 
 	// return the current drift correction integrator value
 	Vector3f	get_gyro_drift(void) {return _omega_I; }
 
-	float		get_health(void) {return _health;}
-	void		set_centripetal(bool b) {_centripetal = b;}
-	bool		get_centripetal(void) {return _centripetal;}
-	void		set_compass(Compass *compass);
-
 	// Methods
-	void 		update_DCM(uint8_t drift_correction_frequency=1);
-	void 		update(void) { update_DCM(); }
-	void 		matrix_reset(bool recover_eulers = false);
-
-	long		roll_sensor;					// Degrees * 100
-	long		pitch_sensor;					// Degrees * 100
-	long		yaw_sensor;					// Degrees * 100
-
-	float		roll;							// Radians
-	float		pitch;							// Radians
-	float		yaw;							// Radians
-
-	uint8_t 	gyro_sat_count;
-	uint8_t 	renorm_range_count;
-	uint8_t 	renorm_blowup_count;
+	void 		update(void);
+	void 		reset(bool recover_eulers = false);
 
 	// status reporting
-	float		get_accel_weight(void);
-	float		get_renorm_val(void);
 	float		get_error_rp(void);
 	float		get_error_yaw(void);
 
@@ -88,13 +47,10 @@ private:
 	float		_ki_roll_pitch;
 	float		_kp_yaw;
 	float		_ki_yaw;
-	float		_gyro_drift_limit;          // radians/s/s
 	bool		_have_initial_yaw;
 
 	// Methods
-	void 		read_adc_raw(void);
 	void 		accel_adjust(Vector3f &accel);
-	float 		read_adc(int select);
 	void 		matrix_update(float _G_Dt);
 	void 		normalize(void);
 	void		check_matrix(void);
@@ -102,34 +58,17 @@ private:
 	void 		drift_correction(float deltat);
 	void 		euler_angles(void);
 
-	// members
-	Compass 	* _compass;
-
-	// 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;                     // note: this is a reference to a pointer owned by the caller
-
-	IMU 		*_imu;
-
+	// primary representation of attitude
 	Matrix3f	_dcm_matrix;
 
-	// sum of accel vectors between drift_correction() calls
-	// this allows the drift correction to run at a different rate
-	// to the main DCM update code
-	Vector3f 	_accel_vector;
-	Vector3f 	_accel_sum;
-
 	Vector3f 	_gyro_vector;		// Store the gyros turn rate in a vector
+	Vector3f 	_accel_vector;		// current accel vector
+
 	Vector3f	_omega_P;		// accel Omega Proportional correction
 	Vector3f	_omega_yaw_P;		// yaw Omega Proportional correction
 	Vector3f 	_omega_I;		// Omega Integrator correction
 	Vector3f 	_omega_integ_corr;	// Partially corrected Gyro_Vector data - used for centrepetal correction
 	Vector3f 	_omega;			// Corrected Gyro_Vector data
-	Vector3f 	_omega_sum;
-	Vector3f 	_omega_smoothed;
-	float		_health;
-	bool		_centripetal;
-	uint8_t		_toggle;
 
 	// state to support status reporting
 	float		_renorm_val_sum;
@@ -139,16 +78,8 @@ private:
 	float		_error_yaw_sum;
 	uint16_t	_error_yaw_count;
 
-	// time in micros when we last got a compass fix
-	uint32_t	_compass_last_update;
-
 	// time in millis when we last got a GPS heading
 	uint32_t	_gps_last_update;
-
-	// counter of calls to update_DCM() without drift correction
-	uint8_t         _drift_correction_count;
-	float		_drift_correction_time;
-
 };
 
-#endif
+#endif // AP_AHRS_DCM_H