From 67c152cef2d58f781036f5b1fc49d14f6bdac063 Mon Sep 17 00:00:00 2001
From: Samuel Tabor <samuelctabor@gmail.com>
Date: Mon, 3 Aug 2020 19:22:00 +0100
Subject: [PATCH] Plane: Move cruise state into cruise flight mode.

---
 ArduPlane/Plane.h         |  7 -------
 ArduPlane/mode.h          |  6 ++++++
 ArduPlane/mode_cruise.cpp | 35 ++++++++++++++++++++---------------
 ArduPlane/soaring.cpp     |  3 ++-
 4 files changed, 28 insertions(+), 23 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 1b62aa5433..02f906b682 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -395,13 +395,6 @@ private:
         int32_t locked_pitch_cd;
     } acro_state;
 
-    // CRUISE controller state
-    struct CruiseState {
-        bool locked_heading;
-        int32_t locked_heading_cd;
-        uint32_t lock_timer_ms;
-    } cruise_state;
-
     struct {
         uint32_t last_tkoff_arm_time;
         uint32_t last_check_ms;
diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h
index 230543e844..1fe7f621b2 100644
--- a/ArduPlane/mode.h
+++ b/ArduPlane/mode.h
@@ -329,9 +329,15 @@ public:
 
     void navigate() override;
 
+    bool get_target_heading_cd(int32_t &target_heading);
+
 protected:
 
     bool _enter() override;
+
+    bool locked_heading;
+    int32_t locked_heading_cd;
+    uint32_t lock_timer_ms;
 };
 
 class ModeAvoidADSB : public Mode
diff --git a/ArduPlane/mode_cruise.cpp b/ArduPlane/mode_cruise.cpp
index 7aa86d9adf..0eb51ec821 100644
--- a/ArduPlane/mode_cruise.cpp
+++ b/ArduPlane/mode_cruise.cpp
@@ -6,8 +6,8 @@ bool ModeCruise::_enter()
     plane.throttle_allows_nudging = false;
     plane.auto_throttle_mode = true;
     plane.auto_navigation_mode = false;
-    plane.cruise_state.locked_heading = false;
-    plane.cruise_state.lock_timer_ms = 0;
+    locked_heading = false;
+    lock_timer_ms = 0;
 
 #if SOARING_ENABLED == ENABLED
     // for ArduSoar soaring_controller
@@ -27,11 +27,11 @@ void ModeCruise::update()
       any aileron or rudder input
     */
     if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) {
-        plane.cruise_state.locked_heading = false;
-        plane.cruise_state.lock_timer_ms = 0;
+        locked_heading = false;
+        lock_timer_ms = 0;
     }
 
-    if (!plane.cruise_state.locked_heading) {
+    if (!locked_heading) {
         plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd;
         plane.update_load_factor();
     } else {
@@ -46,29 +46,34 @@ void ModeCruise::update()
  */
 void ModeCruise::navigate()
 {
-    if (!plane.cruise_state.locked_heading &&
+    if (!locked_heading &&
         plane.channel_roll->get_control_in() == 0 &&
         plane.rudder_input() == 0 &&
         plane.gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
         plane.gps.ground_speed() >= 3 &&
-        plane.cruise_state.lock_timer_ms == 0) {
+        lock_timer_ms == 0) {
         // user wants to lock the heading - start the timer
-        plane.cruise_state.lock_timer_ms = millis();
+        lock_timer_ms = millis();
     }
-    if (plane.cruise_state.lock_timer_ms != 0 &&
-        (millis() - plane.cruise_state.lock_timer_ms) > 500) {
+    if (lock_timer_ms != 0 &&
+        (millis() - lock_timer_ms) > 500) {
         // lock the heading after 0.5 seconds of zero heading input
         // from user
-        plane.cruise_state.locked_heading = true;
-        plane.cruise_state.lock_timer_ms = 0;
-        plane.cruise_state.locked_heading_cd = plane.gps.ground_course_cd();
+        locked_heading = true;
+        lock_timer_ms = 0;
+        locked_heading_cd = plane.gps.ground_course_cd();
         plane.prev_WP_loc = plane.current_loc;
     }
-    if (plane.cruise_state.locked_heading) {
+    if (locked_heading) {
         plane.next_WP_loc = plane.prev_WP_loc;
         // always look 1km ahead
-        plane.next_WP_loc.offset_bearing(plane.cruise_state.locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000);
+        plane.next_WP_loc.offset_bearing(locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000);
         plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
     }
 }
 
+bool ModeCruise::get_target_heading_cd(int32_t &target_heading)
+{
+    target_heading = locked_heading_cd;
+    return locked_heading;
+}
diff --git a/ArduPlane/soaring.cpp b/ArduPlane/soaring.cpp
index 2f99701dc8..d7bade69ea 100644
--- a/ArduPlane/soaring.cpp
+++ b/ArduPlane/soaring.cpp
@@ -169,7 +169,8 @@ bool Plane::soaring_exit_heading_aligned() const
     case Mode::Number::FLY_BY_WIRE_B:
         return (!AP::ahrs().home_is_set() || plane.mode_loiter.isHeadingLinedUp(next_WP_loc, AP::ahrs().get_home()));
     case Mode::Number::CRUISE:
-        return (!cruise_state.locked_heading || plane.mode_loiter.isHeadingLinedUp_cd(cruise_state.locked_heading_cd));
+        int32_t target_heading_cd;
+        return (!plane.mode_cruise.get_target_heading_cd(target_heading_cd) || plane.mode_loiter.isHeadingLinedUp_cd(target_heading_cd));
     default:
         break;
     }