Current testing state
Control system refactor
Recovery trajectory generation working
Restructure state passing
Use attitude delta for reference angular velocity
Bug fixes; HITL driver working
Merge branch 'bendyer-control-recovery'