diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 2902352f9d..792e7bbcd8 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -27,15 +27,17 @@ void set_auto_armed(bool b) } // --------------------------------------------- -void set_simple_mode(bool b) +void set_simple_mode(uint8_t t) { - if(ap.simple_mode != b){ - if(b){ + if(ap.simple_mode != t){ + if(t == 0){ + Log_Write_Event(DATA_SET_SIMPLE_OFF); + }else if(t == 1){ Log_Write_Event(DATA_SET_SIMPLE_ON); }else{ - Log_Write_Event(DATA_SET_SIMPLE_OFF); + Log_Write_Event(DATA_SET_SUPERSIMPLE_ON); } - ap.simple_mode = b; + ap.simple_mode = t; } } diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 82c400ec97..a1ef3dd48f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -361,9 +361,9 @@ static AP_RangeFinder_MaxsonarXL *sonar; static union { struct { uint8_t home_is_set : 1; // 0 - uint8_t simple_mode : 1; // 1 // This is the state of simple mode - uint8_t manual_attitude : 1; // 2 - uint8_t manual_throttle : 1; // 3 + uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE + uint8_t manual_attitude : 1; // 3 + uint8_t manual_throttle : 1; // 4 uint8_t pre_arm_rc_check : 1; // 5 // true if rc input pre-arm checks have been completed successfully uint8_t pre_arm_check : 1; // 6 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed @@ -390,10 +390,11 @@ static struct AP_System{ uint8_t GPS_light : 1; // 0 // Solid indicates we have full 3D lock and can navigate, flash = read uint8_t arming_light : 1; // 1 // Solid indicates armed state, flashing is disarmed, double flashing is disarmed and failing pre-arm checks uint8_t new_radio_frame : 1; // 2 // Set true if we have new PWM data to act on from the Radio - uint8_t CH7_flag : 1; // 3 // true if ch7 aux switch is high - uint8_t CH8_flag : 1; // 4 // true if ch8 aux switch is high - uint8_t usb_connected : 1; // 5 // true if APM is powered from USB connection - uint8_t yaw_stopped : 1; // 6 // Used to manage the Yaw hold capabilities + uint8_t CH7_flag : 2; // 3,4 // ch7 aux switch : 0 is low or false, 1 is center or true, 2 is high + uint8_t CH8_flag : 2; // 5,6 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high + uint8_t usb_connected : 1; // 7 // true if APM is powered from USB connection + uint8_t yaw_stopped : 1; // 8 // Used to manage the Yaw hold capabilities + uint8_t : 7; // 9-15 // Fill bit field to 16 bits } ap_system; @@ -1809,8 +1810,8 @@ void update_simple_mode(void) // should be called after home_bearing has been updated void update_super_simple_bearing() { - // are we in SIMPLE mode? - if(ap.simple_mode && g.super_simple) { + // are we in SUPERSIMPLE mode? + if(ap.simple_mode == 2 || (ap.simple_mode && g.super_simple)) { // get distance to home if(home_distance > SUPER_SIMPLE_RADIUS) { // 10m from home // we reset the angular offset to be a vector from home to the quad diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 8cd260154e..ab38059d4e 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 + // @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 // @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 + // @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 // @User: Standard GSCALAR(ch8_option, "CH8_OPT", CH8_OPTION), diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index cc07c9d00f..822584e6e1 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -17,7 +17,8 @@ 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) { + + 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) { // 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)); @@ -52,30 +53,81 @@ static void reset_control_switch() // read_aux_switches - checks aux switch positions and invokes configured actions static void read_aux_switches() { - // check if ch7 switch has changed position - if (ap_system.CH7_flag != (g.rc_7.radio_in >= AUX_SWITCH_PWM_TRIGGER)) { - // set the ch7 flag + + // 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()) { + // 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); // invoke the appropriate function do_aux_switch_function(g.ch7_option, ap_system.CH7_flag); } - // check if ch8 switch has changed position - if (ap_system.CH8_flag != (g.rc_8.radio_in >= AUX_SWITCH_PWM_TRIGGER)) { - // set the ch8 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()) { + // 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); + // 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() { - // set channel 7 and 8 flags according to switch position - ap_system.CH7_flag = (g.rc_7.radio_in >= AUX_SWITCH_PWM_TRIGGER); - ap_system.CH8_flag = (g.rc_8.radio_in >= AUX_SWITCH_PWM_TRIGGER); + + // 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); + } // init channel 7 options switch(g.ch7_option) { @@ -83,28 +135,24 @@ static void init_aux_switches() case AUX_SWITCH_SONAR: case AUX_SWITCH_FENCE: case AUX_SWITCH_RESETTOARMEDYAW: + case AUX_SWITCH_3POS_SIMPLE_SUPERSIMPLE_MODE: do_aux_switch_function(g.ch7_option, ap_system.CH7_flag); break; } - - // safety check to ensure we ch7 and ch8 have different functions - if (g.ch7_option == g.ch8_option) { - return; - } - // init channel 8 option switch(g.ch8_option) { case AUX_SWITCH_SIMPLE_MODE: case AUX_SWITCH_SONAR: case AUX_SWITCH_FENCE: case AUX_SWITCH_RESETTOARMEDYAW: + case AUX_SWITCH_3POS_SIMPLE_SUPERSIMPLE_MODE: do_aux_switch_function(g.ch8_option, ap_system.CH8_flag); break; } } // do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch -static void do_aux_switch_function(int8_t ch_function, bool ch_flag) +static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) { int8_t tmp_function = ch_function; @@ -128,6 +176,7 @@ static void do_aux_switch_function(int8_t ch_function, bool ch_flag) break; case AUX_SWITCH_SIMPLE_MODE: + case AUX_SWITCH_3POS_SIMPLE_SUPERSIMPLE_MODE: set_simple_mode(ch_flag); break; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 32450d8d56..4832fb1932 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -44,10 +44,14 @@ #define SONAR_SOURCE_ADC 1 #define SONAR_SOURCE_ANALOG_PIN 2 -// Ch7 and Ch8 aux switch control +// 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 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 @@ -63,6 +67,9 @@ #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 +// 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) + // Frame types #define QUAD_FRAME 0 @@ -315,6 +322,7 @@ enum ap_message { #define DATA_SET_HOME 25 #define DATA_SET_SIMPLE_ON 26 #define DATA_SET_SIMPLE_OFF 27 +#define DATA_SET_SUPERSIMPLE_ON 28 // battery monitoring macros #define BATTERY_VOLTAGE(x) (x->voltage_average()*g.volt_div_ratio) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 2d0c982d82..07c1f4fd57 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -234,7 +234,7 @@ static void pre_arm_checks(bool display_failure) } // pre-arm check to ensure ch7 and ch8 have different functions - if (g.ch7_option == g.ch8_option) { + if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Ch7&Ch8 Opt cannot be same")); }