Copter: integrate EPM library

ArduCopter/ArduCopter.pde

@@ -132,6 +132,9 @@
 #if SPRAYER == ENABLED
 #include <AC_Sprayer.h>         // crop sprayer library
 #endif
+#if EPM_ENABLED == ENABLED
+#include <AP_EPM.h>				// EPM cargo gripper stuff
+#endif
 
 // AP_HAL to Arduino compatibility layer
 #include "compat.h"
@@ -829,6 +832,13 @@ AC_Fence    fence(&inertial_nav);
 static AC_Sprayer sprayer(&inertial_nav);
 #endif
 
+////////////////////////////////////////////////////////////////////////////////
+// EPM Cargo Griper
+////////////////////////////////////////////////////////////////////////////////
+#if EPM_ENABLED == ENABLED
+static AP_EPM epm;
+#endif
+
 ////////////////////////////////////////////////////////////////////////////////
 // function definitions to keep compiler from complaining about undeclared functions
 ////////////////////////////////////////////////////////////////////////////////
@@ -898,6 +908,9 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
 
 
 void setup() {
+
+    bool enable_external_leds = true;
+
     // this needs to be the first call, as it fills memory with
     // sentinel values
     memcheck_init();
@@ -906,12 +919,19 @@ void setup() {
     // Load the default values of variables listed in var_info[]s
     AP_Param::setup_sketch_defaults();
 
+    // init EPM cargo gripper
+#if EPM_ENABLED == ENABLED
+    epm.init();
+    enable_external_leds = !epm.enabled();
+#endif
+
     // initialise notify system
-    notify.init(true);
+    // disable external leds if epm is enabled because of pin conflict on the APM
+    notify.init(enable_external_leds);
 
     // initialise battery monitor
     battery.init();
-
+    
 #if CONFIG_SONAR == ENABLED
  #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
     sonar_analog_source = new AP_ADC_AnalogSource(
@@ -1006,6 +1026,7 @@ void loop()
 // Main loop - 100hz
 static void fast_loop()
 {
+
     // IMU DCM Algorithm
     // --------------------
     read_AHRS();

ArduCopter/Parameters.h

@@ -67,6 +67,9 @@ public:
         // relay object
         k_param_relay,
 
+        // EPM object
+        k_param_epm,
+
         // Misc
         //
         k_param_log_bitmask = 20,
@@ -313,7 +316,7 @@ public:
     AP_Int8         wp_yaw_behavior;            // controls how the autopilot controls yaw during missions
     AP_Int16        angle_max;                  // maximum lean angle of the copter in centi-degrees
     AP_Int32        angle_rate_max;             // maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
-
+    
     // Waypoints
     //
     AP_Int8         command_total;

ArduCopter/Parameters.pde

@@ -394,14 +394,14 @@ 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:Sonar, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land
+    // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM
     // @User: Standard
     GSCALAR(ch7_option, "CH7_OPT",                  CH7_OPTION),
 
     // @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:Sonar, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land
+    // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM
     // @User: Standard
     GSCALAR(ch8_option, "CH8_OPT",                  CH8_OPTION),
 
@@ -425,7 +425,7 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Range 90000 250000
     // @User: Advanced
     GSCALAR(angle_rate_max, "ANGLE_RATE_MAX",  ANGLE_RATE_MAX),
-
+    
 #if FRAME_CONFIG ==     HELI_FRAME
     // @Group: HS1_
     // @Path: ../libraries/RC_Channel/RC_Channel.cpp
@@ -981,6 +981,12 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @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
+
     // @Group: COMPASS_
     // @Path: ../libraries/AP_Compass/Compass.cpp
     GOBJECT(compass,        "COMPASS_", Compass),

ArduCopter/config.h

@@ -413,6 +413,12 @@
  # define SPRAYER  DISABLED
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+//	EPM cargo gripper
+#ifndef EPM_ENABLED
+ # define EPM_ENABLED DISABLED
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // RADIO CONFIGURATION
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/control_modes.pde

@@ -107,6 +107,7 @@ static void init_aux_switches()
         case AUX_SWITCH_RESETTOARMEDYAW:
         case AUX_SWITCH_SUPERSIMPLE_MODE:
         case AUX_SWITCH_ACRO_TRAINER:
+        case AUX_SWITCH_EPM:
         case AUX_SWITCH_SPRAYER:
             do_aux_switch_function(g.ch7_option, ap.CH7_flag);
             break;
@@ -119,6 +120,7 @@ static void init_aux_switches()
         case AUX_SWITCH_RESETTOARMEDYAW:
         case AUX_SWITCH_SUPERSIMPLE_MODE:
         case AUX_SWITCH_ACRO_TRAINER:
+        case AUX_SWITCH_EPM:
         case AUX_SWITCH_SPRAYER:
             do_aux_switch_function(g.ch8_option, ap.CH8_flag);
             break;
@@ -298,7 +300,24 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
                     break;
             }
             break;
-
+#if EPM_ENABLED == ENABLED
+        case AUX_SWITCH_EPM:
+            switch(ch_flag) {
+                case AUX_SWITCH_LOW:
+                    epm.off();
+                    Log_Write_Event(DATA_EPM_OFF);
+                    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);
+                    break;
+            }
+            break;
+#endif
 #if SPRAYER == ENABLED
         case AUX_SWITCH_SPRAYER:
             sprayer.enable(ch_flag == AUX_SWITCH_HIGH);

ArduCopter/defines.h

@@ -72,6 +72,7 @@
 #define AUX_SWITCH_AUTO             16      // change to auto flight mode
 #define AUX_SWITCH_AUTOTUNE         17      // auto tune
 #define AUX_SWITCH_LAND             18      // change to LAND flight mode
+#define AUX_SWITCH_EPM              19      // Operate the EPM cargo gripper low=off, middle=neutral, high=on
 
 // values used by the ap.ch7_opt and ap.ch8_opt flags
 #define AUX_SWITCH_LOW              0       // indicates auxiliar switch is in the low position (pwm <1200)
@@ -319,7 +320,9 @@
 #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_NEUTRAL                48
 // RADIANS
 #define RADX100 0.000174532925f
 #define DEGX100 5729.57795f