From 97cd3614ebbef89f3581e9973ae5398cd3417fb9 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 10 Mar 2015 16:32:25 -0400 Subject: [PATCH] Copter: Change Aux Switch function list to enum. --- ArduCopter/ArduCopter.pde | 34 ++++++++++++++ ArduCopter/config.h | 12 ++--- ArduCopter/control_flip.pde | 2 +- ArduCopter/defines.h | 31 ------------- ArduCopter/landing_gear.pde | 2 +- ArduCopter/switches.pde | 92 ++++++++++++++++++------------------- 6 files changed, 88 insertions(+), 85 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f94c13e037..1f55445830 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -361,6 +361,40 @@ static union { uint32_t value; } ap; +// Aux Switch enumeration +enum aux_sw_func { + AUXSW_DO_NOTHING = 0, // aux switch disabled + AUXSW_SET_HOVER, // deprecated + AUXSW_FLIP, // flip + AUXSW_SIMPLE_MODE, // change to simple mode + AUXSW_RTL, // change to RTL flight mode + AUXSW_SAVE_TRIM, // save current position as level + AUXSW_ADC_FILTER, // deprecated + AUXSW_SAVE_WP, // save mission waypoint or RTL if in auto mode + AUXSW_MULTI_MODE, // depending upon CH6 position Flip (if ch6 is low), RTL (if ch6 in middle) or Save WP (if ch6 is high) + AUXSW_CAMERA_TRIGGER, // trigger camera servo or relay + AUXSW_SONAR, // allow enabling or disabling sonar in flight which helps avoid surface tracking when you are far above the ground + AUXSW_FENCE, // allow enabling or disabling fence in flight + AUXSW_RESETTOARMEDYAW, // changes yaw to be same as when quad was armed + AUXSW_SUPERSIMPLE_MODE, // change to simple mode in middle, super simple at top + AUXSW_ACRO_TRAINER, // low = disabled, middle = leveled, high = leveled and limited + AUXSW_SPRAYER, // enable/disable the crop sprayer + AUXSW_AUTO, // change to auto flight mode + AUXSW_AUTOTUNE, // auto tune + AUXSW_LAND, // change to LAND flight mode + AUXSW_EPM, // Operate the EPM cargo gripper low=off, middle=neutral, high=on + AUXSW_EKF, // Deprecated + AUXSW_PARACHUTE_ENABLE, // Parachute enable/disable + AUXSW_PARACHUTE_RELEASE, // Parachute release + AUXSW_PARACHUTE_3POS, // Parachute disable, enable, release with 3 position switch + AUXSW_MISSION_RESET, // Reset auto mission to start from first command + AUXSW_ATTCON_FEEDFWD, // enable/disable the roll and pitch rate feed forward + AUXSW_ATTCON_ACCEL_LIM, // enable/disable the roll, pitch and yaw accel limiting + AUXSW_RETRACT_MOUNT, // Retract Mount + AUXSW_RELAY, // Relay pin on/off (only supports first relay) + AUXSW_LANDING_GEAR, // Landing gear controller +}; + //////////////////////////////////////////////////////////////////////////////// // Radio //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 63ae9565ca..c88a042e19 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -184,27 +184,27 @@ // #ifndef CH7_OPTION - # define CH7_OPTION AUX_SWITCH_DO_NOTHING + # define CH7_OPTION AUXSW_DO_NOTHING #endif #ifndef CH8_OPTION - # define CH8_OPTION AUX_SWITCH_DO_NOTHING + # define CH8_OPTION AUXSW_DO_NOTHING #endif #ifndef CH9_OPTION - # define CH9_OPTION AUX_SWITCH_DO_NOTHING + # define CH9_OPTION AUXSW_DO_NOTHING #endif #ifndef CH10_OPTION - # define CH10_OPTION AUX_SWITCH_DO_NOTHING + # define CH10_OPTION AUXSW_DO_NOTHING #endif #ifndef CH11_OPTION - # define CH11_OPTION AUX_SWITCH_DO_NOTHING + # define CH11_OPTION AUXSW_DO_NOTHING #endif #ifndef CH12_OPTION - # define CH12_OPTION AUX_SWITCH_DO_NOTHING + # define CH12_OPTION AUXSW_DO_NOTHING #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/control_flip.pde b/ArduCopter/control_flip.pde index 6e76ffe6d4..949790f43d 100644 --- a/ArduCopter/control_flip.pde +++ b/ArduCopter/control_flip.pde @@ -6,7 +6,7 @@ * Adapted and updated for AC2 in 2011 by Jason Short * * Controls: - * CH7_OPT - CH12_OPT parameter must be set to "Flip" (AUX_SWITCH_FLIP) which is "2" + * CH7_OPT - CH12_OPT parameter must be set to "Flip" (AUXSW_FLIP) which is "2" * Pilot switches to Stabilize, Acro or AltHold flight mode and puts ch7/ch8 switch to ON position * Vehicle will Roll right by default but if roll or pitch stick is held slightly left, forward or back it will flip in that direction * Vehicle should complete the roll within 2.5sec and will then return to the original flight mode it was in before flip was triggered diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 2da75135c2..a50cc2ff6a 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -28,37 +28,6 @@ #define CH6_PWM_TRIGGER_HIGH 1800 #define CH6_PWM_TRIGGER_LOW 1200 -#define AUX_SWITCH_DO_NOTHING 0 // aux switch disabled -#define AUX_SWITCH_SET_HOVER 1 // deprecated -#define AUX_SWITCH_FLIP 2 // flip -#define AUX_SWITCH_SIMPLE_MODE 3 // change to simple mode -#define AUX_SWITCH_RTL 4 // change to RTL flight mode -#define AUX_SWITCH_SAVE_TRIM 5 // save current position as level -#define AUX_SWITCH_ADC_FILTER 6 // deprecated -#define AUX_SWITCH_SAVE_WP 7 // save mission waypoint or RTL if in auto mode -#define AUX_SWITCH_MULTI_MODE 8 // depending upon CH6 position Flip (if ch6 is low), RTL (if ch6 in middle) or Save WP (if ch6 is high) -#define AUX_SWITCH_CAMERA_TRIGGER 9 // trigger camera servo or relay -#define AUX_SWITCH_SONAR 10 // allow enabling or disabling sonar in flight which helps avoid surface tracking when you are far above the ground -#define AUX_SWITCH_FENCE 11 // allow enabling or disabling fence in flight -#define AUX_SWITCH_RESETTOARMEDYAW 12 // changes yaw to be same as when quad was armed -#define AUX_SWITCH_SUPERSIMPLE_MODE 13 // change to simple mode in middle, super simple at top -#define AUX_SWITCH_ACRO_TRAINER 14 // low = disabled, middle = leveled, high = leveled and limited -#define AUX_SWITCH_SPRAYER 15 // enable/disable the crop sprayer -#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 -#define AUX_SWITCH_EKF 20 // deprecated -#define AUX_SWITCH_PARACHUTE_ENABLE 21 // Parachute enable/disable -#define AUX_SWITCH_PARACHUTE_RELEASE 22 // Parachute release -#define AUX_SWITCH_PARACHUTE_3POS 23 // Parachute disable, enable, release with 3 position switch -#define AUX_SWITCH_MISSIONRESET 24 // Reset auto mission to start from first command -#define AUX_SWITCH_ATTCON_FEEDFWD 25 // enable/disable the roll and pitch rate feed forward -#define AUX_SWITCH_ATTCON_ACCEL_LIM 26 // enable/disable the roll, pitch and yaw accel limiting -#define AUX_SWITCH_RETRACT_MOUNT 27 // Retract Mount -#define AUX_SWITCH_RELAY 28 // Relay pin on/off (only supports first relay) -#define AUX_SWITCH_LANDING_GEAR 29 // Landing gear controller - // 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) #define AUX_SWITCH_MIDDLE 1 // indicates auxiliar switch is in the middle position (pwm >1200, <1800) diff --git a/ArduCopter/landing_gear.pde b/ArduCopter/landing_gear.pde index cf28e02a74..589ebb3385 100644 --- a/ArduCopter/landing_gear.pde +++ b/ArduCopter/landing_gear.pde @@ -4,7 +4,7 @@ static void landinggear_update(){ // If landing gear control is active, run update function. - if (g.ch7_option == AUX_SWITCH_LANDING_GEAR || g.ch8_option == AUX_SWITCH_LANDING_GEAR ){ + if (g.ch7_option == AUXSW_LANDING_GEAR || g.ch8_option == AUXSW_LANDING_GEAR ){ // last status (deployed or retracted) used to check for changes static bool last_deploy_status; diff --git a/ArduCopter/switches.pde b/ArduCopter/switches.pde index 4e15b76bf2..83b607a475 100644 --- a/ArduCopter/switches.pde +++ b/ArduCopter/switches.pde @@ -36,7 +36,7 @@ static void read_control_switch() } } - if(g.ch7_option != AUX_SWITCH_SIMPLE_MODE && g.ch8_option != AUX_SWITCH_SIMPLE_MODE && g.ch7_option != AUX_SWITCH_SUPERSIMPLE_MODE && g.ch8_option != AUX_SWITCH_SUPERSIMPLE_MODE) { + if(g.ch7_option != AUXSW_SIMPLE_MODE && g.ch8_option != AUXSW_SIMPLE_MODE && g.ch7_option != AUXSW_SUPERSIMPLE_MODE && g.ch8_option != AUXSW_SUPERSIMPLE_MODE) { // set Simple mode using stored paramters from Mission planner // rather than by the control switch if (BIT_IS_SET(g.super_simple, switch_position)) { @@ -166,22 +166,22 @@ static void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) { // init channel options switch(ch_option) { - case AUX_SWITCH_SIMPLE_MODE: - case AUX_SWITCH_SONAR: - case AUX_SWITCH_FENCE: - case AUX_SWITCH_RESETTOARMEDYAW: - case AUX_SWITCH_SUPERSIMPLE_MODE: - case AUX_SWITCH_ACRO_TRAINER: - case AUX_SWITCH_EPM: - case AUX_SWITCH_SPRAYER: - case AUX_SWITCH_PARACHUTE_ENABLE: - case AUX_SWITCH_PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release - case AUX_SWITCH_RETRACT_MOUNT: - case AUX_SWITCH_MISSIONRESET: - case AUX_SWITCH_ATTCON_FEEDFWD: - case AUX_SWITCH_ATTCON_ACCEL_LIM: - case AUX_SWITCH_RELAY: - case AUX_SWITCH_LANDING_GEAR: + case AUXSW_SIMPLE_MODE: + case AUXSW_SONAR: + case AUXSW_FENCE: + case AUXSW_RESETTOARMEDYAW: + case AUXSW_SUPERSIMPLE_MODE: + case AUXSW_ACRO_TRAINER: + case AUXSW_EPM: + 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 + case AUXSW_RETRACT_MOUNT: + case AUXSW_MISSION_RESET: + case AUXSW_ATTCON_FEEDFWD: + case AUXSW_ATTCON_ACCEL_LIM: + case AUXSW_RELAY: + case AUXSW_LANDING_GEAR: do_aux_switch_function(ch_option, ch_flag); break; } @@ -193,35 +193,35 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) int8_t tmp_function = ch_function; // multi mode check - if(ch_function == AUX_SWITCH_MULTI_MODE) { + if(ch_function == AUXSW_MULTI_MODE) { if (g.rc_6.radio_in < CH6_PWM_TRIGGER_LOW) { - tmp_function = AUX_SWITCH_FLIP; + tmp_function = AUXSW_FLIP; }else if (g.rc_6.radio_in > CH6_PWM_TRIGGER_HIGH) { - tmp_function = AUX_SWITCH_SAVE_WP; + tmp_function = AUXSW_SAVE_WP; }else{ - tmp_function = AUX_SWITCH_RTL; + tmp_function = AUXSW_RTL; } } switch(tmp_function) { - case AUX_SWITCH_FLIP: + case AUXSW_FLIP: // flip if switch is on, positive throttle and we're actually flying if(ch_flag == AUX_SWITCH_HIGH) { set_mode(FLIP); } break; - case AUX_SWITCH_SIMPLE_MODE: + case AUXSW_SIMPLE_MODE: // low = simple mode off, middle or high position turns simple mode on set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE); break; - case AUX_SWITCH_SUPERSIMPLE_MODE: + case AUXSW_SUPERSIMPLE_MODE: // low = simple mode off, middle = simple mode, high = super simple mode set_simple_mode(ch_flag); break; - case AUX_SWITCH_RTL: + case AUXSW_RTL: if (ch_flag == AUX_SWITCH_HIGH) { // engage RTL (if not possible we remain in current flight mode) set_mode(RTL); @@ -233,13 +233,13 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } break; - case AUX_SWITCH_SAVE_TRIM: + case AUXSW_SAVE_TRIM: if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (g.rc_3.control_in == 0)) { save_trim(); } break; - case AUX_SWITCH_SAVE_WP: + case AUXSW_SAVE_WP: // save waypoint when switch is brought high if (ch_flag == AUX_SWITCH_HIGH) { @@ -294,14 +294,14 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #if CAMERA == ENABLED - case AUX_SWITCH_CAMERA_TRIGGER: + case AUXSW_CAMERA_TRIGGER: if (ch_flag == AUX_SWITCH_HIGH) { do_take_picture(); } break; #endif - case AUX_SWITCH_SONAR: + case AUXSW_SONAR: // enable or disable the sonar #if CONFIG_SONAR == ENABLED if (ch_flag == AUX_SWITCH_HIGH) { @@ -313,7 +313,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #if AC_FENCE == ENABLED - case AUX_SWITCH_FENCE: + case AUXSW_FENCE: // enable or disable the fence if (ch_flag == AUX_SWITCH_HIGH) { fence.enable(true); @@ -325,7 +325,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #endif // To-Do: add back support for this feature - //case AUX_SWITCH_RESETTOARMEDYAW: + //case AUXSW_RESETTOARMEDYAW: // if (ch_flag == AUX_SWITCH_HIGH) { // set_yaw_mode(YAW_RESETTOARMEDYAW); // }else{ @@ -333,7 +333,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) // } // break; - case AUX_SWITCH_ACRO_TRAINER: + case AUXSW_ACRO_TRAINER: switch(ch_flag) { case AUX_SWITCH_LOW: g.acro_trainer = ACRO_TRAINER_DISABLED; @@ -350,7 +350,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) } break; #if EPM_ENABLED == ENABLED - case AUX_SWITCH_EPM: + case AUXSW_EPM: switch(ch_flag) { case AUX_SWITCH_LOW: epm.release(); @@ -364,14 +364,14 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #endif #if SPRAYER == ENABLED - case AUX_SWITCH_SPRAYER: + case AUXSW_SPRAYER: sprayer.enable(ch_flag == AUX_SWITCH_HIGH); // if we are disarmed the pilot must want to test the pump sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors.armed()); break; #endif - case AUX_SWITCH_AUTO: + case AUXSW_AUTO: if (ch_flag == AUX_SWITCH_HIGH) { set_mode(AUTO); }else{ @@ -383,7 +383,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #if AUTOTUNE_ENABLED == ENABLED - case AUX_SWITCH_AUTOTUNE: + case AUXSW_AUTOTUNE: // turn on auto tuner switch(ch_flag) { case AUX_SWITCH_LOW: @@ -401,7 +401,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #endif - case AUX_SWITCH_LAND: + case AUXSW_LAND: if (ch_flag == AUX_SWITCH_HIGH) { set_mode(LAND); }else{ @@ -413,18 +413,18 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #if PARACHUTE == ENABLED - case AUX_SWITCH_PARACHUTE_ENABLE: + case AUXSW_PARACHUTE_ENABLE: // Parachute enable/disable parachute.enabled(ch_flag == AUX_SWITCH_HIGH); break; - case AUX_SWITCH_PARACHUTE_RELEASE: + case AUXSW_PARACHUTE_RELEASE: if (ch_flag == AUX_SWITCH_HIGH) { parachute_manual_release(); } break; - case AUX_SWITCH_PARACHUTE_3POS: + case AUXSW_PARACHUTE_3POS: // Parachute disable, enable, release with 3 position switch switch (ch_flag) { case AUX_SWITCH_LOW: @@ -443,24 +443,24 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #endif - case AUX_SWITCH_MISSIONRESET: + case AUXSW_MISSION_RESET: if (ch_flag == AUX_SWITCH_HIGH) { mission.reset(); } break; - case AUX_SWITCH_ATTCON_FEEDFWD: + case AUXSW_ATTCON_FEEDFWD: // enable or disable feed forward attitude_control.bf_feedforward(ch_flag == AUX_SWITCH_HIGH); break; - case AUX_SWITCH_ATTCON_ACCEL_LIM: + case AUXSW_ATTCON_ACCEL_LIM: // enable or disable accel limiting by restoring defaults attitude_control.accel_limiting(ch_flag == AUX_SWITCH_HIGH); break; #if MOUNT == ENABLE - case AUX_SWITCH_RETRACT_MOUNT: + case AUXSW_RETRACT_MOUNT: switch (ch_flag) { case AUX_SWITCH_HIGH: camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT); @@ -472,11 +472,11 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; #endif - case AUX_SWITCH_RELAY: + case AUXSW_RELAY: ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH); break; - case AUX_SWITCH_LANDING_GEAR: + case AUXSW_LANDING_GEAR: switch (ch_flag) { case AUX_SWITCH_LOW: landinggear.set_cmd_mode(LandingGear_Deploy);