Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Quaternion: change signs in AP_AHRS_Quaternion 

thanks to Justin for the suggestion
This commit is contained in:
Andrew Tridgell 2012-03-12 22:50:38 +11:00
parent d948a28e7c
commit c53b320e00
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 // get current IMU state
gyro = _imu->get_gyro(); gyro = _imu->get_gyro();
accel = _imu->get_accel();
// Quaternion code uses opposite x and y gyro sense from the // the quaternion system uses opposite sign for accel
// rest of APM accel = - _imu->get_accel();
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) { if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) {
// compensate for linear acceleration. This makes a // compensate for linear acceleration. This makes a
// surprisingly large difference in the pitch estimate when // surprisingly large difference in the pitch estimate when
// turning, plus on takeoff and landing // turning, plus on takeoff and landing
float acceleration = _gps->acceleration(); float acceleration = _gps->acceleration();
accel.x -= acceleration; accel.x += acceleration;
// compensate for centripetal acceleration // compensate for centripetal acceleration
float veloc; float veloc;
@ -344,12 +338,12 @@ void AP_AHRS_Quaternion::update(void)
// be careful of the signs in this calculation. the // be careful of the signs in this calculation. the
// quaternion system uses different signs than the // quaternion system uses different signs than the
// rest of APM // rest of APM
accel.y -= (gyro.z - gyro_bias.z) * veloc; accel.y += (gyro.z - gyro_bias.z) * veloc;
accel.z += (gyro.y - gyro_bias.y) * veloc; accel.z -= (gyro.y - gyro_bias.y) * veloc;
} }
if (_compass != NULL && _compass->use_for_yaw()) { 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); update_MARG(deltat, gyro, accel, mag);
} else { } else {
// step the quaternion solution using just gyros and accels // 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 // get corrected gyro vector
Vector3f get_gyro(void) { Vector3f get_gyro(void) {
// notice the sign reversals here // 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) { Vector3f get_gyro_drift(void) {
// notice the sign reversals here. The quaternion // notice the sign reversals here. The quaternion
// system uses a -ve gyro bias, DCM uses a +ve // 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); float get_error_rp(void);