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

Quaternion: fix the gyro bias in centripetal and remove smoothing

Browse Source 
the centripetal code needs to take account of the current gyro
bias.

It turned out that the accel and gyro smoothing was causing
significant control lag, and we're better off just letting the
quaternion code handle it via its own smoothing parameters
This commit is contained in:
Andrew Tridgell 2012-03-05 11:27:45 +11:00
parent 47ddd4dfdd
commit f70dfe440d
2 changed files with 31 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Quaternion/AP_Quaternion.cpp
View File

@ -269,27 +269,30 @@ void AP_Quaternion::update(void)
gyro = _imu->get_gyro();
accel = _imu->get_accel();
// keep a smoothed gyro vector for centripetal and reporting
// to mainline code
_gyro_smoothed = (_gyro_smoothed * 0.95) + (gyro * 0.05);
// Quaternion code uses opposite x and y gyro sense
// Quaternion code uses opposite x and y gyro sense from the
// rest of APM
gyro.x = -gyro.x;
gyro.y = -gyro.y;
// Quaternion code uses opposite z accel as well
accel.z = -accel.z;
if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) {
// compensate for linear acceleration, limited to 1g
float acceleration = _gps->acceleration();
acceleration = constrain(acceleration, 0, 9.8);
accel.x -= acceleration;
// compensate for centripetal acceleration
float veloc;
veloc = _gps->ground_speed / 100;
accel.x -= _gps->acceleration();
accel.y -= _gyro_smoothed.z * veloc;
accel.z += _gyro_smoothed.y * veloc;
// be careful of the signs in this calculation. the
// quaternion system uses different signs than the
// rest of APM
accel.y -= (gyro.z - gyro_bias.z) * veloc;
accel.z += (gyro.y - gyro_bias.y) * veloc;
}
// Quaternion code uses opposite z accel
accel.z = -accel.z;
if (_compass == NULL) {
update_IMU(deltat, gyro, accel);
} else {
@ -298,6 +301,9 @@ void AP_Quaternion::update(void)
update_MARG(deltat, gyro, accel, mag);
}
// keep the corrected gyro for reporting
_gyro_corrected = gyro;
// compute the Eulers
float test = (SEq_1*SEq_3 - SEq_4*SEq_2);
const float singularity = 0.499; // 86.3 degrees?

21
libraries/AP_Quaternion/AP_Quaternion.h
View File

@ -59,8 +59,14 @@ public:
// compatibility methods with DCM
void update_DCM(void) { update(); }
void update_DCM_fast(void) { update(); }
Vector3f get_gyro(void) { return _gyro_smoothed; }
Vector3f get_integrator(void) { return gyro_bias; }
Vector3f get_gyro(void) {
// notice the sign reversals here
return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z);
}
Vector3f get_integrator(void) {
// notice the sign reversals here
return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z);
}
float get_accel_weight(void) { return 0; }
float get_renorm_val(void) { return 0; }
float get_health(void) { return 0; }
@ -91,10 +97,11 @@ private:
// true if we are doing centripetal acceleration correction
bool _centripetal;
// maximum gyroscope measurement error in rad/s (set to 5 degrees/second)
static const float gyroMeasError = 5.0 * (M_PI/180.0);
// maximum gyroscope measurement error in rad/s (set to 10 degrees/second)
static const float gyroMeasError = 10.0 * (M_PI/180.0);
// maximum gyroscope drift rate in radians/s/s (set to 0.02 degrees/s/s)
// maximum gyroscope drift rate in radians/s/s (set to 0.02
// degrees/s/s, which is 1.2 degrees/s/minute)
static const float gyroMeasDrift = 0.02 * (PI/180.0);
float beta;
@ -109,8 +116,8 @@ private:
// estimate gyroscope biases error
Vector3f gyro_bias;
// smoothed gyro estimate
Vector3f _gyro_smoothed;
// the current corrected gyro vector
Vector3f _gyro_corrected;
// estimate of error
float _error_rp_sum;