From ee2bea5256c6c9f7f2f4c8c758375ad665240ad6 Mon Sep 17 00:00:00 2001
From: Willian Galvani <williangalvani@gmail.com>
Date: Wed, 20 Nov 2019 13:40:23 -0300
Subject: [PATCH] Sub: set cam mount to 0,0,0 and RC mode after initialization

---
 ArduSub/system.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index 84293f5574..ad9d767b7f 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -129,6 +129,10 @@ void Sub::init_ardupilot()
 #if MOUNT == ENABLED
     // initialise camera mount
     camera_mount.init();
+    // This step ncessary so the servo is properly initialized
+    camera_mount.set_angle_targets(0, 0, 0);
+    // for some reason the call to set_angle_targets changes the mode to mavlink targeting!
+    camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
 #endif
 
 #ifdef USERHOOK_INIT