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

This commit is contained in:
rmackay9 2012-11-05 13:29:00 +09:00
parent e835cef181
commit 8ab0611261
7 changed files with 67 additions and 31 deletions

View File

@ -56,6 +56,12 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] PROGMEM = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("BARO_USE", 7, AP_AHRS, _baro_use, 0), 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 AP_GROUPEND
}; };
@ -88,3 +94,22 @@ bool AP_AHRS::airspeed_estimate(float *airspeed_ret)
} }
return false; 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));
}

View File

@ -14,7 +14,7 @@
#include <AP_Compass.h> #include <AP_Compass.h>
#include <AP_Airspeed.h> #include <AP_Airspeed.h>
#include <AP_GPS.h> #include <AP_GPS.h>
#include <AP_IMU.h> #include <AP_InertialSensor.h>
#include <AP_Baro.h> #include <AP_Baro.h>
#if defined(ARDUINO) && ARDUINO >= 100 #if defined(ARDUINO) && ARDUINO >= 100
@ -27,8 +27,8 @@ class AP_AHRS
{ {
public: public:
// Constructor // Constructor
AP_AHRS(IMU *imu, GPS *&gps) : AP_AHRS(AP_InertialSensor *ins, GPS *&gps) :
_imu(imu), _ins(ins),
_gps(gps), _gps(gps),
_barometer(NULL) _barometer(NULL)
{ {
@ -37,7 +37,7 @@ public:
// prone than the APM1, so we should have a lower ki, // prone than the APM1, so we should have a lower ki,
// which will make us less prone to increasing omegaI // which will make us less prone to increasing omegaI
// incorrectly due to sensor noise // incorrectly due to sensor noise
_gyro_drift_limit = imu->get_gyro_drift_rate(); _gyro_drift_limit = ins->get_gyro_drift_rate();
} }
// empty init // empty init
@ -58,8 +58,8 @@ public:
_airspeed = airspeed; _airspeed = airspeed;
} }
IMU* get_imu() { AP_InertialSensor* get_ins() {
return _imu; return _ins;
} }
// Methods // Methods
@ -138,6 +138,15 @@ public:
_fast_ground_gains = setting; _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 // settable parameters
AP_Float _kp_yaw; AP_Float _kp_yaw;
AP_Float _kp; 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 // 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 under us without our noticing.
IMU *_imu; AP_InertialSensor *_ins;
GPS *&_gps; GPS *&_gps;
AP_Baro *_barometer; 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 // should we raise the gain on the accelerometers for faster
// convergence, used when disarmed for ArduCopter // convergence, used when disarmed for ArduCopter
bool _fast_ground_gains; bool _fast_ground_gains;
@ -182,6 +194,7 @@ protected:
// acceleration due to gravity in m/s/s // acceleration due to gravity in m/s/s
static const float _gravity = 9.80665; static const float _gravity = 9.80665;
}; };
#include <AP_AHRS_DCM.h> #include <AP_AHRS_DCM.h>

View File

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

View File

@ -14,7 +14,7 @@ class AP_AHRS_DCM : public AP_AHRS
{ {
public: public:
// Constructors // 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(); _dcm_matrix.identity();

View File

@ -5,7 +5,7 @@ class AP_AHRS_HIL : public AP_AHRS
{ {
public: public:
// Constructors // 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 // Methods
void update(void) { void update(void) {
_imu->update(); _ins->update();
} }
void setHil(float roll, float pitch, float yaw, void setHil(float roll, float pitch, float yaw,

View File

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

View File

@ -20,7 +20,7 @@ class AP_AHRS_MPU6000 : public AP_AHRS
{ {
public: public:
// Constructors // 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(); _dcm_matrix.identity();
@ -39,7 +39,7 @@ public:
// return the smoothed gyro vector corrected for drift // return the smoothed gyro vector corrected for drift
Vector3f get_gyro(void) { Vector3f get_gyro(void) {
return _imu->get_gyro(); return _ins->get_gyro();
} }
Matrix3f get_dcm_matrix(void) { Matrix3f get_dcm_matrix(void) {
return _dcm_matrix; return _dcm_matrix;