State machine fixes for HIL

src/modules/commander/commander.cpp

src/modules/commander/state_machine_helper.cpp

src/modules/commander/state_machine_helper.h

src/modules/mavlink/waypoints.c

src/modules/sensors/sensors.cpp