Copter: FlightMode - convert CIRCLE flight mode

ArduCopter/Copter.cpp

@@ -37,7 +37,6 @@ Copter::Copter(void)
     rtl_state(RTL_InitialClimb),
     rtl_state_complete(false),
     smart_rtl_state(SmartRTL_PathFollow),
-    circle_pilot_yaw_override(false),
     simple_cos_yaw(1.0f),
     simple_sin_yaw(0.0f),
     super_simple_last_bearing(0),

ArduCopter/Copter.h

@@ -412,9 +412,6 @@ private:
     // SmartRTL
     SmartRTLState smart_rtl_state;  // records state of SmartRTL
 
-    // Circle
-    bool circle_pilot_yaw_override; // true if pilot is overriding yaw
-
     // SIMPLE Mode
     // Used to track the orientation of the vehicle for Simple mode. This value is reset at each arming
     // or in SuperSimple mode when the vehicle leaves a 20m radius from home.
@@ -839,8 +836,6 @@ private:
     bool brake_init(bool ignore_checks);
     void brake_run();
     void brake_timeout_to_loiter_ms(uint32_t timeout_ms);
-    bool circle_init(bool ignore_checks);
-    void circle_run();
     bool drift_init(bool ignore_checks);
     void drift_run();
     float get_throttle_assist(float velz, float pilot_throttle_scaled);
@@ -1139,6 +1134,8 @@ private:
 
     Copter::FlightMode_AUTO flightmode_auto{*this, mission, circle_nav};
 
+    Copter::FlightMode_CIRCLE flightmode_circle{*this, circle_nav};
+
 #if FRAME_CONFIG == HELI_FRAME
     Copter::FlightMode_STABILIZE_Heli flightmode_stabilize{*this};
 #else

ArduCopter/FlightMode.h

@@ -341,3 +341,35 @@ private:
     AP_Mission &mission;
     AC_Circle *&circle_nav;
 };
+
+
+
+class FlightMode_CIRCLE : public FlightMode {
+
+public:
+
+    FlightMode_CIRCLE(Copter &copter, AC_Circle *& _circle_nav) :
+        Copter::FlightMode(copter),
+        circle_nav(_circle_nav)
+        { }
+
+    bool init(bool ignore_checks) override;
+    void run() override; // should be called at 100hz or more
+
+    bool requires_GPS() const override { return true; }
+    bool has_manual_throttle() const override { return false; }
+    bool allows_arming(bool from_gcs) const override { return false; };
+    bool is_autopilot() const override { return true; }
+
+protected:
+
+    const char *name() const override { return "CIRCLE"; }
+    const char *name4() const override { return "CIRC"; }
+
+private:
+
+    // Circle
+    bool pilot_yaw_override = false; // true if pilot is overriding yaw
+    AC_Circle *&circle_nav;
+
+};

ArduCopter/control_circle.cpp

@@ -5,10 +5,10 @@
  */
 
 // circle_init - initialise circle controller flight mode
-bool Copter::circle_init(bool ignore_checks)
+bool Copter::FlightMode_CIRCLE::init(bool ignore_checks)
 {
-    if (position_ok() || ignore_checks) {
-        circle_pilot_yaw_override = false;
+    if (_copter.position_ok() || ignore_checks) {
+        pilot_yaw_override = false;
 
         // initialize speeds and accelerations
         pos_control->set_speed_xy(wp_nav->get_speed_xy());
@@ -28,7 +28,7 @@ bool Copter::circle_init(bool ignore_checks)
 
 // circle_run - runs the circle flight mode
 // should be called at 100hz or more
-void Copter::circle_run()
+void Copter::FlightMode_CIRCLE::run()
 {
     float target_yaw_rate = 0;
     float target_climb_rate = 0;
@@ -56,11 +56,11 @@ void Copter::circle_run()
     }
 
     // process pilot inputs
-    if (!failsafe.radio) {
+    if (!_copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
-            circle_pilot_yaw_override = true;
+            pilot_yaw_override = true;
         }
 
         // get pilot desired climb rate
@@ -82,14 +82,14 @@ void Copter::circle_run()
     circle_nav->update();
 
     // call attitude controller
-    if (circle_pilot_yaw_override) {
+    if (pilot_yaw_override) {
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
     }else{
         attitude_control->input_euler_angle_roll_pitch_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), circle_nav->get_yaw(),true, 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);
     }

ArduCopter/flight_mode.cpp

@@ -68,7 +68,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
             break;
 
         case CIRCLE:
-            success = circle_init(ignore_checks);
+            success = flightmode_circle.init(ignore_checks);
+            if (success) {
+                flightmode = &flightmode_circle;
+            }
             break;
 
         case LOITER:
@@ -196,10 +199,6 @@ void Copter::update_flight_mode()
     }
 
     switch (control_mode) {
-        case CIRCLE:
-            circle_run();
-            break;
-
         case LOITER:
             loiter_run();
             break;
@@ -330,7 +329,6 @@ bool Copter::mode_requires_GPS()
         case LOITER:
         case RTL:
         case SMART_RTL:
-        case CIRCLE:
         case DRIFT:
         case POSHOLD:
         case BRAKE:
@@ -381,7 +379,6 @@ void Copter::notify_flight_mode()
     switch (control_mode) {
         case GUIDED:
         case RTL:
-        case CIRCLE:
         case AVOID_ADSB:
         case GUIDED_NOGPS:
         case LAND:
@@ -406,9 +403,6 @@ void Copter::notify_flight_mode()
         case RTL:
             notify.set_flight_mode_str("RTL ");
             break;
-        case CIRCLE:
-            notify.set_flight_mode_str("CIRC");
-            break;
         case LAND:
             notify.set_flight_mode_str("LAND");
             break;