PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <PreFlightCheck.hpp>
Public Types | |
enum | arm_requirements_t { ARM_REQ_NONE = 0, ARM_REQ_MISSION_BIT = (1 << 0), ARM_REQ_ARM_AUTH_BIT = (1 << 1), ARM_REQ_GPS_BIT = (1 << 2), ARM_REQ_ESCS_CHECK_BIT = (1 << 3) } |
Public Member Functions | |
PreFlightCheck ()=default | |
~PreFlightCheck ()=default | |
Static Public Member Functions | |
static bool | preflightCheck (orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot) |
Runs a preflight check on all sensors to see if they are properly calibrated and healthy. More... | |
static bool | preArmCheck (orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety, const uint8_t arm_requirements) |
Static Private Member Functions | |
static bool | magnometerCheck (orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail) |
static bool | magConsistencyCheck (orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status) |
static bool | accelerometerCheck (orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail) |
static bool | gyroCheck (orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail) |
static bool | baroCheck (orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail) |
static bool | imuConsistencyCheck (orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status) |
static bool | airspeedCheck (orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, const bool report_fail, const bool prearm) |
static int | rcCalibrationCheck (orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL) |
static bool | powerCheck (orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm) |
static bool | ekf2Check (orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, const bool report_fail, const bool enforce_gps_required) |
static bool | failureDetectorCheck (orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm) |
static bool | check_calibration (const char *param_template, const int32_t device_id) |
Definition at line 49 of file PreFlightCheck.hpp.
Enumerator | |
---|---|
ARM_REQ_NONE | |
ARM_REQ_MISSION_BIT | |
ARM_REQ_ARM_AUTH_BIT | |
ARM_REQ_GPS_BIT | |
ARM_REQ_ESCS_CHECK_BIT |
Definition at line 87 of file PreFlightCheck.hpp.
|
default |
|
default |
|
staticprivate |
Definition at line 47 of file accelerometerCheck.cpp.
References f(), hrt_elapsed_time(), mavlink_log_critical, orb_exists(), ORB_ID, and set_health_flags().
|
staticprivate |
Check if airspeed is higher than 4m/s (accepted max) while the vehicle is landed / not flying Negative and positive offsets are considered. Do not check anymore while arming because pitot cover might have been removed.
Definition at line 46 of file airspeedCheck.cpp.
References airspeed_s::confidence, hrt_elapsed_time(), airspeed_s::indicated_airspeed_m_s, mavlink_log_critical, ORB_ID, set_health_flags(), and airspeed_s::timestamp.
|
staticprivate |
Definition at line 46 of file baroCheck.cpp.
References hrt_elapsed_time(), mavlink_log_critical, orb_exists(), ORB_ID, and set_health_flags().
|
staticprivate |
Definition at line 341 of file PreFlightCheck.cpp.
References ll40ls::instance, param_find_no_notification(), param_get(), and PARAM_INVALID.
|
staticprivate |
Definition at line 44 of file ekf2Check.cpp.
References estimator_status_s::control_mode_flags, estimator_status_s::covariances, estimator_status_s::gps_check_fail_flags, estimator_status_s::hgt_test_ratio, estimator_status_s::mag_test_ratio, mavlink_log_critical, mavlink_log_warning, ORB_ID, param_find(), param_get(), estimator_status_s::pos_test_ratio, estimator_status_s::pre_flt_fail_innov_heading, estimator_status_s::pre_flt_fail_innov_height, estimator_status_s::pre_flt_fail_innov_vel_horiz, estimator_status_s::pre_flt_fail_innov_vel_vert, estimator_status_s::pre_flt_fail_mag_field_disturbed, set_health_flags(), estimator_status_s::states, status, estimator_status_s::timestamp, and estimator_status_s::vel_test_ratio.
|
staticprivate |
Definition at line 38 of file failureDetectorCheck.cpp.
References FAILURE_ALT, vehicle_status_s::failure_detector_status, FAILURE_NONE, FAILURE_PITCH, FAILURE_ROLL, and mavlink_log_critical.
|
staticprivate |
Definition at line 46 of file gyroCheck.cpp.
References hrt_elapsed_time(), mavlink_log_critical, orb_exists(), ORB_ID, and set_health_flags().
|
staticprivate |
Definition at line 44 of file imuConsistencyCheck.cpp.
References sensor_preflight_s::accel_inconsistency_m_s_s, sensor_preflight_s::gyro_inconsistency_rad_s, mavlink_log_critical, mavlink_log_info, ORB_ID, param_find(), param_get(), and set_health_flags_healthy().
|
staticprivate |
Definition at line 46 of file magConsistencyCheck.cpp.
References sensor_preflight_s::mag_inconsistency_angle, mavlink_log_critical, ORB_ID, param_find(), param_get(), set_health_flags_healthy(), and sensor_preflight_s::timestamp.
|
staticprivate |
Definition at line 46 of file magnetometerCheck.cpp.
References hrt_elapsed_time(), mavlink_log_critical, orb_exists(), ORB_ID, and set_health_flags().
|
staticprivate |
Definition at line 43 of file powerCheck.cpp.
References f(), hrt_elapsed_time(), mavlink_log_critical, ORB_ID, system_power_s::timestamp, and system_power_s::voltage5v_v.
|
static |
Definition at line 40 of file preArmCheck.cpp.
References arm_auth_check(), ARM_REQ_ARM_AUTH_BIT, ARM_REQ_ESCS_CHECK_BIT, ARM_REQ_GPS_BIT, ARM_REQ_MISSION_BIT, vehicle_status_flags_s::avoidance_system_required, vehicle_status_flags_s::avoidance_system_valid, vehicle_status_flags_s::circuit_breaker_engaged_power_check, vehicle_status_flags_s::circuit_breaker_engaged_usb_check, vehicle_status_flags_s::condition_auto_mission_available, vehicle_status_flags_s::condition_battery_healthy, vehicle_status_flags_s::condition_escs_error, vehicle_status_flags_s::condition_global_position_valid, vehicle_status_flags_s::condition_power_input_valid, mavlink_log_critical, safety_s::safety_off, safety_s::safety_switch_available, and vehicle_status_flags_s::usb_connected.
Referenced by arming_state_transition(), and commander_main().
|
static |
Runs a preflight check on all sensors to see if they are properly calibrated and healthy.
The function won't fail the test if optional sensors are not found, however, it will fail the test if optional sensors are found but not in working condition.
mavlink_log_pub | Mavlink output orb handle reference for feedback when a sensor fails |
checkMag | true if the magneteometer should be checked |
checkAcc | true if the accelerometers should be checked |
checkGyro | true if the gyroscopes should be checked |
checkBaro | true if the barometer should be checked |
checkAirspeed | true if the airspeed sensor should be checked |
checkRC | true if the Remote Controller should be checked |
checkGNSS | true if the GNSS receiver should be checked |
checkPower | true if the system power should be checked |
Definition at line 58 of file PreFlightCheck.cpp.
References vehicle_status_flags_s::circuit_breaker_engaged_airspd_check, vehicle_status_flags_s::circuit_breaker_engaged_power_check, vehicle_status_flags_s::condition_calibration_enabled, vehicle_status_flags_s::condition_power_input_valid, vehicle_status_flags_s::condition_system_hotplug_timeout, device_id, vehicle_status_s::hil_state, vehicle_status_s::is_vtol, mavlink_log_critical, max_mandatory_accel_count, max_mandatory_baro_count, max_mandatory_gyro_count, max_mandatory_mag_count, max_optional_accel_count, max_optional_baro_count, max_optional_gyro_count, max_optional_mag_count, OK, param_find(), param_get(), vehicle_status_flags_s::rc_calibration_valid, vehicle_status_s::rc_input_mode, vehicle_status_flags_s::rc_signal_found_once, vehicle_status_s::rc_signal_lost, set_health_flags(), vehicle_status_s::vehicle_type, and VEHICLE_TYPE_FIXED_WING.
Referenced by arming_state_transition(), and Commander::preflight_check().
|
staticprivate |
Definition at line 55 of file rcCalibrationCheck.cpp.
References mavlink_log_critical, param_find(), param_get(), PARAM_INVALID, RC_INPUT_HIGHEST_MAX_US, RC_INPUT_LOWEST_MIN_US, and RC_INPUT_MAX_DEADZONE_US.