diff --git a/ArduCopter/RC_Channel.cpp b/ArduCopter/RC_Channel.cpp index b613cb141aee4..7701889147088 100644 --- a/ArduCopter/RC_Channel.cpp +++ b/ArduCopter/RC_Channel.cpp @@ -14,6 +14,12 @@ int8_t RC_Channels_Copter::flight_mode_channel_number() const return copter.g.flight_mode_chan.get(); } +bool RC_Channels_Copter::is_extended_flight_modes_enabled() const +{ + // This is only used in Plane for now, so return false always + return false; +} + void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos) { if (new_pos < 0 || (uint8_t)new_pos > copter.num_flight_modes) { diff --git a/ArduCopter/RC_Channel.h b/ArduCopter/RC_Channel.h index 6d9d9f9426801..aa90968c0a06a 100644 --- a/ArduCopter/RC_Channel.h +++ b/ArduCopter/RC_Channel.h @@ -50,4 +50,6 @@ class RC_Channels_Copter : public RC_Channels int8_t flight_mode_channel_number() const override; + // This must be defined/overridden in every calss that inherits from RC_Channels + bool is_extended_flight_modes_enabled() const override; }; diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index e341fc8259c40..b397cd6b7d3d4 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -505,6 +505,43 @@ const AP_Param::Info Plane::var_info[] = { // @Description: Flight mode for switch position 6 (1750 to 2049) GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), + // @Param: FLTMODE7 + // @DisplayName: FlightMode7 + // @Description: Flight mode for switch position 7 + // @Values: 0:Manual,1:CIRCLE,2:STABILIZE,3:TRAINING,4:ACRO,5:FBWA,6:FBWB,7:CRUISE,8:AUTOTUNE,10:Auto,11:RTL,12:Loiter,13:TAKEOFF,14:AVOID_ADSB,15:Guided,17:QSTABILIZE,18:QHOVER,19:QLOITER,20:QLAND,21:QRTL,22:QAUTOTUNE,23:QACRO,24:THERMAL,25:Loiter to QLand + // @User: Advanced + GSCALAR(flight_mode7, "FLTMODE7", FLIGHT_MODE_7), + + // @Param: FLTMODE8 + // @CopyFieldsFrom: FLTMODE7 + // @DisplayName: FlightMode8 + // @Description: Flight mode for switch position 8 + GSCALAR(flight_mode8, "FLTMODE8", FLIGHT_MODE_8), + + // @Param: FLTMODE9 + // @CopyFieldsFrom: FLTMODE7 + // @DisplayName: FlightMode9 + // @Description: Flight mode for switch position 9 + GSCALAR(flight_mode9, "FLTMODE9", FLIGHT_MODE_9), + + // @Param: FLTMODE10 + // @CopyFieldsFrom: FLTMODE7 + // @DisplayName: FlightMode10 + // @Description: Flight mode for switch position 10 + GSCALAR(flight_mode10, "FLTMODE10", FLIGHT_MODE_10), + + // @Param: FLTMODE11 + // @CopyFieldsFrom: FLTMODE7 + // @DisplayName: FlightMode11 + // @Description: Flight mode for switch position 11 + GSCALAR(flight_mode11, "FLTMODE11", FLIGHT_MODE_11), + + // @Param: FLTMODE12 + // @CopyFieldsFrom: FLTMODE7 + // @DisplayName: FlightMode12 + // @Description: Flight mode for switch position 12 + GSCALAR(flight_mode12, "FLTMODE12", FLIGHT_MODE_12), + // @Param: INITIAL_MODE // @DisplayName: Initial flight mode // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. @@ -512,6 +549,13 @@ const AP_Param::Info Plane::var_info[] = { // @User: Advanced GSCALAR(initial_mode, "INITIAL_MODE", Mode::Number::MANUAL), + // @Param: FLTMODE_EXT + // @DisplayName: Extended flight modes + // @Description: Enables the extra 6 flight modes, bringing the extended total to 12 + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + GSCALAR(flight_modes_extended, "FLTMODE_EXT", 0), + // @Param: ROLL_LIMIT_DEG // @DisplayName: Maximum Bank Angle // @Description: Maximum bank angle commanded in modes with stabilized limits. Increase this value for sharper turns, but decrease to prevent accelerated stalls. diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index b464d33d67997..778893b57dee0 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -357,6 +357,15 @@ class Parameters { k_param_acro_yaw_rate, k_param_takeoff_throttle_max_t, k_param_autotune_options, + + // Extended flight modes + k_param_flight_mode7, + k_param_flight_mode8, + k_param_flight_mode9, + k_param_flight_mode10, + k_param_flight_mode11, + k_param_flight_mode12, + k_param_flight_modes_extended, }; AP_Int16 format_version; @@ -419,7 +428,14 @@ class Parameters { AP_Int8 flight_mode4; AP_Int8 flight_mode5; AP_Int8 flight_mode6; + AP_Int8 flight_mode7; + AP_Int8 flight_mode8; + AP_Int8 flight_mode9; + AP_Int8 flight_mode10; + AP_Int8 flight_mode11; + AP_Int8 flight_mode12; AP_Int8 initial_mode; + AP_Int8 flight_modes_extended; // Navigational manoeuvring limits // diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 5341344d7aaf9..15887ed9eeee7 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -209,6 +209,7 @@ class Plane : public AP_Vehicle { // flight modes convenience array AP_Int8 *flight_modes = &g.flight_mode1; const uint8_t num_flight_modes = 6; + const uint8_t num_ext_flight_modes = 12; AP_FixedWing::Rangefinder_State rangefinder_state; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 5b75b9d8f45c8..3721d7d0eb5a3 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -16,6 +16,11 @@ int8_t RC_Channels_Plane::flight_mode_channel_number() const return plane.g.flight_mode_channel.get(); } +bool RC_Channels_Plane::is_extended_flight_modes_enabled() const +{ + return (plane.g.flight_modes_extended.get() == 1); +} + bool RC_Channels_Plane::in_rc_failsafe() const { return (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe); diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h index 7ecc352c32aa8..362fa281556e1 100644 --- a/ArduPlane/RC_Channel.h +++ b/ArduPlane/RC_Channel.h @@ -57,4 +57,5 @@ class RC_Channels_Plane : public RC_Channels // note that these callbacks are not presently used on Plane: int8_t flight_mode_channel_number() const override; + bool is_extended_flight_modes_enabled() const override; }; diff --git a/ArduPlane/config.h b/ArduPlane/config.h index b89be6703f56a..d895e54d0dad7 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -74,6 +74,24 @@ #if !defined(FLIGHT_MODE_6) # define FLIGHT_MODE_6 Mode::Number::MANUAL #endif +#if !defined(FLIGHT_MODE_7) + # define FLIGHT_MODE_7 Mode::Number::FLY_BY_WIRE_A +#endif +#if !defined(FLIGHT_MODE_8) + # define FLIGHT_MODE_8 Mode::Number::FLY_BY_WIRE_A +#endif +#if !defined(FLIGHT_MODE_9) + # define FLIGHT_MODE_9 Mode::Number::FLY_BY_WIRE_A +#endif +#if !defined(FLIGHT_MODE_10) + # define FLIGHT_MODE_10 Mode::Number::FLY_BY_WIRE_A +#endif +#if !defined(FLIGHT_MODE_11) + # define FLIGHT_MODE_11 Mode::Number::FLY_BY_WIRE_A +#endif +#if !defined(FLIGHT_MODE_12) + # define FLIGHT_MODE_12 Mode::Number::FLY_BY_WIRE_A +#endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp index 7f15e0e2663f9..1ebd29aecbc87 100644 --- a/ArduPlane/control_modes.cpp +++ b/ArduPlane/control_modes.cpp @@ -109,7 +109,11 @@ void RC_Channels_Plane::read_mode_switch() void RC_Channel_Plane::mode_switch_changed(modeswitch_pos_t new_pos) { - if (new_pos < 0 || (uint8_t)new_pos > plane.num_flight_modes) { + uint8_t max_number_of_modes = + plane.g.flight_modes_extended.get() == 1 ? + plane.num_ext_flight_modes : plane.num_flight_modes; + + if (new_pos < 0 || (uint8_t)new_pos > (max_number_of_modes - 1)) { // should not have been called return; } diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 95a818a6142bf..9660c76f85801 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -580,10 +580,38 @@ bool RC_Channel::read_6pos_switch(int8_t& position) return true; } -void RC_Channel::read_mode_switch() +// read a mode switch in extended mode (up to 12 positions, for now) +bool RC_Channel::read_extended_mode_switch(int8_t& position) +{ + // calculate position of 12 pos switch + const uint16_t pulsewidth = get_radio_in(); + if (pulsewidth <= RC_MIN_LIMIT_PWM || pulsewidth >= RC_MAX_LIMIT_PWM) { + return false; // This is an error condition + } + + // Determine the total pulse width range amd step for each position + const int16_t pulsestep = (get_radio_max() - get_radio_min()) / 12U; // Assuming 12 max positions + + position = (pulsewidth - get_radio_min()) / pulsestep; + + if (!debounce_completed(position)) { + return false; + } + + return true; +} + +void RC_Channel::read_mode_switch(bool extended) { int8_t position; - if (read_6pos_switch(position)) { + bool changed = false; + if(extended) { + changed = read_extended_mode_switch(position); + } else { + changed = read_6pos_switch(position); + } + + if (changed) { // set flight mode and simple mode setting mode_switch_changed(modeswitch_pos_t(position)); } diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 2fb05537049b0..189d551de09fc 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -402,6 +402,7 @@ class RC_Channel { bool read_3pos_switch(AuxSwitchPos &ret) const WARN_IF_UNUSED; bool read_6pos_switch(int8_t& position) WARN_IF_UNUSED; + bool read_extended_mode_switch(int8_t &position) WARN_IF_UNUSED; // Structure used to detect and debounce switch changes struct { @@ -411,7 +412,7 @@ class RC_Channel { } switch_state; void reset_mode_switch(); - void read_mode_switch(); + void read_mode_switch(bool extended = false); bool debounce_completed(int8_t position); #if AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED @@ -493,7 +494,7 @@ class RC_Channels { // mode switch handling void reset_mode_switch(); - virtual void read_mode_switch(); + virtual void read_mode_switch(); virtual bool in_rc_failsafe() const { return true; }; virtual bool has_valid_input() const { return false; }; @@ -581,6 +582,9 @@ class RC_Channels { // flight_mode_channel_number must be overridden in vehicle specific code virtual int8_t flight_mode_channel_number() const = 0; + // is_extended_flight_modes_enabled must be overridden in vehicle specific code + virtual bool is_extended_flight_modes_enabled() const = 0; + // set and get calibrating flag, stops arming if true void calibrating(bool b) { gcs_is_calibrating = b; } bool calibrating() { return gcs_is_calibrating; } diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 0ecdaba6d9fbf..009ecb96b1aec 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -222,8 +222,8 @@ void RC_Channels::read_mode_switch() RC_Channel *c = flight_mode_channel(); if (c == nullptr) { return; - } - c->read_mode_switch(); + } + c->read_mode_switch(is_extended_flight_modes_enabled()); } // check if flight mode channel is assigned RC option