From 2b5d8aa5b78ac54d65eef28715b05ed4bbab8d60 Mon Sep 17 00:00:00 2001 From: ctech4285 Date: Tue, 17 Dec 2013 13:58:11 +0900 Subject: [PATCH] Copter: integrate EPM library --- ArduCopter/ArduCopter.pde | 25 +++++++++++++++++++++++-- ArduCopter/Parameters.h | 5 ++++- ArduCopter/Parameters.pde | 12 +++++++++--- ArduCopter/config.h | 6 ++++++ ArduCopter/control_modes.pde | 21 ++++++++++++++++++++- ArduCopter/defines.h | 5 ++++- 6 files changed, 66 insertions(+), 8 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index fd1c2342d9..a019de1c36 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -132,6 +132,9 @@ #if SPRAYER == ENABLED #include // crop sprayer library #endif +#if EPM_ENABLED == ENABLED +#include // 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(); diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index f884a948ec..dade283a2c 100644 --- a/ArduCopter/Parameters.h +++ b/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; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index f76e8cda3e..c176cc16bf 100644 --- a/ArduCopter/Parameters.pde +++ b/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), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 706260667b..5220578d85 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -413,6 +413,12 @@ # define SPRAYER DISABLED #endif +////////////////////////////////////////////////////////////////////////////// +// EPM cargo gripper +#ifndef EPM_ENABLED + # define EPM_ENABLED DISABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 3e4f1bfe9f..fb93ee9796 100644 --- a/ArduCopter/control_modes.pde +++ b/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); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 9ed04b7930..490fc01c74 100644 --- a/ArduCopter/defines.h +++ b/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