AP_AHRS: replace IMU with INS and add roll and pitch trim

libraries/AP_AHRS/AP_AHRS.cpp

@@ -56,6 +56,12 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
     // @User: Advanced
     AP_GROUPINFO("BARO_USE",  7,    AP_AHRS, _baro_use, 0),
 
+    // @Param: TRIM
+    // @DisplayName: AHRS Trim
+    // @Description: Compensates for the difference between the control board and the frame
+    // @User: Advanced
+    AP_GROUPINFO("TRIM", 8, AP_AHRS, _trim, 0),
+
     AP_GROUPEND
 };
 
@@ -88,3 +94,22 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
 	}
 	return false;
 }
+
+// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
+void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians)
+{
+    Vector3f trim = _trim.get();
+
+    // debug -- remove me!
+    Serial.printf_P(PSTR("\nadd_trim before R:%4.2f P:%4.2f\n"),ToDeg(trim.x),ToDeg(trim.y));
+
+    // add new trim
+    trim.x = constrain(trim.x + roll_in_radians, ToRad(-10.0), ToRad(10.0));
+    trim.y = constrain(trim.y + pitch_in_radians, ToRad(-10.0), ToRad(10.0));
+
+    // set and save new trim values
+    _trim.set_and_save(trim);
+
+    // debug -- remove me!
+    Serial.printf_P(PSTR("add_trim after R:%4.2f P:%4.2f\n"),ToDeg(trim.x),ToDeg(trim.y));
+}

libraries/AP_AHRS/AP_AHRS.h

@@ -14,7 +14,7 @@
 #include <AP_Compass.h>
 #include <AP_Airspeed.h>
 #include <AP_GPS.h>
-#include <AP_IMU.h>
+#include <AP_InertialSensor.h>
 #include <AP_Baro.h>
 
 #if defined(ARDUINO) && ARDUINO >= 100
@@ -27,8 +27,8 @@ class AP_AHRS
 {
 public:
     // Constructor
-    AP_AHRS(IMU *imu, GPS *&gps) :
-        _imu(imu),
+    AP_AHRS(AP_InertialSensor *ins, GPS *&gps) :
+        _ins(ins),
         _gps(gps),
         _barometer(NULL)
     {
@@ -37,7 +37,7 @@ public:
         // 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();
+        _gyro_drift_limit = ins->get_gyro_drift_rate();
     }
 
     // empty init
@@ -58,8 +58,8 @@ public:
         _airspeed = airspeed;
     }
 
-    IMU*            get_imu() { 
-	    return _imu; 
+    AP_InertialSensor*            get_ins() {
+	    return _ins;
     }
 
     // Methods
@@ -138,6 +138,15 @@ public:
         _fast_ground_gains = setting;
     }
 
+    // get strim
+    Vector3f                get_trim() { return _trim; }
+
+    // set_trim
+    virtual void            set_trim(Vector3f new_trim) { _trim.set(new_trim); }
+
+    // add_trim - adjust the roll and pitch trim up to a total of 10 degrees
+    virtual void            add_trim(float roll_in_radians, float pitch_in_radians);
+
     // settable parameters
     AP_Float _kp_yaw;
     AP_Float _kp;
@@ -164,10 +173,13 @@ protected:
 
     // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing
     //       IMU under us without our noticing.
-    IMU             *_imu;
+    AP_InertialSensor   *_ins;
     GPS                 *&_gps;
     AP_Baro             *_barometer;
 
+    // a vector to capture the difference between the controller and body frames
+    AP_Vector3f         _trim;
+
     // should we raise the gain on the accelerometers for faster
     // convergence, used when disarmed for ArduCopter
     bool _fast_ground_gains;
@@ -182,6 +194,7 @@ protected:
 
     // acceleration due to gravity in m/s/s
     static const float _gravity = 9.80665;
+
 };
 
 #include <AP_AHRS_DCM.h>

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -38,14 +38,14 @@ AP_AHRS_DCM::update(void)
     float delta_t;
 
     // tell the IMU to grab some data
-    _imu->update();
+    _ins->update();
 
     // ask the IMU how much time this sensor reading represents
-    delta_t = _imu->get_delta_time();
+    delta_t = _ins->get_delta_time();
 
     // Get current values for gyros
-    _gyro_vector  = _imu->get_gyro();
-    _accel_vector = _imu->get_accel();
+    _gyro_vector  = _ins->get_gyro();
+    _accel_vector = _ins->get_accel();
 
     // Integrate the DCM matrix using gyro inputs
     matrix_update(delta_t);
@@ -376,6 +376,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
 void
 AP_AHRS_DCM::drift_correction(float deltat)
 {
+    Matrix3f temp_dcm = _dcm_matrix;
     Vector3f velocity;
     uint32_t last_correction_time;
 
@@ -383,8 +384,11 @@ AP_AHRS_DCM::drift_correction(float deltat)
     // vector
     drift_correction_yaw();
 
+    // apply trim
+    temp_dcm.rotate(_trim);
+
     // integrate the accel vector in the earth frame between GPS readings
-    _ra_sum += _dcm_matrix * (_accel_vector * deltat);
+    _ra_sum += temp_dcm * (_accel_vector * deltat);
 
     // keep a sum of the deltat values, so we know how much time
     // we have integrated over

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -14,7 +14,7 @@ class AP_AHRS_DCM : public AP_AHRS
 {
 public:
     // Constructors
-    AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
+    AP_AHRS_DCM(AP_InertialSensor *ins, GPS *&gps) : AP_AHRS(ins, gps)
     {
         _dcm_matrix.identity();
 

libraries/AP_AHRS/AP_AHRS_HIL.h

@@ -5,7 +5,7 @@ class AP_AHRS_HIL : public AP_AHRS
 {
 public:
     // Constructors
-    AP_AHRS_HIL(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps)
+    AP_AHRS_HIL(AP_InertialSensor *ins, GPS *&gps) : AP_AHRS(ins, gps)
     {
     }
 
@@ -22,7 +22,7 @@ public:
 
     // Methods
     void            update(void) {
-        _imu->update();
+        _ins->update();
     }
     
     void            setHil(float roll, float pitch, float yaw,

libraries/AP_AHRS/AP_AHRS_MPU6000.cpp

@@ -40,7 +40,7 @@ AP_AHRS_MPU6000::init( AP_PeriodicProcess * scheduler )
 
     _mpu6000->dmp_init();
     push_gains_to_dmp();
-    push_offsets_to_ins();
+    _mpu6000->push_gyro_offsets_to_dmp();
 
     // restart timer
     if( timer_running ) {
@@ -56,11 +56,11 @@ AP_AHRS_MPU6000::update(void)
 
     // tell the IMU to grab some data.
     if( !_secondary_ahrs ) {
-        _imu->update();
+        _ins->update();
     }
 
     // ask the IMU how much time this sensor reading represents
-    delta_t = _imu->get_delta_time();
+    delta_t = _ins->get_delta_time();
 
     // convert the quaternions into a DCM matrix
     _mpu6000->quaternion.rotation_matrix(_dcm_matrix);
@@ -95,7 +95,7 @@ void AP_AHRS_MPU6000::drift_correction( float deltat )
     float errorRollPitch[2];
 
     // Get current values for gyros
-    _accel_vector = _imu->get_accel();
+    _accel_vector = _ins->get_accel();
 
     // We take the accelerometer readings and cumulate to average them and obtain the gravity vector
     _accel_filtered += _accel_vector;
@@ -172,16 +172,10 @@ void
 AP_AHRS_MPU6000::push_offsets_to_ins()
 {
     // push down gyro offsets (TO-DO: why are x and y offsets are reversed?!)
-    _mpu6000->set_gyro_offsets_scaled(((AP_IMU_INS*)_imu)->gy(),((AP_IMU_INS*)_imu)->gx(),((AP_IMU_INS*)_imu)->gz());
-    ((AP_IMU_INS*)_imu)->gx(0);
-    ((AP_IMU_INS*)_imu)->gy(0);
-    ((AP_IMU_INS*)_imu)->gz(0);
+    _mpu6000->push_gyro_offsets_to_dmp();
 
     // push down accelerometer offsets (TO-DO: why are x and y offsets are reversed?!)
-    _mpu6000->set_accel_offsets_scaled(((AP_IMU_INS*)_imu)->ay(), ((AP_IMU_INS*)_imu)->ax(), ((AP_IMU_INS*)_imu)->az());
-    //((AP_IMU_INS*)_imu)->ax(0);
-    //((AP_IMU_INS*)_imu)->ay(0);
-    //((AP_IMU_INS*)_imu)->az(0);
+    _mpu6000->push_accel_offsets_to_dmp();
 }
 
 void

libraries/AP_AHRS/AP_AHRS_MPU6000.h

@@ -20,7 +20,7 @@ class AP_AHRS_MPU6000 : public AP_AHRS
 {
 public:
     // Constructors
-    AP_AHRS_MPU6000(AP_IMU_INS *imu, GPS *&gps, AP_InertialSensor_MPU6000 *mpu6000) : AP_AHRS(imu, gps), _mpu6000(mpu6000)
+    AP_AHRS_MPU6000(AP_InertialSensor_MPU6000 *mpu6000, GPS *&gps) : AP_AHRS(mpu6000, gps), _mpu6000(mpu6000)
     {
         _dcm_matrix.identity();
 
@@ -39,7 +39,7 @@ public:
 
     // return the smoothed gyro vector corrected for drift
     Vector3f        get_gyro(void) {
-        return _imu->get_gyro();
+        return _ins->get_gyro();
     }
     Matrix3f        get_dcm_matrix(void) {
         return _dcm_matrix;