void Copter::read_control_switch() { uint32_t tnow_ms = millis(); // calculate position of flight mode switch int8_t switch_position; uint16_t rc5_in = RC_Channels::rc_channel(CH_5)->get_radio_in(); if (rc5_in < 1231) switch_position = 0; else if (rc5_in < 1361) switch_position = 1; else if (rc5_in < 1491) switch_position = 2; else if (rc5_in < 1621) switch_position = 3; else if (rc5_in < 1750) switch_position = 4; else switch_position = 5; // store time that switch last moved if (control_switch_state.last_switch_position != switch_position) { control_switch_state.last_edge_time_ms = tnow_ms; } // debounce switch bool control_switch_changed = control_switch_state.debounced_switch_position != switch_position; bool sufficient_time_elapsed = tnow_ms - control_switch_state.last_edge_time_ms > CONTROL_SWITCH_DEBOUNCE_TIME_MS; bool failsafe_disengaged = !failsafe.radio && failsafe.radio_counter == 0; if (control_switch_changed && sufficient_time_elapsed && failsafe_disengaged) { // set flight mode and simple mode setting if (set_mode((control_mode_t)flight_modes[switch_position].get(), MODE_REASON_TX_COMMAND)) { // play a tone if (control_switch_state.debounced_switch_position != -1) { // alert user to mode change failure (except if autopilot is just starting up) if (ap.initialised) { AP_Notify::events.user_mode_change = 1; } } if (!check_if_auxsw_mode_used(AUXSW_SIMPLE_MODE) && !check_if_auxsw_mode_used(AUXSW_SUPERSIMPLE_MODE)) { // if none of the Aux Switches are set to Simple or Super Simple Mode then // set Simple Mode using stored parameters from EEPROM if (BIT_IS_SET(g.super_simple, switch_position)) { set_simple_mode(2); } else { set_simple_mode(BIT_IS_SET(g.simple_modes, switch_position)); } } } else if (control_switch_state.last_switch_position != -1) { // alert user to mode change failure AP_Notify::events.user_mode_change_failed = 1; } // set the debounced switch position control_switch_state.debounced_switch_position = switch_position; } control_switch_state.last_switch_position = switch_position; }
// perform pre-arm checks and set ap.pre_arm_check flag // return true if the checks pass successfully bool Copter::pre_arm_checks(bool display_failure) { // exit immediately if already armed if (motors.armed()) { return true; } // check if motor interlock and Emergency Stop aux switches are used // at the same time. This cannot be allowed. if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict"); } return false; } // check if motor interlock aux switch is in use // if it is, switch needs to be in disabled position to arm // otherwise exit immediately. This check to be repeated, // as state can change at any time. if (ap.using_interlock && ap.motor_interlock_switch) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled"); } return false; } // exit immediately if we've already successfully performed the pre-arm check if (ap.pre_arm_check) { // run gps checks because results may change and affect LED colour // no need to display failures because arm_checks will do that if the pilot tries to arm pre_arm_gps_checks(false); return true; } // succeed if pre arm checks are disabled if (g.arming_check == ARMING_CHECK_NONE) { set_pre_arm_check(true); set_pre_arm_rc_check(true); return true; } return barometer_checks(display_failure) & rc_calibration_checks(display_failure) & compass_checks(display_failure) & gps_checks(display_failure) & fence_checks(display_failure) & ins_checks(display_failure) & board_voltage_checks(display_failure) & parameter_checks(display_failure) & pilot_throttle_checks(display_failure); }
// check for pilot stick input to trigger lost vehicle alarm void Copter::lost_vehicle_check() { static uint8_t soundalarm_counter; // disable if aux switch is setup to vehicle alarm as the two could interfere if (check_if_auxsw_mode_used(AUXSW_LOST_COPTER_SOUND)) { return; } // ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch if (ap.throttle_zero && !motors.armed() && (channel_roll->control_in > 4000) && (channel_pitch->control_in > 4000)) { if (soundalarm_counter >= LOST_VEHICLE_DELAY) { if (AP_Notify::flags.vehicle_lost == false) { AP_Notify::flags.vehicle_lost = true; gcs_send_text(MAV_SEVERITY_NOTICE,"Locate Copter Alarm!"); } } else { soundalarm_counter++; } } else { soundalarm_counter = 0; if (AP_Notify::flags.vehicle_lost == true) { AP_Notify::flags.vehicle_lost = false; } } }
// Run landing gear controller at 10Hz void Sub::landinggear_update(){ // If landing gear control is active, run update function. if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){ // last status (deployed or retracted) used to check for changes static bool last_deploy_status; // if we are doing an automatic landing procedure, force the landing gear to deploy. // To-Do: should we pause the auto-land procedure to give time for gear to come down? if (control_mode == LAND || (control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || (control_mode == AUTO && auto_mode == Auto_Land) || (control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) { landinggear.force_deploy(true); } landinggear.update(); // send event message to datalog if status has changed if (landinggear.deployed() != last_deploy_status){ if (landinggear.deployed()) { Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED); } else { Log_Write_Event(DATA_LANDING_GEAR_RETRACTED); } } last_deploy_status = landinggear.deployed(); } }
// init_aux_switch_function - initialize aux functions void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag) { // init channel options switch(ch_option) { case AUXSW_SIMPLE_MODE: case AUXSW_SONAR: case AUXSW_FENCE: case AUXSW_RESETTOARMEDYAW: case AUXSW_SUPERSIMPLE_MODE: case AUXSW_ACRO_TRAINER: case AUXSW_EPM: case AUXSW_SPRAYER: case AUXSW_PARACHUTE_ENABLE: case AUXSW_PARACHUTE_3POS: // we trust the vehicle will be disarmed so even if switch is in release position the chute will not release case AUXSW_RETRACT_MOUNT: case AUXSW_MISSION_RESET: case AUXSW_ATTCON_FEEDFWD: case AUXSW_ATTCON_ACCEL_LIM: case AUXSW_RELAY: case AUXSW_LANDING_GEAR: case AUXSW_MOTOR_ESTOP: do_aux_switch_function(ch_option, ch_flag); break; case AUXSW_MOTOR_INTERLOCK: set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); do_aux_switch_function(ch_option, ch_flag); break; } }
void Copter::update_using_interlock() { #if FRAME_CONFIG == HELI_FRAME // helicopters are always using motor interlock ap.using_interlock = true; #else // check if we are using motor interlock control on an aux switch ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK); #endif }
void Copter::update_using_interlock() { #if FRAME_CONFIG == HELI_FRAME // helicopters are always using motor interlock ap.using_interlock = true; #else // check if we are using motor interlock control on an aux switch or are in throw mode // which uses the interlock to stop motors while the copter is being thrown ap.using_interlock = check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK); #endif }
// 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; }
// perform pre-arm checks and set ap.pre_arm_check flag // return true if the checks pass successfully bool Copter::pre_arm_checks(bool display_failure) { // exit immediately if already armed if (motors.armed()) { return true; } // check if motor interlock and Emergency Stop aux switches are used // at the same time. This cannot be allowed. if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict"); } return false; } // check if motor interlock aux switch is in use // if it is, switch needs to be in disabled position to arm // otherwise exit immediately. This check to be repeated, // as state can change at any time. if (ap.using_interlock && ap.motor_interlock_switch) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled"); } return false; } // exit immediately if we've already successfully performed the pre-arm check if (ap.pre_arm_check) { // run gps checks because results may change and affect LED colour // no need to display failures because arm_checks will do that if the pilot tries to arm pre_arm_gps_checks(false); return true; } // succeed if pre arm checks are disabled if (g.arming_check == ARMING_CHECK_NONE) { set_pre_arm_check(true); set_pre_arm_rc_check(true); return true; } // pre-arm rc checks a prerequisite pre_arm_rc_checks(); if (!ap.pre_arm_rc_check) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC not calibrated"); } return false; } // check Baro if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { // barometer health check if (!barometer.all_healthy()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: 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) { if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity"); } return false; } } } // check Compass if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) { //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; } // check the primary compass is healthy if (!compass.healthy()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not healthy"); } return false; } // check compass learning is on or offsets have been set if (!compass.configured()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated"); } return false; } // check for unreasonable compass offsets Vector3f offsets = compass.get_offsets(); if (offsets.length() > COMPASS_OFFSETS_MAX) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass offsets too high"); } return false; } // check for unreasonable mag field length float mag_field = compass.get_field().length(); if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check mag field"); } return false; } // check all compasses point in roughly same direction if (!compass.consistent()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent compasses"); } return false; } } // check GPS if (!pre_arm_gps_checks(display_failure)) { return false; } #if AC_FENCE == ENABLED // check fence is initialised if (!fence.pre_arm_check()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check fence"); } return false; } #endif // check INS if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { // check accelerometers have been calibrated if (!ins.accel_calibrated_ok_all()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accels not calibrated"); } return false; } // check accels are healthy if (!ins.get_accel_health_all()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accelerometers not healthy"); } return false; } //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; } // check all accelerometers point in roughly same direction if (ins.get_accel_count() > 1) { const Vector3f &prime_accel_vec = ins.get_accel(); for(uint8_t i=0; i<ins.get_accel_count(); i++) { // get next accel vector const Vector3f &accel_vec = ins.get_accel(i); Vector3f vec_diff = accel_vec - prime_accel_vec; float threshold = PREARM_MAX_ACCEL_VECTOR_DIFF; if (i >= 2) { /* * for boards with 3 IMUs we only use the first two * in the EKF. Allow for larger accel discrepancy * for IMU3 as it may be running at a different temperature */ threshold *= 2; } // EKF is less sensitive to Z-axis error vec_diff.z *= 0.5f; if (vec_diff.length() > threshold) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Accelerometers"); } return false; } } } // check gyros are healthy if (!ins.get_gyro_health_all()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Gyros not healthy"); } return false; } // check all gyros are consistent if (ins.get_gyro_count() > 1) { for(uint8_t i=0; i<ins.get_gyro_count(); i++) { // get rotation rate difference between gyro #i and primary gyro Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro(); if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Gyros"); } 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,"PreArm: gyros still settling"); } return false; } } #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1 // check board voltage if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { if (hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage"); } return false; } } #endif #endif // 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,"PreArm: Check Battery"); } return false; } } // check various parameter values if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { // ensure ch7 and ch8 have different functions if (check_duplicate_auxsw()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options"); } return false; } // failsafe parameter checks if (g.failsafe_throttle) { // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900 if (channel_throttle->get_radio_min() <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE"); } return false; } } // lean angle parameter check if (aparm.angle_max < 1000 || aparm.angle_max > 8000) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX"); } return false; } // acro balance parameter check if ((g.acro_balance_roll > attitude_control.get_angle_roll_p().kP()) || (g.acro_balance_pitch > attitude_control.get_angle_pitch_p().kP())) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH"); } return false; } #if RANGEFINDER_ENABLED == ENABLED && OPTFLOW == ENABLED // check range finder if optflow enabled if (optflow.enabled() && !rangefinder.pre_arm_check()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder"); } return false; } #endif #if FRAME_CONFIG == HELI_FRAME // check helicopter parameters if (!motors.parameter_check(display_failure)) { return false; } #endif // HELI_FRAME // check for missing terrain data if (!pre_arm_terrain_check(display_failure)) { return false; } // check adsb avoidance failsafe if (failsafe.adsb) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: ADSB threat detected"); } return false; } } // check throttle is above failsafe throttle // this is near the bottom to allow other failures to be displayed before checking pilot throttle if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { 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,"PreArm: Collective below Failsafe"); #else gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe"); #endif } return false; } } return true; }
// one_hz_loop - runs at 1Hz void Copter::one_hz_loop() { if (should_log(MASK_LOG_ANY)) { Log_Write_Data(DATA_AP_STATE, ap.value); } // perform pre-arm checks & display failures every 30 seconds static uint8_t pre_arm_display_counter = 15; pre_arm_display_counter++; if (pre_arm_display_counter >= 30) { pre_arm_checks(true); pre_arm_display_counter = 0; }else{ pre_arm_checks(false); } // auto disarm checks auto_disarm_check(); if (!motors.armed()) { // make it possible to change ahrs orientation at runtime during initial config ahrs.set_orientation(); #if FRAME_CONFIG == HELI_FRAME // helicopters are always using motor interlock set_using_interlock(true); #else // check the user hasn't updated the frame orientation motors.set_frame_orientation(g.frame_orientation); // check if we are using motor interlock control on an aux switch set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); // set all throttle channel settings motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max); // set hover throttle motors.set_hover_throttle(g.throttle_mid); #endif } // update assigned functions and enable auxiliar servos RC_Channel_aux::enable_aux_servos(); check_usb_mux(); #if AP_TERRAIN_AVAILABLE terrain.update(); // tell the rangefinder our height, so it can go into power saving // mode if available #if CONFIG_SONAR == ENABLED float height; if (terrain.height_above_terrain(height, true)) { sonar.set_estimated_terrain_height(height); } #endif #endif // update position controller alt limits update_poscon_alt_max(); // enable/disable raw gyro/accel logging ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); }
// 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; }