From 98fb310c5e8b6c69dd7f833168917fafb5b66e5b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2024 11:14:43 +1000 Subject: [PATCH 1/6] SITL: support up to 32 rotors in a frame --- libraries/SITL/SIM_Frame.cpp | 2 +- libraries/SITL/SIM_Frame.h | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index c3f2fba101295..e31bb817172df 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/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; iget(label_name); if (v.is()) { diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index a097943f6e5c4..7b24c9cc3e867 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -22,6 +22,10 @@ #include "SIM_Motor.h" #include +#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; From 7936e2e5d413218b6845bbb87cdd8491e5ff2114 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2024 11:15:03 +1000 Subject: [PATCH 2/6] SRV_Channel: support 32 rotors in a frame --- libraries/SRV_Channel/SRV_Channel.cpp | 6 +++++- libraries/SRV_Channel/SRV_Channel.h | 25 ++++++++++++++++++++++++- libraries/SRV_Channel/SRV_Channels.cpp | 2 +- 3 files changed, 30 insertions(+), 3 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp index 1cfccbd8bdb29..f05f389e9207a 100644 --- a/libraries/SRV_Channel/SRV_Channel.cpp +++ b/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; } diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index acb6021b89205..886fb3b16de02 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/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(); diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 292aa2bcacb77..c80d57758a28f 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/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 } From a7d29336cd3fb7d17737fd31818944fa2872c946 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2024 11:15:18 +1000 Subject: [PATCH 3/6] AP_Motors: support up to 32 motors --- libraries/AP_Motors/AP_Motors_Class.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 76e4928cb3f66..6b73b59db50c6 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/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 From 4ee45ce5d12fe4a1cc180b9f5736ef495cd96b4e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2024 11:15:36 +1000 Subject: [PATCH 4/6] HAL_SITL: added m32 frame a stack of 8 X quad frames --- libraries/AP_HAL_SITL/SITL_cmdline.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 042bb2ce7f212..a3b8ebd781cd6 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/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 }, From 6dbf197cdbc4b51741a3e094feb86fe78d419459 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2024 11:15:54 +1000 Subject: [PATCH 5/6] autotest: added m32 frame --- Tools/autotest/default_params/copter-32.parm | 2 ++ Tools/autotest/pysim/vehicleinfo.py | 7 +++++++ 2 files changed, 9 insertions(+) create mode 100644 Tools/autotest/default_params/copter-32.parm diff --git a/Tools/autotest/default_params/copter-32.parm b/Tools/autotest/default_params/copter-32.parm new file mode 100644 index 0000000000000..759ead46cb3c7 --- /dev/null +++ b/Tools/autotest/default_params/copter-32.parm @@ -0,0 +1,2 @@ +FRAME_CLASS 15 + diff --git a/Tools/autotest/pysim/vehicleinfo.py b/Tools/autotest/pysim/vehicleinfo.py index ca07c51b8f19d..42273c23b4099 100644 --- a/Tools/autotest/pysim/vehicleinfo.py +++ b/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", From fdda6080283c65e2d0efe7da86c7c0bbf4716821 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 20 Jul 2024 11:18:06 +1000 Subject: [PATCH 6/6] AP_Scripting: added 32 motor example --- .../AP_Scripting/examples/MotorMatrix_32.lua | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 libraries/AP_Scripting/examples/MotorMatrix_32.lua diff --git a/libraries/AP_Scripting/examples/MotorMatrix_32.lua b/libraries/AP_Scripting/examples/MotorMatrix_32.lua new file mode 100644 index 0000000000000..0192ff77b5395 --- /dev/null +++ b/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))