From 236b8ab6b7bd5fb17b2a20d4191d6709f27db5a8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 29 Oct 2016 22:01:29 +1100 Subject: [PATCH] Copter: make EPM a subclass of AP_Gripper_Backend --- ArduCopter/APM_Config.h | 1 - ArduCopter/ArduCopter.cpp | 3 --- ArduCopter/Copter.h | 10 +--------- ArduCopter/GCS_Mavlink.cpp | 8 ++++---- ArduCopter/Parameters.cpp | 6 ------ ArduCopter/Parameters.h | 4 ++-- ArduCopter/commands_logic.cpp | 16 ++++++++-------- ArduCopter/config.h | 6 ------ ArduCopter/defines.h | 6 +++--- ArduCopter/make.inc | 1 - ArduCopter/sensors.cpp | 8 -------- ArduCopter/switches.cpp | 12 +++++------- ArduCopter/system.cpp | 5 ----- ArduCopter/wscript | 1 - 14 files changed, 23 insertions(+), 64 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 22efb269e9..6f431ba7fb 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -28,7 +28,6 @@ //#define AC_RALLY DISABLED // disable rally points library (must also disable terrain which relies on rally) //#define AC_TERRAIN DISABLED // disable terrain library //#define PARACHUTE DISABLED // disable parachute release to save 1k of flash -//#define EPM_ENABLED DISABLED // disable epm cargo gripper to save 500bytes of flash //#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space //#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands //#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 160ca2c75a..4cdc3525af 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -132,9 +132,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(afs_fs_check, 10, 100), #endif SCHED_TASK(terrain_update, 10, 100), -#if EPM_ENABLED == ENABLED - SCHED_TASK(epm_update, 10, 75), -#endif #if GRIPPER_ENABLED == ENABLED SCHED_TASK(gripper_update, 10, 75), #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 75356fa2b2..efa65a675e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -101,9 +101,6 @@ #if SPRAYER == ENABLED #include // crop sprayer library #endif -#if EPM_ENABLED == ENABLED -#include // EPM cargo gripper stuff -#endif #if GRIPPER_ENABLED == ENABLED #include // gripper stuff #endif @@ -568,11 +565,6 @@ private: AC_Sprayer sprayer; #endif - // EPM Cargo Griper -#if EPM_ENABLED == ENABLED - AP_EPM epm; -#endif - // Parachute release #if PARACHUTE == ENABLED AP_Parachute parachute; @@ -1122,7 +1114,7 @@ private: #if PARACHUTE == ENABLED void do_parachute(const AP_Mission::Mission_Command& cmd); #endif -#if EPM_ENABLED == ENABLED +#if GRIPPER_ENABLED == ENABLED void do_gripper(const AP_Mission::Mission_Command& cmd); #endif bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 18bdf0bc8f..4c334ccc88 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1377,20 +1377,20 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) result = copter.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4); break; -#if EPM_ENABLED == ENABLED +#if GRIPPER_ENABLED == ENABLED case MAV_CMD_DO_GRIPPER: // param1 : gripper number (ignored) // param2 : action (0=release, 1=grab). See GRIPPER_ACTIONS enum. - if(!copter.epm.enabled()) { + if(!copter.g2.gripper.enabled()) { result = MAV_RESULT_FAILED; } else { result = MAV_RESULT_ACCEPTED; switch ((uint8_t)packet.param2) { case GRIPPER_ACTION_RELEASE: - copter.epm.release(); + copter.g2.gripper.release(); break; case GRIPPER_ACTION_GRAB: - copter.epm.grab(); + copter.g2.gripper.grab(); break; default: result = MAV_RESULT_FAILED; diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 6e9ab0efa4..f92eb112ca 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -707,12 +707,6 @@ const AP_Param::Info Copter::var_info[] = { // @Path: ../libraries/AP_Relay/AP_Relay.cpp GOBJECT(relay, "RELAY_", AP_Relay), -#if EPM_ENABLED == ENABLED - // @Group: EPM_ - // @Path: ../libraries/AP_EPM/AP_EPM.cpp - GOBJECT(epm, "EPM_", AP_EPM), -#endif - #if PARACHUTE == ENABLED // @Group: CHUTE_ // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index ca3709a6b8..b82b5c7a15 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -67,8 +67,8 @@ public: // relay object k_param_relay, - // EPM object - k_param_epm, + // (old) EPM object + k_param_epm_unused, // BoardConfig object k_param_BoardConfig, diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index a329be9f81..281f61119c 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -132,8 +132,8 @@ bool Copter::start_command(const AP_Mission::Mission_Command& cmd) break; #endif -#if EPM_ENABLED == ENABLED - case MAV_CMD_DO_GRIPPER: // Mission command to control EPM gripper +#if GRIPPER_ENABLED == ENABLED + case MAV_CMD_DO_GRIPPER: // Mission command to control gripper do_gripper(cmd); break; #endif @@ -571,19 +571,19 @@ void Copter::do_parachute(const AP_Mission::Mission_Command& cmd) } #endif -#if EPM_ENABLED == ENABLED -// do_gripper - control EPM gripper +#if GRIPPER_ENABLED == ENABLED +// do_gripper - control gripper void Copter::do_gripper(const AP_Mission::Mission_Command& cmd) { // Note: we ignore the gripper num parameter because we only support one gripper switch (cmd.content.gripper.action) { case GRIPPER_ACTION_RELEASE: - epm.release(); - Log_Write_Event(DATA_EPM_RELEASE); + g2.gripper.release(); + Log_Write_Event(DATA_GRIPPER_RELEASE); break; case GRIPPER_ACTION_GRAB: - epm.grab(); - Log_Write_Event(DATA_EPM_GRAB); + g2.gripper.grab(); + Log_Write_Event(DATA_GRIPPER_GRAB); break; default: // do nothing diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 54285071c9..fe1065c5a8 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -295,12 +295,6 @@ # define PRECISION_LANDING ENABLED #endif -////////////////////////////////////////////////////////////////////////////// -// EPM cargo gripper -#ifndef EPM_ENABLED - # define EPM_ENABLED ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // gripper #ifndef GRIPPER_ENABLED diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 0937ab65ba..f9c62ffcbd 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -49,7 +49,7 @@ enum aux_sw_func { AUXSW_AUTO = 16, // change to auto flight mode AUXSW_AUTOTUNE = 17, // auto tune AUXSW_LAND = 18, // change to LAND flight mode - AUXSW_EPM = 19, // Operate the EPM cargo gripper low=off, middle=neutral, high=on + 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 @@ -361,8 +361,8 @@ enum DevOptions { #define DATA_ACRO_TRAINER_DISABLED 43 #define DATA_ACRO_TRAINER_LEVELING 44 #define DATA_ACRO_TRAINER_LIMITED 45 -#define DATA_EPM_GRAB 46 -#define DATA_EPM_RELEASE 47 +#define DATA_GRIPPER_GRAB 46 +#define DATA_GRIPPER_RELEASE 47 #define DATA_PARACHUTE_DISABLED 49 #define DATA_PARACHUTE_ENABLED 50 #define DATA_PARACHUTE_RELEASED 51 diff --git a/ArduCopter/make.inc b/ArduCopter/make.inc index 067bfa5d4d..38e3cfc87f 100644 --- a/ArduCopter/make.inc +++ b/ArduCopter/make.inc @@ -51,7 +51,6 @@ LIBRARIES += AP_BattMonitor LIBRARIES += AP_BoardConfig LIBRARIES += AP_Frsky_Telem LIBRARIES += AC_Sprayer -LIBRARIES += AP_EPM LIBRARIES += AP_Parachute LIBRARIES += AP_LandingGear LIBRARIES += AP_Terrain diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 80db51eeeb..424a74f191 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -233,14 +233,6 @@ void Copter::accel_cal_update() #endif } -#if EPM_ENABLED == ENABLED -// epm update - moves epm pwm output back to neutral after grab or release is completed -void Copter::epm_update() -{ - epm.update(); -} -#endif - #if GRIPPER_ENABLED == ENABLED // gripper update void Copter::gripper_update() diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index 3beabbd5b2..11e84290b6 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -221,7 +221,7 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) case AUXSW_FENCE: case AUXSW_SUPERSIMPLE_MODE: case AUXSW_ACRO_TRAINER: - case AUXSW_EPM: + 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 @@ -379,18 +379,16 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; } break; -#if EPM_ENABLED == ENABLED - case AUXSW_EPM: +#if GRIPPER_ENABLED == ENABLED + case AUXSW_GRIPPER: switch(ch_flag) { case AUX_SWITCH_LOW: g2.gripper.release(); - epm.release(); - Log_Write_Event(DATA_EPM_RELEASE); + Log_Write_Event(DATA_GRIPPER_RELEASE); break; case AUX_SWITCH_HIGH: g2.gripper.grab(); - epm.grab(); - Log_Write_Event(DATA_EPM_GRAB); + Log_Write_Event(DATA_GRIPPER_GRAB); break; } break; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 9a175a0072..2fc4d82300 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -128,11 +128,6 @@ void Copter::init_ardupilot() BoardConfig.init(); - // init EPM cargo gripper -#if EPM_ENABLED == ENABLED - epm.init(); -#endif - // init cargo gripper #if GRIPPER_ENABLED == ENABLED g2.gripper.init(); diff --git a/ArduCopter/wscript b/ArduCopter/wscript index 37edbc65f0..2011823140 100644 --- a/ArduCopter/wscript +++ b/ArduCopter/wscript @@ -17,7 +17,6 @@ def build(bld): 'AC_Sprayer', 'AC_WPNav', 'AP_Camera', - 'AP_EPM', 'AP_Frsky_Telem', 'AP_IRLock', 'AP_InertialNav',