From 7d528330d570fa4900b1a24c2a1f851c3a7dcba0 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Tue, 6 Jan 2015 00:57:54 +0200 Subject: [PATCH 1/4] Implemented SK450 DeadCat frame support Implemented the way to specify motor output scale which is required for SK450 DeadCat as it has asymetrical arms (front arms are longer than back ones) --- .../px4fmu_common/init.d/10019_sk450_deadcat | 32 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++ .../px4fmu_common/mixers/FMU_quad_deadcat.mix | 25 +++++++++++++++ src/modules/systemlib/mixer/mixer.h | 3 +- .../systemlib/mixer/mixer_multirotor.cpp | 9 +++++- src/modules/systemlib/mixer/multi_tables.py | 21 +++++++++--- 6 files changed, 89 insertions(+), 6 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/10019_sk450_deadcat create mode 100644 ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat new file mode 100644 index 0000000000..96463d5ef5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -0,0 +1,32 @@ +#!nsh +# +# HobbyKing SK450 DeadCat modification +# +# Anton Matosov +# + +sh /etc/init.d/rc.mc_defaults + +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 6.0 + param set MC_ROLLRATE_P 0.04 + param set MC_ROLLRATE_I 0.1 + param set MC_ROLLRATE_D 0.0015 + + param set MC_PITCH_P 6.0 + param set MC_PITCHRATE_P 0.08 + param set MC_PITCHRATE_I 0.2 + param set MC_PITCHRATE_D 0.0015 + + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.1 + param set MC_YAWRATE_I 0.07 + param set MC_YAWRATE_D 0.0 +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_deadcat + +set PWM_OUTPUTS 1234 +set PWM_MIN 1075 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 20f2be0d9e..b83687fbd4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -226,6 +226,11 @@ then sh /etc/init.d/10018_tbs_endurance fi +if param compare SYS_AUTOSTART 10019 +then + sh /etc/init.d/10019_sk450_deadcat +fi + # # Hexa Coaxial # diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix new file mode 100644 index 0000000000..a8c5b716d6 --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix @@ -0,0 +1,25 @@ +Multirotor mixer for PX4FMU +=========================== + +This file defines a single mixer for a quadrotor in SK450 DeadCat configuration. All controls are mixed 100%. + +R: 4dc 10000 10000 10000 0 + +Gimbal / payload mixer for last four channels +----------------------------------------------------- + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 4 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 5 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 6 10000 10000 0 -10000 10000 + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 7 10000 10000 0 -10000 10000 diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 864ce21a58..67ef521b4e 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -441,7 +441,6 @@ private: SimpleMixer operator=(const SimpleMixer&); }; - /** * Supported multirotor geometries. * @@ -460,12 +459,14 @@ class __EXPORT MultirotorMixer : public Mixer { public: /** + * Precalculated rotor mix. */ struct Rotor { float roll_scale; /**< scales roll for this rotor */ float pitch_scale; /**< scales pitch for this rotor */ float yaw_scale; /**< scales yaw for this rotor */ + float out_scale; /**< scales total out for this rotor */ }; /** diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 5cfbe47f07..2ab5b5e8e4 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -75,7 +75,8 @@ float constrain(float val, float min, float max) { return (val < min) ? min : ((val > max) ? max : val); } -} + +} // anonymous namespace MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, @@ -89,6 +90,7 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, _pitch_scale(pitch_scale), _yaw_scale(yaw_scale), _idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */ + _limits_pub(), _rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]), _rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]) { @@ -152,6 +154,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl } else if (!strcmp(geomname, "4w")) { geometry = MultirotorGeometry::QUAD_WIDE; + } else if (!strcmp(geomname, "4dc")) { + geometry = MultirotorGeometry::QUAD_DEADCAT; + } else if (!strcmp(geomname, "6+")) { geometry = MultirotorGeometry::HEX_PLUS; @@ -212,6 +217,8 @@ MultirotorMixer::mix(float *outputs, unsigned space) pitch * _rotors[i].pitch_scale + thrust; + out *= _rotors[i].out_scale; + /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py index 21c0affd95..ba59e0536d 100755 --- a/src/modules/systemlib/mixer/multi_tables.py +++ b/src/modules/systemlib/mixer/multi_tables.py @@ -69,6 +69,13 @@ quad_plus = [ [ 180, CW], ] +quad_deadcat = [ + [ 63, CCW, 1.0], + [-135, CCW, 0.964], + [ -63, CW, 1.0], + [ 135, CW, 0.964], +] + quad_v = [ [ 18.8, 0.4242], [ -18.8, 1.0], @@ -148,13 +155,18 @@ twin_engine = [ [-90, 0.0], ] + +tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine] + def variableName(variable): for variableName, value in list(globals().items()): if value is variable: return variableName -tables = [quad_x, quad_plus, quad_v, quad_wide, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine] - +def unpackScales(scalesList): + if len(scalesList) == 2: + scalesList += [1.0] #Add thrust scale + return scalesList def printEnum(): print("enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {") @@ -167,10 +179,11 @@ def printEnum(): def printScaleTables(): for table in tables: print("const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table))) - for (angle, yawScale) in table: + for row in table: + angle, yawScale, thrustScale = unpackScales(row) rollScale = rcos(angle + 90) pitchScale = rcos(angle) - print("\t{{ {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale)) + print("\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale, thrustScale)) print("};\n") def printScaleTablesIndex(): From 2776a705f46434e4cbbaa88f28e04078ad5ff558 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Tue, 6 Jan 2015 19:30:45 +0200 Subject: [PATCH 2/4] Adopted SK540 config to the latest changes in the init scripts Lowered the pwm min --- ROMFS/px4fmu_common/init.d/10019_sk450_deadcat | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index 96463d5ef5..d42721adaa 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -25,8 +25,7 @@ then param set MC_YAWRATE_D 0.0 fi -set VEHICLE_TYPE mc set MIXER FMU_quad_deadcat -set PWM_OUTPUTS 1234 -set PWM_MIN 1075 +set PWM_OUT 1234 +set PWM_MIN 1050 From 8624e2a77582b400206042918352418b4892790f Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Wed, 7 Jan 2015 00:42:35 +0200 Subject: [PATCH 3/4] Fixed naming of the mixer Added mixer check to set MAV_TYPE correctly --- ROMFS/px4fmu_common/init.d/10019_sk450_deadcat | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- .../mixers/{FMU_quad_deadcat.mix => sk450_deadcat.mix} | 0 3 files changed, 2 insertions(+), 2 deletions(-) rename ROMFS/px4fmu_common/mixers/{FMU_quad_deadcat.mix => sk450_deadcat.mix} (100%) diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index d42721adaa..e68f57f252 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -25,7 +25,7 @@ then param set MC_YAWRATE_D 0.0 fi -set MIXER FMU_quad_deadcat +set MIXER sk450_deadcat set PWM_OUT 1234 set PWM_MIN 1050 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 31d6a8a2d1..2d9f56a40b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -522,7 +522,7 @@ then then set MAV_TYPE 2 fi - if [ $MIXER == quad_w ] + if [ $MIXER == quad_w -o $MIXER == sk450_deadcat ] then set MAV_TYPE 2 fi diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix b/ROMFS/px4fmu_common/mixers/sk450_deadcat.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_quad_deadcat.mix rename to ROMFS/px4fmu_common/mixers/sk450_deadcat.mix From ba89c9eae26ba990317ff0b66d42e114b8c7dffa Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Wed, 7 Jan 2015 00:39:22 +0200 Subject: [PATCH 4/4] Adopted sk450_deadcat mixer file name --- .../mixers/{sk450_deadcat.mix => sk450_deadcat.main.mix} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename ROMFS/px4fmu_common/mixers/{sk450_deadcat.mix => sk450_deadcat.main.mix} (100%) diff --git a/ROMFS/px4fmu_common/mixers/sk450_deadcat.mix b/ROMFS/px4fmu_common/mixers/sk450_deadcat.main.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/sk450_deadcat.mix rename to ROMFS/px4fmu_common/mixers/sk450_deadcat.main.mix