From 920868145c71a062e7799a7899ed1cea8d0b9138 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 29 Oct 2016 09:08:22 +1100 Subject: [PATCH] Copter: support for AP_Gripper --- ArduCopter/ArduCopter.cpp | 3 +++ ArduCopter/Copter.h | 4 ++++ ArduCopter/Parameters.cpp | 19 +++++++++++++------ ArduCopter/Parameters.h | 4 ++++ ArduCopter/config.h | 6 ++++++ ArduCopter/make.inc | 1 + ArduCopter/sensors.cpp | 8 ++++++++ ArduCopter/switches.cpp | 2 ++ ArduCopter/system.cpp | 5 +++++ ArduCopter/wscript | 1 + 10 files changed, 47 insertions(+), 6 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index ab7eecdf40..160ca2c75a 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -135,6 +135,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #if EPM_ENABLED == ENABLED SCHED_TASK(epm_update, 10, 75), #endif +#if GRIPPER_ENABLED == ENABLED + SCHED_TASK(gripper_update, 10, 75), +#endif #ifdef USERHOOK_FASTLOOP SCHED_TASK(userhook_FastLoop, 100, 75), #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 5d2a37c7e6..75356fa2b2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -104,6 +104,9 @@ #if EPM_ENABLED == ENABLED #include // EPM cargo gripper stuff #endif +#if GRIPPER_ENABLED == ENABLED +#include // gripper stuff +#endif #if PARACHUTE == ENABLED #include // Parachute release library #endif @@ -1041,6 +1044,7 @@ private: void read_battery(void); void read_receiver_rssi(void); void epm_update(); + void gripper_update(); void terrain_update(); void terrain_logging(); bool terrain_use(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index f49830113f..6e9ab0efa4 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -395,42 +395,42 @@ const AP_Param::Info Copter::var_info[] = { // @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: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, 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:Avoidance + // @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:Avoidance // @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: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, 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:Avoidance + // @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:Avoidance // @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: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, 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:Avoidance + // @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:Avoidance // @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: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, 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:Avoidance + // @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:Avoidance // @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: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, 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:Avoidance + // @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:Avoidance // @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: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, 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:Avoidance + // @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:Avoidance // @User: Standard GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), @@ -1039,6 +1039,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_Stats/AP_Stats.cpp AP_SUBGROUPINFO(stats, "STAT", 12, ParametersG2, AP_Stats), +#if GRIPPER_ENABLED == ENABLED + // @Group: GRIP_ + // @Path: ../libraries/AP_Gripper/AP_Gripper.cpp + AP_SUBGROUPINFO(gripper, "GRIP_", 13, ParametersG2, AP_Gripper), +#endif + + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4266ee6d42..ca3709a6b8 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -553,6 +553,10 @@ public: // vehicle statistics AP_Stats stats; +#if GRIPPER_ENABLED + AP_Gripper gripper; +#endif + // Throw mode parameters AP_Int8 throw_nextmode; AP_Int8 throw_type; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 6ab6bad26b..54285071c9 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -301,6 +301,12 @@ # define EPM_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// gripper +#ifndef GRIPPER_ENABLED + # define GRIPPER_ENABLED ENABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // Parachute release #ifndef PARACHUTE diff --git a/ArduCopter/make.inc b/ArduCopter/make.inc index e392b01c82..067bfa5d4d 100644 --- a/ArduCopter/make.inc +++ b/ArduCopter/make.inc @@ -63,3 +63,4 @@ LIBRARIES += AP_ADSB LIBRARIES += AP_Avoidance LIBRARIES += AP_Proximity LIBRARIES += AP_Stats +LIBRARIES += AP_Gripper diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index fe6779896c..80db51eeeb 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -241,6 +241,14 @@ void Copter::epm_update() } #endif +#if GRIPPER_ENABLED == ENABLED +// gripper update +void Copter::gripper_update() +{ + g2.gripper.update(); +} +#endif + /* update AP_Button */ diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 7360d49161..3beabbd5b2 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -383,10 +383,12 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_EPM: switch(ch_flag) { case AUX_SWITCH_LOW: + g2.gripper.release(); epm.release(); Log_Write_Event(DATA_EPM_RELEASE); break; case AUX_SWITCH_HIGH: + g2.gripper.grab(); epm.grab(); Log_Write_Event(DATA_EPM_GRAB); break; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 031aeca298..9a175a0072 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -133,6 +133,11 @@ void Copter::init_ardupilot() epm.init(); #endif + // init cargo gripper +#if GRIPPER_ENABLED == ENABLED + g2.gripper.init(); +#endif + // initialise notify system // disable external leds if epm is enabled because of pin conflict on the APM notify.init(true); diff --git a/ArduCopter/wscript b/ArduCopter/wscript index d09ac46219..37edbc65f0 100644 --- a/ArduCopter/wscript +++ b/ArduCopter/wscript @@ -32,6 +32,7 @@ def build(bld): 'AP_AdvancedFailsafe', 'AP_Proximity', 'AP_Stats', + 'AP_Gripper', ], )