Actually finish the flight controller

[yameatmeyourdead]

Cascaded PID controller
Steps:

• Position PID to calculate desired velocity.
• Velocity PID to calculate desired force.
• V_desired = PID(P_desired-P_measured)
• F_des = PID(V_desired-V_measured)
• G*T=F_des
  [6x8] [8x1]=[6x1]
  G is full-rank:
• T = G^+ F_des

Use published T200 data to map T -> U where U is pwm required for the desired thrust.
Might want a QP optimizer here for T?
min T (G*T - F)^2 s.t. T_min <= T <= T_max
@Hermanoid thoughts?