mirror of https://github.com/ArduPilot/ardupilot
AP_AHRS: replace IMU with INS and add roll and pitch trim
This commit is contained in:
parent
e835cef181
commit
8ab0611261
|
@ -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));
|
||||
}
|
|
@ -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,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 <AP_AHRS_DCM.h>
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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();
|
||||
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue