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 49de46a548
commit 83ae8e47be
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
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));
}

View File

@ -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>

View File

@ -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

View File

@ -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();

View File

@ -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,

View File

@ -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

View File

@ -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;