From 3b9a35da27e9523a71366724dce2a7a7ff2b4394 Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Tue, 19 Mar 2019 20:19:38 +0000
Subject: [PATCH] AP_Motors: add reversed tricopter option

---
 libraries/AP_Motors/AP_MotorsTri.cpp | 17 +++++++++++++++++
 libraries/AP_Motors/AP_MotorsTri.h   |  3 +++
 2 files changed, 20 insertions(+)

diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp
index 9851b10875..836adb32cc 100644
--- a/libraries/AP_Motors/AP_MotorsTri.cpp
+++ b/libraries/AP_Motors/AP_MotorsTri.cpp
@@ -51,6 +51,11 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
     // allow mapping of motor7
     add_motor_num(AP_MOTORS_CH_TRI_YAW);
 
+    // check for reverse tricopter
+    if (frame_type == MOTOR_FRAME_TYPE_PLUSREV) {
+        _pitch_reversed = true;
+    }
+
     // record successful initialisation if what we setup was the desired frame_class
     _flags.initialised_ok = (frame_class == MOTOR_FRAME_TRI);
 }
@@ -58,6 +63,13 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
 // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
 void AP_MotorsTri::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
 {
+    // check for reverse tricopter
+    if (frame_type == MOTOR_FRAME_TYPE_PLUSREV) {
+        _pitch_reversed = true;
+    } else {
+        _pitch_reversed = false;
+    }
+
     _flags.initialised_ok = (frame_class == MOTOR_FRAME_TRI);
 }
 
@@ -153,6 +165,11 @@ void AP_MotorsTri::output_armed_stabilizing()
     throttle_thrust = get_throttle() * compensation_gain;
     throttle_avg_max = _throttle_avg_max * compensation_gain;
 
+    // check for reversed pitch
+    if (_pitch_reversed) {
+        pitch_thrust *= -1.0f;
+    }
+
     // calculate angle of yaw pivot
     _pivot_angle = safe_asin(yaw_thrust);
     if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) {
diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h
index d0a956dec6..494efa7878 100644
--- a/libraries/AP_Motors/AP_MotorsTri.h
+++ b/libraries/AP_Motors/AP_MotorsTri.h
@@ -71,4 +71,7 @@ protected:
     float           _thrust_right;
     float           _thrust_rear;
     float           _thrust_left;
+
+    // reverse pitch
+    bool _pitch_reversed;
 };