void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag)
{
switch (ch_option) {
case AUX_FUNC::DO_NOTHING:
break;
case AUX_FUNC::SAVE_WP:
if (ch_flag == HIGH) {
// do nothing if in AUTO mode
if (rover.control_mode == &rover.mode_auto) {
return;
}
// if disarmed clear mission and set home to current location
if (!rover.arming.is_armed()) {
rover.mode_auto.mission.clear();
if (!rover.set_home_to_current_location(false)) {
// ignored
}
return;
}
// record the waypoint if not in auto mode
if (rover.control_mode != &rover.mode_auto) {
if (rover.mode_auto.mission.num_commands() == 0) {
// add a home location....
add_waypoint_for_current_loc();
}
add_waypoint_for_current_loc();
}
}
break;
// learn cruise speed and throttle
case AUX_FUNC::LEARN_CRUISE:
if (ch_flag == HIGH) {
rover.cruise_learn_start();
}
break;
// arm or disarm the motors
case AUX_FUNC::ARMDISARM:
if (ch_flag == HIGH) {
rover.arm_motors(AP_Arming::Method::RUDDER);
} else if (ch_flag == LOW) {
rover.disarm_motors();
}
break;
// set mode to Manual
case AUX_FUNC::MANUAL:
do_aux_function_change_mode(rover.mode_manual, ch_flag);
break;
// set mode to Acro
case AUX_FUNC::ACRO:
do_aux_function_change_mode(rover.mode_acro, ch_flag);
break;
// set mode to Steering
case AUX_FUNC::STEERING:
do_aux_function_change_mode(rover.mode_steering, ch_flag);
break;
// set mode to Hold
case AUX_FUNC::HOLD:
do_aux_function_change_mode(rover.mode_hold, ch_flag);
break;
// set mode to Auto
case AUX_FUNC::AUTO:
do_aux_function_change_mode(rover.mode_auto, ch_flag);
break;
// set mode to RTL
case AUX_FUNC::RTL:
do_aux_function_change_mode(rover.mode_rtl, ch_flag);
break;
// set mode to SmartRTL
case AUX_FUNC::SMART_RTL:
do_aux_function_change_mode(rover.mode_smartrtl, ch_flag);
break;
// set mode to Guided
case AUX_FUNC::GUIDED:
do_aux_function_change_mode(rover.mode_guided, ch_flag);
break;
// Set mode to LOITER
case AUX_FUNC::LOITER:
do_aux_function_change_mode(rover.mode_loiter, ch_flag);
break;
// Set mode to Follow
case AUX_FUNC::FOLLOW:
do_aux_function_change_mode(rover.mode_follow, ch_flag);
break;
// set mode to Simple
case AUX_FUNC::SIMPLE:
do_aux_function_change_mode(rover.mode_simple, ch_flag);
break;
// trigger sailboat tack
case AUX_FUNC::SAILBOAT_TACK:
// any switch movement interpreted as request to tack
rover.control_mode->handle_tack_request();
break;
default:
RC_Channel::do_aux_function(ch_option, ch_flag);
break;
}
}
// do_aux_function - implement the function invoked by auxillary switches
void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_switch_pos_t ch_flag)
{
switch(ch_option) {
case FLIP:
// flip if switch is on, positive throttle and we're actually flying
if (ch_flag == aux_switch_pos_t::HIGH) {
copter.set_mode(control_mode_t::FLIP, MODE_REASON_TX_COMMAND);
}
break;
case SIMPLE_MODE:
// low = simple mode off, middle or high position turns simple mode on
copter.set_simple_mode(ch_flag == HIGH || ch_flag == MIDDLE);
break;
case SUPERSIMPLE_MODE:
// low = simple mode off, middle = simple mode, high = super simple mode
copter.set_simple_mode(ch_flag);
break;
case RTL:
#if MODE_RTL_ENABLED == ENABLED
do_aux_function_change_mode(control_mode_t::RTL, ch_flag);
#endif
break;
case SAVE_TRIM:
if ((ch_flag == HIGH) && (copter.control_mode <= control_mode_t::ACRO) && (copter.channel_throttle->get_control_in() == 0)) {
copter.save_trim();
}
break;
case SAVE_WP:
#if MODE_AUTO_ENABLED == ENABLED
// save waypoint when switch is brought high
if (ch_flag == HIGH) {
// do not allow saving new waypoints while we're in auto or disarmed
if (copter.control_mode == control_mode_t::AUTO || !copter.motors->armed()) {
return;
}
// do not allow saving the first waypoint with zero throttle
if ((copter.mode_auto.mission.num_commands() == 0) && (copter.channel_throttle->get_control_in() == 0)) {
return;
}
// create new mission command
AP_Mission::Mission_Command cmd = {};
// if the mission is empty save a takeoff command
if (copter.mode_auto.mission.num_commands() == 0) {
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
cmd.id = MAV_CMD_NAV_TAKEOFF;
cmd.content.location.alt = MAX(copter.current_loc.alt,100);
// use the current altitude for the target alt for takeoff.
// only altitude will matter to the AP mission script for takeoff.
if (copter.mode_auto.mission.add_cmd(cmd)) {
// log event
copter.Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
// set new waypoint to current location
cmd.content.location = copter.current_loc;
// if throttle is above zero, create waypoint command
if (copter.channel_throttle->get_control_in() > 0) {
cmd.id = MAV_CMD_NAV_WAYPOINT;
} else {
// with zero throttle, create LAND command
cmd.id = MAV_CMD_NAV_LAND;
}
// save command
if (copter.mode_auto.mission.add_cmd(cmd)) {
// log event
copter.Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
#endif
break;
case MISSION_RESET:
#if MODE_AUTO_ENABLED == ENABLED
if (ch_flag == HIGH) {
copter.mode_auto.mission.reset();
}
#endif
break;
case AUTO:
#if MODE_AUTO_ENABLED == ENABLED
do_aux_function_change_mode(control_mode_t::AUTO, ch_flag);
#endif
break;
case RANGEFINDER:
// enable or disable the rangefinder
#if RANGEFINDER_ENABLED == ENABLED
if ((ch_flag == HIGH) && copter.rangefinder.has_orientation(ROTATION_PITCH_270)) {
copter.rangefinder_state.enabled = true;
} else {
copter.rangefinder_state.enabled = false;
}
#endif
break;
case FENCE:
#if AC_FENCE == ENABLED
// enable or disable the fence
if (ch_flag == HIGH) {
copter.fence.enable(true);
copter.Log_Write_Event(DATA_FENCE_ENABLE);
} else {
copter.fence.enable(false);
copter.Log_Write_Event(DATA_FENCE_DISABLE);
}
#endif
break;
case ACRO_TRAINER:
#if MODE_ACRO_ENABLED == ENABLED
switch(ch_flag) {
case LOW:
copter.g.acro_trainer = ACRO_TRAINER_DISABLED;
copter.Log_Write_Event(DATA_ACRO_TRAINER_DISABLED);
break;
case MIDDLE:
copter.g.acro_trainer = ACRO_TRAINER_LEVELING;
copter.Log_Write_Event(DATA_ACRO_TRAINER_LEVELING);
break;
case HIGH:
copter.g.acro_trainer = ACRO_TRAINER_LIMITED;
copter.Log_Write_Event(DATA_ACRO_TRAINER_LIMITED);
break;
}
#endif
break;
case AUTOTUNE:
#if AUTOTUNE_ENABLED == ENABLED
do_aux_function_change_mode(control_mode_t::AUTOTUNE, ch_flag);
#endif
break;
case LAND:
do_aux_function_change_mode(control_mode_t::LAND, ch_flag);
break;
case GUIDED:
do_aux_function_change_mode(control_mode_t::GUIDED, ch_flag);
break;
case PARACHUTE_ENABLE:
#if PARACHUTE == ENABLED
// Parachute enable/disable
copter.parachute.enabled(ch_flag == HIGH);
#endif
break;
case PARACHUTE_RELEASE:
#if PARACHUTE == ENABLED
if (ch_flag == HIGH) {
copter.parachute_manual_release();
}
#endif
break;
case PARACHUTE_3POS:
#if PARACHUTE == ENABLED
// Parachute disable, enable, release with 3 position switch
switch (ch_flag) {
case LOW:
copter.parachute.enabled(false);
copter.Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case MIDDLE:
copter.parachute.enabled(true);
copter.Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case HIGH:
copter.parachute.enabled(true);
copter.parachute_manual_release();
break;
}
#endif
break;
case ATTCON_FEEDFWD:
// enable or disable feed forward
copter.attitude_control->bf_feedforward(ch_flag == HIGH);
break;
case ATTCON_ACCEL_LIM:
// enable or disable accel limiting by restoring defaults
copter.attitude_control->accel_limiting(ch_flag == HIGH);
break;
case RETRACT_MOUNT:
#if MOUNT == ENABLE
switch (ch_flag) {
case HIGH:
copter.camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);
break;
case MIDDLE:
// nothing
break;
case LOW:
copter.camera_mount.set_mode_to_default();
break;
}
#endif
break;
case MOTOR_ESTOP:
// Turn on Emergency Stop logic when channel is high
copter.set_motor_emergency_stop(ch_flag == HIGH);
break;
case MOTOR_INTERLOCK:
// Turn on when above LOW, because channel will also be used for speed
// control signal in tradheli
copter.ap.motor_interlock_switch = (ch_flag == HIGH || ch_flag == MIDDLE);
break;
case BRAKE:
#if MODE_BRAKE_ENABLED == ENABLED
do_aux_function_change_mode(control_mode_t::BRAKE, ch_flag);
#endif
break;
case THROW:
#if MODE_THROW_ENABLED == ENABLED
do_aux_function_change_mode(control_mode_t::THROW, ch_flag);
#endif
break;
case AVOID_ADSB:
#if ADSB_ENABLED == ENABLED
// enable or disable AP_Avoidance
if (ch_flag == HIGH) {
copter.avoidance_adsb.enable();
copter.Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE);
} else {
copter.avoidance_adsb.disable();
copter.Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE);
}
#endif
break;
case PRECISION_LOITER:
#if PRECISION_LANDING == ENABLED && MODE_LOITER_ENABLED == ENABLED
switch (ch_flag) {
case HIGH:
copter.mode_loiter.set_precision_loiter_enabled(true);
break;
case MIDDLE:
// nothing
break;
case LOW:
copter.mode_loiter.set_precision_loiter_enabled(false);
break;
}
#endif
break;
case ARMDISARM:
// arm or disarm the vehicle
switch (ch_flag) {
case HIGH:
copter.init_arm_motors(AP_Arming::ArmingMethod::AUXSWITCH);
// remember that we are using an arming switch, for use by set_throttle_zero_flag
copter.ap.armed_with_switch = true;
break;
case MIDDLE:
// nothing
break;
case LOW:
copter.init_disarm_motors();
break;
}
break;
case SMART_RTL:
#if MODE_SMARTRTL_ENABLED == ENABLED
do_aux_function_change_mode(control_mode_t::SMART_RTL, ch_flag);
#endif
break;
case INVERTED:
#if FRAME_CONFIG == HELI_FRAME
switch (ch_flag) {
case HIGH:
copter.motors->set_inverted_flight(true);
copter.attitude_control->set_inverted_flight(true);
copter.heli_flags.inverted_flight = true;
break;
case MIDDLE:
// nothing
break;
case LOW:
copter.motors->set_inverted_flight(false);
copter.attitude_control->set_inverted_flight(false);
copter.heli_flags.inverted_flight = false;
break;
}
#endif
break;
case WINCH_ENABLE:
#if WINCH_ENABLED == ENABLED
switch (ch_flag) {
case HIGH:
// high switch maintains current position
copter.g2.winch.release_length(0.0f);
copter.Log_Write_Event(DATA_WINCH_LENGTH_CONTROL);
break;
default:
// all other position relax winch
copter.g2.winch.relax();
copter.Log_Write_Event(DATA_WINCH_RELAXED);
break;
}
#endif
break;
case WINCH_CONTROL:
#if WINCH_ENABLED == ENABLED
switch (ch_flag) {
case LOW:
// raise winch at maximum speed
copter.g2.winch.set_desired_rate(-copter.g2.winch.get_rate_max());
break;
case HIGH:
// lower winch at maximum speed
copter.g2.winch.set_desired_rate(copter.g2.winch.get_rate_max());
break;
case MIDDLE:
copter.g2.winch.set_desired_rate(0.0f);
break;
}
#endif
break;
#ifdef USERHOOK_AUXSWITCH
case USER_FUNC1:
userhook_auxSwitch1(ch_flag);
break;
case USER_FUNC2:
userhook_auxSwitch2(ch_flag);
break;
case USER_FUNC3:
userhook_auxSwitch3(ch_flag);
break;
#endif
case ZIGZAG:
#if MODE_ZIGZAG_ENABLED == ENABLED
do_aux_function_change_mode(control_mode_t::ZIGZAG, ch_flag);
#endif
break;
case ZIGZAG_SaveWP:
#if MODE_ZIGZAG_ENABLED == ENABLED
if (copter.flightmode == &copter.mode_zigzag) {
switch (ch_flag) {
case LOW:
copter.mode_zigzag.save_or_move_to_destination(0);
break;
case MIDDLE:
copter.mode_zigzag.return_to_manual_control();
break;
case HIGH:
copter.mode_zigzag.save_or_move_to_destination(1);
break;
}
}
#endif
break;
default:
RC_Channel::do_aux_function(ch_option, ch_flag);
break;
}
}
