From e958c313a1c1494da6e4c17b962bc1189b55b6d0 Mon Sep 17 00:00:00 2001
From: yaapu <alex.apostoli@gmail.com>
Date: Fri, 23 Sep 2022 14:27:26 +0200
Subject: [PATCH] ArduPlane: fixed roll and pitch for OSD VTOL view

---
 ArduPlane/ArduPlane.cpp             | 25 ++++++++++---------------
 ArduPlane/Plane.h                   |  2 --
 libraries/AP_Vehicle/AP_Vehicle.cpp |  3 +--
 libraries/AP_Vehicle/AP_Vehicle.h   |  2 --
 4 files changed, 11 insertions(+), 21 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index 4cd9f071c2..d2c6f70e6c 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -798,26 +798,21 @@ bool Plane::set_velocity_match(const Vector2f &velocity)
 
 #endif // AP_SCRIPTING_ENABLED
 
-#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 HAL_QUADPLANE_ENABLED
-   if (quadplane.show_vtol_view()) {
-       return;
-   }
-#endif
-   if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) {  // correct for TRIM_PITCH_CD
-      pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
-      return;
-   }
-#if HAL_QUADPLANE_ENABLED
-   pitch = quadplane.ahrs_view->pitch;
-   roll = quadplane.ahrs_view->roll;
+    if (quadplane.show_vtol_view()) {
+        pitch = quadplane.ahrs_view->pitch;
+        roll = quadplane.ahrs_view->roll;
+        return;
+    }
 #endif
+    pitch = ahrs.pitch;
+    roll = ahrs.roll;
+    if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) {  // correct for TRIM_PITCH_CD
+        pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
+    }
 }
-#endif
 
 AP_HAL_MAIN_CALLBACKS(&plane);
diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index cb9d7ed544..03597cc758 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -985,9 +985,7 @@ 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,
diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
index 56e9b21604..178864e1b3 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.cpp
+++ b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -635,6 +635,7 @@ void AP_Vehicle::publish_osd_info()
     nav_info.wp_number = mission->get_current_nav_index();
     osd->set_nav_info(nav_info);
 }
+#endif
 
 void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const
 {
@@ -642,8 +643,6 @@ void AP_Vehicle::get_osd_roll_pitch_rad(float &roll, float &pitch) const
     pitch = ahrs.pitch;
 }
 
-#endif
-
 #if HAL_INS_ACCELCAL_ENABLED
 
 #ifndef HAL_CAL_ALWAYS_REBOOT
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index d32a942c0d..87e55daf22 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -292,10 +292,8 @@ public:
      */
     virtual bool get_pan_tilt_norm(float &pan_norm, float &tilt_norm) const { return false; }
 
-#if OSD_ENABLED
    // Returns roll and  pitch for OSD Horizon, Plane overrides to correct for VTOL view and fixed wing TRIM_PITCH_CD
     virtual void get_osd_roll_pitch_rad(float &roll, float &pitch) const;
-#endif
 
     /*
      get the target body-frame angular velocities in rad/s (Z-axis component used by some gimbals)