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

AP_AHRS: cache trim rotation

Browse Source
This commit is contained in:
Jonathan Challinger 2016-08-09 11:47:59 -07:00 committed by Andrew Tridgell
parent 80d9092993
commit e20687fbce
2 changed files with 17 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -173,14 +173,6 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_
}
}
Matrix3f AP_AHRS::get_rotation_autopilot_body_to_vehicle_body(void) const
{
Matrix3f ret;
ret.from_euler(_trim.x, _trim.y, 0.0f);
return ret;
}
// return a ground speed estimate in m/s
Vector2f AP_AHRS::groundspeed_vector(void)
{
@ -238,6 +230,12 @@ Vector2f AP_AHRS::groundspeed_vector(void)
// should be called after _dcm_matrix is updated
void AP_AHRS::update_trig(void)
{
if (_last_trim != _trim.get()) {
_last_trim = _trim.get();
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
}
Vector2f yaw_vector;
const Matrix3f &temp = get_rotation_body_to_ned();

13
libraries/AP_AHRS/AP_AHRS.h
View File

@ -85,6 +85,10 @@ public:
_home.alt = 0;
_home.lng = 0;
_home.lat = 0;
_last_trim = _trim.get();
_rotation_autopilot_body_to_vehicle_body.from_euler(_last_trim.x, _last_trim.y, 0.0f);
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
}
// empty virtual destructor
@ -240,8 +244,8 @@ public:
// return a DCM rotation matrix representing our current
// attitude
virtual const Matrix3f &get_rotation_body_to_ned(void) const = 0;
Matrix3f get_rotation_autopilot_body_to_vehicle_body(void) const;
Matrix3f get_rotation_vehicle_body_to_autopilot_body(void) const { return get_rotation_autopilot_body_to_vehicle_body().transposed(); }
const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
// get our current position estimate. Return true if a position is available,
// otherwise false. This call fills in lat, lng and alt
@ -513,6 +517,11 @@ protected:
// a vector to capture the difference between the controller and body frames
AP_Vector3f _trim;
// cached trim rotations
Vector3f _last_trim;
Matrix3f _rotation_autopilot_body_to_vehicle_body;
Matrix3f _rotation_vehicle_body_to_autopilot_body;
// the limit of the gyro drift claimed by the sensors, in
// radians/s/s
float _gyro_drift_limit;