AP_Motors: allow Matrix config from scripting

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -26,6 +26,11 @@ void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame
     _last_frame_class = frame_class;
     _last_frame_type = frame_type;
 
+    if (frame_class == MOTOR_FRAME_SCRIPTING_MATRIX) {
+        // if Scripting frame class, do nothing scripting must call its own dedicated init function
+        return;
+    }
+
     // setup the motors
     setup_motors(frame_class, frame_type);
 
@@ -33,6 +38,63 @@ void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame
     set_update_rate(_speed_hz);
 }
 
+#ifdef ENABLE_SCRIPTING
+// dedicated init for lua scripting
+bool AP_MotorsMatrix::init(uint8_t expected_num_motors)
+{
+    if (_last_frame_class != MOTOR_FRAME_SCRIPTING_MATRIX) {
+        // not the correct class
+        return false;
+    }
+
+    // Make sure the correct number of motors have been added
+    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;
+    }
+
+    normalise_rpy_factors();
+
+    set_update_rate(_speed_hz);
+
+    set_initialised_ok(true);
+
+    return true;
+}
+#endif // ENABLE_SCRIPTING
+
 // set update rate to motors - a value in hertz
 void AP_MotorsMatrix::set_update_rate(uint16_t speed_hz)
 {
@@ -55,14 +117,9 @@ void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, mo
     if (armed() || (frame_class == _last_frame_class && _last_frame_type == frame_type)) {
         return;
     }
-    _last_frame_class = frame_class;
-    _last_frame_type = frame_type;
 
-    // setup the motors
-    setup_motors(frame_class, frame_type);
+    init(frame_class, frame_type);
 
-    // enable fast channels or instant pwm
-    set_update_rate(_speed_hz);
 }
 
 void AP_MotorsMatrix::output_to_motors()
@@ -429,6 +486,11 @@ bool AP_MotorsMatrix::output_test_num(uint8_t output_channel, int16_t pwm)
 // add_motor
 void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order)
 {
+    if (initialised_ok()) {
+        // do not allow motors to be set if the current frame type has init correctly
+        return;
+    }
+
     // ensure valid motor number is provided
     if (motor_num >= 0 && motor_num < AP_MOTORS_MAX_NUM_MOTORS) {
 
@@ -486,7 +548,7 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
     for (int8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
         remove_motor(i);
     }
-
+    set_initialised_ok(false);
     bool success = true;
 
     switch (frame_class) {
@@ -988,3 +1050,6 @@ void AP_MotorsMatrix::disable_yaw_torque(void)
         _yaw_factor[i] = 0;
     }
 }
+
+// singleton instance
+AP_MotorsMatrix *AP_MotorsMatrix::_singleton;

libraries/AP_Motors/AP_MotorsMatrix.h

@@ -17,11 +17,26 @@ public:
     /// Constructor
     AP_MotorsMatrix(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
         AP_MotorsMulticopter(loop_rate, speed_hz)
-    {};
+    {
+        if (_singleton != nullptr) {
+            AP_HAL::panic("AP_MotorsMatrix must be singleton");
+        }
+        _singleton = this;
+    };
+
+    // get singleton instance
+    static AP_MotorsMatrix *get_singleton() {
+        return _singleton;
+    }
 
     // init
     void                init(motor_frame_class frame_class, motor_frame_type frame_type) override;
 
+#ifdef ENABLE_SCRIPTING
+    // Init to be called from scripting
+    bool                init(uint8_t expected_num_motors);
+#endif // ENABLE_SCRIPTING
+
     // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
     void                set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) override;
 
@@ -59,6 +74,9 @@ public:
     // as vectoring is used for yaw control
     void                disable_yaw_torque(void) override;
 
+    // add_motor using raw roll, pitch, throttle and yaw factors
+    void                add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order);
+
 protected:
     // output - sends commands to the motors
     void                output_armed_stabilizing() override;
@@ -66,9 +84,6 @@ protected:
     // check for failed motor
     void                check_for_failed_motor(float throttle_thrust_best);
 
-    // add_motor using raw roll, pitch, throttle and yaw factors
-    void                add_motor_raw(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order);
-
     // add_motor using just position and yaw_factor (or prop direction)
     void                add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order);
 
@@ -98,4 +113,7 @@ protected:
     // motor failure handling
     float               _thrust_rpyt_out_filt[AP_MOTORS_MAX_NUM_MOTORS];    // filtered thrust outputs with 1 second time constant
     uint8_t             _motor_lost_index;  // index number of the lost motor
+
+private:
+    static AP_MotorsMatrix *_singleton;
 };

libraries/AP_Motors/AP_Motors_Class.h

@@ -45,6 +45,7 @@ public:
         MOTOR_FRAME_DODECAHEXA = 12,
         MOTOR_FRAME_HELI_QUAD = 13,
         MOTOR_FRAME_DECA = 14,
+        MOTOR_FRAME_SCRIPTING_MATRIX = 15,
     };
     enum motor_frame_type {
         MOTOR_FRAME_TYPE_PLUS = 0,