From 7c552d3545833adfa38074229354aa5307cfdfed Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Thu, 9 Mar 2017 15:53:32 -0500 Subject: [PATCH] Sub: Remove unsused aux switch code --- ArduSub/AP_State.cpp | 6 - ArduSub/ArduSub.cpp | 3 - ArduSub/Parameters.cpp | 44 ----- ArduSub/Parameters.h | 18 +- ArduSub/Sub.h | 10 +- ArduSub/config.h | 8 - ArduSub/defines.h | 55 ------ ArduSub/motors.cpp | 5 - ArduSub/radio.cpp | 38 ++++ ArduSub/switches.cpp | 384 ----------------------------------------- ArduSub/system.cpp | 8 - 11 files changed, 41 insertions(+), 538 deletions(-) delete mode 100644 ArduSub/switches.cpp diff --git a/ArduSub/AP_State.cpp b/ArduSub/AP_State.cpp index cf5f0f3253..dff8847c70 100644 --- a/ArduSub/AP_State.cpp +++ b/ArduSub/AP_State.cpp @@ -74,12 +74,6 @@ void Sub::set_pre_arm_check(bool b) } } -void Sub::update_using_interlock() -{ - // check if we are using motor interlock control on an aux switch - ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK); -} - void Sub::set_motor_emergency_stop(bool b) { if (ap.motor_emergency_stop != b) { diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index c10526ef9f..2b9e9912a3 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -31,7 +31,6 @@ const AP_Scheduler::Task Sub::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(auto_disarm_check, 10, 50), SCHED_TASK(auto_trim, 10, 75), SCHED_TASK(read_rangefinder, 20, 100), @@ -381,8 +380,6 @@ void Sub::one_hz_loop() // make it possible to change ahrs orientation at runtime during initial config ahrs.set_orientation(); - update_using_interlock(); - // set all throttle channel settings motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); } diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 031e4a9dba..a2f5ae837f 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -309,50 +309,6 @@ const AP_Param::Info Sub::var_info[] = { GSCALAR(radio_tuning_high, "TUNE_HIGH", 1000), #endif -#if AUXSW_ENABLED == ENABLED - // @Param: CH7_OPT - // @DisplayName: Channel 7 option - // @Description: Select which function if 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, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 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 vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw - // @User: Standard - GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING), - - // @Param: CH8_OPT - // @DisplayName: Channel 8 option - // @Description: Select which function if 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, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 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 vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw - // @User: Standard - GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING), - - // @Param: CH9_OPT - // @DisplayName: Channel 9 option - // @Description: Select which function if 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, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 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 vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw - // @User: Standard - GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING), - - // @Param: CH10_OPT - // @DisplayName: Channel 10 option - // @Description: Select which function if 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, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 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 vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw - // @User: Standard - GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING), - - // @Param: CH11_OPT - // @DisplayName: Channel 11 option - // @Description: Select which function if 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, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 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 vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw - // @User: Standard - GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING), - - // @Param: CH12_OPT - // @DisplayName: Channel 12 option - // @Description: Select which function if 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, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 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 vehicle Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw - // @User: Standard - GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), -#endif - // @Param: DISARM_DELAY // @DisplayName: Disarm delay // @Description: Delay before automatic disarm in seconds. A value of zero disables auto disarm. diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 0048b7d476..03cd8389ed 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -216,17 +216,8 @@ public: k_param_acro_balance_roll, k_param_acro_balance_pitch, - - // AUX switch options - k_param_ch7_option, // Disabled - k_param_ch8_option, // Disabled - k_param_ch9_option, // Disabled - k_param_ch10_option, // Disabled - k_param_ch11_option, // Disabled - k_param_ch12_option, // Disabled - // RPM Sensor - k_param_rpm_sensor, // Disabled + k_param_rpm_sensor = 232, // Disabled // RC_Mapper Library k_param_rcmap, // Disabled @@ -301,12 +292,7 @@ public: AP_Int16 radio_tuning_high; AP_Int16 radio_tuning_low; #endif - 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 fs_ekf_action; diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 655eaade52..9079033541 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -241,7 +241,7 @@ private: uint8_t compass_mot : 1; // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // true if we are currently performing the motors test uint8_t initialised : 1; // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes - uint8_t throttle_zero : 1; // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock + uint8_t throttle_zero : 1; // true if the throttle stick is at zero uint8_t system_time_set : 1; // true if the system time has been set from the GPS uint8_t gps_base_pos_set : 1; // true when the gps base position has been set (used for RTK gps only) enum HomeState home_state : 2; // home status (unset, set, locked) @@ -500,7 +500,6 @@ private: void set_simple_mode(uint8_t b); void set_failsafe_battery(bool b); void set_pre_arm_check(bool b); - void update_using_interlock(); void set_motor_emergency_stop(bool b); float get_smoothing_gain(); void get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max); @@ -744,13 +743,6 @@ private: void print_divider(void); void print_enabled(bool b); void report_version(); - bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check); - bool check_duplicate_auxsw(void); - uint8_t read_3pos_switch(int16_t radio_in); - 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); void save_trim(); void auto_trim(); void init_ardupilot(); diff --git a/ArduSub/config.h b/ArduSub/config.h index b5e507ca55..4aa466af78 100644 --- a/ArduSub/config.h +++ b/ArduSub/config.h @@ -79,14 +79,6 @@ # define CH6_TUNE_ENABLED DISABLED #endif -////////////////////////////////////////////////////////////////////////////// -// Aux Switches -// - -#ifndef AUXSW_ENABLED -# define AUXSW_ENABLED DISABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // RPM // diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 89a64cbcc4..96e4b388f1 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -29,61 +29,6 @@ enum autopilot_yaw_mode { AUTO_YAW_CORRECT_XTRACK = 6 // steer the sub in order to correct for crosstrack error during line following }; -// 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 -#define CH6_PWM_TRIGGER_HIGH 1800 -#define CH6_PWM_TRIGGER_LOW 1200 - -// 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 - - // No RTL mode for Sub - // 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, // changes yaw to be same as when quad was armed - 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 - - // No sprayer for Sub, remove - // AUXSW_SPRAYER = 15, // enable/disable the crop sprayer - - AUXSW_AUTO = 16, // change to auto flight mode - AUXSW_GRIPPER = 19, // Operate cargo grippers low=off, middle=neutral, high=on - - // No parachute for Sub, remove - // 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_LOST_VEHICLE_SOUND = 30, // Play lost vehicle 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) -}; - // HIL enumerations #define HIL_MODE_DISABLED 0 #define HIL_MODE_SENSORS 1 diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index f7b1137320..817491c7ee 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -181,11 +181,6 @@ void Sub::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_VEHICLE_SOUND)) { - return; - } - // ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch if (ap.throttle_zero && !motors.armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) { if (soundalarm_counter >= LOST_VEHICLE_DELAY) { diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index 5fed248700..d290145ba7 100644 --- a/ArduSub/radio.cpp +++ b/ArduSub/radio.cpp @@ -108,3 +108,41 @@ void Sub::set_throttle_zero_flag(int16_t throttle_control) } } +// save_trim - adds roll and pitch trims from the radio to ahrs +void Sub::save_trim() +{ + // save roll and pitch trim + float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f); + float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f); + ahrs.add_trim(roll_trim, pitch_trim); + Log_Write_Event(DATA_SAVE_TRIM); + gcs_send_text(MAV_SEVERITY_INFO, "Trim saved"); +} + +// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions +// meant to be called continuously while the pilot attempts to keep the vehicle level +void Sub::auto_trim() +{ + if (auto_trim_counter > 0) { + auto_trim_counter--; + + // flash the leds + AP_Notify::flags.save_trim = true; + + // calculate roll trim adjustment + float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f); + + // calculate pitch trim adjustment + float pitch_trim_adjustment = ToRad((float)channel_pitch->get_control_in() / 4000.0f); + + // add trim to ahrs object + // save to eeprom on last iteration + ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0)); + + // on last iteration restore leds and accel gains to normal + if (auto_trim_counter == 0) { + AP_Notify::flags.save_trim = false; + } + } +} + diff --git a/ArduSub/switches.cpp b/ArduSub/switches.cpp deleted file mode 100644 index 150a0f7484..0000000000 --- a/ArduSub/switches.cpp +++ /dev/null @@ -1,384 +0,0 @@ -#include "Sub.h" - -#define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200 - -//Documentation of Aux Switch Flags: -static union { - 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 - }; - uint32_t value; -} aux_con; - -// check_if_auxsw_mode_used - Check to see if any of the Aux Switches are set to a given mode. -bool Sub::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 Sub::check_duplicate_auxsw(void) -{ - bool ret = ((g.ch7_option != AUXSW_DO_NOTHING) && (g.ch7_option == g.ch8_option || - g.ch7_option == g.ch9_option || g.ch7_option == g.ch10_option || - g.ch7_option == g.ch11_option || g.ch7_option == g.ch12_option)); - - ret = ret || ((g.ch8_option != AUXSW_DO_NOTHING) && (g.ch8_option == g.ch9_option || - g.ch8_option == g.ch10_option || g.ch8_option == g.ch11_option || - g.ch8_option == g.ch12_option)); - - ret = ret || ((g.ch9_option != AUXSW_DO_NOTHING) && (g.ch9_option == g.ch10_option || - g.ch9_option == g.ch11_option || g.ch9_option == g.ch12_option)); - - ret = ret || ((g.ch10_option != AUXSW_DO_NOTHING) && (g.ch10_option == g.ch11_option || - g.ch10_option == g.ch12_option)); - - ret = ret || ((g.ch11_option != AUXSW_DO_NOTHING) && (g.ch11_option == g.ch12_option)); - - return ret; -} - -// read_3pos_switch -uint8_t Sub::read_3pos_switch(int16_t 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 (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 Sub::read_aux_switches() -{ - uint8_t switch_position; - - // exit immediately during radio failsafe - if (failsafe.manual_control) { - 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); - -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN - read_aux_switch(CH_12, aux_con.CH12_flag, g.ch12_option); -#endif -} - -#undef read_aux_switch - -// init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so -void Sub::init_aux_switches() -{ - // 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); -} - -// init_aux_switch_function - initialize aux functions -void Sub::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) -{ - // init channel options - switch (ch_option) { - case AUXSW_SIMPLE_MODE: - case AUXSW_RANGEFINDER: - case AUXSW_FENCE: - case AUXSW_RESETTOARMEDYAW: - case AUXSW_SUPERSIMPLE_MODE: - case AUXSW_ACRO_TRAINER: - case AUXSW_GRIPPER: - case AUXSW_RETRACT_MOUNT: - case AUXSW_MISSION_RESET: - case AUXSW_ATTCON_FEEDFWD: - case AUXSW_ATTCON_ACCEL_LIM: - case AUXSW_RELAY: - case AUXSW_MOTOR_ESTOP: - case AUXSW_MOTOR_INTERLOCK: - do_aux_switch_function(ch_option, ch_flag); - break; - } -} - -// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch -void Sub::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) -{ - - switch (ch_function) { - - case AUXSW_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); - break; - - case AUXSW_SUPERSIMPLE_MODE: - // low = simple mode off, middle = simple mode, high = super simple mode - set_simple_mode(ch_flag); - break; - - case AUXSW_SAVE_TRIM: - if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) { - save_trim(); - } - break; - - case AUXSW_SAVE_WP: - // save waypoint when switch is brought high - if (ch_flag == AUX_SWITCH_HIGH) { - - // do not allow saving new waypoints while we're in auto or disarmed - if (control_mode == AUTO || !motors.armed()) { - return; - } - - // do not allow saving the first waypoint with zero throttle - if ((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)) { - return; - } - - // create new mission command - AP_Mission::Mission_Command cmd = {}; - - // set new waypoint to current location - cmd.content.location = current_loc; - - // if throttle is above zero, create waypoint command - if (channel_throttle->get_control_in() > 0) { - cmd.id = MAV_CMD_NAV_WAYPOINT; - } else { - // with zero throttle, create LAND command - cmd.id = MAV_CMD_NAV_LAND; - } - - // save command - if (mission.add_cmd(cmd)) { - // log event - Log_Write_Event(DATA_SAVEWP_ADD_WP); - } - } - break; - -#if CAMERA == ENABLED - case AUXSW_CAMERA_TRIGGER: - if (ch_flag == AUX_SWITCH_HIGH) { - do_take_picture(); - } - break; -#endif - - case AUXSW_RANGEFINDER: - // enable or disable the sonar -#if RANGEFINDER_ENABLED == ENABLED - if ((ch_flag == AUX_SWITCH_HIGH) && rangefinder.has_orientation(ROTATION_PITCH_270)) { - rangefinder_state.enabled = true; - } else { - rangefinder_state.enabled = false; - } -#endif - break; - -#if AC_FENCE == ENABLED - case AUXSW_FENCE: - // enable or disable the fence - if (ch_flag == AUX_SWITCH_HIGH) { - fence.enable(true); - Log_Write_Event(DATA_FENCE_ENABLE); - } else { - fence.enable(false); - Log_Write_Event(DATA_FENCE_DISABLE); - } - break; -#endif - // To-Do: add back support for this feature - //case AUXSW_RESETTOARMEDYAW: - // if (ch_flag == AUX_SWITCH_HIGH) { - // set_yaw_mode(YAW_RESETTOARMEDYAW); - // }else{ - // set_yaw_mode(YAW_HOLD); - // } - // break; - - case AUXSW_ACRO_TRAINER: - switch (ch_flag) { - case AUX_SWITCH_LOW: - g.acro_trainer = ACRO_TRAINER_DISABLED; - 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); - break; - case AUX_SWITCH_HIGH: - g.acro_trainer = ACRO_TRAINER_LIMITED; - Log_Write_Event(DATA_ACRO_TRAINER_LIMITED); - break; - } - break; -#if GRIPPER_ENABLED == ENABLED - case AUXSW_GRIPPER: - switch (ch_flag) { - case AUX_SWITCH_LOW: - g2.gripper.release(); - Log_Write_Event(DATA_GRIPPER_RELEASE); - break; - case AUX_SWITCH_HIGH: - g2.gripper.grab(); - Log_Write_Event(DATA_GRIPPER_GRAB); - break; - } - break; -#endif - - case AUXSW_AUTO: - 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(); - } - } - break; - - case AUXSW_MISSION_RESET: - if (ch_flag == AUX_SWITCH_HIGH) { - mission.reset(); - } - break; - - case AUXSW_ATTCON_FEEDFWD: - // enable or disable feed forward - attitude_control.bf_feedforward(ch_flag == AUX_SWITCH_HIGH); - break; - - case AUXSW_ATTCON_ACCEL_LIM: - // enable or disable accel limiting by restoring defaults - attitude_control.accel_limiting(ch_flag == AUX_SWITCH_HIGH); - break; - -#if MOUNT == ENABLE - case AUXSW_RETRACT_MOUNT: - switch (ch_flag) { - case AUX_SWITCH_HIGH: - camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT); - break; - case AUX_SWITCH_LOW: - camera_mount.set_mode_to_default(); - break; - } - break; -#endif - - case AUXSW_RELAY: - ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH); - break; - - case AUXSW_LOST_VEHICLE_SOUND: - switch (ch_flag) { - case AUX_SWITCH_HIGH: - AP_Notify::flags.vehicle_lost = true; - break; - case AUX_SWITCH_LOW: - AP_Notify::flags.vehicle_lost = false; - break; - } - break; - - case AUXSW_MOTOR_ESTOP: - // Turn on Emergency Stop logic when channel is high - set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH); - break; - - case AUXSW_MOTOR_INTERLOCK: - // Turn on when above LOW, because channel will also be used for speed - // control signal in tradheli - motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); - - // Log new status - if (motors.get_interlock()) { - Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED); - } else { - Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED); - } - break; - } -} - -// save_trim - adds roll and pitch trims from the radio to ahrs -void Sub::save_trim() -{ - // save roll and pitch trim - float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f); - float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f); - ahrs.add_trim(roll_trim, pitch_trim); - Log_Write_Event(DATA_SAVE_TRIM); - gcs_send_text(MAV_SEVERITY_INFO, "Trim saved"); -} - -// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions -// meant to be called continuously while the pilot attempts to keep the vehicle level -void Sub::auto_trim() -{ - if (auto_trim_counter > 0) { - auto_trim_counter--; - - // flash the leds - AP_Notify::flags.save_trim = true; - - // calculate roll trim adjustment - float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f); - - // calculate pitch trim adjustment - float pitch_trim_adjustment = ToRad((float)channel_pitch->get_control_in() / 4000.0f); - - // add trim to ahrs object - // save to eeprom on last iteration - ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0)); - - // on last iteration restore leds and accel gains to normal - if (auto_trim_counter == 0) { - AP_Notify::flags.save_trim = false; - } - } -} - diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index a4b9cb63b3..9234c4556f 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -149,9 +149,6 @@ void Sub::init_ardupilot() gcs().set_dataflash(&DataFlash); - // update motor interlock state - update_using_interlock(); - init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up motors and output to escs init_joystick(); // joystick initialization @@ -275,11 +272,6 @@ void Sub::init_ardupilot() // initialise mission library mission.init(); - // initialise the flight mode and aux switch - // --------------------------- - // reset_control_switch(); - init_aux_switches(); - startup_INS_ground(); // we don't want writes to the serial port to cause us to pause