From 14d80910ec737bf0dbd7093edd726d7a56fc7e64 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 15 Sep 2014 15:52:21 +0900
Subject: [PATCH] Copter: integrate EPM ver2

---
 ArduCopter/ArduCopter.pde | 6 ++++++
 ArduCopter/defines.h      | 4 ++--
 ArduCopter/sensors.pde    | 8 ++++++++
 ArduCopter/switches.pde   | 8 ++++----
 ArduCopter/system.pde     | 5 +----
 5 files changed, 21 insertions(+), 10 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 964649be15..17ac08e5f1 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -823,6 +823,9 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
 #if FRSKY_TELEM_ENABLED == ENABLED
     { telemetry_send,       80,     10 },	
 #endif
+#if EPM_ENABLED == ENABLED
+    { epm_update,           40,     10 },
+#endif
 #ifdef USERHOOK_FASTLOOP
     { userhook_FastLoop,     4,     10 },
 #endif
@@ -888,6 +891,9 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
 #if FRSKY_TELEM_ENABLED == ENABLED
     { telemetry_send,       20,     100 },	
 #endif
+#if EPM_ENABLED == ENABLED
+    { epm_update,           10,      20 },
+#endif
 #ifdef USERHOOK_FASTLOOP
     { userhook_FastLoop,     1,    100  },
 #endif
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index e3bd52b6b1..64bdef433a 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -292,8 +292,8 @@ enum FlipState {
 #define DATA_ACRO_TRAINER_DISABLED      43
 #define DATA_ACRO_TRAINER_LEVELING      44
 #define DATA_ACRO_TRAINER_LIMITED       45
-#define DATA_EPM_ON                     46
-#define DATA_EPM_OFF                    47
+#define DATA_EPM_GRAB                   46
+#define DATA_EPM_RELEASE                47
 #define DATA_EPM_NEUTRAL                48
 #define DATA_PARACHUTE_DISABLED         49
 #define DATA_PARACHUTE_ENABLED          50
diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde
index 03aea80782..10428af05a 100644
--- a/ArduCopter/sensors.pde
+++ b/ArduCopter/sensors.pde
@@ -133,3 +133,11 @@ void read_receiver_rssi(void)
         receiver_rssi = constrain_int16(ret, 0, 255);
     }
 }
+
+#if EPM_ENABLED == ENABLED
+// epm update - moves epm pwm output back to neutral after grab or release is completed
+void epm_update()
+{
+    epm.update();
+}
+#endif
diff --git a/ArduCopter/switches.pde b/ArduCopter/switches.pde
index 7795743a35..662d4c2b2c 100644
--- a/ArduCopter/switches.pde
+++ b/ArduCopter/switches.pde
@@ -310,16 +310,16 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
         case AUX_SWITCH_EPM:
             switch(ch_flag) {
                 case AUX_SWITCH_LOW:
-                    epm.off();
-                    Log_Write_Event(DATA_EPM_OFF);
+                    epm.release();
+                    Log_Write_Event(DATA_EPM_RELEASE);
                     break;
                 case AUX_SWITCH_MIDDLE:
                     epm.neutral();
                     Log_Write_Event(DATA_EPM_NEUTRAL);
                     break;
                 case AUX_SWITCH_HIGH:
-                    epm.on();
-                    Log_Write_Event(DATA_EPM_ON);
+                    epm.grab();
+                    Log_Write_Event(DATA_EPM_GRAB);
                     break;
             }
             break;
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index e0fa98560d..317bb91a71 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -131,17 +131,14 @@ static void init_ardupilot()
 
     BoardConfig.init();
 
-    bool enable_external_leds = true;
-
     // init EPM cargo gripper
 #if EPM_ENABLED == ENABLED
     epm.init();
-    enable_external_leds = !epm.enabled();
 #endif
 
     // initialise notify system
     // disable external leds if epm is enabled because of pin conflict on the APM
-    notify.init(enable_external_leds);
+    notify.init(true);
 
     // initialise battery monitor
     battery.init();