diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 751f958aa7..a03cc36b2a 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -969,6 +969,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPPTR(mode_zigzag_ptr, "ZIGZ_", 38, ParametersG2, ModeZigZag), #endif +#if MODE_ACRO_ENABLED == ENABLED + // @Param: ACRO_OPTIONS + // @DisplayName: Acro mode options + // @Description: A range of options that can be applied to change acro mode behaviour. Air-mode enables ATC_THR_MIX_MAN at all times (air-mode has no effect on helicopters). Rate Loop Only disables the use of angle stabilization and uses angular rate stabilization only. + // @Bitmask: 0:Air-mode,1:Rate Loop Only + // @User: Advanced + AP_GROUPINFO("ACRO_OPTIONS", 39, ParametersG2, acro_options, 0), +#endif + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 319bd4cfa2..00ba8a89de 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -616,6 +616,10 @@ public: void *mode_zigzag_ptr; #endif +#if MODE_ACRO_ENABLED == ENABLED + AP_Int8 acro_options; +#endif + }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index 16332e2421..616e82a76e 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -112,9 +112,11 @@ void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const AuxS case AUX_FUNC::SUPERSIMPLE_MODE: case AUX_FUNC::SURFACE_TRACKING: case AUX_FUNC::WINCH_ENABLE: - case AUX_FUNC::AIRMODE: do_aux_function(ch_option, ch_flag); break; + case AUX_FUNC::AIRMODE: + do_aux_function_change_air_mode(ch_flag); + break; default: RC_Channel::init_aux_function(ch_option, ch_flag); break; @@ -596,17 +598,11 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi break; case AUX_FUNC::AIRMODE: - switch (ch_flag) { - case AuxSwitchPos::HIGH: - copter.air_mode = AirMode::AIRMODE_ENABLED; - break; - case AuxSwitchPos::MIDDLE: - break; - case AuxSwitchPos::LOW: - copter.air_mode = AirMode::AIRMODE_DISABLED; - break; - } - break; + do_aux_function_change_air_mode(ch_flag); +#if MODE_ACRO_ENABLED == ENABLED && FRAME_CONFIG != HELI_FRAME + copter.mode_acro.air_mode_aux_changed(); +#endif + break; default: RC_Channel::do_aux_function(ch_option, ch_flag); @@ -614,6 +610,21 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const AuxSwi } } +// change air-mode status +void RC_Channel_Copter::do_aux_function_change_air_mode(const AuxSwitchPos ch_flag) +{ + switch (ch_flag) { + case AuxSwitchPos::HIGH: + copter.air_mode = AirMode::AIRMODE_ENABLED; + break; + case AuxSwitchPos::MIDDLE: + break; + case AuxSwitchPos::LOW: + copter.air_mode = AirMode::AIRMODE_DISABLED; + break; + } +} + // save_trim - adds roll and pitch trims from the radio to ahrs void Copter::save_trim() { diff --git a/ArduCopter/RC_Channel.h b/ArduCopter/RC_Channel.h index 37b7c997d4..e55c64c081 100644 --- a/ArduCopter/RC_Channel.h +++ b/ArduCopter/RC_Channel.h @@ -19,6 +19,7 @@ private: void do_aux_function_armdisarm(const AuxSwitchPos ch_flag) override; void do_aux_function_change_mode(const Mode::Number mode, const AuxSwitchPos ch_flag); + void do_aux_function_change_air_mode(const AuxSwitchPos ch_flag); // called when the mode switch changes position: void mode_switch_changed(modeswitch_pos_t new_pos) override; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 2c4e958cd5..09e82f8c62 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -362,6 +362,12 @@ void Copter::exit_mode(Mode *&old_flightmode, } #endif +#if MODE_ACRO_ENABLED == ENABLED + if (old_flightmode == &mode_acro) { + mode_acro.exit(); + } +#endif + #if FRAME_CONFIG == HELI_FRAME // firmly reset the flybar passthrough to false when exiting acro mode. if (old_flightmode == &mode_acro) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 155b029be9..b04ad94647 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -265,12 +265,21 @@ public: LIMITED = 2, }; + enum class AcroOptions { + AIR_MODE = 1 << 0, + RATE_LOOP_ONLY = 1 << 1, + }; + virtual void run() override; bool requires_GPS() const override { return false; } bool has_manual_throttle() const override { return true; } bool allows_arming(bool from_gcs) const override { return true; }; bool is_autopilot() const override { return false; } + bool init(bool ignore_checks) override; + void exit(); + // whether an air-mode aux switch has been toggled + void air_mode_aux_changed(); protected: @@ -282,7 +291,7 @@ protected: float throttle_hover() const override; private: - + bool disable_air_mode_reset; }; #endif diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index a13b25a27c..54795a1f93 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -51,7 +51,11 @@ void ModeAcro::run() } // run attitude controller - attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); + if (g2.acro_options.get() & uint8_t(AcroOptions::RATE_LOOP_ONLY)) { + attitude_control->input_rate_bf_roll_pitch_yaw_2(target_roll, target_pitch, target_yaw); + } else { + attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); + } // output pilot's throttle without angle boost attitude_control->set_throttle_out(get_pilot_desired_throttle(), @@ -59,6 +63,29 @@ void ModeAcro::run() copter.g.throttle_filt); } +bool ModeAcro::init(bool ignore_checks) +{ + if (g2.acro_options.get() & uint8_t(AcroOptions::AIR_MODE)) { + disable_air_mode_reset = false; + copter.air_mode = AirMode::AIRMODE_ENABLED; + } + + return true; +} + +void ModeAcro::exit() +{ + if (!disable_air_mode_reset && (g2.acro_options.get() & uint8_t(AcroOptions::AIR_MODE))) { + copter.air_mode = AirMode::AIRMODE_DISABLED; + } + disable_air_mode_reset = false; +} + +void ModeAcro::air_mode_aux_changed() +{ + disable_air_mode_reset = true; +} + float ModeAcro::throttle_hover() const { if (g2.acro_thr_mid > 0) { diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index a5856cb487..76feaf7d1c 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -88,7 +88,11 @@ void ModeAcro_Heli::run() } // run attitude controller - attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); + if (g2.acro_options.get() & uint8_t(AcroOptions::RATE_LOOP_ONLY)) { + attitude_control->input_rate_bf_roll_pitch_yaw_2(target_roll, target_pitch, target_yaw); + } else { + attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); + } }else{ /* for fly-bar passthrough use control_in values with no