// do_aux_switch_function - implement the function invoked by the ch7 or ch8 switch
void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
{
switch(ch_function) {
case AUXSW_FLIP:
// flip if switch is on, positive throttle and we're actually flying
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(FLIP, MODE_REASON_TX_COMMAND);
}
break;
case AUXSW_SIMPLE_MODE:
// low = simple mode off, middle or high position turns simple mode on
set_simple_mode(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
break;
case AUXSW_SUPERSIMPLE_MODE:
// low = simple mode off, middle = simple mode, high = super simple mode
set_simple_mode(ch_flag);
break;
case AUXSW_RTL:
if (ch_flag == AUX_SWITCH_HIGH) {
// engage RTL (if not possible we remain in current flight mode)
set_mode(RTL, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in RTL
if (control_mode == RTL) {
reset_control_switch();
}
}
break;
case AUXSW_SAVE_TRIM:
if ((ch_flag == AUX_SWITCH_HIGH) && (control_mode <= ACRO) && (channel_throttle->get_control_in() == 0)) {
save_trim();
}
break;
case AUXSW_SAVE_WP:
// save waypoint when switch is brought high
if (ch_flag == AUX_SWITCH_HIGH) {
// do not allow saving new waypoints while we're in auto or disarmed
if (control_mode == AUTO || !motors.armed()) {
return;
}
// do not allow saving the first waypoint with zero throttle
if ((mission.num_commands() == 0) && (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 (mission.num_commands() == 0) {
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
cmd.id = MAV_CMD_NAV_TAKEOFF;
cmd.content.location.options = 0;
cmd.p1 = 0;
cmd.content.location.lat = 0;
cmd.content.location.lng = 0;
cmd.content.location.alt = MAX(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 (mission.add_cmd(cmd)) {
// log event
Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
// set new waypoint to current location
cmd.content.location = current_loc;
// if throttle is above zero, create waypoint command
if (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 (mission.add_cmd(cmd)) {
// log event
Log_Write_Event(DATA_SAVEWP_ADD_WP);
}
}
break;
case AUXSW_CAMERA_TRIGGER:
#if CAMERA == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
do_take_picture();
}
#endif
break;
case AUXSW_RANGEFINDER:
// enable or disable the rangefinder
#if RANGEFINDER_ENABLED == ENABLED
if ((ch_flag == AUX_SWITCH_HIGH) && (rangefinder.num_sensors() >= 1)) {
rangefinder_state.enabled = true;
} else {
rangefinder_state.enabled = false;
}
#endif
break;
case AUXSW_FENCE:
#if AC_FENCE == ENABLED
// enable or disable the fence
if (ch_flag == AUX_SWITCH_HIGH) {
fence.enable(true);
Log_Write_Event(DATA_FENCE_ENABLE);
} else {
fence.enable(false);
Log_Write_Event(DATA_FENCE_DISABLE);
}
#endif
break;
case AUXSW_ACRO_TRAINER:
switch(ch_flag) {
case AUX_SWITCH_LOW:
g.acro_trainer = ACRO_TRAINER_DISABLED;
Log_Write_Event(DATA_ACRO_TRAINER_DISABLED);
break;
case AUX_SWITCH_MIDDLE:
g.acro_trainer = ACRO_TRAINER_LEVELING;
Log_Write_Event(DATA_ACRO_TRAINER_LEVELING);
break;
case AUX_SWITCH_HIGH:
g.acro_trainer = ACRO_TRAINER_LIMITED;
Log_Write_Event(DATA_ACRO_TRAINER_LIMITED);
break;
}
break;
case AUXSW_GRIPPER:
#if GRIPPER_ENABLED == ENABLED
switch(ch_flag) {
case AUX_SWITCH_LOW:
g2.gripper.release();
Log_Write_Event(DATA_GRIPPER_RELEASE);
break;
case AUX_SWITCH_HIGH:
g2.gripper.grab();
Log_Write_Event(DATA_GRIPPER_GRAB);
break;
}
#endif
break;
case AUXSW_SPRAYER:
#if SPRAYER == ENABLED
sprayer.run(ch_flag == AUX_SWITCH_HIGH);
// if we are disarmed the pilot must want to test the pump
sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors.armed());
#endif
break;
case AUXSW_AUTO:
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(AUTO, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in AUTO
if (control_mode == AUTO) {
reset_control_switch();
}
}
break;
case AUXSW_AUTOTUNE:
#if AUTOTUNE_ENABLED == ENABLED
// turn on auto tuner
switch(ch_flag) {
case AUX_SWITCH_LOW:
case AUX_SWITCH_MIDDLE:
// restore flight mode based on flight mode switch position
if (control_mode == AUTOTUNE) {
reset_control_switch();
}
break;
case AUX_SWITCH_HIGH:
// start an autotuning session
set_mode(AUTOTUNE, MODE_REASON_TX_COMMAND);
break;
}
#endif
break;
case AUXSW_LAND:
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(LAND, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in LAND
if (control_mode == LAND) {
reset_control_switch();
}
}
break;
case AUXSW_PARACHUTE_ENABLE:
#if PARACHUTE == ENABLED
// Parachute enable/disable
parachute.enabled(ch_flag == AUX_SWITCH_HIGH);
#endif
break;
case AUXSW_PARACHUTE_RELEASE:
#if PARACHUTE == ENABLED
if (ch_flag == AUX_SWITCH_HIGH) {
parachute_manual_release();
}
#endif
break;
case AUXSW_PARACHUTE_3POS:
#if PARACHUTE == ENABLED
// Parachute disable, enable, release with 3 position switch
switch (ch_flag) {
case AUX_SWITCH_LOW:
parachute.enabled(false);
Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case AUX_SWITCH_MIDDLE:
parachute.enabled(true);
Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case AUX_SWITCH_HIGH:
parachute.enabled(true);
parachute_manual_release();
break;
}
#endif
break;
case AUXSW_MISSION_RESET:
if (ch_flag == AUX_SWITCH_HIGH) {
mission.reset();
}
break;
case AUXSW_ATTCON_FEEDFWD:
// enable or disable feed forward
attitude_control.bf_feedforward(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_ATTCON_ACCEL_LIM:
// enable or disable accel limiting by restoring defaults
attitude_control.accel_limiting(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RETRACT_MOUNT:
#if MOUNT == ENABLE
switch (ch_flag) {
case AUX_SWITCH_HIGH:
camera_mount.set_mode(MAV_MOUNT_MODE_RETRACT);
break;
case AUX_SWITCH_LOW:
camera_mount.set_mode_to_default();
break;
}
#endif
break;
case AUXSW_RELAY:
ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY2:
ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY3:
ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY4:
ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_LANDING_GEAR:
switch (ch_flag) {
case AUX_SWITCH_LOW:
landinggear.set_cmd_mode(LandingGear_Deploy);
break;
case AUX_SWITCH_MIDDLE:
landinggear.set_cmd_mode(LandingGear_Auto);
break;
case AUX_SWITCH_HIGH:
landinggear.set_cmd_mode(LandingGear_Retract);
break;
}
break;
case AUXSW_LOST_COPTER_SOUND:
switch (ch_flag) {
case AUX_SWITCH_HIGH:
AP_Notify::flags.vehicle_lost = true;
break;
case AUX_SWITCH_LOW:
AP_Notify::flags.vehicle_lost = false;
break;
}
break;
case AUXSW_MOTOR_ESTOP:
// Turn on Emergency Stop logic when channel is high
set_motor_emergency_stop(ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_MOTOR_INTERLOCK:
// Turn on when above LOW, because channel will also be used for speed
// control signal in tradheli
ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
break;
case AUXSW_BRAKE:
// brake flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(BRAKE, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in BRAKE
if (control_mode == BRAKE) {
reset_control_switch();
}
}
break;
case AUXSW_THROW:
// throw flight mode
if (ch_flag == AUX_SWITCH_HIGH) {
set_mode(THROW, MODE_REASON_TX_COMMAND);
} else {
// return to flight mode switch's flight mode if we are currently in throw mode
if (control_mode == THROW) {
reset_control_switch();
}
}
break;
case AUXSW_AVOID_ADSB:
// enable or disable AP_Avoidance
if (ch_flag == AUX_SWITCH_HIGH) {
avoidance_adsb.enable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_ENABLE);
} else {
avoidance_adsb.disable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE);
}
break;
case AUXSW_PRECISION_LOITER:
#if PRECISION_LANDING == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
set_precision_loiter_enabled(true);
break;
case AUX_SWITCH_LOW:
set_precision_loiter_enabled(false);
break;
}
#endif
break;
case AUXSW_AVOID_PROXIMITY:
#if PROXIMITY_ENABLED == ENABLED
switch (ch_flag) {
case AUX_SWITCH_HIGH:
avoid.proximity_avoidance_enable(true);
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_ENABLE);
break;
case AUX_SWITCH_LOW:
avoid.proximity_avoidance_enable(false);
Log_Write_Event(DATA_AVOIDANCE_PROXIMITY_DISABLE);
break;
}
#endif
break;
}
}
// arm_checks - perform final checks before arming
// always called just before arming. Return true if ok to arm
// has side-effect that logging is started
bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
{
#if LOGGING_ENABLED == ENABLED
// start dataflash
start_logging();
#endif
// check accels and gyro are healthy
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
//check if accelerometers have calibrated and require reboot
if (ins.accel_cal_requires_reboot()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accelerometers calibrated requires reboot");
}
return false;
}
if (!ins.get_accel_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Accelerometers not healthy");
}
return false;
}
if (!ins.get_gyro_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Gyros not healthy");
}
return false;
}
// get ekf attitude (if bad, it's usually the gyro biases)
if (!pre_arm_ekf_attitude_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: gyros still settling");
}
return false;
}
}
// always check if inertial nav has started and is ready
if (!ahrs.healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Nav Checks");
}
return false;
}
if (compass.is_calibrating()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running");
}
return false;
}
//check if compass has calibrated and requires reboot
if (compass.compass_cal_requires_reboot()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot");
}
return false;
}
// always check if the current mode allows arming
if (!mode_allows_arming(control_mode, arming_from_gcs)) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable");
}
return false;
}
// always check gps
if (!pre_arm_gps_checks(display_failure)) {
return false;
}
// if we are using motor interlock switch and it's enabled, fail to arm
// skip check in Throw mode which takes control of the motor interlock
if (ap.using_interlock && motors.get_interlock()) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled");
return false;
}
// if we are not using Emergency Stop switch option, force Estop false to ensure motors
// can run normally
if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
set_motor_emergency_stop(false);
// if we are using motor Estop switch, it must not be in Estop position
} else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped");
return false;
}
// succeed if arming checks are disabled
if (g.arming_check == ARMING_CHECK_NONE) {
return true;
}
// baro checks
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
// baro health check
if (!barometer.all_healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Barometer not healthy");
}
return false;
}
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Altitude disparity");
}
return false;
}
}
#if AC_FENCE == ENABLED
// check vehicle is within fence
if (!fence.pre_arm_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: check fence");
}
return false;
}
#endif
// check lean angle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning");
}
return false;
}
}
// check battery voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery");
}
return false;
}
}
// check for missing terrain data
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
if (!pre_arm_terrain_check(display_failure)) {
return false;
}
}
// check adsb
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
if (failsafe.adsb) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ADSB threat detected");
}
return false;
}
}
// check throttle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) {
// check throttle is not too low - must be above failsafe throttle
if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->get_radio_in() < g.failsafe_throttle_value) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe");
#endif
}
return false;
}
// check throttle is not too high - skips checks if arming from GCS in Guided
if (!(arming_from_gcs && (control_mode == GUIDED || control_mode == GUIDED_NOGPS))) {
// above top of deadband is too always high
if (get_pilot_desired_climb_rate(channel_throttle->get_control_in()) > 0.0f) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high");
#endif
}
return false;
}
// in manual modes throttle must be at zero
if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->get_control_in() > 0) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high");
#endif
}
return false;
}
}
}
// check if safety switch has been pushed
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Safety Switch");
}
return false;
}
// if we've gotten this far all is ok
return true;
}
// init_arm_motors - performs arming process including initialisation of barometer and gyros
// returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure
bool Copter::init_arm_motors(bool arming_from_gcs)
{
static bool in_arm_motors = false;
// exit immediately if already in this function
if (in_arm_motors) {
return false;
}
in_arm_motors = true;
// run pre-arm-checks and display failures
if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
// disable cpu failsafe because initialising everything takes a while
failsafe_disable();
// reset battery failsafe
set_failsafe_battery(false);
// notify that arming will occur (we do this early to give plenty of warning)
AP_Notify::flags.armed = true;
// call update_notify a few times to ensure the message gets out
for (uint8_t i=0; i<=10; i++) {
update_notify();
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("ARMING MOTORS"));
#endif
// Remember Orientation
// --------------------
init_simple_bearing();
initial_armed_bearing = ahrs.yaw_sensor;
if (ap.home_state == HOME_UNSET) {
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
ahrs.resetHeightDatum();
Log_Write_Event(DATA_EKF_ALT_RESET);
} else if (ap.home_state == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location();
}
calc_distance_and_bearing();
#if FRAME_CONFIG == HELI_FRAME
// helicopters are always using motor interlock
set_using_interlock(true);
#else
// check if we are using motor interlock control on an aux switch
set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK));
#endif
// if we are using motor interlock switch and it's enabled, fail to arm
if (ap.using_interlock && motors.get_interlock()){
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Motor Interlock Enabled"));
AP_Notify::flags.armed = false;
in_arm_motors = false;
return false;
}
// if we are not using Emergency Stop switch option, force Estop false to ensure motors
// can run normally
if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
set_motor_emergency_stop(false);
// if we are using motor Estop switch, it must not be in Estop position
} else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){
gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("Arm: Motor Emergency Stopped"));
AP_Notify::flags.armed = false;
in_arm_motors = false;
return false;
}
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);
#if SPRAYER == ENABLED
// turn off sprayer's test if on
sprayer.test_pump(false);
#endif
// short delay to allow reading of rc inputs
delay(30);
// enable output to motors
enable_motor_output();
// finally actually arm the motors
motors.armed(true);
// log arming to dataflash
Log_Write_Event(DATA_ARMED);
// log flight mode in case it was changed while vehicle was disarmed
DataFlash.Log_Write_Mode(control_mode);
// reenable failsafe
failsafe_enable();
// perf monitor ignores delay due to arming
perf_ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
// return success
return true;
}