diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 4913d411bf..9f70263a61 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -153,3 +153,9 @@ void set_using_interlock(bool b) } } +void set_motor_estop(bool b) +{ + if(ap.motor_estop != b) { + ap.motor_estop = b; + } +} \ No newline at end of file diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 95a0e6468f..cb79a6fe7c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -359,6 +359,7 @@ static union { enum HomeState home_state : 2; // 30,31 // home status (unset, set, locked) uint8_t motor_interlock : 1; // 32 // motor interlock status, final control for motors on/off uint8_t using_interlock : 1; // 33 // aux switch motor interlock function is in use + uint8_t motor_estop : 1; // 34 // motor estop switch, shuts off motors when enabled }; uint32_t value; } ap; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 507a34a3a0..a4e190aba4 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -379,42 +379,42 @@ const AP_Param::Info var_info[] PROGMEM = { // @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, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound, 31:E-Stop, 32:Motor Interlock // @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, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound, 31:E-Stop, 32:Motor Interlock // @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, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound, 31:E-Stop, 32:Motor Interlock // @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, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound, 31:E-Stop, 32:Motor Interlock // @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, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound, 31:E-Stop, 32:Motor Interlock // @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, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 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, 29:Landing Gear, 30:Lost Copter Sound, 31:E-Stop, 32:Motor Interlock // @User: Standard GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 2088dd0333..e3d6217cf1 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -66,6 +66,7 @@ enum aux_sw_func { AUXSW_RELAY, // Relay pin on/off (only supports first relay) AUXSW_LANDING_GEAR, // Landing gear controller AUXSW_LOST_COPTER_SOUND, // Play lost copter sound + AUXSW_MOTOR_ESTOP, // Emergency Stop Switch AUXSW_MOTOR_INTERLOCK, // Motor On/Off switch }; diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index e6bb84275a..3a3ef600ae 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -167,6 +167,17 @@ static bool init_arm_motors(bool arming_from_gcs) return false; } + // if we are not E-Stop switch option, force Estop false to ensure motors + // can run normally + if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ + set_motor_estop(false); + // if we are using motor Estop switch, it must not be in Estop position + } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_estop){ + gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Emergency Stopped")); + AP_Notify::flags.armed = false; + return false; + } + // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); hal.util->set_soft_armed(true); @@ -228,6 +239,15 @@ static bool pre_arm_checks(bool display_failure) return false; } + // if we are using Motor E-Stop aux switch, check it is not enabled + // and warn if it is + if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_estop){ + if (display_failure) { + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Motor Emergency Stopped")); + } + return false; + } + // exit immediately if we've already successfully performed the pre-arm check if (ap.pre_arm_check) { // run gps checks because results may change and affect LED colour @@ -757,8 +777,11 @@ static void motors_output() // true means motors run, false motors don't run motors.set_interlock(ap.motor_interlock); } else { - // if not using interlock switch, force interlock true - motors.set_interlock(true); + // if not using interlock switch, set according to E-Stop status + // where E-Stop is forced false during arming if E-Stop switch + // is not used. Interlock enabled means motors run, so we must + // invert motor_estop status for motors to run. + motors.set_interlock(!ap.motor_estop); } motors.output(); } diff --git a/ArduCopter/switches.pde b/ArduCopter/switches.pde index b13d60c06c..99b8ca13e1 100644 --- a/ArduCopter/switches.pde +++ b/ArduCopter/switches.pde @@ -227,6 +227,8 @@ static void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) case AUXSW_ATTCON_ACCEL_LIM: case AUXSW_RELAY: case AUXSW_LANDING_GEAR: + case AUXSW_MOTOR_ESTOP: + case AUXSW_MOTOR_INTERLOCK: do_aux_switch_function(ch_option, ch_flag); break; } @@ -533,7 +535,12 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) landinggear.set_cmd_mode(LandingGear_Retract); break; } - break; + break; + + case AUXSW_MOTOR_ESTOP: + // Turn on E-Stop logic when channel is high + set_motor_estop(ch_flag == AUX_SWITCH_HIGH); + break; case AUXSW_LOST_COPTER_SOUND: switch (ch_flag) {