From a5af151a9115760204c80b64b40e31278af37281 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Tue, 10 Mar 2015 14:21:31 -0400 Subject: [PATCH] Copter: Initial expansion of Aux Switch to Ch7-12 --- ArduCopter/ArduCopter.pde | 38 +++++++++-------- ArduCopter/Parameters.h | 8 ++++ ArduCopter/Parameters.pde | 28 +++++++++++++ ArduCopter/config.h | 16 +++++++ ArduCopter/control_flip.pde | 2 +- ArduCopter/defines.h | 2 +- ArduCopter/landing_gear.pde | 2 +- ArduCopter/motors.pde | 2 +- ArduCopter/switches.pde | 83 ++++++++++++++++++++++++++----------- 9 files changed, 135 insertions(+), 46 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 9215be35d1..f94c13e037 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -334,25 +334,29 @@ static bool sonar_enabled = true; // enable user switch for sonar static union { struct { uint8_t unused1 : 1; // 0 - uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE - uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully - uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed - uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised - uint8_t logging_started : 1; // 6 // true if dataflash logging has started - uint8_t land_complete : 1; // 7 // true if we have detected a landing + uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE + uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully + uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed + uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised + uint8_t logging_started : 1; // 6 // true if dataflash logging has started + uint8_t land_complete : 1; // 7 // true if we have detected a landing uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio - uint8_t CH7_flag : 2; // 9,10 // ch7 aux switch : 0 is low or false, 1 is center or true, 2 is high + uint8_t CH7_flag : 2; // 9,10 // ch7 aux switch : 0 is low or false, 1 is center or true, 2 is high uint8_t CH8_flag : 2; // 11,12 // ch8 aux switch : 0 is low or false, 1 is center or true, 2 is high - uint8_t usb_connected : 1; // 13 // true if APM is powered from USB connection - uint8_t rc_receiver_present : 1; // 14 // true if we have an rc receiver present (i.e. if we've ever received an update - uint8_t compass_mot : 1; // 15 // true if we are currently performing compassmot calibration - uint8_t motor_test : 1; // 16 // true if we are currently performing the motors test - uint8_t initialised : 1; // 17 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes - uint8_t land_complete_maybe : 1; // 18 // true if we may have landed (less strict version of land_complete) - uint8_t throttle_zero : 1; // 19 // true if the throttle stick is at zero, debounced - uint8_t system_time_set : 1; // 20 // true if the system time has been set from the GPS - uint8_t gps_base_pos_set : 1; // 21 // true when the gps base position has been set (used for RTK gps only) - enum HomeState home_state : 2; // 22,23 - home status (unset, set, locked) + uint8_t CH9_flag : 2; // 13,14 // ch9 aux switch : 0 is low or false, 1 is center or true, 2 is high + uint8_t CH10_flag : 2; // 15,16 // ch10 aux switch : 0 is low or false, 1 is center or true, 2 is high + uint8_t CH11_flag : 2; // 17,18 // ch11 aux switch : 0 is low or false, 1 is center or true, 2 is high + uint8_t CH12_flag : 2; // 19,20 // ch12 aux switch : 0 is low or false, 1 is center or true, 2 is high + uint8_t usb_connected : 1; // 21 // true if APM is powered from USB connection + uint8_t rc_receiver_present : 1; // 22 // true if we have an rc receiver present (i.e. if we've ever received an update + uint8_t compass_mot : 1; // 23 // true if we are currently performing compassmot calibration + uint8_t motor_test : 1; // 24 // true if we are currently performing the motors test + uint8_t initialised : 1; // 25 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes + uint8_t land_complete_maybe : 1; // 26 // true if we may have landed (less strict version of land_complete) + uint8_t throttle_zero : 1; // 27 // true if the throttle stick is at zero, debounced + uint8_t system_time_set : 1; // 28 // true if the system time has been set from the GPS + uint8_t gps_base_pos_set : 1; // 29 // true when the gps base position has been set (used for RTK gps only) + enum HomeState home_state : 2; // 30,31 // home status (unset, set, locked) }; uint32_t value; } ap; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 613b704daa..4819322b1c 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -185,6 +185,10 @@ public: k_param_serial2_baud_old, // deprecated k_param_serial2_protocol, // deprecated k_param_serial_manager, // 119 + k_param_ch9_option, + k_param_ch10_option, + k_param_ch11_option, + k_param_ch12_option, // 123 // // 140: Sensor parameters @@ -392,6 +396,10 @@ public: AP_Int8 frame_orientation; AP_Int8 ch7_option; AP_Int8 ch8_option; + AP_Int8 ch9_option; + AP_Int8 ch10_option; + AP_Int8 ch11_option; + AP_Int8 ch12_option; AP_Int8 arming_check; AP_Int8 land_repositioning; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 487fa50dd1..f3c54c3381 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -357,6 +357,34 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(ch8_option, "CH8_OPT", CH8_OPTION), + // @Param: CH9_OPT + // @DisplayName: Channel 9 option + // @Description: Select which function if performed when CH9 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:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear + // @User: Standard + GSCALAR(ch9_option, "CH9_OPT", CH9_OPTION), + + // @Param: CH10_OPT + // @DisplayName: Channel 10 option + // @Description: Select which function if performed when CH10 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:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear + // @User: Standard + GSCALAR(ch10_option, "CH10_OPT", CH10_OPTION), + + // @Param: CH11_OPT + // @DisplayName: Channel 11 option + // @Description: Select which function if performed when CH11 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:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear + // @User: Standard + GSCALAR(ch11_option, "CH11_OPT", CH11_OPTION), + + // @Param: CH12_OPT + // @DisplayName: Channel 12 option + // @Description: Select which function if performed when CH12 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:RangeFinder, 11:Fence, 12:ResetToArmedYaw, 13:Super Simple Mode, 14:Acro Trainer, 16:Auto, 17:AutoTune, 18:Land, 19:EPM, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 29:Landing Gear + // @User: Standard + GSCALAR(ch12_option, "CH12_OPT", CH12_OPTION), + // @Param: ARMING_CHECK // @DisplayName: Arming check // @Description: Allows enabling or disabling of pre-arming checks of receiver, accelerometer, barometer, compass and GPS diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 1f99e20062..63ae9565ca 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -191,6 +191,22 @@ # define CH8_OPTION AUX_SWITCH_DO_NOTHING #endif +#ifndef CH9_OPTION + # define CH9_OPTION AUX_SWITCH_DO_NOTHING +#endif + +#ifndef CH10_OPTION + # define CH10_OPTION AUX_SWITCH_DO_NOTHING +#endif + +#ifndef CH11_OPTION + # define CH11_OPTION AUX_SWITCH_DO_NOTHING +#endif + +#ifndef CH12_OPTION + # define CH12_OPTION AUX_SWITCH_DO_NOTHING +#endif + ////////////////////////////////////////////////////////////////////////////// // HIL_MODE OPTIONAL diff --git a/ArduCopter/control_flip.pde b/ArduCopter/control_flip.pde index c0f2309c03..6e76ffe6d4 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 or CH8_OPT parameter must be set to "Flip" (AUX_SWITCH_FLIP) which is "2" + * CH7_OPT - CH12_OPT parameter must be set to "Flip" (AUX_SWITCH_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 7d229fbbe4..2da75135c2 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -22,7 +22,7 @@ #define AUTO_YAW_LOOK_AHEAD 4 // point in the direction the copter is moving #define AUTO_YAW_RESETTOARMEDYAW 5 // point towards heading at time motors were armed -// Ch6, Ch7 and Ch8 aux switch control +// Ch6... Ch12 aux switch control #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 diff --git a/ArduCopter/landing_gear.pde b/ArduCopter/landing_gear.pde index 2a6e6f634d..cf28e02a74 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 == AUX_SWITCH_LANDING_GEAR || g.ch8_option == AUX_SWITCH_LANDING_GEAR ){ // last status (deployed or retracted) used to check for changes static bool last_deploy_status; diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index de82d4ded2..200c621572 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -406,7 +406,7 @@ static bool pre_arm_checks(bool display_failure) // ensure ch7 and ch8 have different functions 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 Option cannot be same")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Duplicate Aux Switch Options")); } return false; } diff --git a/ArduCopter/switches.pde b/ArduCopter/switches.pde index 6ad7ef5957..4e15b76bf2 100644 --- a/ArduCopter/switches.pde +++ b/ArduCopter/switches.pde @@ -100,6 +100,46 @@ static void read_aux_switches() // invoke the appropriate function do_aux_switch_function(g.ch8_option, ap.CH8_flag); } + + // check if Ch9 switch has changed position + switch_position = read_3pos_switch(g.rc_9.radio_in); + if (ap.CH9_flag != switch_position) { + // set the CH9 flag + ap.CH9_flag = switch_position; + + // invoke the appropriate function + do_aux_switch_function(g.ch9_option, ap.CH9_flag); + } + + // check if Ch10 switch has changed position + switch_position = read_3pos_switch(g.rc_10.radio_in); + if (ap.CH10_flag != switch_position) { + // set the CH10 flag + ap.CH10_flag = switch_position; + + // invoke the appropriate function + do_aux_switch_function(g.ch10_option, ap.CH10_flag); + } + + // check if Ch11 switch has changed position + switch_position = read_3pos_switch(g.rc_11.radio_in); + if (ap.CH11_flag != switch_position) { + // set the CH11 flag + ap.CH11_flag = switch_position; + + // invoke the appropriate function + do_aux_switch_function(g.ch11_option, ap.CH11_flag); + } + + // check if Ch12 switch has changed position + switch_position = read_3pos_switch(g.rc_12.radio_in); + if (ap.CH12_flag != switch_position) { + // set the CH12 flag + ap.CH12_flag = switch_position; + + // invoke the appropriate function + do_aux_switch_function(g.ch12_option, ap.CH12_flag); + } } // init_aux_switches - invoke configured actions at start-up for aux function where it is safe to do so @@ -108,9 +148,24 @@ static void init_aux_switches() // set the CH7 flag ap.CH7_flag = read_3pos_switch(g.rc_7.radio_in); ap.CH8_flag = read_3pos_switch(g.rc_8.radio_in); + ap.CH9_flag = read_3pos_switch(g.rc_9.radio_in); + ap.CH10_flag = read_3pos_switch(g.rc_10.radio_in); + ap.CH11_flag = read_3pos_switch(g.rc_11.radio_in); + ap.CH12_flag = read_3pos_switch(g.rc_12.radio_in); + + init_aux_switch_function(g.ch7_option, ap.CH7_flag); + init_aux_switch_function(g.ch8_option, ap.CH8_flag); + init_aux_switch_function(g.ch9_option, ap.CH9_flag); + init_aux_switch_function(g.ch10_option, ap.CH10_flag); + init_aux_switch_function(g.ch11_option, ap.CH11_flag); + init_aux_switch_function(g.ch12_option, ap.CH12_flag); +} - // init channel 7 options - switch(g.ch7_option) { +// init_aux_switch_function - initialize aux functions +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: @@ -127,29 +182,7 @@ static void init_aux_switches() case AUX_SWITCH_ATTCON_ACCEL_LIM: case AUX_SWITCH_RELAY: case AUX_SWITCH_LANDING_GEAR: - do_aux_switch_function(g.ch7_option, ap.CH7_flag); - break; - } - - // 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_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: - do_aux_switch_function(g.ch8_option, ap.CH8_flag); + do_aux_switch_function(ch_option, ch_flag); break; } }