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

Quaternion: change signs in AP_AHRS_Quaternion

Browse Source 
thanks to Justin for the suggestion
This commit is contained in:
Andrew Tridgell 2012-03-12 22:50:38 +11:00
parent df79703ed1
commit fbe8592d3c
2 changed files with 8 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_AHRS/AP_AHRS_Quaternion.cpp
View File

@ -321,22 +321,16 @@ void AP_AHRS_Quaternion::update(void)
// get current IMU state
gyro = _imu->get_gyro();
accel = _imu->get_accel();
// 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;
// the quaternion system uses opposite sign for accel
accel = - _imu->get_accel();
if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) {
// compensate for linear acceleration. This makes a
// surprisingly large difference in the pitch estimate when
// turning, plus on takeoff and landing
float acceleration = _gps->acceleration();
accel.x -= acceleration;
accel.x += acceleration;
// compensate for centripetal acceleration
float veloc;
@ -344,12 +338,12 @@ void AP_AHRS_Quaternion::update(void)
// 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;
accel.y += (gyro.z - gyro_bias.z) * veloc;
accel.z -= (gyro.y - gyro_bias.y) * veloc;
}
if (_compass != NULL && _compass->use_for_yaw()) {
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z);
update_MARG(deltat, gyro, accel, mag);
} else {
// step the quaternion solution using just gyros and accels

4
libraries/AP_AHRS/AP_AHRS_Quaternion.h
View File

@ -34,13 +34,13 @@ public:
// get corrected gyro vector
Vector3f get_gyro(void) {
// notice the sign reversals here
return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z);
return Vector3f(_gyro_corrected.x, _gyro_corrected.y, _gyro_corrected.z);
}
Vector3f get_gyro_drift(void) {
// notice the sign reversals here. The quaternion
// system uses a -ve gyro bias, DCM uses a +ve
return Vector3f(gyro_bias.x, gyro_bias.y, -gyro_bias.z);
return Vector3f(-gyro_bias.x, -gyro_bias.y, -gyro_bias.z);
}
float get_error_rp(void);