Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Quaternion: added DCM compatibility interfaces

Browse Source 
useful for getting the code going quickly
This commit is contained in:
Andrew Tridgell 2012-03-03 18:32:50 +11:00
parent 626f8598ed
commit a85ba80246
2 changed files with 41 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Quaternion/AP_Quaternion.cpp
View File

@ -249,19 +249,36 @@ void AP_Quaternion::update(void)
_imu->update();
delta_t = _imu->get_delta_time();
deltat = _imu->get_delta_time();
// get current IMU state
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
gyro.x = -gyro.x;
gyro.y = -gyro.y;
accel = _imu->get_accel();
if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) {
// compensate for centripetal acceleration
float veloc;
veloc = _gps->ground_speed / 100;
accel.y -= _gyro_smoothed.z * veloc;
accel.z += _gyro_smoothed.y * veloc;
}
// Quaternion code uses opposite z accel
accel.z = -accel.z;
if (_compass == NULL) {
update_IMU(deltat, gyro, accel);
} else {
// mag.z is reversed in quaternion code
Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z);
update_MARG(deltat, gyro, accel, mag);
}
@ -289,6 +306,10 @@ void AP_Quaternion::update(void)
1 - 2.0*(SEq_3*SEq_3 + SEq_4*SEq_4));
}
if (_compass != NULL) {
yaw += _compass->get_declination();
}
// and integer Eulers
roll_sensor = 100 * ToDeg(roll);
pitch_sensor = 100 * ToDeg(pitch);

18
libraries/AP_Quaternion/AP_Quaternion.h
View File

@ -56,6 +56,21 @@ public:
int32_t yaw_sensor;
// 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 Vector3f(0,0,0); }
float get_accel_weight(void) { return 0; }
float get_renorm_val(void) { return 0; }
float get_error_rp(void) { return 0; }
float get_error_yaw(void) { return 0; }
float get_health(void) { return 0; }
void matrix_reset(void) { }
uint8_t gyro_sat_count;
uint8_t renorm_range_count;
uint8_t renorm_blowup_count;
private:
void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel);
void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag);
@ -93,6 +108,9 @@ private:
// estimate gyroscope biases error
Vector3f gyro_bias;
// smoothed gyro estimate
Vector3f _gyro_smoothed;
};
#endif