path: root/src/modules/systemlib/mixer/multi_tables

#!/usr/bin/tclsh
#
# Generate multirotor mixer scale tables compatible with the ArduCopter layout
#

proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) }
proc rcos {a} { expr cos([rad $a])}


set quad_x {
	  45 CCW
	-135 CCW
	 -45 CW
	 135 CW
}

set quad_plus {
	  90 CCW
	 -90 CCW
	   0 CW
	 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
	-129 CCW
	 -68 CW
	 129 CW
}

set hex_x {
	  90 CW
	 -90 CCW
	 -30 CW
	 150 CCW
	  30 CCW
	-150 CW
}

set hex_plus {
	   0 CW
	 180 CCW
	-120 CW
	  60 CCW
	 -60 CCW
	 120 CW
}

set hex_cox {
	  60 CW
	  60 CCW
	 180 CW
	 180 CCW
	 -60 CW
	 -60 CCW
}

set octa_x {
	  22.5 CW
	-157.5 CW
	  67.5 CCW
	 157.5 CCW
	 -22.5 CCW
	-112.5 CCW
	 -67.5 CW
	 112.5 CW
}

set octa_plus {
	   0 CW
	 180 CW
	  45 CCW
	 135 CCW
	 -45 CCW
	-135 CCW
	 -90 CW
	  90 CW
}

set octa_cox {
	  45 CCW
	 -45 CW
	-135 CCW
	 135 CW
	 -45 CCW
	  45 CW
	 135 CCW
	-135 CW
}

set twin_engine {
      90 0.0
     -90 0.0
}

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, %9.6f }," [rcos [expr $a + 90]] [rcos $a] [expr $d]]}

foreach table $tables {
	puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table]

	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 $dir
		}
		factors $angle $dd
	}
	puts "};"
}

puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::MAX_GEOMETRY\] = {"
foreach table $tables {
	puts [format "\t&_config_%s\[0\]," $table]
}
puts "};"

puts "const unsigned _config_rotor_count\[MultirotorMixer::MAX_GEOMETRY\] = {"
foreach table $tables {
	upvar #0 $table angles
	puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table]
}
puts "};"