From 0e2e077d96118bccf6777297f76994be68172798 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Mon, 4 Feb 2019 10:12:29 -0700
Subject: [PATCH] AP_AHRS: add method to set pitch trim
---
libraries/AP_AHRS/AP_AHRS_View.cpp | 11 ++++++++---
libraries/AP_AHRS/AP_AHRS_View.h | 4 ++++
2 files changed, 12 insertions(+), 3 deletions(-)
diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp
index d075d45a07..0db18ecf6c 100644
--- a/libraries/AP_AHRS/AP_AHRS_View.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_View.cpp
@@ -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)
{
diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h
index 027732cef4..ca456178ef 100644
--- a/libraries/AP_AHRS/AP_AHRS_View.h
+++ b/libraries/AP_AHRS/AP_AHRS_View.h
@@ -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;
};