AP_Motors: add 6DoF scripting matrix mixer

libraries/AP_Motors/AP_Motors.h

@@ -11,3 +11,4 @@
 #include "AP_MotorsCoax.h"
 #include "AP_MotorsTailsitter.h"
 #include "AP_Motors6DOF.h"
+#include "AP_MotorsMatrix_6DoF_Scripting.h"

libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.cpp

@@ -0,0 +1,325 @@
+/*
+   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/>.
+ */
+
+#ifdef ENABLE_SCRIPTING
+
+#include <AP_HAL/AP_HAL.h>
+#include "AP_MotorsMatrix_6DoF_Scripting.h"
+#include <AP_Vehicle/AP_Vehicle.h>
+
+extern const AP_HAL::HAL& hal;
+
+void AP_MotorsMatrix_6DoF_Scripting::output_to_motors()
+{
+    switch (_spool_state) {
+        case SpoolState::SHUT_DOWN:
+        case SpoolState::GROUND_IDLE:
+        {
+            // no output, cant spin up for ground idle because we don't know which way motors should be spining
+            for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+                if (motor_enabled[i]) {
+                    _actuator[i] = 0.0f;
+                }
+            }
+            break;
+        }
+        case SpoolState::SPOOLING_UP:
+        case SpoolState::THROTTLE_UNLIMITED:
+        case SpoolState::SPOOLING_DOWN:
+            // set motor output based on thrust requests
+            for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+                if (motor_enabled[i]) {
+                    if (_reversible[i]) {
+                        // revesible motor can provide both positive and negative thrust, +- spin max, spin min does not apply
+                        if (is_positive(_thrust_rpyt_out[i])) { 
+                            _actuator[i] = apply_thrust_curve_and_volt_scaling(_thrust_rpyt_out[i]) * _spin_max;
+
+                        } else if (is_negative(_thrust_rpyt_out[i])) {
+                            _actuator[i] = -apply_thrust_curve_and_volt_scaling(-_thrust_rpyt_out[i]) * _spin_max;
+
+                        } else {
+                            _actuator[i] = 0.0f;
+                        }
+                    } else {
+                        // motor can only provide trust in a single direction, spin min to spin max as 'normal' copter
+                         _actuator[i] = thrust_to_actuator(_thrust_rpyt_out[i]);
+                    }
+                }
+            }
+            break;
+    }
+
+    // Send to each motor
+    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _actuator[i] * 4500);
+        }
+    }
+}
+
+// output_armed - sends commands to the motors
+void AP_MotorsMatrix_6DoF_Scripting::output_armed_stabilizing()
+{
+    uint8_t i;                          // general purpose counter
+    float   roll_thrust;                // roll thrust input value, +/- 1.0
+    float   pitch_thrust;               // pitch thrust input value, +/- 1.0
+    float   yaw_thrust;                 // yaw thrust input value, +/- 1.0
+    float   throttle_thrust;            // throttle thrust input value, 0.0 - 1.0
+    float   forward_thrust;             // forward thrust input value, +/- 1.0
+    float   right_thrust;               // right thrust input value, +/- 1.0
+
+    // note that the throttle, forwards and right inputs are not in bodyframe, they are in the frame of the 'normal' 4DoF copter were pretending to be
+
+    // apply voltage and air pressure compensation
+    const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
+    roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
+    pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
+    yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
+    throttle_thrust = get_throttle() * compensation_gain;
+
+    // scale horizontal thrust with throttle, this mimics a normal copter
+    // so we don't break the lean angle proportional acceleration assumption made by the position controller
+    forward_thrust = get_forward() * throttle_thrust;
+    right_thrust = get_lateral() * throttle_thrust;
+
+
+    // set throttle limit flags
+    if (throttle_thrust <= 0) {
+        throttle_thrust = 0;
+        // we cant thrust down, the vehicle can do it, but it would break a lot of assumptions further up the control stack
+        // 1G decent probably plenty anyway....
+        limit.throttle_lower = true;
+    }
+    if (throttle_thrust >= 1) {
+        throttle_thrust = 1;
+        limit.throttle_upper = true;
+    }
+
+    // rotate the thrust into bodyframe
+    Matrix3f rot;
+    Vector3f thrust_vec;
+    rot.from_euler312(_roll_offset, _pitch_offset, 0.0f);
+
+
+    /*
+        upwards thrust, independent of orientation
+    */
+    thrust_vec.x = 0.0f;
+    thrust_vec.y = 0.0f;
+    thrust_vec.z = throttle_thrust;
+    thrust_vec = rot * thrust_vec;
+    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            _thrust_rpyt_out[i] =  thrust_vec.x * _forward_factor[i];
+            _thrust_rpyt_out[i] += thrust_vec.y * _right_factor[i];
+            _thrust_rpyt_out[i] += thrust_vec.z * _throttle_factor[i];
+
+            if (fabsf(_thrust_rpyt_out[i]) >= 1) {
+                // if we hit this the mixer is probably scaled incorrectly
+                limit.throttle_upper = true;
+            }
+            _thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i],-1.0f,1.0f);
+        }
+    }
+
+
+    /*
+        rotations: roll, pitch and yaw
+    */
+    float rpy_ratio = 1.0f;  // scale factor, output will be scaled by this ratio so it can all fit evenly
+    float thrust[AP_MOTORS_MAX_NUM_MOTORS];
+    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            thrust[i] =  roll_thrust * _roll_factor[i];
+            thrust[i] += pitch_thrust * _pitch_factor[i];
+            thrust[i] += yaw_thrust * _yaw_factor[i];
+            float total_thrust = _thrust_rpyt_out[i] + thrust[i];
+            // control input will be limited by motor range
+            if (total_thrust > 1.0f) {
+                rpy_ratio = MIN(rpy_ratio,(1.0f - _thrust_rpyt_out[i]) / thrust[i]);
+            } else if (total_thrust < -1.0f) {
+                rpy_ratio = MIN(rpy_ratio,(-1.0f -_thrust_rpyt_out[i]) / thrust[i]);
+            }
+        }
+    }
+
+    // set limit flags if output is being scaled
+    if (rpy_ratio < 1) {
+        limit.roll = true;
+        limit.pitch = true;
+        limit.yaw = true;
+    }
+
+    // scale back rotations evenly so it will all fit
+    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            _thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i] + thrust[i] * rpy_ratio,-1.0f,1.0f);
+        }
+    }
+
+    /*
+        forward and lateral, independent of orentaiton
+    */
+    thrust_vec.x = forward_thrust;
+    thrust_vec.y = right_thrust;
+    thrust_vec.z = 0.0f;
+    thrust_vec = rot * thrust_vec;
+
+    float horz_ratio = 1.0f; 
+    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            thrust[i] =  thrust_vec.x * _forward_factor[i];
+            thrust[i] += thrust_vec.y * _right_factor[i];
+            thrust[i] += thrust_vec.z * _throttle_factor[i];
+            float total_thrust = _thrust_rpyt_out[i] + thrust[i];
+            // control input will be limited by motor range
+            if (total_thrust > 1.0f) {
+                horz_ratio = MIN(horz_ratio,(1.0f - _thrust_rpyt_out[i]) / thrust[i]);
+            } else if (total_thrust < -1.0f) {
+                horz_ratio = MIN(horz_ratio,(-1.0f -_thrust_rpyt_out[i]) / thrust[i]);
+            }
+        }
+    }
+
+    // scale back evenly so it will all fit
+    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            _thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i] + thrust[i] * horz_ratio,-1.0f,1.0f);
+        }
+    }
+
+    /*
+        apply deadzone to revesible motors, this stops motors from reversing direction too often
+        re-use yaw headroom param for deadzone, constain to a max of 25%
+    */
+    const float deadzone = constrain_float(_yaw_headroom.get() * 0.001f,0.0f,0.25f);
+    for (i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i] && _reversible[i]) {
+            if (is_negative(_thrust_rpyt_out[i])) {
+                if ((_thrust_rpyt_out[i] > -deadzone) && is_positive(_last_thrust_out[i])) {
+                    _thrust_rpyt_out[i] = 0.0f;
+                } else {
+                    _last_thrust_out[i] = _thrust_rpyt_out[i];
+                }
+            } else if (is_positive(_thrust_rpyt_out[i])) {
+                if ((_thrust_rpyt_out[i] < deadzone) && is_negative(_last_thrust_out[i])) {
+                    _thrust_rpyt_out[i] = 0.0f;
+                } else {
+                    _last_thrust_out[i] = _thrust_rpyt_out[i];
+                }
+            }
+        }
+    }
+
+}
+
+// sets the roll and pitch offset, this rotates the thrust vector in body frame
+// these are typically set such that the throttle thrust vector is earth frame up
+void AP_MotorsMatrix_6DoF_Scripting::set_roll_pitch(float roll_deg, float pitch_deg)
+{
+    _roll_offset = radians(roll_deg);
+    _pitch_offset = radians(pitch_deg);
+}
+
+// add_motor, take roll, pitch, yaw, throttle(up), forward, right factors along with a bool if the motor is reversible and the testing order, called from scripting
+void AP_MotorsMatrix_6DoF_Scripting::add_motor(int8_t motor_num, float roll_factor, float pitch_factor, float yaw_factor, float throttle_factor, float forward_factor, float right_factor, bool reversible, uint8_t testing_order)
+{
+    if (initialised_ok()) {
+        // don't allow matrix to be changed after init
+        return;
+    }
+
+    // ensure valid motor number is provided
+    if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
+        motor_enabled[motor_num] = true;
+
+        _roll_factor[motor_num] = roll_factor;
+        _pitch_factor[motor_num] = pitch_factor;
+        _yaw_factor[motor_num] = yaw_factor;
+
+        _throttle_factor[motor_num] = throttle_factor;
+        _forward_factor[motor_num] = forward_factor;
+        _right_factor[motor_num] = right_factor;
+
+        // set order that motor appears in test
+        _test_order[motor_num] = testing_order;
+
+        // ensure valid motor number is provided
+        SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(motor_num);
+        SRV_Channels::set_aux_channel_default(function, motor_num);
+
+        uint8_t chan;
+        if (!SRV_Channels::find_channel(function, chan)) {
+            gcs().send_text(MAV_SEVERITY_ERROR, "Motors: unable to setup motor %u", motor_num);
+            return;
+        }
+
+        _reversible[motor_num] = reversible;
+        if (_reversible[motor_num]) {
+            // reversible, set to angle type hard code trim to 1500
+            SRV_Channels::set_angle(function, 4500);
+            SRV_Channels::set_trim_to_pwm_for(function, 1500);
+        } else {
+            SRV_Channels::set_range(function, 4500);
+        }
+        SRV_Channels::set_output_min_max(function, get_pwm_output_min(), get_pwm_output_max());
+    }
+}
+
+bool AP_MotorsMatrix_6DoF_Scripting::init(uint8_t expected_num_motors) {
+    uint8_t num_motors = 0;
+    for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
+        if (motor_enabled[i]) {
+            num_motors++;
+        }
+    }
+
+    set_initialised_ok(expected_num_motors == num_motors);
+
+    if (!initialised_ok()) {
+        _mav_type = MAV_TYPE_GENERIC;
+        return false;
+    }
+
+    switch (num_motors) {
+        case 3:
+            _mav_type = MAV_TYPE_TRICOPTER;
+            break;
+        case 4:
+            _mav_type = MAV_TYPE_QUADROTOR;
+            break;
+        case 6:
+            _mav_type = MAV_TYPE_HEXAROTOR;
+            break;
+        case 8:
+            _mav_type = MAV_TYPE_OCTOROTOR;
+            break;
+        case 10:
+            _mav_type = MAV_TYPE_DECAROTOR;
+            break;
+        case 12:
+            _mav_type = MAV_TYPE_DODECAROTOR;
+            break;
+        default:
+            _mav_type = MAV_TYPE_GENERIC;
+    }
+
+    return true;
+}
+
+// singleton instance
+AP_MotorsMatrix_6DoF_Scripting *AP_MotorsMatrix_6DoF_Scripting::_singleton;
+
+#endif // ENABLE_SCRIPTING

libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h

@@ -0,0 +1,66 @@
+#pragma once
+#ifdef ENABLE_SCRIPTING
+
+#include <AP_Common/AP_Common.h>
+#include <AP_Math/AP_Math.h>
+#include <RC_Channel/RC_Channel.h>
+#include "AP_MotorsMatrix.h"
+
+class AP_MotorsMatrix_6DoF_Scripting : public AP_MotorsMatrix {
+public:
+
+    /// Constructor
+    AP_MotorsMatrix_6DoF_Scripting(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
+        AP_MotorsMatrix(loop_rate, speed_hz)
+    {
+        if (_singleton != nullptr) {
+            AP_HAL::panic("AP_MotorsMatrix 6DoF must be singleton");
+        }
+        _singleton = this;
+    };
+
+    // get singleton instance
+    static AP_MotorsMatrix_6DoF_Scripting *get_singleton() {
+        return _singleton;
+    }
+
+    // output_to_motors - sends minimum values out to the motors
+    void output_to_motors() override;
+
+    // sets the roll and pitch offset, this rotates the thrust vector in body frame
+    // these are typically set such that the throttle thrust vector is earth frame up
+    void set_roll_pitch(float roll_deg, float pitch_deg) override;
+
+    // add_motor using raw roll, pitch, throttle and yaw factors, to be called from scripting
+    void add_motor(int8_t motor_num, float roll_factor, float pitch_factor, float yaw_factor, float throttle_factor, float forward_factor, float right_factor, bool reversible, uint8_t testing_order);
+
+    // if the expected number of motors have been setup then set as initalized
+    bool init(uint8_t expected_num_motors) override;
+
+protected:
+    // output - sends commands to the motors
+    void output_armed_stabilizing() override;
+
+    // nothing to do for setup, scripting will mark as initalized when done
+    void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override {};
+
+    float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS];     // each motors contribution to up thrust
+    float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS];      // each motors contribution to forward thrust
+    float _right_factor[AP_MOTORS_MAX_NUM_MOTORS];        // each motors contribution to right thrust
+
+    // true if motor is revesible, it can go from -Spin max to +Spin max, if false motor is can go from Spin min to Spin max
+    bool _reversible[AP_MOTORS_MAX_NUM_MOTORS];
+
+    // store last values to allow deadzone
+    float _last_thrust_out[AP_MOTORS_MAX_NUM_MOTORS];
+
+    // Current offset angles, radians
+    float _roll_offset;
+    float _pitch_offset;
+
+private:
+    static AP_MotorsMatrix_6DoF_Scripting *_singleton;
+
+};
+
+#endif // ENABLE_SCRIPTING

libraries/AP_Motors/AP_Motors_Class.h

@@ -46,6 +46,7 @@ public:
         MOTOR_FRAME_HELI_QUAD = 13,
         MOTOR_FRAME_DECA = 14,
         MOTOR_FRAME_SCRIPTING_MATRIX = 15,
+        MOTOR_FRAME_6DOF_SCRIPTING = 16,
     };
 
     // return string corresponding to frame_class
@@ -106,6 +107,9 @@ public:
     void                set_forward(float forward_in) { _forward_in = forward_in; }; // range -1 ~ +1
     void                set_lateral(float lateral_in) { _lateral_in = lateral_in; };     // range -1 ~ +1
 
+    // for 6DoF vehicles, sets the roll and pitch offset, this rotates the thrust vector in body frame
+    virtual void        set_roll_pitch(float roll_deg, float pitch_deg) {};
+
     // accessors for roll, pitch, yaw and throttle inputs to motors
     float               get_roll() const { return _roll_in; }
     float               get_pitch() const { return _pitch_in; }