Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: add method to set pitch trim

This commit is contained in:
Mark Whitehorn 2019-02-04 10:12:29 -07:00 committed by Andrew Tridgell
parent 1648a6544b
commit 0e2e077d96
2 changed files with 12 additions and 3 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

@ -39,15 +39,20 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation, float pitch_
AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation);
}
_pitch_trim_deg = pitch_trim_deg;
// Add pitch trim
y_angle = wrap_360(y_angle + pitch_trim_deg);
rot_view.from_euler(0, radians(y_angle), 0);
rot_view.from_euler(0, radians(wrap_360(y_angle + pitch_trim_deg)), 0);
// setup initial state
update();
}
// apply pitch trim
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);
};
// update state
void AP_AHRS_View::update(bool skip_ins_update)
{

4
libraries/AP_AHRS/AP_AHRS_View.h
View File

@ -48,6 +48,9 @@ public:
return rot_body_to_ned;
}
// apply pitch trim
void set_pitch_trim(float trim_deg);
// helper trig value accessors
float cos_roll() const {
return trig.cos_roll;
@ -190,4 +193,5 @@ private:
} trig;
float y_angle;
float _pitch_trim_deg;
};