From 1ac74e2fe56f9f1da65120717bf181057584bc8c Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Sun, 10 Mar 2019 10:54:17 +0000
Subject: [PATCH] Tracker: add initial mode parameter

---
 AntennaTracker/Parameters.cpp |  6 ++++++
 AntennaTracker/Parameters.h   |  2 ++
 AntennaTracker/system.cpp     | 19 ++++++++++++++++++-
 3 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp
index 20c5eab1ae..2a64482f23 100644
--- a/AntennaTracker/Parameters.cpp
+++ b/AntennaTracker/Parameters.cpp
@@ -403,6 +403,12 @@ const AP_Param::Info Tracker::var_info[] = {
     // @User: Standard
     GSCALAR(scan_speed_pitch,       "SCAN_SPEED_PIT",   5),
 
+    // @Param: INITIAL_MODE
+    // @DisplayName: Mode tracker will switch into after initialization
+    // @Description: 0:MANUAL, 1:STOP, 2:SCAN, 10:AUTO
+    // @User: Standard
+    GSCALAR(initial_mode,            "INITIAL_MODE",     10),
+
     AP_VAREND
 };
 
diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h
index b0d7ad438b..c8ef320703 100644
--- a/AntennaTracker/Parameters.h
+++ b/AntennaTracker/Parameters.h
@@ -119,6 +119,7 @@ public:
         k_param_gcs_pid_mask = 225,
         k_param_scan_speed_yaw,
         k_param_scan_speed_pitch,
+        k_param_initial_mode
     };
 
     AP_Int16 format_version;
@@ -156,6 +157,7 @@ public:
     AP_Int16 pitch_min;
     AP_Int16 pitch_max;
     AP_Int16 gcs_pid_mask;
+    AP_Int8  initial_mode;
 
     // Waypoints
     //
diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp
index 58016952ed..73eb61ce6b 100644
--- a/AntennaTracker/system.cpp
+++ b/AntennaTracker/system.cpp
@@ -101,7 +101,24 @@ void Tracker::init_tracker()
     gcs().send_text(MAV_SEVERITY_INFO,"Ready to track");
     hal.scheduler->delay(1000); // Why????
 
-    set_mode(AUTO, MODE_REASON_STARTUP); // tracking
+    switch (g.initial_mode) {
+    case MANUAL:
+        set_mode(MANUAL, MODE_REASON_STARTUP);
+        break;
+
+    case SCAN:
+        set_mode(SCAN, MODE_REASON_STARTUP);
+        break;
+
+    case STOP:
+        set_mode(STOP, MODE_REASON_STARTUP);
+        break;
+
+    case AUTO:
+    default:
+        set_mode(AUTO, MODE_REASON_STARTUP);
+        break;
+    }
 
     if (g.startup_delay > 0) {
         // arm servos with trim value to allow them to start up (required