3 #define CONTROL_SWITCH_DEBOUNCE_TIME_MS 200 18 if (g.flight_mode_chan <= 0) {
23 uint32_t tnow_ms =
millis();
26 int8_t switch_position;
28 if (mode_in < 1231) switch_position = 0;
29 else if (mode_in < 1361) switch_position = 1;
30 else if (mode_in < 1491) switch_position = 2;
31 else if (mode_in < 1621) switch_position = 3;
32 else if (mode_in < 1750) switch_position = 4;
33 else switch_position = 5;
36 if (control_switch_state.last_switch_position != switch_position) {
37 control_switch_state.last_edge_time_ms = tnow_ms;
41 bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position;
43 bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0;
45 if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) {
49 if (control_switch_state.debounced_switch_position != -1) {
59 if (
BIT_IS_SET(g.super_simple, switch_position)) {
62 set_simple_mode(
BIT_IS_SET(g.simple_modes, switch_position));
66 }
else if (control_switch_state.last_switch_position != -1) {
72 control_switch_state.debounced_switch_position = switch_position;
75 control_switch_state.last_switch_position = switch_position;
81 bool ret = g.ch7_option == auxsw_mode_check || g.ch8_option == auxsw_mode_check || g.ch9_option == auxsw_mode_check
82 || g.ch10_option == auxsw_mode_check || g.ch11_option == auxsw_mode_check || g.ch12_option == auxsw_mode_check;
91 auxsw_option_counts[g.ch7_option]++;
92 auxsw_option_counts[g.ch8_option]++;
93 auxsw_option_counts[g.ch9_option]++;
94 auxsw_option_counts[g.ch10_option]++;
95 auxsw_option_counts[g.ch11_option]++;
96 auxsw_option_counts[g.ch12_option]++;
98 for (uint8_t i=0; i<
sizeof(auxsw_option_counts); i++) {
102 if (auxsw_option_counts[i] > 1) {
111 control_switch_state.last_switch_position = control_switch_state.debounced_switch_position = -1;
112 read_control_switch();
125 #define read_aux_switch(chan, flag, option) \ 127 switch_position = read_3pos_switch(chan); \ 128 if (debounce_aux_switch(chan, flag) && flag != switch_position) { \ 129 flag = switch_position; \ 130 do_aux_switch_function(option, flag); \ 137 uint8_t switch_position;
140 if (failsafe.radio || failsafe.radio_counter != 0) {
150 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN 155 #undef read_aux_switch 171 init_aux_switch_function(g.ch7_option,
aux_con.CH7_flag);
172 init_aux_switch_function(g.ch8_option,
aux_con.CH8_flag);
173 init_aux_switch_function(g.ch10_option,
aux_con.CH10_flag);
174 init_aux_switch_function(g.ch11_option,
aux_con.CH11_flag);
177 init_aux_switch_function(g.ch9_option,
aux_con.CH9_flag);
178 init_aux_switch_function(g.ch12_option,
aux_con.CH12_flag);
207 do_aux_switch_function(ch_option, ch_flag);
220 const uint8_t debounce_count = 2;
222 if (chan < CH_7 || chan >
CH_12) {
232 if (db.
count < debounce_count) {
235 return db.
count >= debounce_count;
242 switch(ch_function) {
257 set_simple_mode(ch_flag);
261 #if MODE_RTL_ENABLED == ENABLED 267 if (control_mode ==
RTL) {
268 reset_control_switch();
275 if ((ch_flag ==
AUX_SWITCH_HIGH) && (control_mode <=
ACRO) && (channel_throttle->get_control_in() == 0)) {
281 #if MODE_AUTO_ENABLED == ENABLED 291 if ((mission.num_commands() == 0) && (channel_throttle->get_control_in() == 0)) {
299 if (mission.num_commands() == 0) {
301 cmd.
id = MAV_CMD_NAV_TAKEOFF;
310 if (mission.add_cmd(cmd)) {
320 if (channel_throttle->get_control_in() > 0) {
321 cmd.
id = MAV_CMD_NAV_WAYPOINT;
324 cmd.
id = MAV_CMD_NAV_LAND;
328 if (mission.add_cmd(cmd)) {
337 #if MODE_AUTO_ENABLED == ENABLED 345 #if MODE_AUTO_ENABLED == ENABLED 350 if (control_mode ==
AUTO) {
351 reset_control_switch();
358 #if CAMERA == ENABLED 360 camera.take_picture();
367 #if RANGEFINDER_ENABLED == ENABLED 369 rangefinder_state.enabled =
true;
371 rangefinder_state.enabled =
false;
377 #if AC_FENCE == ENABLED 390 #if MODE_ACRO_ENABLED == ENABLED 409 #if GRIPPER_ENABLED == ENABLED 412 g2.gripper.release();
424 #if SPRAYER == ENABLED 432 #if AUTOTUNE_ENABLED == ENABLED 439 reset_control_switch();
455 if (control_mode ==
LAND) {
456 reset_control_switch();
462 #if PARACHUTE == ENABLED 469 #if PARACHUTE == ENABLED 471 parachute_manual_release();
477 #if PARACHUTE == ENABLED 481 parachute.enabled(
false);
485 parachute.enabled(
true);
489 parachute.enabled(
true);
490 parachute_manual_release();
510 camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);
513 camera_mount.set_mode_to_default();
569 #if MODE_BRAKE_ENABLED == ENABLED 575 if (control_mode ==
BRAKE) {
576 reset_control_switch();
583 #if MODE_THROW_ENABLED == ENABLED 589 if (control_mode ==
THROW) {
590 reset_control_switch();
597 #if ADSB_ENABLED == ENABLED 600 avoidance_adsb.enable();
603 avoidance_adsb.disable();
610 #if PRECISION_LANDING == ENABLED && MODE_LOITER_ENABLED == ENABLED 613 mode_loiter.set_precision_loiter_enabled(
true);
616 mode_loiter.set_precision_loiter_enabled(
false);
623 #if PROXIMITY_ENABLED == ENABLED && AC_AVOID_ENABLED == ENABLED 626 avoid.proximity_avoidance_enable(
true);
630 avoid.proximity_avoidance_enable(
false);
640 init_arm_motors(
false);
642 ap.armed_with_switch =
true;
645 init_disarm_motors();
651 #if MODE_SMARTRTL_ENABLED == ENABLED 658 reset_control_switch();
665 #if FRAME_CONFIG == HELI_FRAME 670 motors->set_inverted_flight(
true);
671 attitude_control->set_inverted_flight(
true);
672 heli_flags.inverted_flight =
true;
675 motors->set_inverted_flight(
false);
676 attitude_control->set_inverted_flight(
false);
677 heli_flags.inverted_flight =
false;
685 #if WINCH_ENABLED == ENABLED 689 g2.winch.release_length(0.0
f);
702 #if WINCH_ENABLED == ENABLED 706 g2.winch.set_desired_rate(-g2.winch.get_rate_max());
710 g2.winch.set_desired_rate(g2.winch.get_rate_max());
714 g2.winch.set_desired_rate(0.0
f);
724 ap.rc_override_enable =
true;
728 ap.rc_override_enable =
false;
740 float roll_trim =
ToRad((
float)channel_roll->get_control_in()/100.0f);
741 float pitch_trim =
ToRad((
float)channel_pitch->get_control_in()/100.0f);
751 if (auto_trim_counter > 0) {
758 float roll_trim_adjustment =
ToRad((
float)channel_roll->get_control_in() / 4000.0f);
761 float pitch_trim_adjustment =
ToRad((
float)channel_pitch->get_control_in() / 4000.0f);
765 ahrs.
add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
768 if (auto_trim_counter == 0) {
void init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
uint32_t user_mode_change_failed
uint32_t user_mode_change
#define AUX_SWITCH_PWM_TRIGGER_LOW
void read_control_switch()
#define DATA_ACRO_TRAINER_LEVELING
#define DATA_FENCE_ENABLE
int16_t get_radio_in() const
static RC_Channel * rc_channel(uint8_t chan)
#define DATA_ACRO_TRAINER_LIMITED
#define DATA_GRIPPER_RELEASE
#define read_aux_switch(chan, flag, option)
#define DATA_AVOIDANCE_PROXIMITY_ENABLE
#define CONTROL_SWITCH_DEBOUNCE_TIME_MS
#define ACRO_TRAINER_DISABLED
#define ACRO_TRAINER_LEVELING
virtual void add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom=true)
uint8_t read_3pos_switch(uint8_t chan)
#define DATA_PARACHUTE_ENABLED
AP_MotorsMatrix motors(400)
void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
void reset_control_switch()
bool check_if_auxsw_mode_used(uint8_t auxsw_mode_check)
bool check_duplicate_auxsw(void)
#define DATA_WINCH_LENGTH_CONTROL
#define AUX_SWITCH_PWM_TRIGGER_HIGH
#define DATA_AVOIDANCE_ADSB_DISABLE
#define DATA_PARACHUTE_DISABLED
#define DATA_WINCH_RELAXED
bool debounce_aux_switch(uint8_t chan, uint8_t ch_flag)
#define BIT_IS_SET(value, bitnumber)
#define DATA_AVOIDANCE_ADSB_ENABLE
#define DATA_FENCE_DISABLE
void send_text(MAV_SEVERITY severity, const char *fmt,...)
#define DATA_GRIPPER_GRAB
#define DATA_AVOIDANCE_PROXIMITY_DISABLE
static struct notify_flags_and_values_type flags
#define DATA_ACRO_TRAINER_DISABLED
#define ACRO_TRAINER_LIMITED
#define AUX_SWITCH_MIDDLE
static struct notify_events_type events
#define DATA_SAVEWP_ADD_WP