diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 5664feb46c..fc56a50383 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -26,6 +26,7 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); Copter::Copter(void) : DataFlash(DataFlash_Class::create(fwver.fw_string, g.log_bitmask)), flight_modes(&g.flight_mode1), + flightmode(&flightmode_stabilize), control_mode(STABILIZE), scaleLongDown(1), wp_bearing(0), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 1cad9bd062..d52299c364 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -961,8 +961,6 @@ private: void smart_rtl_save_position(); bool sport_init(bool ignore_checks); void sport_run(); - bool stabilize_init(bool ignore_checks); - void stabilize_run(); void crash_check(); void parachute_check(); void parachute_release(); @@ -1011,8 +1009,6 @@ private: void update_heli_control_dynamics(void); void heli_update_landing_swash(); void heli_update_rotor_speed_targets(); - bool heli_stabilize_init(bool ignore_checks); - void heli_stabilize_run(); void read_inertia(); bool land_complete_maybe(); void update_land_and_crash_detectors(); @@ -1171,6 +1167,13 @@ private: Copter::FlightMode_ALTHOLD flightmode_althold{*this}; +#if FRAME_CONFIG == HELI_FRAME + Copter::FlightMode_STABILIZE_Heli flightmode_stabilize{*this}; +#else + Copter::FlightMode_STABILIZE flightmode_stabilize{*this}; +#endif + + public: void mavlink_delay_cb(); void failsafe_check(); diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index 1ea78732ef..2efa20e9e9 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -224,3 +224,49 @@ protected: private: }; + + + +class FlightMode_STABILIZE : public FlightMode { + +public: + + FlightMode_STABILIZE(Copter &copter) : + Copter::FlightMode(copter) + { } + + virtual bool init(bool ignore_checks) override; + virtual void run() override; // should be called at 100hz or more + + virtual bool requires_GPS() const override { return false; } + virtual bool has_manual_throttle() const override { return true; } + virtual bool allows_arming(bool from_gcs) const override { return true; }; + virtual bool is_autopilot() const override { return false; } + +protected: + + const char *name() const override { return "STABILIZE"; } + const char *name4() const override { return "STAB"; } + +private: + +}; + +#if FRAME_CONFIG == HELI_FRAME +class FlightMode_STABILIZE_Heli : public FlightMode_STABILIZE { + +public: + + FlightMode_STABILIZE_Heli(Copter &copter) : + Copter::FlightMode_STABILIZE(copter) + { } + + bool init(bool ignore_checks) override; + void run() override; // should be called at 100hz or more + +protected: + +private: + +}; +#endif diff --git a/ArduCopter/control_stabilize.cpp b/ArduCopter/control_stabilize.cpp index a2e5f3ffad..e8adf8a6c6 100644 --- a/ArduCopter/control_stabilize.cpp +++ b/ArduCopter/control_stabilize.cpp @@ -5,10 +5,10 @@ */ // stabilize_init - initialise stabilize controller -bool Copter::stabilize_init(bool ignore_checks) +bool Copter::FlightMode_STABILIZE::init(bool ignore_checks) { // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high - if (motors->armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) && + if (motors->armed() && ap.land_complete && !_copter.mode_has_manual_throttle(_copter.control_mode) && (get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) { return false; } @@ -20,7 +20,7 @@ bool Copter::stabilize_init(bool ignore_checks) // stabilize_run - runs the main stabilize controller // should be called at 100hz or more -void Copter::stabilize_run() +void Copter::FlightMode_STABILIZE::run() { float target_roll, target_pitch; float target_yaw_rate; @@ -41,6 +41,8 @@ void Copter::stabilize_run() // apply SIMPLE mode transform to pilot inputs update_simple_mode(); + AP_Vehicle::MultiCopter &aparm = _copter.aparm; + // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index e8a42d4548..eb2f14e9a7 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -47,11 +47,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) break; case STABILIZE: - #if FRAME_CONFIG == HELI_FRAME - success = heli_stabilize_init(ignore_checks); - #else - success = stabilize_init(ignore_checks); - #endif + success = flightmode_stabilize.init(ignore_checks); + if (success) { + flightmode = &flightmode_stabilize; + } break; case ALT_HOLD: @@ -194,14 +193,6 @@ void Copter::update_flight_mode() } switch (control_mode) { - case STABILIZE: - #if FRAME_CONFIG == HELI_FRAME - heli_stabilize_run(); - #else - stabilize_run(); - #endif - break; - case AUTO: auto_run(); break; @@ -409,9 +400,6 @@ void Copter::notify_flight_mode() // set flight mode string switch (control_mode) { - case STABILIZE: - notify.set_flight_mode_str("STAB"); - break; case AUTO: notify.set_flight_mode_str("AUTO"); break; diff --git a/ArduCopter/heli_control_stabilize.cpp b/ArduCopter/heli_control_stabilize.cpp index 09a9668154..e7407ed97e 100644 --- a/ArduCopter/heli_control_stabilize.cpp +++ b/ArduCopter/heli_control_stabilize.cpp @@ -6,21 +6,21 @@ */ // stabilize_init - initialise stabilize controller -bool Copter::heli_stabilize_init(bool ignore_checks) +bool Copter::FlightMode_STABILIZE_Heli::init(bool ignore_checks) { // set target altitude to zero for reporting // To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error? pos_control->set_alt_target(0); // set stab collective true to use stabilize scaled collective pitch range - input_manager.set_use_stab_col(true); + _copter.input_manager.set_use_stab_col(true); return true; } // stabilize_run - runs the main stabilize controller // should be called at 100hz or more -void Copter::heli_stabilize_run() +void Copter::FlightMode_STABILIZE_Heli::run() { float target_roll, target_pitch; float target_yaw_rate; @@ -33,16 +33,16 @@ void Copter::heli_stabilize_run() // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move if(!motors->armed()) { - heli_flags.init_targets_on_arming=true; + _copter.heli_flags.init_targets_on_arming = true; attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); } - if(motors->armed() && heli_flags.init_targets_on_arming) { + if(motors->armed() && _copter.heli_flags.init_targets_on_arming) { attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); if (motors->get_interlock()) { - heli_flags.init_targets_on_arming=false; + _copter.heli_flags.init_targets_on_arming=false; } } @@ -56,13 +56,13 @@ void Copter::heli_stabilize_run() // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats - get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); + get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, _copter.aparm.angle_max); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot's desired throttle - pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); + pilot_throttle_scaled = _copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());