Plane: Add TRIM_PITCH_CD FLIGHT_OPTIONS bits for GCS and OSD

Icatme · Aug 30, 2021 · d31f3eb · d31f3eb
1 parent 4b67490
commit d31f3eb
Show file tree

Hide file tree

Showing 4 changed files with 10 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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 {