mirror of https://github.com/ArduPilot/ardupilot
AHRS: new functions to support APM_Control library
This commit is contained in:
parent
2bc2460c07
commit
ba41612e7d
|
@ -58,6 +58,10 @@ public:
|
||||||
_airspeed = airspeed;
|
_airspeed = airspeed;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
IMU* get_imu() {
|
||||||
|
return _imu;
|
||||||
|
}
|
||||||
|
|
||||||
// Methods
|
// Methods
|
||||||
virtual void update(void) = 0;
|
virtual void update(void) = 0;
|
||||||
|
|
||||||
|
@ -71,6 +75,15 @@ public:
|
||||||
int32_t pitch_sensor;
|
int32_t pitch_sensor;
|
||||||
int32_t yaw_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
|
// return a smoothed and corrected gyro vector
|
||||||
virtual Vector3f get_gyro(void) = 0;
|
virtual Vector3f get_gyro(void) = 0;
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue