From 79c06974b711250e51db59abfd2cfc7e4c8099aa Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 21 Mar 2016 15:23:41 +1100
Subject: [PATCH] Copter: FlightMode - convert ALT_HOLD flight mode

---
 ArduCopter/Copter.h            |  3 +--
 ArduCopter/FlightMode.h        | 26 ++++++++++++++++++++++++++
 ArduCopter/control_althold.cpp |  8 ++++----
 ArduCopter/flight_mode.cpp     | 12 ++++--------
 4 files changed, 35 insertions(+), 14 deletions(-)

diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h
index 2acf965bab..1cad9bd062 100644
--- a/ArduCopter/Copter.h
+++ b/ArduCopter/Copter.h
@@ -796,8 +796,6 @@ private:
     MAV_RESULT mavlink_compassmot(mavlink_channel_t chan);
     void delay(uint32_t ms);
     void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
-    bool althold_init(bool ignore_checks);
-    void althold_run();
     bool auto_init(bool ignore_checks);
     void auto_run();
     void auto_takeoff_start(const Location& dest_loc);
@@ -1171,6 +1169,7 @@ private:
     Copter::FlightMode_ACRO flightmode_acro{*this};
 #endif
 
+    Copter::FlightMode_ALTHOLD flightmode_althold{*this};
 
 public:
     void mavlink_delay_cb();
diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h
index efb6f48ed0..1ea78732ef 100644
--- a/ArduCopter/FlightMode.h
+++ b/ArduCopter/FlightMode.h
@@ -198,3 +198,29 @@ private:
 };
 #endif
 
+
+
+class FlightMode_ALTHOLD : public FlightMode {
+
+public:
+
+    FlightMode_ALTHOLD(Copter &copter) :
+        Copter::FlightMode(copter)
+        { }
+
+    bool init(bool ignore_checks) override;
+    void run() override; // should be called at 100hz or more
+
+    bool requires_GPS() const override { return false; }
+    bool has_manual_throttle() const override { return false; }
+    bool allows_arming(bool from_gcs) const override { return true; };
+    bool is_autopilot() const override { return false; }
+
+protected:
+
+    const char *name() const override { return "ALT_HOLD"; }
+    const char *name4() const override { return "ALTH"; }
+
+private:
+
+};
diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp
index bf8bb0a427..f6025a5558 100644
--- a/ArduCopter/control_althold.cpp
+++ b/ArduCopter/control_althold.cpp
@@ -6,7 +6,7 @@
  */
 
 // althold_init - initialise althold controller
-bool Copter::althold_init(bool ignore_checks)
+bool Copter::FlightMode_ALTHOLD::init(bool ignore_checks)
 {
     // initialize vertical speeds and leash lengths
     pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
@@ -26,7 +26,7 @@ bool Copter::althold_init(bool ignore_checks)
 
 // althold_run - runs the althold controller
 // should be called at 100hz or more
-void Copter::althold_run()
+void Copter::FlightMode_ALTHOLD::run()
 {
     AltHoldModeState althold_state;
     float takeoff_climb_rate = 0.0f;
@@ -149,14 +149,14 @@ void Copter::althold_run()
 
 #if AC_AVOID_ENABLED == ENABLED
         // apply avoidance
-        avoid.adjust_roll_pitch(target_roll, target_pitch, aparm.angle_max);
+        _copter.avoid.adjust_roll_pitch(target_roll, target_pitch, _copter.aparm.angle_max);
 #endif
 
         // call attitude controller
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
 
         // adjust climb rate using rangefinder
-        if (rangefinder_alt_ok()) {
+        if (_copter.rangefinder_alt_ok()) {
             // if rangefinder is ok, use surface tracking
             target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
         }
diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp
index 8919dd6128..e8a42d4548 100644
--- a/ArduCopter/flight_mode.cpp
+++ b/ArduCopter/flight_mode.cpp
@@ -55,7 +55,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
             break;
 
         case ALT_HOLD:
-            success = althold_init(ignore_checks);
+            success = flightmode_althold.init(ignore_checks);
+            if (success) {
+                flightmode = &flightmode_althold;
+            }
             break;
 
         case AUTO:
@@ -199,10 +202,6 @@ void Copter::update_flight_mode()
             #endif
             break;
 
-        case ALT_HOLD:
-            althold_run();
-            break;
-
         case AUTO:
             auto_run();
             break;
@@ -413,9 +412,6 @@ void Copter::notify_flight_mode()
         case STABILIZE:
             notify.set_flight_mode_str("STAB");
             break;
-        case ALT_HOLD:
-            notify.set_flight_mode_str("ALTH");
-            break;
         case AUTO:
             notify.set_flight_mode_str("AUTO");
             break;