diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index 7ffb3af76a..eca3086a99 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -34,7 +34,8 @@ bool AP_Arming_Copter::pre_arm_checks(bool display_failure) // check if motor interlock and Emergency Stop aux switches are used // at the same time. This cannot be allowed. - if (copter.check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ + if (rc().find_channel_for_option(RC_Channel::aux_func::MOTOR_INTERLOCK) && + rc().find_channel_for_option(RC_Channel::aux_func::MOTOR_ESTOP)){ check_failed(ARMING_CHECK_NONE, display_failure, "Interlock/E-Stop Conflict"); return false; } @@ -159,8 +160,8 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) // check various parameter values if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) { - // ensure ch7 and ch8 have different functions - if (copter.check_duplicate_auxsw()) { + // ensure all rc channels have different functions + if (rc().duplicate_options_exist()) { check_failed(ARMING_CHECK_PARAMETERS, display_failure, "Duplicate Aux Switch Options"); return false; } @@ -203,7 +204,8 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) return false; } // Inverted flight feature disabled for Heli Single and Dual frames - if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD && (copter.g.ch7_option == 43 || copter.g.ch8_option == 43 || copter.g.ch9_option == 43 || copter.g.ch10_option == 43 || copter.g.ch11_option == 43 || copter.g.ch12_option == 43)) { + if (copter.g2.frame_class.get() != AP_Motors::MOTOR_FRAME_HELI_QUAD && + rc().find_channel_for_option(RC_Channel::aux_func_t::INVERTED) != nullptr) { if (display_failure) { gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Inverted flight option not supported"); } @@ -556,10 +558,10 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, AP_Arming::ArmingMethod // if we are not using Emergency Stop switch option, force Estop false to ensure motors // can run normally - if (!copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ + if (!rc().find_channel_for_option(RC_Channel::aux_func::MOTOR_ESTOP)){ copter.set_motor_emergency_stop(false); // if we are using motor Estop switch, it must not be in Estop position - } else if (copter.check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && copter.ap.motor_emergency_stop){ + } else if (rc().find_channel_for_option(RC_Channel::aux_func::MOTOR_ESTOP) && copter.ap.motor_emergency_stop){ gcs().send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); return false; } diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index 843071cf90..0edb3069ed 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -86,7 +86,7 @@ void Copter::update_using_interlock() #else // check if we are using motor interlock control on an aux switch or are in throw mode // which uses the interlock to stop motors while the copter is being thrown - ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK); + ap.using_interlock = rc().find_channel_for_option(RC_Channel::aux_func::MOTOR_INTERLOCK) != nullptr; #endif } diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 0b6fb6b5bc..2a0732664e 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -90,7 +90,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(update_optical_flow, 200, 160), #endif SCHED_TASK(update_batt_compass, 10, 120), - SCHED_TASK(read_aux_switches, 10, 50), + SCHED_TASK(read_aux_all, 10, 50), SCHED_TASK(arm_motors_check, 10, 50), #if TOY_MODE_ENABLED == ENABLED SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50), @@ -193,6 +193,12 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #endif }; +void Copter::read_aux_all() +{ + copter.g2.rc_channels.read_aux_all(); +} + + constexpr int8_t Copter::_failsafe_priorities[7]; void Copter::setup() @@ -270,7 +276,7 @@ void Copter::rc_loop() // Read radio and 3-position switch on radio // ----------------------------------------- read_radio(); - read_control_switch(); + rc().read_mode_switch(); } // throttle_loop - should be run at 50 hz diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index bda55be404..d8b497b9a9 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -55,7 +55,6 @@ #include // Attitude control library #include // Attitude control library for traditional helicopter #include // Position control library -#include // RC Channel Library #include // AP Motors library #include // statistics library #include // RSSI Library @@ -88,6 +87,8 @@ #include "defines.h" #include "config.h" +#include "RC_Channel.h" // RC Channel Library + #include "GCS_Mavlink.h" #include "GCS_Copter.h" #include "AP_Rally.h" // Rally point library @@ -194,6 +195,8 @@ public: #endif friend class AP_Arming_Copter; friend class ToyMode; + friend class RC_Channel_Copter; + friend class RC_Channels_Copter; Copter(void); @@ -233,6 +236,7 @@ private: // flight modes convenience array AP_Int8 *flight_modes; + const uint8_t num_flight_modes = 6; AP_Baro barometer; Compass compass; @@ -353,19 +357,6 @@ private: control_mode_t prev_control_mode; mode_reason_t prev_control_mode_reason = MODE_REASON_UNKNOWN; - // Structure used to detect changes in the flight mode control switch - struct { - int8_t debounced_switch_position; // currently used switch position - int8_t last_switch_position; // switch position in previous iteration - uint32_t last_edge_time_ms; // system time that switch position was last changed - } control_switch_state; - - // de-bounce counters for switches.cpp - struct debounce { - uint8_t count; - uint8_t ch_flag; - } aux_debounce[(CH_12 - CH_7)+1]; - RCMapper rcmap; // intertial nav alt when we armed @@ -664,6 +655,7 @@ private: void rc_loop(); void throttle_loop(); void update_batt_compass(void); + void read_aux_all(void); void fourhundred_hz_logging(); void ten_hz_logging_loop(); void twentyfive_hz_logging(); @@ -892,15 +884,6 @@ private: // switches.cpp void read_control_switch(); - bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check); - bool check_duplicate_auxsw(void); - void reset_control_switch(); - uint8_t read_3pos_switch(uint8_t chan); - void read_aux_switches(); - void init_aux_switches(); - void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag); - void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag); - bool debounce_aux_switch(uint8_t chan, uint8_t ch_flag); void save_trim(); void auto_trim(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 992b874bdd..49f6ac31ed 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -366,48 +366,6 @@ const AP_Param::Info Copter::var_info[] = { // @RebootRequired: True GSCALAR(frame_type, "FRAME_TYPE", AP_Motors::MOTOR_FRAME_TYPE_X), - // @Param: CH7_OPT - // @DisplayName: Channel 7 option - // @Description: Select which function is performed when CH7 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl - // @User: Standard - GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING), - - // @Param: CH8_OPT - // @DisplayName: Channel 8 option - // @Description: Select which function is performed when CH8 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl - // @User: Standard - GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING), - - // @Param: CH9_OPT - // @DisplayName: Channel 9 option - // @Description: Select which function is performed when CH9 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl - // @User: Standard - GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING), - - // @Param: CH10_OPT - // @DisplayName: Channel 10 option - // @Description: Select which function is performed when CH10 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl - // @User: Standard - GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING), - - // @Param: CH11_OPT - // @DisplayName: Channel 11 option - // @Description: Select which function is performed when CH11 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl - // @User: Standard - GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING), - - // @Param: CH12_OPT - // @DisplayName: Channel 12 option - // @Description: Select which function is performed when CH12 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL, 43:InvertedFlight, 44:Winch Enable, 45:WinchControl - // @User: Standard - GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), - // @Group: ARMING_ // @Path: ../libraries/AP_Arming/AP_Arming.cpp GOBJECT(arming, "ARMING_", AP_Arming_Copter), @@ -922,7 +880,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Group: RC // @Path: ../libraries/RC_Channel/RC_Channels.cpp - AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels), + AP_SUBGROUPINFO(rc_channels, "RC", 17, ParametersG2, RC_Channels_Copter), #if VISUAL_ODOMETRY_ENABLED == ENABLED // @Group: VISO @@ -1046,6 +1004,13 @@ const AP_Param::ConversionInfo conversion_table[] = { { Parameters::k_param_fs_batt_voltage, 0, AP_PARAM_INT8, "BATT_FS_LOW_VOLT" }, { Parameters::k_param_fs_batt_mah, 0, AP_PARAM_INT8, "BATT_FS_LOW_MAH" }, { Parameters::k_param_failsafe_battery_enabled,0, AP_PARAM_INT8, "BATT_FS_LOW_ACT" }, + + { Parameters::Parameters::k_param_ch7_option_old, 0, AP_PARAM_INT8, "RC7_OPTION" }, + { Parameters::Parameters::k_param_ch8_option_old, 0, AP_PARAM_INT8, "RC8_OPTION" }, + { Parameters::Parameters::k_param_ch9_option_old, 0, AP_PARAM_INT8, "RC9_OPTION" }, + { Parameters::Parameters::k_param_ch10_option_old, 0, AP_PARAM_INT8, "RC10_OPTION" }, + { Parameters::Parameters::k_param_ch11_option_old, 0, AP_PARAM_INT8, "RC11_OPTION" }, + { Parameters::Parameters::k_param_ch12_option_old, 0, AP_PARAM_INT8, "RC12_OPTION" }, }; void Copter::load_parameters(void) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 89475534ee..7ce2f70d76 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -94,7 +94,7 @@ public: k_param_pilot_speed_up, // renamed from k_param_pilot_velocity_z_max k_param_circle_rate, // deprecated - remove k_param_rangefinder_gain, - k_param_ch8_option, + k_param_ch8_option_old, // deprecated k_param_arming_check_old, // deprecated - remove k_param_sprayer, k_param_angle_max, @@ -203,10 +203,10 @@ public: k_param_serial2_baud_old, // deprecated k_param_serial2_protocol, // deprecated k_param_serial_manager, - k_param_ch9_option, - k_param_ch10_option, - k_param_ch11_option, - k_param_ch12_option, + k_param_ch9_option_old, + k_param_ch10_option_old, + k_param_ch11_option_old, + k_param_ch12_option_old, k_param_takeoff_trigger_dz, k_param_gcs3, k_param_gcs_pid_mask, // 126 @@ -233,7 +233,7 @@ public: k_param_frame_type, k_param_optflow_enabled, // deprecated k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor - k_param_ch7_option, + k_param_ch7_option_old, k_param_auto_slew_rate, // deprecated - can be deleted k_param_rangefinder_type_old, // deprecated k_param_super_simple = 155, @@ -432,12 +432,6 @@ public: AP_Int16 radio_tuning_high; AP_Int16 radio_tuning_low; AP_Int8 frame_type; - AP_Int8 ch7_option; - AP_Int8 ch8_option; - AP_Int8 ch9_option; - AP_Int8 ch10_option; - AP_Int8 ch11_option; - AP_Int8 ch12_option; AP_Int8 disarm_delay; AP_Int8 land_repositioning; @@ -551,7 +545,7 @@ public: AP_Int8 frame_class; // RC input channels - RC_Channels rc_channels; + RC_Channels_Copter rc_channels; // control over servo output ranges SRV_Channels servo_channels; diff --git a/ArduCopter/RC_Channel.h b/ArduCopter/RC_Channel.h new file mode 100644 index 0000000000..52a52d4758 --- /dev/null +++ b/ArduCopter/RC_Channel.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include "Copter.h" + +class RC_Channel_Copter : public RC_Channel +{ + +public: + +protected: + + void init_aux_function(aux_func_t ch_option, aux_switch_pos_t) override; + void do_aux_function(aux_func_t ch_option, aux_switch_pos_t) override; + +private: + + void do_aux_function_change_mode(const control_mode_t mode, + const aux_switch_pos_t ch_flag); + + // called when the mode switch changes position: + void mode_switch_changed(modeswitch_pos_t new_pos) override; + +}; + +class RC_Channels_Copter : public RC_Channels +{ +public: + + // this must be implemented for the AP_Scheduler functor to work: + void read_aux_all() override { + RC_Channels::read_aux_all(); + } + + bool has_valid_input() const override; + + RC_Channel_Copter obj_channels[NUM_RC_CHANNELS]; + RC_Channel_Copter *channel(const uint8_t chan) override { + if (chan > NUM_RC_CHANNELS) { + return nullptr; + } + return &obj_channels[chan]; + } + +protected: + + int8_t flight_mode_channel_number() const override; + +}; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 5987596bd8..bceb2e5ab7 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -21,66 +21,6 @@ enum autopilot_yaw_mode { AUTO_YAW_RATE = 6, // turn at a specified rate (held in auto_yaw_rate) }; -// Ch6... Ch12 aux switch control -#define AUX_SWITCH_PWM_TRIGGER_HIGH 1800 // pwm value above which the ch7 or ch8 option will be invoked -#define AUX_SWITCH_PWM_TRIGGER_LOW 1200 // pwm value below which the ch7 or ch8 option will be disabled - -// values used by the ap.ch7_opt and ap.ch8_opt flags -#define AUX_SWITCH_LOW 0 // indicates auxiliary switch is in the low position (pwm <1200) -#define AUX_SWITCH_MIDDLE 1 // indicates auxiliary switch is in the middle position (pwm >1200, <1800) -#define AUX_SWITCH_HIGH 2 // indicates auxiliary switch is in the high position (pwm >1800) - -// Aux Switch enumeration -enum aux_sw_func { - AUXSW_DO_NOTHING = 0, // aux switch disabled - AUXSW_FLIP = 2, // flip - AUXSW_SIMPLE_MODE = 3, // change to simple mode - AUXSW_RTL = 4, // change to RTL flight mode - AUXSW_SAVE_TRIM = 5, // save current position as level - AUXSW_SAVE_WP = 7, // save mission waypoint or RTL if in auto mode - AUXSW_CAMERA_TRIGGER = 9, // trigger camera servo or relay - AUXSW_RANGEFINDER = 10, // allow enabling or disabling rangefinder in flight which helps avoid surface tracking when you are far above the ground - AUXSW_FENCE = 11, // allow enabling or disabling fence in flight - AUXSW_RESETTOARMEDYAW = 12, // UNUSED - AUXSW_SUPERSIMPLE_MODE = 13, // change to simple mode in middle, super simple at top - AUXSW_ACRO_TRAINER = 14, // low = disabled, middle = leveled, high = leveled and limited - AUXSW_SPRAYER = 15, // enable/disable the crop sprayer - AUXSW_AUTO = 16, // change to auto flight mode - AUXSW_AUTOTUNE = 17, // auto tune - AUXSW_LAND = 18, // change to LAND flight mode - AUXSW_GRIPPER = 19, // Operate cargo grippers low=off, middle=neutral, high=on - AUXSW_PARACHUTE_ENABLE = 21, // Parachute enable/disable - AUXSW_PARACHUTE_RELEASE = 22, // Parachute release - AUXSW_PARACHUTE_3POS = 23, // Parachute disable, enable, release with 3 position switch - AUXSW_MISSION_RESET = 24, // Reset auto mission to start from first command - AUXSW_ATTCON_FEEDFWD = 25, // enable/disable the roll and pitch rate feed forward - AUXSW_ATTCON_ACCEL_LIM = 26, // enable/disable the roll, pitch and yaw accel limiting - AUXSW_RETRACT_MOUNT = 27, // Retract Mount - AUXSW_RELAY = 28, // Relay pin on/off (only supports first relay) - AUXSW_LANDING_GEAR = 29, // Landing gear controller - AUXSW_LOST_COPTER_SOUND = 30, // Play lost copter sound - AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch - AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch - AUXSW_BRAKE = 33, // Brake flight mode - AUXSW_RELAY2 = 34, // Relay2 pin on/off (in Mission planner set CH8_OPT = 34) - AUXSW_RELAY3 = 35, // Relay3 pin on/off (in Mission planner set CH9_OPT = 35) - AUXSW_RELAY4 = 36, // Relay4 pin on/off (in Mission planner set CH10_OPT = 36) - AUXSW_THROW = 37, // change to THROW flight mode - AUXSW_AVOID_ADSB = 38, // enable AP_Avoidance library - AUXSW_PRECISION_LOITER = 39, // enable precision loiter - AUXSW_AVOID_PROXIMITY = 40, // enable object avoidance using proximity sensors (ie. horizontal lidar) - AUXSW_ARMDISARM = 41, // arm or disarm vehicle - AUXSW_SMART_RTL = 42, // change to SmartRTL flight mode - AUXSW_INVERTED = 43, // enable inverted flight - AUXSW_WINCH_ENABLE = 44, // winch enable/disable - AUXSW_WINCH_CONTROL = 45, // winch control - AUXSW_RC_OVERRIDE_ENABLE = 46, // enable RC Override - AUXSW_USER_FUNC1 = 47, // user function #1 - AUXSW_USER_FUNC2 = 48, // user function #2 - AUXSW_USER_FUNC3 = 49, // user function #3 - AUXSW_SWITCH_MAX, -}; - // Frame types #define UNDEFINED_FRAME 0 #define MULTICOPTER_FRAME 1 diff --git a/ArduCopter/mode_flip.cpp b/ArduCopter/mode_flip.cpp index 914b729ca6..5e357a225b 100644 --- a/ArduCopter/mode_flip.cpp +++ b/ArduCopter/mode_flip.cpp @@ -6,7 +6,7 @@ * Adapted and updated for AC2 in 2011 by Jason Short * * Controls: - * CH7_OPT - CH12_OPT parameter must be set to "Flip" (AUXSW_FLIP) which is "2" + * RC7_OPTION - RC12_OPTION parameter must be set to "Flip" (AUXSW_FLIP) which is "2" * Pilot switches to Stabilize, Acro or AltHold flight mode and puts ch7/ch8 switch to ON position * Vehicle will Roll right by default but if roll or pitch stick is held slightly left, forward or back it will flip in that direction * Vehicle should complete the roll within 2.5sec and will then return to the original flight mode it was in before flip was triggered diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 986f262dc0..bef8497d34 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -342,7 +342,7 @@ void Copter::lost_vehicle_check() static uint8_t soundalarm_counter; // disable if aux switch is setup to vehicle alarm as the two could interfere - if (check_if_auxsw_mode_used(AUXSW_LOST_COPTER_SOUND)) { + if (rc().find_channel_for_option(RC_Channel::aux_func::LOST_COPTER_SOUND)) { return; } diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index cd9326b0a6..f1cbe6069e 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -11,20 +11,20 @@ void Copter::default_dead_zones() #if FRAME_CONFIG == HELI_FRAME channel_throttle->set_default_dead_zone(10); channel_yaw->set_default_dead_zone(15); - RC_Channels::rc_channel(CH_6)->set_default_dead_zone(10); + rc().channel(CH_6)->set_default_dead_zone(10); #else channel_throttle->set_default_dead_zone(30); channel_yaw->set_default_dead_zone(20); #endif - RC_Channels::rc_channel(CH_6)->set_default_dead_zone(0); + rc().channel(CH_6)->set_default_dead_zone(0); } void Copter::init_rc_in() { - channel_roll = RC_Channels::rc_channel(rcmap.roll()-1); - channel_pitch = RC_Channels::rc_channel(rcmap.pitch()-1); - channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1); - channel_yaw = RC_Channels::rc_channel(rcmap.yaw()-1); + channel_roll = rc().channel(rcmap.roll()-1); + channel_pitch = rc().channel(rcmap.pitch()-1); + channel_throttle = rc().channel(rcmap.throttle()-1); + channel_yaw = rc().channel(rcmap.yaw()-1); // set rc channel ranges channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX); @@ -33,10 +33,10 @@ void Copter::init_rc_in() channel_throttle->set_range(1000); //set auxiliary servo ranges - RC_Channels::rc_channel(CH_5)->set_range(1000); - RC_Channels::rc_channel(CH_6)->set_range(1000); - RC_Channels::rc_channel(CH_7)->set_range(1000); - RC_Channels::rc_channel(CH_8)->set_range(1000); + rc().channel(CH_5)->set_range(1000); + rc().channel(CH_6)->set_range(1000); + rc().channel(CH_7)->set_range(1000); + rc().channel(CH_8)->set_range(1000); // set default dead zones default_dead_zones(); @@ -95,7 +95,7 @@ void Copter::read_radio() { uint32_t tnow_ms = millis(); - if (RC_Channels::read_input()) { + if (rc().read_input()) { ap.new_radio_frame = true; set_throttle_and_failsafe(channel_throttle->get_radio_in()); diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index f241d00195..a0d45b0065 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -1,291 +1,169 @@ #include "Copter.h" -#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200 +#include "RC_Channel.h" -//Documentation of Aux Switch Flags: -struct { - uint8_t CH6_flag : 2; // 0, 1 // ch6 aux switch : 0 is low or false, 1 is center or true, 2 is high - uint8_t CH7_flag : 2; // 2, 3 // ch7 aux switch : 0 is low or false, 1 is center or true, 2 is high - uint8_t CH8_flag : 2; // 4, 5 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high - uint8_t CH9_flag : 2; // 6, 7 // ch9 aux switch : 0 is low or false, 1 is center or true, 2 is high - uint8_t CH10_flag : 2; // 8, 9 // ch10 aux switch : 0 is low or false, 1 is center or true, 2 is high - uint8_t CH11_flag : 2; // 10,11 // ch11 aux switch : 0 is low or false, 1 is center or true, 2 is high - uint8_t CH12_flag : 2; // 12,13 // ch12 aux switch : 0 is low or false, 1 is center or true, 2 is high -} aux_con; +// defining these two macros and including the RC_Channels_VarInfo header defines the parmaeter information common to all vehicle types +#define RC_CHANNELS_SUBCLASS RC_Channels_Copter +#define RC_CHANNEL_SUBCLASS RC_Channel_Copter -void Copter::read_control_switch() +#include + +int8_t RC_Channels_Copter::flight_mode_channel_number() const { - if (g.flight_mode_chan <= 0) { - // no flight mode channel + return copter.g.flight_mode_chan.get(); +} + +void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos) +{ + if (new_pos < 0 || (uint8_t)new_pos > copter.num_flight_modes) { + // should not have been called return; } - - uint32_t tnow_ms = millis(); - // calculate position of flight mode switch - int8_t switch_position; - uint16_t mode_in = RC_Channels::rc_channel(g.flight_mode_chan-1)->get_radio_in(); - if (mode_in < 1231) switch_position = 0; - else if (mode_in < 1361) switch_position = 1; - else if (mode_in < 1491) switch_position = 2; - else if (mode_in < 1621) switch_position = 3; - else if (mode_in < 1750) switch_position = 4; - else switch_position = 5; - - // store time that switch last moved - if (control_switch_state.last_switch_position != switch_position) { - control_switch_state.last_edge_time_ms = tnow_ms; - } - - // debounce switch - bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position; - bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS; - bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0; - - if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) { - // set flight mode and simple mode setting - if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND)) { - // play a tone - if (control_switch_state.debounced_switch_position != -1) { - // alert user to mode change (except if autopilot is just starting up) - if (ap.initialised) { - AP_Notify::events.user_mode_change = 1; - } - } - - if (!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_SUPERSIMPLE_MODE)) { - // if none of the Aux Switches are set to Simple or Super Simple Mode then - // set Simple Mode using stored parameters from EEPROM - if (BIT_IS_SET(g.super_simple, switch_position)) { - set_simple_mode(2); - } else { - set_simple_mode(BIT_IS_SET(g.simple_modes, switch_position)); - } - } - - } else if (control_switch_state.last_switch_position != -1) { - // alert user to mode change failure + if (!copter.set_mode((control_mode_t)copter.flight_modes[new_pos].get(), MODE_REASON_TX_COMMAND)) { + // alert user to mode change failure + if (copter.ap.initialised) { AP_Notify::events.user_mode_change_failed = 1; } - - // set the debounced switch position - control_switch_state.debounced_switch_position = switch_position; - } - - control_switch_state.last_switch_position = switch_position; -} - -// check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode. -bool Copter::check_if_auxsw_mode_used(uint8_t auxsw_mode_check) -{ - bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check - || g.ch10_option == auxsw_mode_check || g.ch11_option == auxsw_mode_check || g.ch12_option == auxsw_mode_check; - - return ret; -} - -// check_duplicate_auxsw - Check to see if any Aux Switch Functions are duplicated -bool Copter::check_duplicate_auxsw(void) -{ - uint8_t auxsw_option_counts[AUXSW_SWITCH_MAX] = {}; - auxsw_option_counts[g.ch7_option]++; - auxsw_option_counts[g.ch8_option]++; - auxsw_option_counts[g.ch9_option]++; - auxsw_option_counts[g.ch10_option]++; - auxsw_option_counts[g.ch11_option]++; - auxsw_option_counts[g.ch12_option]++; - - for (uint8_t i=0; i 1) { - return true; - } - } - return false; -} - -void Copter::reset_control_switch() -{ - control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1; - read_control_switch(); -} - -// read_3pos_switch -uint8_t Copter::read_3pos_switch(uint8_t chan) -{ - uint16_t radio_in = RC_Channels::rc_channel(chan)->get_radio_in(); - if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW; // switch is in low position - if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; // switch is in high position - return AUX_SWITCH_MIDDLE; // switch is in middle position -} - -// can't take reference to a bitfield member, thus a #define: -#define read_aux_switch(chan, flag, option) \ - do { \ - switch_position = read_3pos_switch(chan); \ - if (debounce_aux_switch(chan, flag) && flag != switch_position) { \ - flag = switch_position; \ - do_aux_switch_function(option, flag); \ - } \ - } while (false) - -// read_aux_switches - checks aux switch positions and invokes configured actions -void Copter::read_aux_switches() -{ - uint8_t switch_position; - - // exit immediately during radio failsafe - if (failsafe.radio || failsafe.radio_counter != 0) { return; } - read_aux_switch(CH_7, aux_con.CH7_flag, g.ch7_option); - read_aux_switch(CH_8, aux_con.CH8_flag, g.ch8_option); - read_aux_switch(CH_9, aux_con.CH9_flag, g.ch9_option); - read_aux_switch(CH_10, aux_con.CH10_flag, g.ch10_option); - read_aux_switch(CH_11, aux_con.CH11_flag, g.ch11_option); - read_aux_switch(CH_12, aux_con.CH12_flag, g.ch12_option); + // play a tone + // alert user to mode change (except if autopilot is just starting up) + if (copter.ap.initialised) { + AP_Notify::events.user_mode_change = 1; + } + + if (!rc().find_channel_for_option(SIMPLE_MODE) && + !rc().find_channel_for_option(SUPERSIMPLE_MODE)) { + // if none of the Aux Switches are set to Simple or Super Simple Mode then + // set Simple Mode using stored parameters from EEPROM + if (BIT_IS_SET(copter.g.super_simple, new_pos)) { + copter.set_simple_mode(2); + } else { + copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos)); + } + } } -#undef read_aux_switch - -// init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so -void Copter::init_aux_switches() +bool RC_Channels_Copter::has_valid_input() const { - // set the CH7 ~ CH12 flags - aux_con.CH7_flag = read_3pos_switch(CH_7); - aux_con.CH8_flag = read_3pos_switch(CH_8); - aux_con.CH10_flag = read_3pos_switch(CH_10); - aux_con.CH11_flag = read_3pos_switch(CH_11); - - // ch9, ch12 only supported on some boards - aux_con.CH9_flag = read_3pos_switch(CH_9); - aux_con.CH12_flag = read_3pos_switch(CH_12); - - // initialise functions assigned to switches - init_aux_switch_function(g.ch7_option, aux_con.CH7_flag); - init_aux_switch_function(g.ch8_option, aux_con.CH8_flag); - init_aux_switch_function(g.ch10_option, aux_con.CH10_flag); - init_aux_switch_function(g.ch11_option, aux_con.CH11_flag); - - // ch9, ch12 only supported on some boards - init_aux_switch_function(g.ch9_option, aux_con.CH9_flag); - init_aux_switch_function(g.ch12_option, aux_con.CH12_flag); + if (copter.failsafe.radio) { + return false; + } + if (copter.failsafe.radio_counter != 0) { + return false; + } + return true; } + // init_aux_switch_function - initialize aux functions -void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) +void RC_Channel_Copter::init_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) { // init channel options switch(ch_option) { - case AUXSW_SIMPLE_MODE: - case AUXSW_RANGEFINDER: - case AUXSW_FENCE: - case AUXSW_SUPERSIMPLE_MODE: - case AUXSW_ACRO_TRAINER: - case AUXSW_GRIPPER: - case AUXSW_SPRAYER: - case AUXSW_PARACHUTE_ENABLE: - case AUXSW_PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release - case AUXSW_RETRACT_MOUNT: - case AUXSW_MISSION_RESET: - case AUXSW_ATTCON_FEEDFWD: - case AUXSW_ATTCON_ACCEL_LIM: - case AUXSW_MOTOR_ESTOP: - case AUXSW_MOTOR_INTERLOCK: - case AUXSW_AVOID_ADSB: - case AUXSW_PRECISION_LOITER: - case AUXSW_AVOID_PROXIMITY: - case AUXSW_INVERTED: - case AUXSW_WINCH_ENABLE: - case AUXSW_RC_OVERRIDE_ENABLE: - do_aux_switch_function(ch_option, ch_flag); - break; + case SIMPLE_MODE: + case RANGEFINDER: + case FENCE: + case SUPERSIMPLE_MODE: + case ACRO_TRAINER: + case GRIPPER: + case SPRAYER: + case PARACHUTE_ENABLE: + case PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release + case RETRACT_MOUNT: + case MISSION_RESET: + case ATTCON_FEEDFWD: + case ATTCON_ACCEL_LIM: + case MOTOR_ESTOP: + case MOTOR_INTERLOCK: + case AVOID_ADSB: + case PRECISION_LOITER: + case AVOID_PROXIMITY: + case INVERTED: + case WINCH_ENABLE: + case RC_OVERRIDE_ENABLE: + do_aux_function(ch_option, ch_flag); + break; + // the following functions do not need to be initialised: + case FLIP: + case RTL: + case SAVE_TRIM: + case SAVE_WP: + case RESETTOARMEDYAW: + case AUTO: + break; + default: + RC_Channel::init_aux_function(ch_option, ch_flag); + break; } } -/* - debounce an aux switch using a counter in copter.debounce - structure. This will return false until the same ch_flag is seen debounce_count times - */ -bool Copter::debounce_aux_switch(uint8_t chan, uint8_t ch_flag) +// do_aux_function_change_mode - change mode based on an aux switch +// being moved +void RC_Channel_Copter::do_aux_function_change_mode(const control_mode_t mode, + const aux_switch_pos_t ch_flag) { - // a value of 2 means we need 3 values in a row with the same - // value to activate - const uint8_t debounce_count = 2; - - if (chan < CH_7 || chan > CH_12) { - // someone has forgotten to expand the debounce channel range - return false; + switch(ch_flag) { + case HIGH: + // engage mode (if not possible we remain in current flight mode) + copter.set_mode(mode, MODE_REASON_TX_COMMAND); + break; + default: + // return to flight mode switch's flight mode if we are currently + // in this mode + if (copter.control_mode == mode) { + rc().reset_mode_switch(); + } } - struct debounce &db = aux_debounce[chan-CH_7]; - if (db.ch_flag != ch_flag) { - db.ch_flag = ch_flag; - db.count = 0; - return false; - } - if (db.count < debounce_count) { - db.count++; - } - return db.count >= debounce_count; } -// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch -void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) +// do_aux_function - implement the function invoked by auxillary switches +void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag) { - - switch(ch_function) { - case AUXSW_FLIP: + switch(ch_option) { + case FLIP: // flip if switch is on, positive throttle and we're actually flying - if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(FLIP, MODE_REASON_TX_COMMAND); + if (ch_flag == aux_switch_pos::HIGH) { + copter.set_mode(control_mode_t::FLIP, MODE_REASON_TX_COMMAND); } break; - case AUXSW_SIMPLE_MODE: + case SIMPLE_MODE: // low = simple mode off, middle or high position turns simple mode on - set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); + copter.set_simple_mode(ch_flag == HIGH || ch_flag == MIDDLE); break; - case AUXSW_SUPERSIMPLE_MODE: + case SUPERSIMPLE_MODE: // low = simple mode off, middle = simple mode, high = super simple mode - set_simple_mode(ch_flag); + copter.set_simple_mode(ch_flag); break; - case AUXSW_RTL: + case RTL: #if MODE_RTL_ENABLED == ENABLED - if (ch_flag == AUX_SWITCH_HIGH) { - // engage RTL (if not possible we remain in current flight mode) - set_mode(RTL, MODE_REASON_TX_COMMAND); - } else { - // return to flight mode switch's flight mode if we are currently in RTL - if (control_mode == RTL) { - reset_control_switch(); - } - } + do_aux_function_change_mode(control_mode_t::RTL, ch_flag); #endif break; - case AUXSW_SAVE_TRIM: - if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) { - save_trim(); + case SAVE_TRIM: + if ((ch_flag == HIGH) && (copter.control_mode <= control_mode_t::ACRO) && (copter.channel_throttle->get_control_in() == 0)) { + copter.save_trim(); } break; - case AUXSW_SAVE_WP: + case SAVE_WP: #if MODE_AUTO_ENABLED == ENABLED // save waypoint when switch is brought high - if (ch_flag == AUX_SWITCH_HIGH) { + if (ch_flag == HIGH) { // do not allow saving new waypoints while we're in auto or disarmed - if (control_mode == AUTO || !motors->armed()) { + if (copter.control_mode == control_mode_t::AUTO || !copter.motors->armed()) { return; } // do not allow saving the first waypoint with zero throttle - if ((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)) { + if ((copter.mission.num_commands() == 0) && (copter.channel_throttle->get_control_in() == 0)) { return; } @@ -293,28 +171,28 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) AP_Mission::Mission_Command cmd = {}; // if the mission is empty save a takeoff command - if (mission.num_commands() == 0) { + if (copter.mission.num_commands() == 0) { // set our location ID to 16, MAV_CMD_NAV_WAYPOINT cmd.id = MAV_CMD_NAV_TAKEOFF; cmd.content.location.options = 0; cmd.p1 = 0; cmd.content.location.lat = 0; cmd.content.location.lng = 0; - cmd.content.location.alt = MAX(current_loc.alt,100); + cmd.content.location.alt = MAX(copter.current_loc.alt,100); // use the current altitude for the target alt for takeoff. // only altitude will matter to the AP mission script for takeoff. - if (mission.add_cmd(cmd)) { + if (copter.mission.add_cmd(cmd)) { // log event - Log_Write_Event(DATA_SAVEWP_ADD_WP); + copter.Log_Write_Event(DATA_SAVEWP_ADD_WP); } } // set new waypoint to current location - cmd.content.location = current_loc; + cmd.content.location = copter.current_loc; // if throttle is above zero, create waypoint command - if (channel_throttle->get_control_in() > 0) { + if (copter.channel_throttle->get_control_in() > 0) { cmd.id = MAV_CMD_NAV_WAYPOINT; } else { // with zero throttle, create LAND command @@ -322,425 +200,380 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } // save command - if (mission.add_cmd(cmd)) { + if (copter.mission.add_cmd(cmd)) { // log event - Log_Write_Event(DATA_SAVEWP_ADD_WP); + copter.Log_Write_Event(DATA_SAVEWP_ADD_WP); } } #endif break; - case AUXSW_MISSION_RESET: + case MISSION_RESET: #if MODE_AUTO_ENABLED == ENABLED - if (ch_flag == AUX_SWITCH_HIGH) { - mission.reset(); + if (ch_flag == HIGH) { + copter.mission.reset(); } #endif break; - case AUXSW_AUTO: + case AUTO: #if MODE_AUTO_ENABLED == ENABLED - if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(AUTO, MODE_REASON_TX_COMMAND); - } else { - // return to flight mode switch's flight mode if we are currently in AUTO - if (control_mode == AUTO) { - reset_control_switch(); - } - } + do_aux_function_change_mode(control_mode_t::AUTO, ch_flag); #endif break; - case AUXSW_CAMERA_TRIGGER: -#if CAMERA == ENABLED - if (ch_flag == AUX_SWITCH_HIGH) { - camera.take_picture(); - } -#endif - break; - - case AUXSW_RANGEFINDER: + case RANGEFINDER: // enable or disable the rangefinder #if RANGEFINDER_ENABLED == ENABLED - if ((ch_flag == AUX_SWITCH_HIGH) && rangefinder.has_orientation(ROTATION_PITCH_270)) { - rangefinder_state.enabled = true; + if ((ch_flag == HIGH) && copter.rangefinder.has_orientation(ROTATION_PITCH_270)) { + copter.rangefinder_state.enabled = true; } else { - rangefinder_state.enabled = false; + copter.rangefinder_state.enabled = false; } #endif break; - case AUXSW_FENCE: + case FENCE: #if AC_FENCE == ENABLED // enable or disable the fence - if (ch_flag == AUX_SWITCH_HIGH) { - fence.enable(true); - Log_Write_Event(DATA_FENCE_ENABLE); + if (ch_flag == HIGH) { + copter.fence.enable(true); + copter.Log_Write_Event(DATA_FENCE_ENABLE); } else { - fence.enable(false); - Log_Write_Event(DATA_FENCE_DISABLE); + copter.fence.enable(false); + copter.Log_Write_Event(DATA_FENCE_DISABLE); } #endif break; - case AUXSW_ACRO_TRAINER: + case ACRO_TRAINER: #if MODE_ACRO_ENABLED == ENABLED switch(ch_flag) { - case AUX_SWITCH_LOW: - g.acro_trainer = ACRO_TRAINER_DISABLED; - Log_Write_Event(DATA_ACRO_TRAINER_DISABLED); + case LOW: + copter.g.acro_trainer = ACRO_TRAINER_DISABLED; + copter.Log_Write_Event(DATA_ACRO_TRAINER_DISABLED); break; - case AUX_SWITCH_MIDDLE: - g.acro_trainer = ACRO_TRAINER_LEVELING; - Log_Write_Event(DATA_ACRO_TRAINER_LEVELING); + case MIDDLE: + copter.g.acro_trainer = ACRO_TRAINER_LEVELING; + copter.Log_Write_Event(DATA_ACRO_TRAINER_LEVELING); break; - case AUX_SWITCH_HIGH: - g.acro_trainer = ACRO_TRAINER_LIMITED; - Log_Write_Event(DATA_ACRO_TRAINER_LIMITED); + case HIGH: + copter.g.acro_trainer = ACRO_TRAINER_LIMITED; + copter.Log_Write_Event(DATA_ACRO_TRAINER_LIMITED); break; } #endif break; - case AUXSW_GRIPPER: + case GRIPPER: #if GRIPPER_ENABLED == ENABLED switch(ch_flag) { - case AUX_SWITCH_LOW: - g2.gripper.release(); - Log_Write_Event(DATA_GRIPPER_RELEASE); + case LOW: + copter.g2.gripper.release(); + copter.Log_Write_Event(DATA_GRIPPER_RELEASE); break; - case AUX_SWITCH_HIGH: - g2.gripper.grab(); - Log_Write_Event(DATA_GRIPPER_GRAB); + case MIDDLE: + // nothing + break; + case HIGH: + copter.g2.gripper.grab(); + copter.Log_Write_Event(DATA_GRIPPER_GRAB); break; } #endif break; - case AUXSW_SPRAYER: + case SPRAYER: #if SPRAYER_ENABLED == ENABLED - sprayer.run(ch_flag == AUX_SWITCH_HIGH); + copter.sprayer.run(ch_flag == HIGH); // if we are disarmed the pilot must want to test the pump - sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors->armed()); + copter.sprayer.test_pump((ch_flag == HIGH) && !copter.motors->armed()); #endif break; - case AUXSW_AUTOTUNE: + case AUTOTUNE: #if AUTOTUNE_ENABLED == ENABLED - // turn on auto tuner - switch(ch_flag) { - case AUX_SWITCH_LOW: - case AUX_SWITCH_MIDDLE: - // restore flight mode based on flight mode switch position - if (control_mode == AUTOTUNE) { - reset_control_switch(); - } - break; - case AUX_SWITCH_HIGH: - // start an autotuning session - set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND); - break; - } + do_aux_function_change_mode(control_mode_t::AUTOTUNE, ch_flag); #endif break; - case AUXSW_LAND: - if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(LAND, MODE_REASON_TX_COMMAND); - } else { - // return to flight mode switch's flight mode if we are currently in LAND - if (control_mode == LAND) { - reset_control_switch(); - } - } + case LAND: + do_aux_function_change_mode(control_mode_t::LAND, ch_flag); break; - case AUXSW_PARACHUTE_ENABLE: + case GUIDED: + do_aux_function_change_mode(control_mode_t::GUIDED, ch_flag); + break; + + case PARACHUTE_ENABLE: #if PARACHUTE == ENABLED // Parachute enable/disable - parachute.enabled(ch_flag == AUX_SWITCH_HIGH); + copter.parachute.enabled(ch_flag == HIGH); #endif break; - case AUXSW_PARACHUTE_RELEASE: + case PARACHUTE_RELEASE: #if PARACHUTE == ENABLED - if (ch_flag == AUX_SWITCH_HIGH) { - parachute_manual_release(); + if (ch_flag == HIGH) { + copter.parachute_manual_release(); } #endif break; - case AUXSW_PARACHUTE_3POS: + case PARACHUTE_3POS: #if PARACHUTE == ENABLED // Parachute disable, enable, release with 3 position switch switch (ch_flag) { - case AUX_SWITCH_LOW: - parachute.enabled(false); - Log_Write_Event(DATA_PARACHUTE_DISABLED); + case LOW: + copter.parachute.enabled(false); + copter.Log_Write_Event(DATA_PARACHUTE_DISABLED); break; - case AUX_SWITCH_MIDDLE: - parachute.enabled(true); - Log_Write_Event(DATA_PARACHUTE_ENABLED); + case MIDDLE: + copter.parachute.enabled(true); + copter.Log_Write_Event(DATA_PARACHUTE_ENABLED); break; - case AUX_SWITCH_HIGH: - parachute.enabled(true); - parachute_manual_release(); + case HIGH: + copter.parachute.enabled(true); + copter.parachute_manual_release(); break; } #endif break; - case AUXSW_ATTCON_FEEDFWD: + case ATTCON_FEEDFWD: // enable or disable feed forward - attitude_control->bf_feedforward(ch_flag == AUX_SWITCH_HIGH); + copter.attitude_control->bf_feedforward(ch_flag == HIGH); break; - case AUXSW_ATTCON_ACCEL_LIM: + case ATTCON_ACCEL_LIM: // enable or disable accel limiting by restoring defaults - attitude_control->accel_limiting(ch_flag == AUX_SWITCH_HIGH); + copter.attitude_control->accel_limiting(ch_flag == HIGH); break; - case AUXSW_RETRACT_MOUNT: + case RETRACT_MOUNT: #if MOUNT == ENABLE switch (ch_flag) { - case AUX_SWITCH_HIGH: - camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT); + case HIGH: + copter.camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT); break; - case AUX_SWITCH_LOW: - camera_mount.set_mode_to_default(); + case MIDDLE: + // nothing + break; + case LOW: + copter.camera_mount.set_mode_to_default(); break; } #endif break; - case AUXSW_RELAY: - ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH); - break; - - case AUXSW_RELAY2: - ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH); - break; - - case AUXSW_RELAY3: - ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH); - break; - - case AUXSW_RELAY4: - ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH); - break; - - case AUXSW_LANDING_GEAR: + case LANDING_GEAR: switch (ch_flag) { - case AUX_SWITCH_LOW: - landinggear.set_position(AP_LandingGear::LandingGear_Deploy); + case LOW: + copter.landinggear.set_position(AP_LandingGear::LandingGear_Deploy); break; - case AUX_SWITCH_HIGH: - landinggear.set_position(AP_LandingGear::LandingGear_Retract); + case MIDDLE: + // nothing + break; + case HIGH: + copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract); break; } break; - case AUXSW_LOST_COPTER_SOUND: + case LOST_COPTER_SOUND: switch (ch_flag) { - case AUX_SWITCH_HIGH: + case HIGH: AP_Notify::flags.vehicle_lost = true; break; - case AUX_SWITCH_LOW: + case MIDDLE: + // nothing + break; + case LOW: AP_Notify::flags.vehicle_lost = false; break; } break; - case AUXSW_MOTOR_ESTOP: + case MOTOR_ESTOP: // Turn on Emergency Stop logic when channel is high - set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH); + copter.set_motor_emergency_stop(ch_flag == HIGH); break; - case AUXSW_MOTOR_INTERLOCK: + case MOTOR_INTERLOCK: // Turn on when above LOW, because channel will also be used for speed // control signal in tradheli - ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); + copter.ap.motor_interlock_switch = (ch_flag == HIGH || ch_flag == MIDDLE); break; - case AUXSW_BRAKE: + case BRAKE: #if MODE_BRAKE_ENABLED == ENABLED - // brake flight mode - if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(BRAKE, MODE_REASON_TX_COMMAND); - } else { - // return to flight mode switch's flight mode if we are currently in BRAKE - if (control_mode == BRAKE) { - reset_control_switch(); - } - } + do_aux_function_change_mode(control_mode_t::BRAKE, ch_flag); #endif break; - case AUXSW_THROW: + case THROW: #if MODE_THROW_ENABLED == ENABLED - // throw flight mode - if (ch_flag == AUX_SWITCH_HIGH) { - set_mode(THROW, MODE_REASON_TX_COMMAND); - } else { - // return to flight mode switch's flight mode if we are currently in throw mode - if (control_mode == THROW) { - reset_control_switch(); - } - } + do_aux_function_change_mode(control_mode_t::THROW, ch_flag); #endif break; - case AUXSW_AVOID_ADSB: + case AVOID_ADSB: #if ADSB_ENABLED == ENABLED // enable or disable AP_Avoidance - if (ch_flag == AUX_SWITCH_HIGH) { - avoidance_adsb.enable(); - Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE); + if (ch_flag == HIGH) { + copter.avoidance_adsb.enable(); + copter.Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE); } else { - avoidance_adsb.disable(); - Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE); + copter.avoidance_adsb.disable(); + copter.Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE); } #endif break; - case AUXSW_PRECISION_LOITER: + case PRECISION_LOITER: #if PRECISION_LANDING == ENABLED && MODE_LOITER_ENABLED == ENABLED switch (ch_flag) { - case AUX_SWITCH_HIGH: - mode_loiter.set_precision_loiter_enabled(true); + case HIGH: + copter.mode_loiter.set_precision_loiter_enabled(true); break; - case AUX_SWITCH_LOW: - mode_loiter.set_precision_loiter_enabled(false); + case MIDDLE: + // nothing + break; + case LOW: + copter.mode_loiter.set_precision_loiter_enabled(false); break; } #endif break; - case AUXSW_AVOID_PROXIMITY: + case AVOID_PROXIMITY: #if PROXIMITY_ENABLED == ENABLED && AC_AVOID_ENABLED == ENABLED switch (ch_flag) { - case AUX_SWITCH_HIGH: - avoid.proximity_avoidance_enable(true); - Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_ENABLE); + case HIGH: + copter.avoid.proximity_avoidance_enable(true); + copter.Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_ENABLE); break; - case AUX_SWITCH_LOW: - avoid.proximity_avoidance_enable(false); - Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_DISABLE); + case MIDDLE: + // nothing + break; + case LOW: + copter.avoid.proximity_avoidance_enable(false); + copter.Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_DISABLE); break; } #endif break; - case AUXSW_ARMDISARM: + case ARMDISARM: // arm or disarm the vehicle switch (ch_flag) { - case AUX_SWITCH_HIGH: - init_arm_motors(AP_Arming::ArmingMethod::AUXSWITCH); + case HIGH: + copter.init_arm_motors(AP_Arming::ArmingMethod::AUXSWITCH); // remember that we are using an arming switch, for use by set_throttle_zero_flag - ap.armed_with_switch = true; + copter.ap.armed_with_switch = true; break; - case AUX_SWITCH_LOW: - init_disarm_motors(); + case MIDDLE: + // nothing + break; + case LOW: + copter.init_disarm_motors(); break; } break; - case AUXSW_SMART_RTL: + case SMART_RTL: #if MODE_SMARTRTL_ENABLED == ENABLED - if (ch_flag == AUX_SWITCH_HIGH) { - // engage SmartRTL (if not possible we remain in current flight mode) - set_mode(SMART_RTL, MODE_REASON_TX_COMMAND); - } else { - // return to flight mode switch's flight mode if we are currently in RTL - if (control_mode == SMART_RTL) { - reset_control_switch(); - } - } + do_aux_function_change_mode(control_mode_t::SMART_RTL, ch_flag); #endif break; - case AUXSW_INVERTED: + case INVERTED: #if FRAME_CONFIG == HELI_FRAME - // inverted flight option is disabled for heli single and dual frames - if (g2.frame_class.get() == AP_Motors::MOTOR_FRAME_HELI_QUAD) { - switch (ch_flag) { - case AUX_SWITCH_HIGH: - motors->set_inverted_flight(true); - attitude_control->set_inverted_flight(true); - heli_flags.inverted_flight = true; - break; - case AUX_SWITCH_LOW: - motors->set_inverted_flight(false); - attitude_control->set_inverted_flight(false); - heli_flags.inverted_flight = false; - break; - } + switch (ch_flag) { + case HIGH: + copter.motors->set_inverted_flight(true); + copter.attitude_control->set_inverted_flight(true); + copter.heli_flags.inverted_flight = true; + break; + case MIDDLE: + // nothing + break; + case LOW: + copter.motors->set_inverted_flight(false); + copter.attitude_control->set_inverted_flight(false); + copter.heli_flags.inverted_flight = false; + break; } #endif break; - case AUXSW_WINCH_ENABLE: + case WINCH_ENABLE: #if WINCH_ENABLED == ENABLED switch (ch_flag) { - case AUX_SWITCH_HIGH: + case HIGH: // high switch maintains current position - g2.winch.release_length(0.0f); - Log_Write_Event(DATA_WINCH_LENGTH_CONTROL); + copter.g2.winch.release_length(0.0f); + copter.Log_Write_Event(DATA_WINCH_LENGTH_CONTROL); break; default: // all other position relax winch - g2.winch.relax(); - Log_Write_Event(DATA_WINCH_RELAXED); + copter.g2.winch.relax(); + copter.Log_Write_Event(DATA_WINCH_RELAXED); break; } #endif break; - case AUXSW_WINCH_CONTROL: + case WINCH_CONTROL: #if WINCH_ENABLED == ENABLED switch (ch_flag) { - case AUX_SWITCH_LOW: + case LOW: // raise winch at maximum speed - g2.winch.set_desired_rate(-g2.winch.get_rate_max()); + copter.g2.winch.set_desired_rate(-copter.g2.winch.get_rate_max()); break; - case AUX_SWITCH_HIGH: + case HIGH: // lower winch at maximum speed - g2.winch.set_desired_rate(g2.winch.get_rate_max()); + copter.g2.winch.set_desired_rate(copter.g2.winch.get_rate_max()); break; - case AUX_SWITCH_MIDDLE: - default: - g2.winch.set_desired_rate(0.0f); + case MIDDLE: + copter.g2.winch.set_desired_rate(0.0f); break; } #endif break; - case AUXSW_RC_OVERRIDE_ENABLE: + case RC_OVERRIDE_ENABLE: // Allow or disallow RC_Override switch (ch_flag) { - case AUX_SWITCH_HIGH: { - ap.rc_override_enable = true; + case HIGH: { + copter.ap.rc_override_enable = true; break; } - case AUX_SWITCH_LOW: { - ap.rc_override_enable = false; + case MIDDLE: + // nothing + break; + case LOW: { + copter.ap.rc_override_enable = false; break; } } break; #ifdef USERHOOK_AUXSWITCH - case AUXSW_USER_FUNC1: + case USER_FUNC1: userhook_auxSwitch1(ch_flag); break; - case AUXSW_USER_FUNC2: + case USER_FUNC2: userhook_auxSwitch2(ch_flag); break; - case AUXSW_USER_FUNC3: + case USER_FUNC3: userhook_auxSwitch3(ch_flag); break; #endif + default: + RC_Channel::do_aux_function(ch_option, ch_flag); + break; } } @@ -781,4 +614,3 @@ void Copter::auto_trim() } } } - diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 2f5692d4fc..7b5f4238ed 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -251,10 +251,8 @@ void Copter::init_ardupilot() #endif DataFlash.setVehicle_Startup_Log_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void)); - // initialise the flight mode and aux switch - // --------------------------- - reset_control_switch(); - init_aux_switches(); + // initialise rc channels including setting mode + rc().init(); startup_INS_ground(); diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index b9a8f3d7f8..67c90c2e65 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -8,7 +8,7 @@ // tuning - updates parameters based on the ch6 tuning knob's position // should be called at 3.3hz void Copter::tuning() { - RC_Channel *rc6 = RC_Channels::rc_channel(CH_6); + RC_Channel *rc6 = rc().channel(CH_6); // exit immediately if not using tuning function, or when radio failsafe is invoked, so tuning values are not set to zero if ((g.radio_tuning <= 0) || failsafe.radio || failsafe.radio_counter != 0 || rc6->get_radio_in() == 0) {