// 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; } }
// heli_init - perform any special initialisation required for the tradheli void Copter::heli_init() { // helicopters are always using motor interlock set_using_interlock(true); /* automatically set H_RSC_MIN and H_RSC_MAX from RC8_MIN and RC8_MAX so that when users upgrade from tradheli version 3.3 to 3.4 they get the same throttle range as in previous versions of the code */ if (!g.heli_servo_rsc.radio_min.configured()) { g.heli_servo_rsc.radio_min.set_and_save(g.rc_8.radio_min.get()); } if (!g.heli_servo_rsc.radio_max.configured()) { g.heli_servo_rsc.radio_max.set_and_save(g.rc_8.radio_max.get()); } }
// 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)); }
// 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_P(MAV_SEVERITY_CRITICAL,PSTR("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. set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK)); if (ap.using_interlock && motors.get_interlock()){ if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Motor Interlock Enabled")); } return false; } // if we are using Motor Emergency Stop aux switch, check it is not enabled // and warn if it is if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Motor Emergency Stopped")); } 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_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Altitude disparity")); } return false; } } } // check Compass if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) { // check the primary compass is healthy if(!compass.healthy()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check mag field")); } return false; } // check all compasses point in roughly same direction if (!compass.consistent()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Accels not calibrated")); } return false; } // check accels are healthy if(!ins.get_accel_health_all()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Accelerometers not healthy")); } 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; } if (vec_diff.length() > threshold) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent Accelerometers")); } return false; } } } // check gyros are healthy if(!ins.get_gyro_health_all()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("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->radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("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_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Check ANGLE_MAX")); } return false; } // acro balance parameter check if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: ACRO_BAL_ROLL/PITCH")); } return false; } #if CONFIG_SONAR == ENABLED && OPTFLOW == ENABLED // check range finder if optflow enabled if (optflow.enabled() && !sonar.pre_arm_check()) { if (display_failure) { gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("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 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->radio_in < g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Collective below Failsafe")); #else gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("PreArm: Throttle below Failsafe")); #endif } return false; } } // if we've gotten this far then pre arm checks have completed set_pre_arm_check(true); 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; }
// heli_init - perform any special initialisation required for the tradheli void Copter::heli_init() { // helicopters are always using motor interlock set_using_interlock(true); }