diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index fe904a499f..60be9851a7 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -113,7 +113,7 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &sta if (!mag_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u invalid", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Mag #%u invalid", instance); } } @@ -123,13 +123,13 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &sta if (!calibration_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Mag #%u uncalibrated", instance); } } } else { if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no Mag Sensor #%u", instance); } } @@ -164,7 +164,7 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s if (sensors.accel_inconsistency_m_s_s > test_limit) { if (report_status) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accels inconsistent - Check Cal"); set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status); set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status); } @@ -174,7 +174,7 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s } else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) { if (report_status) { - mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: ACCELS INCONSISTENT - CHECK CAL"); + mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accels inconsistent - Check Cal"); } } @@ -183,7 +183,7 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s if (sensors.gyro_inconsistency_rad_s > test_limit) { if (report_status) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyros inconsistent - Check Cal"); set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status); set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status); } @@ -193,7 +193,7 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s } else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) { if (report_status) { - mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: GYROS INCONSISTENT - CHECK CAL"); + mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyros inconsistent - Check Cal"); } } @@ -223,7 +223,7 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s if (sensors.mag_inconsistency_ga > test_limit) { if (report_status) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Mag Sensors inconsistent"); set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status); set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status); } @@ -249,7 +249,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & if (!accel_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u invalid", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel #%u invalid", instance); } } @@ -259,7 +259,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & if (!calibration_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel #%u uncalibrated", instance); } } else { @@ -271,7 +271,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming"); } /* this is frickin' fatal */ @@ -282,7 +282,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & } else { if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: No Accel Sensor #%u", instance); } } @@ -313,7 +313,7 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, u if (!gyro_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u invalid", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro #%u invalid", instance); } } @@ -323,13 +323,13 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, u if (!calibration_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro #%u uncalibrated", instance); } } } else { if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no Gyro Sensor #%u", instance); } } @@ -356,14 +356,14 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, u if (!baro_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u invalid", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro #%u invalid", instance); } } } else { if (!optional && report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no Baro Sensor #%u", instance); } } @@ -389,7 +389,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu if ((orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure) != PX4_OK) || (hrt_elapsed_time(&differential_pressure.timestamp) > 1_s)) { if (report_fail && !optional) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing"); } present = false; @@ -400,7 +400,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu if ((orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed) != PX4_OK) || (hrt_elapsed_time(&airspeed.timestamp) > 1_s)) { if (report_fail && !optional) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing"); } present = false; @@ -416,7 +416,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu */ if (prearm && fabsf(airspeed.confidence) < 0.95f) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR STUCK"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor stuck"); } present = true; @@ -431,7 +431,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu */ if (fabsf(differential_pressure.differential_pressure_filtered_pa) > 15.0f && !prearm) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot"); } present = true; @@ -474,17 +474,17 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, success = false; if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); } } else if (avionics_power_rail_voltage < 4.9f) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)avionics_power_rail_voltage); + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); } } else if (avionics_power_rail_voltage > 5.4f) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)avionics_power_rail_voltage); + mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage); } } } @@ -508,7 +508,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s // Get estimator status data if available and exit with a fail recorded if not int sub = orb_subscribe(ORB_ID(estimator_status)); - estimator_status_s status = {}; + estimator_status_s status; if (orb_copy(ORB_ID(estimator_status), sub, &status) != PX4_OK) { present = false; @@ -518,7 +518,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s // Check if preflight check performed by estimator has failed if (status.pre_flt_fail) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF INTERNAL CHECKS"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: EKF internal checks"); } success = false; @@ -530,7 +530,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s if (status.hgt_test_ratio > test_limit) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HGT ERROR"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: EKF Height Error"); } success = false; @@ -542,7 +542,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s if (status.vel_test_ratio > test_limit) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF VEL ERROR"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: EKF Velocity Error"); } success = false; @@ -554,7 +554,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s if (status.pos_test_ratio > test_limit) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HORIZ POS ERROR"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: EKF Horizontal Position Error"); } success = false; @@ -566,7 +566,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s if (status.mag_test_ratio > test_limit) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF YAW ERROR"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: EKF Yaw Error"); } success = false; @@ -579,7 +579,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit || fabsf(status.states[15]) > test_limit) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU ACCEL BIAS"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: EKF High IMU Accel Bias"); } success = false; @@ -592,7 +592,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit || fabsf(status.states[12]) > test_limit) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU GYRO BIAS"); + mavlink_log_critical(mavlink_log_pub, "Preflight Fail: EKF High IMU Gyro Bias"); } success = false;