Full state control differs from Crazyflie

The state control interface differs from [full state setpoint](https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/crtp/crtp_generic_setpoint/#full-state) in the firmware. With body rate control exposed, we still need to restructure the controllers in some way to enable the full state setpoints.