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; _pitch_trim_deg = pitch_trim_deg;
// Add pitch trim // Add pitch trim
rot_view.from_euler(0, radians(wrap_360(y_angle + pitch_trim_deg)), 0); 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 // setup initial state
update(); 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) { void AP_AHRS_View::set_pitch_trim(float trim_deg) {
_pitch_trim_deg = trim_deg; _pitch_trim_deg = trim_deg;
rot_view.from_euler(0, radians(wrap_360(y_angle + _pitch_trim_deg)), 0); 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 // update state
@ -60,11 +64,8 @@ void AP_AHRS_View::update(bool skip_ins_update)
gyro = ahrs.get_gyro(); gyro = ahrs.get_gyro();
if (!is_zero(y_angle + _pitch_trim_deg)) { if (!is_zero(y_angle + _pitch_trim_deg)) {
Matrix3f &r = rot_body_to_ned; rot_body_to_ned = rot_body_to_ned * rot_view_T;
r.transpose(); gyro = rot_view * gyro;
r = rot_view * r;
r.transpose();
gyro.rotate(rotation);
} }
rot_body_to_ned.to_euler(&roll, &pitch, &yaw); 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; const enum Rotation rotation;
AP_AHRS &ahrs; AP_AHRS &ahrs;
// body frame rotation for this View
Matrix3f rot_view; Matrix3f rot_view;
// transpose of rot_view
Matrix3f rot_view_T;
Matrix3f rot_body_to_ned; Matrix3f rot_body_to_ned;
Vector3f gyro; Vector3f gyro;