[rotorcraft] fix MODE_STARTUP

- init nav, guidance and stabilization from autopilot_init
- after init of the above, set the autopilot_mode so it propagates to the guidance submodes.
- also call autopilot_init after the ahrs_init for ahrs_is_aligned check...

This should work for the ARDrone2 case where you want to start in NAV mode, but don't have RC via datalink yet and autopilot_set_mode is hence not called.
However you have to make sure that either the AHRS status is already running (aligned) at startup or use AUTOPILOT_DISABLE_AHRS_KILL