Sub: Initialize control mode to flightmode 1

ArduSub/Sub.cpp

@@ -27,7 +27,7 @@ Sub::Sub(void) :
             FUNCTOR_BIND_MEMBER(&Sub::start_command, bool, const AP_Mission::Mission_Command &),
             FUNCTOR_BIND_MEMBER(&Sub::verify_command_callback, bool, const AP_Mission::Mission_Command &),
             FUNCTOR_BIND_MEMBER(&Sub::exit_mission, void)),
-    control_mode(STABILIZE),
+    control_mode(MANUAL),
     motors(MAIN_LOOP_RATE),
     scaleLongDown(1),
     wp_bearing(0),

ArduSub/joystick.cpp

@@ -25,6 +25,8 @@ namespace {
 }
 
 void Sub::init_joystick() {
+	set_mode((control_mode_t)flight_modes[0].get(), MODE_REASON_TX_COMMAND); // Initialize flight mode
+
     if(g.numGainSettings < 1) g.numGainSettings.set_and_save(1);
 
     if(g.numGainSettings == 1 || (g.gain_default < g.maxGain + 0.01 && g.gain_default > g.minGain - 0.01)) {