Motor/Moment mapping

[skulumani]

The mapping from controller moment to motor commands is done using

  // Given the UAV arm length of 0.31 m and a prop. angle of 15 deg.
  invFMmat <<  0.0000,    1.2879,   -0.1725,   -0.0000,    1.1132,    0.3071,
  -1.1154,    0.6440,    0.1725,    0.9641,   -0.3420,    0.7579,
  -1.1154,   -0.6440,   -0.1725,   -0.9641,   -0.7712,    1.7092,
  -0.0000,   -1.2879,    0.1725,         0,    1.1132,    0.3071,
  1.1154,   -0.6440,   -0.1725,    0.9641,   -0.3420,    0.7579,
  1.1154,    0.6440,    0.1725,   -0.9641,   -0.7712,    1.7092;

which is found here. It might make sense to make this a function rather than hard coded.

Some ideas:

Inputs: Motor arm length, angle of rotors, number of rotors
Outputs: Mapping matrix

I'll also give this a shot since it's easy and I probably can't screw it up too much