From 2c48c20088d6b23169fca4215d52828ec8d03c75 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 29 Jul 2013 16:28:04 +0900 Subject: [PATCH] Copter: generalize 3 pos switch for all ch7/ch8 options --- ArduCopter/Parameters.pde | 4 +- ArduCopter/control_modes.pde | 109 +++++++++++++---------------------- ArduCopter/defines.h | 15 +++-- 3 files changed, 49 insertions(+), 79 deletions(-) diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index ab38059d4e..18925816a1 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -405,14 +405,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, 100:3pos Simple Mode + // @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 // @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, 100:3pos Simple Mode + // @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 // @User: Standard GSCALAR(ch8_option, "CH8_OPT", CH8_OPTION), diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 822584e6e1..df0a734223 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -18,7 +18,7 @@ static void read_control_switch() // set flight mode and simple mode setting if (set_mode(flight_modes[switchPosition])) { - if(g.ch7_option != AUX_SWITCH_SIMPLE_MODE && g.ch8_option != AUX_SWITCH_SIMPLE_MODE && g.ch7_option != AUX_SWITCH_3POS_SIMPLE_SUPERSIMPLE_MODE && g.ch8_option != AUX_SWITCH_3POS_SIMPLE_SUPERSIMPLE_MODE) { + 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) { // set Simple mode using stored paramters from Mission planner // rather than by the control switch set_simple_mode(BIT_IS_SET(g.simple_modes, switchPosition)); @@ -50,84 +50,47 @@ static void reset_control_switch() read_control_switch(); } +// read_3pos_switch +static uint8_t read_3pos_switch(int16_t radio_in){ + if (radio_in < AUX_SWITCH_PWM_TRIGGER_LOW) return AUX_SWITCH_LOW; // switch is in low position + if (radio_in > AUX_SWITCH_PWM_TRIGGER_HIGH) return AUX_SWITCH_HIGH; // switch is in high position + return AUX_SWITCH_MIDDLE; // switch is in middle position +} + // read_aux_switches - checks aux switch positions and invokes configured actions static void read_aux_switches() { + uint8_t switch_position; - // Check if we have a 3 positions switch function for Ch7 - if (g.ch7_option >= 100) { - // check if CH7 switch has changed position - if (ap_system.CH7_flag != read3posCH7Switch()) { + // check if ch7 switch has changed position + switch_position = read_3pos_switch(g.rc_7.radio_in); + if (ap_system.CH7_flag != switch_position) { // set the CH7 flag - ap_system.CH7_flag = read3posCH7Switch(); - - // invoke the appropriate function - do_aux_switch_function(g.ch7_option, ap_system.CH7_flag); - } - } - // Else we have a 2 positions switch function - check if ch7 switch has changed position - else if (ap_system.CH7_flag != (g.rc_7.radio_in >= AUX_SWITCH_PWM_TRIGGER)) { - // set the CH7 flag - ap_system.CH7_flag = (g.rc_7.radio_in >= AUX_SWITCH_PWM_TRIGGER); + ap_system.CH7_flag = switch_position; // invoke the appropriate function do_aux_switch_function(g.ch7_option, ap_system.CH7_flag); } - - // Check if we have a 3 positions switch function for Ch8 - if (g.ch8_option >= 100) { - // check if Ch8 switch has changed position - if (ap_system.CH8_flag != read3posCH8Switch()) { + // check if Ch8 switch has changed position + switch_position = read_3pos_switch(g.rc_8.radio_in); + if (ap_system.CH8_flag != switch_position) { // set the CH8 flag - ap_system.CH8_flag = read3posCH8Switch(); - - // invoke the appropriate function - do_aux_switch_function(g.ch8_option, ap_system.CH8_flag); - } - } - // Else we have a 2 positions switch function - check if Ch8 switch has changed position - else if (ap_system.CH8_flag != (g.rc_8.radio_in >= AUX_SWITCH_PWM_TRIGGER)) { - // set the CH8 flag - ap_system.CH8_flag = (g.rc_8.radio_in >= AUX_SWITCH_PWM_TRIGGER); + ap_system.CH8_flag = switch_position; // invoke the appropriate function do_aux_switch_function(g.ch8_option, ap_system.CH8_flag); } } -static uint8_t read3posCH7Switch(void){ - int16_t pulsewidth = g.rc_7.radio_in; // Read CH7 - - if (pulsewidth < CH7_PWM_TRIGGER_LOW) return 0; // Ch7 switch is in low position - if (pulsewidth > CH7_PWM_TRIGGER_HIGH) return 2; // Ch7 switch is in high position - return 1; // Ch7 switch is in center position -} - -static uint8_t read3posCH8Switch(void){ - int16_t pulsewidth = g.rc_8.radio_in; // Read CH8 - - if (pulsewidth < CH8_PWM_TRIGGER_LOW) return 0; // Ch8 switch is in low position - if (pulsewidth > CH8_PWM_TRIGGER_HIGH) return 2; // Ch8 switch is in high position - return 1; // Ch8 switch is in center position -} - // init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so static void init_aux_switches() { + uint8_t switch_position; - // set channel 7 and 8 flags according to switch position and switch type (2 or 3 positions) - if (g.ch7_option >= 100) { - // set the CH7 flag - ap_system.CH7_flag = read3posCH7Switch(); - } else { - ap_system.CH7_flag = (g.rc_7.radio_in >= AUX_SWITCH_PWM_TRIGGER); - } - if (g.ch8_option >= 100) { - ap_system.CH8_flag = read3posCH8Switch(); - } else { - ap_system.CH8_flag = (g.rc_8.radio_in >= AUX_SWITCH_PWM_TRIGGER); - } + // set the CH7 flag + ap_system.CH7_flag = read_3pos_switch(g.rc_7.radio_in); + ap_system.CH8_flag = read_3pos_switch(g.rc_8.radio_in); // init channel 7 options switch(g.ch7_option) { @@ -135,7 +98,7 @@ static void init_aux_switches() case AUX_SWITCH_SONAR: case AUX_SWITCH_FENCE: case AUX_SWITCH_RESETTOARMEDYAW: - case AUX_SWITCH_3POS_SIMPLE_SUPERSIMPLE_MODE: + case AUX_SWITCH_SUPERSIMPLE_MODE: do_aux_switch_function(g.ch7_option, ap_system.CH7_flag); break; } @@ -145,7 +108,7 @@ static void init_aux_switches() case AUX_SWITCH_SONAR: case AUX_SWITCH_FENCE: case AUX_SWITCH_RESETTOARMEDYAW: - case AUX_SWITCH_3POS_SIMPLE_SUPERSIMPLE_MODE: + case AUX_SWITCH_SUPERSIMPLE_MODE: do_aux_switch_function(g.ch8_option, ap_system.CH8_flag); break; } @@ -170,18 +133,18 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) switch(tmp_function) { case AUX_SWITCH_FLIP: // flip if switch is on, positive throttle and we're actually flying - if(ch_flag && g.rc_3.control_in >= 0 && ap.takeoff_complete) { + if((ch_flag == AUX_SWITCH_HIGH) && (g.rc_3.control_in >= 0) && ap.takeoff_complete) { init_flip(); } break; case AUX_SWITCH_SIMPLE_MODE: - case AUX_SWITCH_3POS_SIMPLE_SUPERSIMPLE_MODE: + case AUX_SWITCH_SUPERSIMPLE_MODE: set_simple_mode(ch_flag); break; case AUX_SWITCH_RTL: - if (ch_flag) { + if (ch_flag == AUX_SWITCH_HIGH) { // engage RTL (if not possible we remain in current flight mode) set_mode(RTL); }else{ @@ -193,14 +156,14 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) break; case AUX_SWITCH_SAVE_TRIM: - if(ch_flag && control_mode <= ACRO && g.rc_3.control_in == 0) { + if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (g.rc_3.control_in == 0)) { save_trim(); } break; case AUX_SWITCH_SAVE_WP: // save waypoint when switch is switched off - if (ch_flag == false) { + if (ch_flag == AUX_SWITCH_LOW) { // if in auto mode, reset the mission if(control_mode == AUTO) { @@ -252,7 +215,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) #if CAMERA == ENABLED case AUX_SWITCH_CAMERA_TRIGGER: - if(ch_flag) { + if (ch_flag == AUX_SWITCH_HIGH) { do_take_picture(); } break; @@ -260,17 +223,25 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUX_SWITCH_SONAR: // enable or disable the sonar - g.sonar_enabled = ch_flag; + if (ch_flag == AUX_SWITCH_HIGH) { + g.sonar_enabled = true; + }else{ + g.sonar_enabled = false; + } break; #if AC_FENCE == ENABLED case AUX_SWITCH_FENCE: // enable or disable the fence - fence.enable(ch_flag); + if (ch_flag == AUX_SWITCH_HIGH) { + fence.enable(true); + }else{ + fence.enable(false); + } break; #endif case AUX_SWITCH_RESETTOARMEDYAW: - if (ch_flag) { + if (ch_flag == AUX_SWITCH_HIGH) { set_yaw_mode(YAW_RESETTOARMEDYAW); }else{ set_yaw_mode(YAW_HOLD); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 4832fb1932..bda9f56f9e 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -45,13 +45,10 @@ #define SONAR_SOURCE_ANALOG_PIN 2 // Ch6, Ch7 and Ch8 aux switch control -#define AUX_SWITCH_PWM_TRIGGER 1800 // pwm value above which the ch7 or ch8 option will be invoked +#define AUX_SWITCH_PWM_TRIGGER_HIGH 1800 // pwm value above which the ch7 or ch8 option will be invoked +#define AUX_SWITCH_PWM_TRIGGER_LOW 1200 // pwm value below which the ch7 or ch8 option will be disabled #define CH6_PWM_TRIGGER_HIGH 1800 #define CH6_PWM_TRIGGER_LOW 1200 -#define CH7_PWM_TRIGGER_HIGH 1800 -#define CH7_PWM_TRIGGER_LOW 1200 -#define CH8_PWM_TRIGGER_HIGH 1800 -#define CH8_PWM_TRIGGER_LOW 1200 #define AUX_SWITCH_DO_NOTHING 0 // aux switch disabled #define AUX_SWITCH_SET_HOVER 1 // deprecated @@ -66,10 +63,12 @@ #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 -// Functions >= 100 need a 3 positions switch -#define AUX_SWITCH_3POS_SIMPLE_SUPERSIMPLE_MODE 100 // depending upon CH7 or CH8 position : disable Simple mode (if CH7 or CH8 is low) ; select Simple mode (if CH7 or CH8 in middle) ; select SuperSimple mode (if CH7 or CH8 is high) - +// 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) +#define AUX_SWITCH_HIGH 2 // indicates auxiliar switch is in the high position (pwm >1800) // Frame types #define QUAD_FRAME 0