diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index f303d8778a..b0f58f020c 100644 --- a/ArduCopter/Copter.cpp +++ b/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), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9784ef90b3..9288d00802 100644 --- a/ArduCopter/Copter.h +++ b/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 diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index 784925272f..7541ab6738 100644 --- a/ArduCopter/FlightMode.h +++ b/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; + +}; diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index 05fa2c0957..a78001686b 100644 --- a/ArduCopter/control_circle.cpp +++ b/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); } diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 7a48a8c768..ea550a82ec 100644 --- a/ArduCopter/flight_mode.cpp +++ b/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;