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

AP_AHRS: refactor View

Browse Source 
* AP_AHRS: refactor View
This commit is contained in:
Mark Whitehorn 2019-03-23 10:45:57 -06:00 committed by Andrew Tridgell
parent ffc94f19cd
commit da1e5bc61f
2 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_AHRS/AP_AHRS_View.cpp
View File

@ -42,6 +42,8 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_
_pitch_trim_deg = pitch_trim_deg;
// Add pitch trim
rot_view.from_euler(0, radians(wrap_360(y_angle + pitch_trim_deg)), 0);
rot_view_T = rot_view;
rot_view_T.transpose();
// setup initial state
update();
@ -51,6 +53,8 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_
void AP_AHRS_View::set_pitch_trim(float trim_deg) {
_pitch_trim_deg = trim_deg;
rot_view.from_euler(0, radians(wrap_360(y_angle + _pitch_trim_deg)), 0);
rot_view_T = rot_view;
rot_view_T.transpose();
};
// update state
@ -60,11 +64,8 @@ void AP_AHRS_View::update(bool skip_ins_update)
gyro = ahrs.get_gyro();
if (!is_zero(y_angle + _pitch_trim_deg)) {
Matrix3f &r = rot_body_to_ned;
r.transpose();
r = rot_view * r;
r.transpose();
gyro.rotate(rotation);
rot_body_to_ned = rot_body_to_ned * rot_view_T;
gyro = rot_view * gyro;
}
rot_body_to_ned.to_euler(&roll, &pitch, &yaw);

3
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -183,7 +183,10 @@ private:
const enum Rotation rotation;
AP_AHRS &ahrs;
// body frame rotation for this View
Matrix3f rot_view;
// transpose of rot_view
Matrix3f rot_view_T;
Matrix3f rot_body_to_ned;
Vector3f gyro;