diff options
Diffstat (limited to 'src/modules/systemlib')
-rw-r--r-- | src/modules/systemlib/mixer/mixer_multirotor.cpp | 126 | ||||
-rwxr-xr-x | src/modules/systemlib/mixer/multi_tables | 19 |
2 files changed, 77 insertions, 68 deletions
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 24187c9bc..eb1aef6c1 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -78,89 +78,87 @@ float constrain(float val, float min, float max) */ const MultirotorMixer::Rotor _config_quad_x[] = { - { -0.707107, 0.707107, 1.00 }, - { 0.707107, -0.707107, 1.00 }, - { 0.707107, 0.707107, -1.00 }, - { -0.707107, -0.707107, -1.00 }, + { -0.707107, 0.707107, 1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { 0.707107, 0.707107, -1.000000 }, + { -0.707107, -0.707107, -1.000000 }, }; const MultirotorMixer::Rotor _config_quad_plus[] = { - { -1.000000, 0.000000, 1.00 }, - { 1.000000, 0.000000, 1.00 }, - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, -1.00 }, + { -1.000000, 0.000000, 1.000000 }, + { 1.000000, 0.000000, 1.000000 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, -1.000000 }, }; -//Add table for quad in V configuration, which is not generated by multi_tables! const MultirotorMixer::Rotor _config_quad_v[] = { - { -0.3223, 0.9466, 0.4242 }, - { 0.3223, -0.9466, 1.0000 }, - { 0.3223, 0.9466, -0.4242 }, - { -0.3223, -0.9466, -1.0000 }, + { -0.322266, 0.946649, 0.424200 }, + { 0.322266, 0.946649, 1.000000 }, + { 0.322266, 0.946649, -0.424200 }, + { -0.322266, 0.946649, -1.000000 }, }; const MultirotorMixer::Rotor _config_quad_wide[] = { - { -0.927184, 0.374607, 1.00 }, - { 0.777146, -0.629320, 1.00 }, - { 0.927184, 0.374607, -1.00 }, - { -0.777146, -0.629320, -1.00 }, + { -0.927184, 0.374607, 1.000000 }, + { 0.777146, -0.629320, 1.000000 }, + { 0.927184, 0.374607, -1.000000 }, + { -0.777146, -0.629320, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_x[] = { - { -1.000000, 0.000000, -1.00 }, - { 1.000000, 0.000000, 1.00 }, - { 0.500000, 0.866025, -1.00 }, - { -0.500000, -0.866025, 1.00 }, - { -0.500000, 0.866025, 1.00 }, - { 0.500000, -0.866025, -1.00 }, + { -1.000000, 0.000000, -1.000000 }, + { 1.000000, 0.000000, 1.000000 }, + { 0.500000, 0.866025, -1.000000 }, + { -0.500000, -0.866025, 1.000000 }, + { -0.500000, 0.866025, 1.000000 }, + { 0.500000, -0.866025, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_plus[] = { - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, 1.00 }, - { 0.866025, -0.500000, -1.00 }, - { -0.866025, 0.500000, 1.00 }, - { 0.866025, 0.500000, 1.00 }, - { -0.866025, -0.500000, -1.00 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, 1.000000 }, + { 0.866025, -0.500000, -1.000000 }, + { -0.866025, 0.500000, 1.000000 }, + { 0.866025, 0.500000, 1.000000 }, + { -0.866025, -0.500000, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_cox[] = { - { -0.866025, 0.500000, -1.00 }, - { -0.866025, 0.500000, 1.00 }, - { -0.000000, -1.000000, -1.00 }, - { -0.000000, -1.000000, 1.00 }, - { 0.866025, 0.500000, -1.00 }, - { 0.866025, 0.500000, 1.00 }, + { -0.866025, 0.500000, -1.000000 }, + { -0.866025, 0.500000, 1.000000 }, + { -0.000000, -1.000000, -1.000000 }, + { -0.000000, -1.000000, 1.000000 }, + { 0.866025, 0.500000, -1.000000 }, + { 0.866025, 0.500000, 1.000000 }, }; const MultirotorMixer::Rotor _config_octa_x[] = { - { -0.382683, 0.923880, -1.00 }, - { 0.382683, -0.923880, -1.00 }, - { -0.923880, 0.382683, 1.00 }, - { -0.382683, -0.923880, 1.00 }, - { 0.382683, 0.923880, 1.00 }, - { 0.923880, -0.382683, 1.00 }, - { 0.923880, 0.382683, -1.00 }, - { -0.923880, -0.382683, -1.00 }, + { -0.382683, 0.923880, -1.000000 }, + { 0.382683, -0.923880, -1.000000 }, + { -0.923880, 0.382683, 1.000000 }, + { -0.382683, -0.923880, 1.000000 }, + { 0.382683, 0.923880, 1.000000 }, + { 0.923880, -0.382683, 1.000000 }, + { 0.923880, 0.382683, -1.000000 }, + { -0.923880, -0.382683, -1.000000 }, }; const MultirotorMixer::Rotor _config_octa_plus[] = { - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, -1.00 }, - { -0.707107, 0.707107, 1.00 }, - { -0.707107, -0.707107, 1.00 }, - { 0.707107, 0.707107, 1.00 }, - { 0.707107, -0.707107, 1.00 }, - { 1.000000, 0.000000, -1.00 }, - { -1.000000, 0.000000, -1.00 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, -1.000000 }, + { -0.707107, 0.707107, 1.000000 }, + { -0.707107, -0.707107, 1.000000 }, + { 0.707107, 0.707107, 1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { 1.000000, 0.000000, -1.000000 }, + { -1.000000, 0.000000, -1.000000 }, }; const MultirotorMixer::Rotor _config_octa_cox[] = { - { -0.707107, 0.707107, 1.00 }, - { 0.707107, 0.707107, -1.00 }, - { 0.707107, -0.707107, 1.00 }, - { -0.707107, -0.707107, -1.00 }, - { 0.707107, 0.707107, 1.00 }, - { -0.707107, 0.707107, -1.00 }, - { -0.707107, -0.707107, 1.00 }, - { 0.707107, -0.707107, -1.00 }, + { -0.707107, 0.707107, 1.000000 }, + { 0.707107, 0.707107, -1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { -0.707107, -0.707107, -1.000000 }, + { 0.707107, 0.707107, 1.000000 }, + { -0.707107, 0.707107, -1.000000 }, + { -0.707107, -0.707107, 1.000000 }, + { 0.707107, -0.707107, -1.000000 }, }; -const MultirotorMixer::Rotor _config_duorotor[] = { - { -1.000000, 0.000000, 0.00 }, - { 1.000000, 0.000000, 0.00 }, +const MultirotorMixer::Rotor _config_twin_engine[] = { + { -1.000000, 0.000000, 0.000000 }, + { 1.000000, 0.000000, 0.000000 }, }; - const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], @@ -172,7 +170,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_octa_x[0], &_config_octa_plus[0], &_config_octa_cox[0], - &_config_duorotor[0], + &_config_twin_engine[0], }; const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 18c828578..bdb62f812 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -21,6 +21,12 @@ set quad_plus { 180 CW } +set quad_v { + 18.8 0.4242 + -18.8 1.0 + -18.8 -0.4242 + 18.8 -1.0 +} set quad_wide { 68 CCW @@ -89,11 +95,14 @@ set octa_cox { -135 CW } +set twin_engine { + 90 0.0 + -90 0.0 +} -set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox} - +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox twin_engine} -proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} +proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %9.6f }," [rcos [expr $a + 90]] [rcos $a] [expr $d]]} foreach table $tables { puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] @@ -101,9 +110,11 @@ foreach table $tables { upvar #0 $table angles foreach {angle dir} $angles { if {$dir == "CW"} { + set dd -1.0 + } elseif {$dir == "CCW"} { set dd 1.0 } else { - set dd -1.0 + set dd $dir } factors $angle $dd } |