// init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration void Copter::init_rc_out() { motors.set_update_rate(g.rc_speed); motors.set_frame_orientation(g.frame_orientation); motors.set_loop_rate(scheduler.get_loop_rate_hz()); motors.Init(); // motor initialisation #if FRAME_CONFIG != HELI_FRAME motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); #endif for(uint8_t i = 0; i < 5; i++) { delay(20); read_radio(); } // we want the input to be scaled correctly channel_throttle->set_range_out(0,1000); // setup correct scaling for ESCs like the UAVCAN PX4ESC which // take a proportion of speed. hal.rcout->set_esc_scaling(channel_throttle->get_radio_min(), channel_throttle->get_radio_max()); // check if we should enter esc calibration mode esc_calibration_startup_check(); // enable output to motors pre_arm_rc_checks(); if (ap.pre_arm_rc_check) { enable_motor_output(); } // refresh auxiliary channel to function map RC_Channel_aux::update_aux_servo_function(); }
// check if we should enter esc calibration mode void Copter::esc_calibration_startup_check() { #if FRAME_CONFIG != HELI_FRAME // exit immediately if pre-arm rc checks fail pre_arm_rc_checks(); if (!ap.pre_arm_rc_check) { // clear esc flag for next time if ((g.esc_calibrate != ESCCAL_NONE) && (g.esc_calibrate != ESCCAL_DISABLED)) { g.esc_calibrate.set_and_save(ESCCAL_NONE); } return; } // check ESC parameter switch (g.esc_calibrate) { case ESCCAL_NONE: // check if throttle is high if (channel_throttle->control_in >= ESC_CALIBRATION_HIGH_THROTTLE) { // we will enter esc_calibrate mode on next reboot g.esc_calibrate.set_and_save(ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH); // send message to gcs gcs_send_text_P(MAV_SEVERITY_CRITICAL,PSTR("ESC Calibration: restart board")); // turn on esc calibration notification AP_Notify::flags.esc_calibration = true; // block until we restart while(1) { delay(5); } } break; case ESCCAL_PASSTHROUGH_IF_THROTTLE_HIGH: // check if throttle is high if (channel_throttle->control_in >= ESC_CALIBRATION_HIGH_THROTTLE) { // pass through pilot throttle to escs esc_calibration_passthrough(); } break; case ESCCAL_PASSTHROUGH_ALWAYS: // pass through pilot throttle to escs esc_calibration_passthrough(); break; case ESCCAL_AUTO: // perform automatic ESC calibration esc_calibration_auto(); break; case ESCCAL_DISABLED: default: // do nothing break; } // clear esc flag for next time if (g.esc_calibrate != ESCCAL_DISABLED) { g.esc_calibrate.set_and_save(ESCCAL_NONE); } #endif // FRAME_CONFIG != HELI_FRAME }
bool Copter::rc_calibration_checks(bool display_failure) { // 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; } return true; }
// mavlink_motor_test_check - perform checks before motor tests can begin // return true if tests can continue, false if not bool Sub::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) { // check rc has been calibrated pre_arm_rc_checks(); if (check_rc && !ap.pre_arm_rc_check) { gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated"); return false; } // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { gcs_chan[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety switch"); return false; } // if we got this far the check was successful and the motor test can continue 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; }
// setup_compassmot - sets compass's motor interference parameters uint8_t Copter::mavlink_compassmot(mavlink_channel_t chan) { #if FRAME_CONFIG == HELI_FRAME // compassmot not implemented for tradheli return 1; #else int8_t comp_type; // throttle or current based compensation Vector3f compass_base[COMPASS_MAX_INSTANCES]; // compass vector when throttle is zero Vector3f motor_impact[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector Vector3f motor_impact_scaled[COMPASS_MAX_INSTANCES]; // impact of motors on compass vector scaled with throttle Vector3f motor_compensation[COMPASS_MAX_INSTANCES]; // final compensation to be stored to eeprom float throttle_pct; // throttle as a percentage 0.0 ~ 1.0 float throttle_pct_max = 0.0f; // maximum throttle reached (as a percentage 0~1.0) float current_amps_max = 0.0f; // maximum current reached float interference_pct[COMPASS_MAX_INSTANCES]; // interference as a percentage of total mag field (for reporting purposes only) uint32_t last_run_time; uint32_t last_send_time; bool updated = false; // have we updated the compensation vector at least once uint8_t command_ack_start = command_ack_counter; // exit immediately if we are already in compassmot if (ap.compass_mot) { // ignore restart messages return 1; }else{ ap.compass_mot = true; } // initialise output for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) { interference_pct[i] = 0.0f; } // check compass is enabled if (!g.compass_enabled) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Compass disabled"); ap.compass_mot = false; return 1; } // check compass health compass.read(); for (uint8_t i=0; i<compass.get_count(); i++) { if (!compass.healthy(i)) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Check compass"); ap.compass_mot = false; return 1; } } // check if radio is calibrated pre_arm_rc_checks(); if (!ap.pre_arm_rc_check) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "RC not calibrated"); ap.compass_mot = false; return 1; } // check throttle is at zero read_radio(); if (channel_throttle->control_in != 0) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Throttle not zero"); ap.compass_mot = false; return 1; } // check we are landed if (!ap.land_complete) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL, "Not landed"); ap.compass_mot = false; return 1; } // disable cpu failsafe failsafe_disable(); // initialise compass init_compass(); // default compensation type to use current if possible if (battery.has_current()) { comp_type = AP_COMPASS_MOT_COMP_CURRENT; }else{ comp_type = AP_COMPASS_MOT_COMP_THROTTLE; } // send back initial ACK mavlink_msg_command_ack_send(chan, MAV_CMD_PREFLIGHT_CALIBRATION,0); // flash leds AP_Notify::flags.esc_calibration = true; // warn user we are starting calibration gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Starting calibration"); // inform what type of compensation we are attempting if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) { gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Current"); } else{ gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Throttle"); } // disable throttle and battery failsafe g.failsafe_throttle = FS_THR_DISABLED; g.failsafe_battery_enabled = FS_BATT_DISABLED; // disable motor compensation compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); for (uint8_t i=0; i<compass.get_count(); i++) { compass.set_motor_compensation(i, Vector3f(0,0,0)); } // get initial compass readings last_run_time = millis(); while ( millis() - last_run_time < 500 ) { compass.accumulate(); } compass.read(); // store initial x,y,z compass values // initialise interference percentage for (uint8_t i=0; i<compass.get_count(); i++) { compass_base[i] = compass.get_field(i); interference_pct[i] = 0.0f; } // enable motors and pass through throttle init_rc_out(); enable_motor_output(); motors.armed(true); // initialise run time last_run_time = millis(); last_send_time = millis(); // main run while there is no user input and the compass is healthy while (command_ack_start == command_ack_counter && compass.healthy(compass.get_primary()) && motors.armed()) { // 50hz loop if (millis() - last_run_time < 20) { // grab some compass values compass.accumulate(); hal.scheduler->delay(5); continue; } last_run_time = millis(); // read radio input read_radio(); // pass through throttle to motors motors.throttle_pass_through(channel_throttle->radio_in); // read some compass values compass.read(); // read current read_battery(); // calculate scaling for throttle throttle_pct = (float)channel_throttle->control_in / 1000.0f; throttle_pct = constrain_float(throttle_pct,0.0f,1.0f); // if throttle is near zero, update base x,y,z values if (throttle_pct <= 0.0f) { for (uint8_t i=0; i<compass.get_count(); i++) { compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f; } // causing printing to happen as soon as throttle is lifted } else { // calculate diff from compass base and scale with throttle for (uint8_t i=0; i<compass.get_count(); i++) { motor_impact[i] = compass.get_field(i) - compass_base[i]; } // throttle based compensation if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { // for each compass for (uint8_t i=0; i<compass.get_count(); i++) { // scale by throttle motor_impact_scaled[i] = motor_impact[i] / throttle_pct; // adjust the motor compensation to negate the impact motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f; } updated = true; } else { // for each compass for (uint8_t i=0; i<compass.get_count(); i++) { // current based compensation if more than 3amps being drawn motor_impact_scaled[i] = motor_impact[i] / battery.current_amps(); // adjust the motor compensation to negate the impact if drawing over 3amps if (battery.current_amps() >= 3.0f) { motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f; updated = true; } } } // calculate interference percentage at full throttle as % of total mag field if (comp_type == AP_COMPASS_MOT_COMP_THROTTLE) { for (uint8_t i=0; i<compass.get_count(); i++) { // interference is impact@fullthrottle / mag field * 100 interference_pct[i] = motor_compensation[i].length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f; } }else{ for (uint8_t i=0; i<compass.get_count(); i++) { // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100 interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f; } } // record maximum throttle and current throttle_pct_max = MAX(throttle_pct_max, throttle_pct); current_amps_max = MAX(current_amps_max, battery.current_amps()); } if (AP_HAL::millis() - last_send_time > 500) { last_send_time = AP_HAL::millis(); mavlink_msg_compassmot_status_send(chan, channel_throttle->control_in, battery.current_amps(), interference_pct[compass.get_primary()], motor_compensation[compass.get_primary()].x, motor_compensation[compass.get_primary()].y, motor_compensation[compass.get_primary()].z); } } // stop motors motors.output_min(); motors.armed(false); // set and save motor compensation if (updated) { compass.motor_compensation_type(comp_type); for (uint8_t i=0; i<compass.get_count(); i++) { compass.set_motor_compensation(i, motor_compensation[i]); } compass.save_motor_compensation(); // display success message gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_INFO, "Calibration successful"); } else { // compensation vector never updated, report failure gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_NOTICE, "Failed"); compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); } // display new motor offsets and save report_compass(); // turn off notify leds AP_Notify::flags.esc_calibration = false; // re-enable cpu failsafe failsafe_enable(); // re-enable failsafes g.failsafe_throttle.load(); g.failsafe_battery_enabled.load(); // flag we have completed ap.compass_mot = false; return 0; #endif // FRAME_CONFIG != HELI_FRAME }