aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AHRS: add correct_centrifugal and flags structure

Browse Source
This commit is contained in:
Randy Mackay 2013-05-06 13:31:49 +09:00
parent 7e36935140
commit ceda7d26b0
3 changed files with 34 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_AHRS/AP_AHRS.h
View File

@ -39,6 +39,9 @@ public:
// which will make us less prone to increasing omegaI
// incorrectly due to sensor noise
_gyro_drift_limit = ins->get_gyro_drift_rate();
// enable centrifugal correction by default
_flags.correct_centrifugal = true;
}
// init sets up INS board orientation
@ -48,7 +51,7 @@ public:
// Accessors
void set_fly_forward(bool b) {
_fly_forward = b;
_flags.fly_forward = b;
}
void set_compass(Compass *compass) {
_compass = compass;
@ -144,12 +147,18 @@ public:
// return true if yaw has been initialised
bool yaw_initialised(void) const {
return _have_initial_yaw;
return _flags.have_initial_yaw;
}
// set the fast gains flag
void set_fast_gains(bool setting) {
_fast_ground_gains = setting;
_flags.fast_ground_gains = setting;
}
// set the correct centrifugal flag
// allows arducopter to disable corrections when disarmed
void set_correct_centrifugal(bool setting) {
_flags.correct_centrifugal = setting;
}
// get trim
@ -175,8 +184,14 @@ public:
static const struct AP_Param::GroupInfo var_info[];
protected:
// whether the yaw value has been intialised with a reference
bool _have_initial_yaw;
// flags structure
struct ahrs_flags {
uint8_t have_initial_yaw : 1; // whether the yaw value has been intialised with a reference
uint8_t fast_ground_gains : 1; // should we raise the gain on the accelerometers for faster convergence, used when disarmed for ArduCopter
uint8_t fly_forward : 1; // 1 if we can assume the aircraft will be flying forward on its X axis
uint8_t correct_centrifugal : 1; // 1 if we should correct for centrifugal forces (allows arducopter to turn this off when motors are disarmed)
} _flags;
// pointer to compass object, if available
Compass * _compass;
@ -195,14 +210,6 @@ protected:
// 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;
// true if we can assume the aircraft will be flying forward
// on its X axis
bool _fly_forward;
// the limit of the gyro drift claimed by the sensors, in
// radians/s/s
float _gyro_drift_limit;

24
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -296,7 +296,7 @@ bool AP_AHRS_DCM::use_compass(void)
// no compass available
return false;
}
if (!_fly_forward || !have_gps()) {
if (!_flags.fly_forward || !have_gps()) {
// we don't have any alterative to the compass
return true;
}
@ -336,24 +336,24 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// we force an additional compass read()
// here. This has the effect of throwing away
// the first compass value, which can be bad
if (!_have_initial_yaw && _compass->read()) {
if (!_flags.have_initial_yaw && _compass->read()) {
float heading = _compass->calculate_heading(_dcm_matrix);
_dcm_matrix.from_euler(roll, pitch, heading);
_omega_yaw_P.zero();
_have_initial_yaw = true;
_flags.have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_compass();
}
} else if (_fly_forward && have_gps()) {
} else if (_flags.fly_forward && have_gps()) {
if (_gps->last_fix_time != _gps_last_update &&
_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3f;
_gps_last_update = _gps->last_fix_time;
if (!_have_initial_yaw) {
if (!_flags.have_initial_yaw) {
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01f));
_omega_yaw_P.zero();
_have_initial_yaw = true;
_flags.have_initial_yaw = true;
}
new_value = true;
yaw_error = yaw_error_gps();
@ -381,7 +381,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
// paper from Bill Premerlani
_omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw;
if (_fast_ground_gains) {
if (_flags.fast_ground_gains) {
_omega_yaw_P.z *= 8;
}
@ -507,8 +507,8 @@ AP_AHRS_DCM::drift_correction(float deltat)
// are m/s/s
Vector3f GA_e;
GA_e = Vector3f(0, 0, -1.0f);
if (_have_gps_lock || _fly_forward) {
if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
// limit vertical acceleration correction to 0.5 gravities. The
@ -584,11 +584,11 @@ AP_AHRS_DCM::drift_correction(float deltat)
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error * _P_gain(spin_rate) * _kp;
if (_fast_ground_gains) {
if (_flags.fast_ground_gains) {
_omega_P *= 8;
}
if (_fly_forward && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D &&
if (_flags.fly_forward && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D &&
_gps->ground_speed < GPS_SPEED_MIN &&
_accel_vector.x >= 7 &&
pitch_sensor > -3000 && pitch_sensor < 3000) {
@ -626,7 +626,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
// remember the velocity for next time
_last_velocity = velocity;
if (_have_gps_lock && _fly_forward) {
if (_have_gps_lock && _flags.fly_forward) {
// update wind estimate
estimate_wind(velocity);
}

4
libraries/AP_AHRS/AP_AHRS_MPU6000.cpp
View File

@ -280,8 +280,8 @@ AP_AHRS_MPU6000::drift_correction_yaw(void)
heading = _compass->calculate_heading(_dcm_matrix);
// if this is the first good compass reading then set yaw to this heading
if( !_have_initial_yaw ) {
_have_initial_yaw = true;
if( !_flags.have_initial_yaw ) {
_flags.have_initial_yaw = true;
yaw_corrected = wrap_PI(heading);
}