Bulk of flight_controller-end code is written; demands tuning and maybe debugging on specific calls made in apply_wrench_to_target (mostly to get reference frame happy and right, and to get forces in the right direction and magnitude.)