From 1e4a6c92364859ce76e1af5b98c8b9cd0622945f Mon Sep 17 00:00:00 2001
From: Hwurzburg <hwurzburg@yahoo.com>
Date: Sun, 1 Aug 2021 13:34:37 -0500
Subject: [PATCH] ArduPlane: Correct OSD horizon for VTOL modes and
 TRIM_PITCH_CD in Fixed Wing

---
 ArduPlane/ArduPlane.cpp | 15 +++++++++++++++
 ArduPlane/Plane.h       |  3 +++
 2 files changed, 18 insertions(+)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 854e5ab2ae..dbb6a343fa 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -689,4 +689,19 @@ bool Plane::get_target_location(Location& target_loc)
 }
 #endif // ENABLE_SCRIPTING
 
+#if OSD_ENABLED
+// correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL
+void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
+{
+   pitch = ahrs.pitch;
+   roll = ahrs.roll;
+   if (!quadplane.show_vtol_view()) {  // correct for TRIM_PITCH_CD
+      pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
+   } else {
+      pitch = quadplane.ahrs_view->pitch;
+      roll = quadplane.ahrs_view->roll;
+   }
+}
+#endif
+
 AP_HAL_MAIN_CALLBACKS(&plane);
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 6a9c522f1c..fecf4a804a 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -950,6 +950,9 @@ private:
 
     // ArduPlane.cpp
     void disarm_if_autoland_complete();
+# if OSD_ENABLED
+    void get_osd_roll_pitch_rad(float &roll, float &pitch) const override;
+#endif
     float tecs_hgt_afe(void);
     void efi_update(void);
     void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,