AHRS: new functions to support APM_Control library

This commit is contained in:
Andrew Tridgell 2012-08-22 12:14:39 +10:00
parent 2bc2460c07
commit ba41612e7d
1 changed files with 13 additions and 0 deletions

View File

@ -58,6 +58,10 @@ public:
_airspeed = airspeed;
}
IMU* get_imu() {
return _imu;
}
// Methods
virtual void update(void) = 0;
@ -71,6 +75,15 @@ public:
int32_t pitch_sensor;
int32_t yaw_sensor;
float get_pitch_rate_earth(void) {
Vector3f omega = get_gyro();
return cos(roll) * omega.y - sin(roll) * omega.z;
}
float get_roll_rate_earth(void) {
Vector3f omega = get_gyro();
return omega.x + tan(pitch)*(omega.y*sin(roll) + omega.z*cos(roll));
}
// return a smoothed and corrected gyro vector
virtual Vector3f get_gyro(void) = 0;