diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 7f86721a11..56fe27c634 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/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)); +} \ No newline at end of file diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 9dd4e40775..c971dc8f40 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include #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,9 +173,12 @@ 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; - GPS *&_gps; - AP_Baro *_barometer; + 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 @@ -182,6 +194,7 @@ protected: // acceleration due to gravity in m/s/s static const float _gravity = 9.80665; + }; #include diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index f66a442347..39fce9afa5 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index f6263252ca..6f078a1714 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/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(); diff --git a/libraries/AP_AHRS/AP_AHRS_HIL.h b/libraries/AP_AHRS/AP_AHRS_HIL.h index 469f799bff..52e17ca557 100644 --- a/libraries/AP_AHRS/AP_AHRS_HIL.h +++ b/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, diff --git a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp b/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp index e7e1d15228..bfbd954df5 100644 --- a/libraries/AP_AHRS/AP_AHRS_MPU6000.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_MPU6000.h b/libraries/AP_AHRS/AP_AHRS_MPU6000.h index ba22c82900..73d1dd9424 100644 --- a/libraries/AP_AHRS/AP_AHRS_MPU6000.h +++ b/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;