From d31f3eb4c51249faab224597b2b435a37075eb8d Mon Sep 17 00:00:00 2001
From: TunaLobster <charliepjohnson@gmail.com>
Date: Mon, 26 Jul 2021 19:07:27 -0500
Subject: [PATCH] Plane: Add TRIM_PITCH_CD FLIGHT_OPTIONS bits for GCS and OSD

---
 ArduPlane/ArduPlane.cpp   | 4 ++--
 ArduPlane/GCS_Mavlink.cpp | 6 +++++-
 ArduPlane/Parameters.cpp  | 2 +-
 ArduPlane/defines.h       | 2 ++
 4 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp
index a7e6472e893cbd..ba942a140c7037 100644
--- a/ArduPlane/ArduPlane.cpp
+++ b/ArduPlane/ArduPlane.cpp
@@ -700,9 +700,9 @@ 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
+   if (!quadplane.show_vtol_view() && !(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) {  // correct for TRIM_PITCH_CD
       pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
-   } else {
+   } else if (!quadplane.show_vtol_view()) {
       pitch = quadplane.ahrs_view->pitch;
       roll = quadplane.ahrs_view->roll;
    }
diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
index 21cd1aff47ff56..b27add78dfbc01 100644
--- a/ArduPlane/GCS_Mavlink.cpp
+++ b/ArduPlane/GCS_Mavlink.cpp
@@ -114,8 +114,12 @@ void GCS_MAVLINK_Plane::send_attitude() const
     const AP_AHRS &ahrs = AP::ahrs();
 
     float r = ahrs.roll;
-    float p = ahrs.pitch - radians(plane.g.pitch_trim_cd*0.01f);
+    float p = ahrs.pitch;
     float y = ahrs.yaw;
+
+    if (!(plane.g2.flight_options & FlightOptions::GCS_REMOVE_TRIM_PITCH_CD)) {
+        p -= radians(plane.g.pitch_trim_cd * 0.01f);
+    }
     
     if (plane.quadplane.show_vtol_view()) {
         r = plane.quadplane.ahrs_view->roll;
diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index f20d2e1306eac2..732918bfd4d156 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -1105,7 +1105,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Param: FLIGHT_OPTIONS
     // @DisplayName: Flight mode options
     // @Description: Flight mode specific options
-    // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5: Enable yaw damper in acro mode, 6: Surpress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airpseed sensor, 7:EnableDefaultAirspeed for takeoff
+    // @Bitmask: 0:Rudder mixing in direct flight modes only (Manual / Stabilize / Acro),1:Use centered throttle in Cruise or FBWB to indicate trim airspeed, 2:Disable attitude check for takeoff arming, 3:Force target airspeed to trim airspeed in Cruise or FBWB, 4: Climb to ALT_HOLD_RTL before turning for RTL, 5: Enable yaw damper in acro mode, 6: Surpress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airpseed sensor., 7:EnableDefaultAirspeed for takeoff, 8: Remove the TRIM_PITCH_CD on the GCS horizon, 9: Remove the TRIM_PITCH_CD on the OSD horizon
     // @User: Advanced
     AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0),
 
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index 1bd9ade168a420..5f3d7aceedd85e 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -161,6 +161,8 @@ enum FlightOptions {
     ACRO_YAW_DAMPER = (1 << 5),
     SURPRESS_TKOFF_SCALING = (1<<6),
     ENABLE_DEFAULT_AIRSPEED = (1<<7),
+    GCS_REMOVE_TRIM_PITCH_CD = (1 << 8),
+    OSD_REMOVE_TRIM_PITCH_CD = (1 << 9),
 };
 
 enum CrowFlapOptions {