Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: add method to set pitch trim

Browse Source
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); AP_HAL::panic("Unsupported AHRS view %u\n", (unsigned)rotation);
} }
_pitch_trim_deg = pitch_trim_deg;
// Add pitch trim // Add pitch trim
y_angle = wrap_360(y_angle + pitch_trim_deg); rot_view.from_euler(0, radians(wrap_360(y_angle + pitch_trim_deg)), 0);
rot_view.from_euler(0, radians(y_angle), 0);
// setup initial state // setup initial state
update(); 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 // update state
void AP_AHRS_View::update(bool skip_ins_update) 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; return rot_body_to_ned;
} }
// apply pitch trim
void set_pitch_trim(float trim_deg);
// helper trig value accessors // helper trig value accessors
float cos_roll() const { float cos_roll() const {
return trig.cos_roll; return trig.cos_roll;
@ -190,4 +193,5 @@ private:
} trig; } trig;
float y_angle; float y_angle;
float _pitch_trim_deg;
}; };