From 58a128f430e5688ac9a2d1b2e0688907cd7f1bbf Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Thu, 17 Jan 2019 10:56:28 -0700
Subject: [PATCH] AP_Motors: add support for copter tailsitters

add MotorsMatrixTS tailsitter class
add X quadcopter tailsitter frame
add support for trimotor tailsitter
---
 libraries/AP_Motors/AP_Motors.h           |   1 +
 libraries/AP_Motors/AP_MotorsMatrixTS.cpp | 125 ++++++++++++++++++++++
 libraries/AP_Motors/AP_MotorsMatrixTS.h   |  30 ++++++
 3 files changed, 156 insertions(+)
 create mode 100644 libraries/AP_Motors/AP_MotorsMatrixTS.cpp
 create mode 100644 libraries/AP_Motors/AP_MotorsMatrixTS.h

diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h
index 0243666a53..fa93d194a6 100644
--- a/libraries/AP_Motors/AP_Motors.h
+++ b/libraries/AP_Motors/AP_Motors.h
@@ -11,3 +11,4 @@
 #include "AP_MotorsCoax.h"
 #include "AP_MotorsTailsitter.h"
 #include "AP_Motors6DOF.h"
+#include "AP_MotorsMatrixTS.h"
diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.cpp b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp
new file mode 100644
index 0000000000..32bd0bf0c7
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsMatrixTS.cpp
@@ -0,0 +1,125 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *       AP_MotorsMatrixTS.cpp - tailsitters with multicopter motor configuration
+ */
+
+#include <AP_BattMonitor/AP_BattMonitor.h>
+#include <AP_HAL/AP_HAL.h>
+#include "AP_MotorsMatrixTS.h"
+
+extern const AP_HAL::HAL& hal;
+
+#define SERVO_OUTPUT_RANGE  4500
+
+// output a thrust to all motors that match a given motor mask. This
+// is used to control motors enabled for forward flight. Thrust is in
+// the range 0 to 1
+void AP_MotorsMatrixTS::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
+{
+    const int16_t pwm_min = get_pwm_output_min();
+    const int16_t pwm_range = get_pwm_output_max() - pwm_min;
+
+    for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            int16_t motor_out;
+            if (mask & (1U<<i)) {
+                /*
+                    apply rudder mixing differential thrust
+                    copter frame roll is plane frame yaw (this is only
+                    used by tiltrotors and tailsitters)
+                */
+                float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
+                motor_out = pwm_min + pwm_range * constrain_float(thrust + diff_thrust, 0.0f, 1.0f);
+            } else {
+                motor_out = pwm_min;
+            }
+            rc_write(i, motor_out);
+        }
+    }
+}
+
+void AP_MotorsMatrixTS::output_to_motors()
+{
+    // calls calc_thrust_to_pwm(_thrust_rpyt_out[i]) for each enabled motor
+    AP_MotorsMatrix::output_to_motors();
+
+    // also actuate control surfaces
+    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron,  -_yaw_in * SERVO_OUTPUT_RANGE);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, _pitch_in * SERVO_OUTPUT_RANGE);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, _roll_in * SERVO_OUTPUT_RANGE);
+}
+
+void AP_MotorsMatrixTS::output_armed_stabilizing()
+{
+    // calculates thrust values _thrust_rpyt_out
+    AP_MotorsMatrix::output_armed_stabilizing();
+}
+
+void AP_MotorsMatrixTS::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
+{
+    // remove existing motors
+    for (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        remove_motor(i);
+    }
+
+    bool success = false;
+
+    switch (frame_class) {
+
+        case MOTOR_FRAME_TRI:
+            // frame_type ignored since only one frame type is currently supported
+            add_motor(AP_MOTORS_MOT_1,  90, 0, 2);
+            add_motor(AP_MOTORS_MOT_2, -90, 0, 4);
+            add_motor(AP_MOTORS_MOT_4, 180, 0, 3);
+            success = true;
+            break;
+        case MOTOR_FRAME_QUAD:
+            switch (frame_type) {
+                case MOTOR_FRAME_TYPE_PLUS:
+                    // motors 1,2 on wings, motors 3,4 on vertical tail/subfin
+                    // motors 1,2 are counter-rotating, as are motors 3,4
+                    // left wing motor is CW (looking from front)
+                    // don't think it matters which of 3,4 is CW
+                    add_motor(AP_MOTORS_MOT_1,  90, 0, 2);
+                    add_motor(AP_MOTORS_MOT_2, -90, 0, 4);
+                    add_motor(AP_MOTORS_MOT_3,   0, 0, 1);
+                    add_motor(AP_MOTORS_MOT_4, 180, 0, 3);
+                    success = true;
+                    break;
+                case MOTOR_FRAME_TYPE_X:
+                    // PLUS_TS layout rotated 45 degrees about X axis
+                    add_motor(AP_MOTORS_MOT_1,   45, 0, 1);
+                    add_motor(AP_MOTORS_MOT_2, -135, 0, 3);
+                    add_motor(AP_MOTORS_MOT_3,  -45, 0, 4);
+                    add_motor(AP_MOTORS_MOT_4,  135, 0, 2);
+                    success = true;
+                    break;
+                default:
+                    // matrixTS doesn't support the configured frame_type
+                    break;
+            }
+            break;
+        default:
+            // matrixTS doesn't support the configured frame_class
+            break;
+        } // switch frame_class
+
+    // normalise factors to magnitude 0.5
+    normalise_rpy_factors();
+
+    _flags.initialised_ok = success;
+}
diff --git a/libraries/AP_Motors/AP_MotorsMatrixTS.h b/libraries/AP_Motors/AP_MotorsMatrixTS.h
new file mode 100644
index 0000000000..197e18739f
--- /dev/null
+++ b/libraries/AP_Motors/AP_MotorsMatrixTS.h
@@ -0,0 +1,30 @@
+/// @file	AP_MotorsMatrixTS.h
+/// @brief	Motor control class for tailsitters with multicopter motor configurations
+
+#pragma once
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include <SRV_Channel/SRV_Channel.h>
+#include "AP_MotorsMatrix.h"
+
+/// @class      AP_MotorsMatrix
+class AP_MotorsMatrixTS : public AP_MotorsMatrix {
+public:
+
+    AP_MotorsMatrixTS(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
+        AP_MotorsMatrix(loop_rate, speed_hz) {
+        AP_Param::setup_object_defaults(this, var_info);
+    };
+
+    virtual void        output_to_motors() override;
+
+    virtual void        output_motor_mask(float thrust, uint8_t mask, float rudder_dt) override;
+
+protected:
+    // configures the motors for the defined frame_class and frame_type
+    virtual void        setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override;
+
+    // calculate motor outputs
+    void                output_armed_stabilizing() override;
+};