Since this is flushing out an architecture I don't think I'll just straight into implementing a failsafe system.  It will need to be an FSM and know how to deal with things like RTH for X amount of time, then land etc.

Setting ManualControlCommand in this architecture would no longer have an influence if failsafe mode is enabled.  That is used by the transmitter_control_select() which is not called during failsafe.  Thus the failsafe_control_select method needs to actually specify a control input for the other modules - in this case stabilized control with no throttle (which will eventually be selectable).