Copter: 3 positions switch support for CH7 and CH8 - new 3 positions SIMPLE / SUPERSIMPLE mode

3 positions modes must use mode number >= 100
pre-arm check correction for CH7 - CH8 check
This commit is contained in:
Olivier-ADLER 2013-07-28 01:28:00 +02:00 committed by Randy Mackay
parent c173f0c7d9
commit 87d062905e
6 changed files with 95 additions and 35 deletions

View File

@ -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;
}
}

View File

@ -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

View File

@ -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),

View File

@ -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;

View File

@ -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)

View File

@ -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"));
}