From 7b1347da79ef1d79f8972a7a3cf643505c2e10e2 Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Tue, 6 Dec 2016 21:22:47 -0500
Subject: [PATCH] Sub: Initialize control mode to flightmode 1

---
 ArduSub/Sub.cpp      | 2 +-
 ArduSub/joystick.cpp | 2 ++
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp
index 6f01c9cf0c..08165119a0 100644
--- a/ArduSub/Sub.cpp
+++ b/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),
diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp
index 7b527eb6b4..348df1b556 100644
--- a/ArduSub/joystick.cpp
+++ b/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)) {