SITL: added support for up to 32 rotors

Tools/autotest/default_params/copter-32.parm

@@ -0,0 +1,2 @@
+FRAME_CLASS     15
+

Tools/autotest/pysim/vehicleinfo.py

@@ -129,6 +129,13 @@ def __init__(self):
                 "default_params_filename": ["default_params/copter.parm",
                                             "default_params/copter-dodecahexa.parm" ],
             },
+            "m32": {
+                "waf_target": "bin/arducopter",
+                "default_params_filename": [
+                    "default_params/copter.parm",
+                    "default_params/copter-32.parm",
+                ],
+            },
             # SIM
             "IrisRos": {
                 "waf_target": "bin/arducopter",

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -143,6 +143,7 @@ static const struct {
     { "octa-cwx",           MultiCopter::create },
     { "octa-dji",           MultiCopter::create },
     { "octa-quad-cwx",      MultiCopter::create },
+    { "m32",                MultiCopter::create },
     { "dodeca-hexa",        MultiCopter::create },
     { "tri",                MultiCopter::create },
     { "y6",                 MultiCopter::create },

libraries/AP_Motors/AP_Motors_Class.h

@@ -21,7 +21,9 @@
 #define AP_MOTORS_MOT_11 10U
 #define AP_MOTORS_MOT_12 11U
 
-#define AP_MOTORS_MAX_NUM_MOTORS 12
+#ifndef AP_MOTORS_MAX_NUM_MOTORS
+#define AP_MOTORS_MAX_NUM_MOTORS 32
+#endif
 
 #ifndef AP_MOTORS_FRAME_DEFAULT_ENABLED
 #define AP_MOTORS_FRAME_DEFAULT_ENABLED 1

libraries/AP_Scripting/examples/MotorMatrix_32.lua

@@ -0,0 +1,17 @@
+-- This script is an example setting up a custom motor matrix mix
+
+local NUM_BANKS = 8
+
+-- duplicate a quad 8x for 32 motors
+for i = 1, NUM_BANKS do
+   local base = (i-1)*4
+   gcs:send_text(0, string.format("Setting up motor bank %u at %u", i, base))
+   MotorsMatrix:add_motor_raw(base+0, -1,  1,  1, base+2)
+   MotorsMatrix:add_motor_raw(base+1,  1, -1,  1, base+4)
+   MotorsMatrix:add_motor_raw(base+2,  1,  1, -1, base+1)
+   MotorsMatrix:add_motor_raw(base+3, -1, -1, -1, base+3)
+end
+
+assert(MotorsMatrix:init(NUM_BANKS*4), "Failed to init MotorsMatrix")
+
+motors:set_frame_string(string.format("Motors%u", NUM_BANKS*4))

libraries/SITL/SIM_Frame.cpp

@@ -409,7 +409,7 @@ void Frame::load_frame_params(const char *model_json)
     };
     char label_name[20];
     for (uint8_t i=0; i<ARRAY_SIZE(per_motor_vars); i++) {
-        for (uint8_t j=0; j<12; j++) {
+        for (uint8_t j=0; j<SIM_FRAME_MAX_ACTUATORS; j++) {
             snprintf(label_name, 20, "motor%i_%s", j+1, per_motor_vars[i].label);
             auto v = obj->get(label_name);
             if (v.is<AP_JSON::null>()) {

libraries/SITL/SIM_Frame.h

@@ -22,6 +22,10 @@
 #include "SIM_Motor.h"
 #include <AP_JSON/AP_JSON.h>
 
+#ifndef SIM_FRAME_MAX_ACTUATORS
+#define SIM_FRAME_MAX_ACTUATORS 32
+#endif
+
 namespace SITL {
 
 /*
@@ -133,9 +137,9 @@ class Frame {
         Vector3f moment_of_inertia;
 
         // if zero will no be used
-        Vector3f motor_pos[12];
-        Vector3f motor_thrust_vec[12];
-        float yaw_factor[12] = {0};
+        Vector3f motor_pos[SIM_FRAME_MAX_ACTUATORS];
+        Vector3f motor_thrust_vec[SIM_FRAME_MAX_ACTUATORS];
+        float yaw_factor[SIM_FRAME_MAX_ACTUATORS] {0,};
 
         // number of motors
         float num_motors = 4;

libraries/SRV_Channel/SRV_Channel.cpp

@@ -306,7 +306,8 @@ uint16_t SRV_Channel::get_limit_pwm(Limit limit) const
 bool SRV_Channel::is_motor(SRV_Channel::Aux_servo_function_t function)
 {
     return ((function >= SRV_Channel::k_motor1 && function <= SRV_Channel::k_motor8) ||
-            (function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12));
+            (function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12) ||
+            (function >= SRV_Channel::k_motor13 && function <= SRV_Channel::k_motor32));
 }
 
 // return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
@@ -332,6 +333,7 @@ bool SRV_Channel::should_e_stop(SRV_Channel::Aux_servo_function_t function)
     case Aux_servo_function_t::k_motor10:
     case Aux_servo_function_t::k_motor11:
     case Aux_servo_function_t::k_motor12:
+    case Aux_servo_function_t::k_motor13 ... Aux_servo_function_t::k_motor32:
     case Aux_servo_function_t::k_engine_run_enable:
         return true;
     default:
@@ -380,6 +382,8 @@ int8_t SRV_Channel::get_motor_num(void) const
         return int8_t(uint16_t(k_function) - k_motor1);
     case k_motor9 ... k_motor12:
         return 8 + int8_t(uint16_t(k_function) - k_motor9);
+    case k_motor13 ... k_motor32:
+        return 12 + int8_t(uint16_t(k_function) - k_motor13);
     default:
         break;
     }

libraries/SRV_Channel/SRV_Channel.h

@@ -189,6 +189,26 @@ class SRV_Channel {
         k_rcin15_mapped         = 154,
         k_rcin16_mapped         = 155,
         k_lift_release          = 156,
+        k_motor13               = 160,
+        k_motor14               = 161,
+        k_motor15               = 162,
+        k_motor16               = 163,
+        k_motor17               = 164,
+        k_motor18               = 165,
+        k_motor19               = 166,
+        k_motor20               = 167,
+        k_motor21               = 168,
+        k_motor22               = 169,
+        k_motor23               = 170,
+        k_motor24               = 171,
+        k_motor25               = 172,
+        k_motor26               = 173,
+        k_motor27               = 174,
+        k_motor28               = 175,
+        k_motor29               = 176,
+        k_motor30               = 177,
+        k_motor31               = 178,
+        k_motor32               = 179,
         k_nr_aux_servo_functions         ///< This must be the last enum value (only add new values _before_ this one)
     } Aux_servo_function_t;
 
@@ -539,7 +559,10 @@ class SRV_Channels {
         if (channel < 8) {
             return SRV_Channel::Aux_servo_function_t(SRV_Channel::k_motor1+channel);
         }
-        return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor9+(channel-8)));
+        if (channel < 12) {
+            return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor9+(channel-8)));
+        }
+        return SRV_Channel::Aux_servo_function_t((SRV_Channel::k_motor13+(channel-12)));
     }
 
     void cork();

libraries/SRV_Channel/SRV_Channels.cpp

@@ -355,7 +355,7 @@ SRV_Channels::SRV_Channels(void)
 #if NUM_SERVO_CHANNELS > 16
         if (i >= 16) {
             // default to GPIO, this disables the pin and stops logging
-            channels[i].function.set_default(SRV_Channel::k_GPIO);
+            channels[i].function.set_default(SRV_Channel::k_none);
         }
 #endif
     }