dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

PreFlightCheck: refactor sensors preflight checks

This commit is contained in:
bresch 2022-03-10 17:35:18 +01:00 committed by Mathieu Bresciani
parent b1c1163ee4
commit 75aa11c955
7 changed files with 168 additions and 171 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
View File

@ -45,14 +45,10 @@
using namespace time_literals;
static constexpr unsigned max_mandatory_gyro_count = 1;
static constexpr unsigned max_optional_gyro_count = 4;
static constexpr unsigned max_mandatory_accel_count = 1;
static constexpr unsigned max_optional_accel_count = 4;
static constexpr unsigned max_mandatory_mag_count = 1;
static constexpr unsigned max_optional_mag_count = 4;
static constexpr unsigned max_mandatory_gyro_count = 1;
static constexpr unsigned max_mandatory_accel_count = 1;
static constexpr unsigned max_mandatory_baro_count = 1;
static constexpr unsigned max_optional_baro_count = 4;
bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
@ -72,8 +68,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
param_get(param_find("SYS_HAS_MAG"), &sys_has_mag);
if (sys_has_mag == 1) {
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count, max_optional_mag_count,
mavlink_log_pub, status, magnetometerCheck, isMagRequired);
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count,
mavlink_log_pub, status, magnetometerCheck);
/* mag consistency checks (need to be performed after the individual checks) */
if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
@ -84,16 +80,16 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- ACCEL ---- */
{
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count, max_optional_accel_count,
mavlink_log_pub, status, accelerometerCheck, isAccelRequired);
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count,
mavlink_log_pub, status, accelerometerCheck);
// TODO: highest priority (from params)
}
/* ---- GYRO ---- */
{
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count, max_optional_gyro_count,
mavlink_log_pub, status, gyroCheck, isGyroRequired);
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count,
mavlink_log_pub, status, gyroCheck);
// TODO: highest priority (from params)
}
@ -104,8 +100,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
if (sys_has_baro == 1) {
static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, max_optional_baro_count,
mavlink_log_pub, status, baroCheck, isBaroRequired));
static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count,
mavlink_log_pub, status, baroCheck));
}
}
@ -123,8 +119,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
param_get(param_find("SYS_HAS_NUM_DIST"), &sys_has_num_dist_sens);
if (sys_has_num_dist_sens > 0) {
static_cast<void>(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens, sys_has_num_dist_sens,
mavlink_log_pub, status, distSensCheck, isDistSensRequired));
static_cast<void>(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens,
mavlink_log_pub, status, distSensCheck));
}
}
@ -236,23 +232,19 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
return !failed;
}
bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub,
vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired)
bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure,
const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub,
vehicle_status_s &status, sens_check_func_t sens_check)
{
bool pass_check = true;
bool report_fail = report_failure;
/* check all sensors, but fail only for mandatory ones */
for (uint8_t i = 0u; i < max_optional_count; i++) {
const bool required = (i < max_mandatory_count) || isSensorRequired(i);
for (uint8_t i = 0u; i < ORB_MULTI_MAX_INSTANCES; i++) {
const bool is_mandatory = i < nb_mandatory_instances;
if (!sens_check(mavlink_log_pub, status, i, !required, report_fail)) {
if (required) {
pass_check = false;
}
report_fail = false; // only report the first failure
if (!sens_check(mavlink_log_pub, status, i, is_mandatory, report_fail)) {
pass_check = false;
}
}

20
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
View File

@ -48,8 +48,7 @@
#include <drivers/drv_hrt.h>
typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail);
typedef bool (*is_sens_req_func_t)(uint8_t instance);
const bool is_mandatory, bool &report_fail);
class PreFlightCheck
{
@ -100,25 +99,24 @@ public:
bool report_fail = true);
private:
static bool sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub,
vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired);
static bool sensorAvailabilityCheck(const bool report_failure,
const uint8_t nb_mandatory_instances, orb_advert_t *mavlink_log_pub,
vehicle_status_s &status, sens_check_func_t sens_check);
static bool isMagRequired(uint8_t instance);
static bool magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail);
const bool is_mandatory, bool &report_fail);
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
static bool isAccelRequired(uint8_t instance);
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail);
const bool is_mandatory, bool &report_fail);
static bool isGyroRequired(uint8_t instance);
static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail);
const bool is_mandatory, bool &report_fail);
static bool isBaroRequired(uint8_t instance);
static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail);
static bool isDistSensRequired(uint8_t instance);
const bool is_mandatory, bool &report_fail);
static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail);
const bool is_mandatory, 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, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed);

79
src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp
View File

@ -50,70 +50,71 @@ bool PreFlightCheck::isAccelRequired(const uint8_t instance)
uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
const uint32_t device_id = static_cast<uint32_t>(accel.get().device_id);
bool is_used_by_nav = false;
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().accel_device_id == device_id) {
return true;
is_used_by_nav = true;
break;
}
}
return false;
return is_used_by_nav;
}
bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail)
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
bool calibration_valid = false;
bool valid = true;
const bool is_required = is_mandatory || isAccelRequired(instance);
bool is_valid = false;
bool is_calibration_valid = false;
bool is_value_valid = false;
if (exists) {
uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0);
if (!valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance);
}
}
is_valid = (accel.get().device_id != 0) && (accel.get().timestamp != 0);
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
calibration_valid = true;
is_calibration_valid = true;
} else {
calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0);
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("ACC", accel.get().device_id) >= 0);
}
if (!calibration_valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance);
}
const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
+ accel.get().y * accel.get().y
+ accel.get().z * accel.get().z);
} else {
const float accel_magnitude = sqrtf(accel.get().x * accel.get().x
+ accel.get().y * accel.get().y
+ accel.get().z * accel.get().z);
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");
}
// this is fatal
valid = false;
}
}
} else {
if (!optional && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance);
if (accel_magnitude > 4.0f && accel_magnitude < 15.0f /* m/s^2 */) {
is_value_valid = true;
}
}
const bool success = calibration_valid && valid;
if (report_fail && is_required) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Sensor %u missing", instance);
report_fail = false;
return success;
} else if (!is_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Accel %u", instance);
report_fail = false;
} else if (!is_calibration_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u uncalibrated", instance);
report_fail = false;
} else if (!is_value_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel Range, hold still on arming");
report_fail = false;
}
}
const bool is_sensor_ok = is_valid && is_calibration_valid && is_value_valid;
return is_sensor_ok || !is_required;
}

37
src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp
View File

@ -48,43 +48,48 @@ bool PreFlightCheck::isBaroRequired(const uint8_t instance)
uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
const uint32_t device_id = static_cast<uint32_t>(baro.get().device_id);
bool is_used_by_nav = false;
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().baro_device_id == device_id) {
return true;
is_used_by_nav = true;
break;
}
}
return false;
return is_used_by_nav;
}
bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail)
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK);
const bool is_required = is_mandatory || isBaroRequired(instance);
bool valid = false;
if (exists) {
uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
valid = (baro.get().device_id != 0) && (baro.get().timestamp != 0);
if (!valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro %u", instance);
}
}
} else {
if (!optional && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor %u missing", instance);
}
}
if (instance == 0) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, valid, status);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, is_required, valid, status);
}
return valid;
if (report_fail && is_required) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Baro Sensor %u missing", instance);
report_fail = false;
} else if (!valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Baro %u", instance);
report_fail = false;
}
}
return valid || !is_required;
}

29
src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp
View File

@ -42,35 +42,30 @@
using namespace time_literals;
bool PreFlightCheck::isDistSensRequired(const uint8_t instance)
{
return false;
}
bool PreFlightCheck::distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail)
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(distance_sensor), instance) == PX4_OK);
bool check_valid = false;
bool valid = false;
if (exists) {
uORB::SubscriptionData<distance_sensor_s> dist_sens_sub{ORB_ID(distance_sensor), instance};
dist_sens_sub.update();
const distance_sensor_s &dist_sens_data = dist_sens_sub.get();
check_valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s);
valid = (hrt_elapsed_time(&dist_sens_data.timestamp) < 1_s);
}
if (!check_valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from distance sensor %u", instance);
}
}
} else {
if (!optional && report_fail) {
if (report_fail && is_mandatory) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: distance sensor %u missing", instance);
report_fail = false;
} else if (!valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from distance sensor %u", instance);
report_fail = false;
}
}
return check_valid;
return valid || !is_mandatory;
}

58
src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp
View File

@ -49,54 +49,58 @@ bool PreFlightCheck::isGyroRequired(const uint8_t instance)
uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
const uint32_t device_id = static_cast<uint32_t>(gyro.get().device_id);
bool is_used_by_nav = false;
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().gyro_device_id == device_id) {
return true;
is_used_by_nav = true;
break;
}
}
return false;
return is_used_by_nav;
}
bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail)
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK);
bool calibration_valid = false;
bool valid = false;
const bool is_required = is_mandatory || isGyroRequired(instance);
bool is_valid = false;
bool is_calibration_valid = false;
if (exists) {
uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0);
if (!valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro %u", instance);
}
}
is_valid = (gyro.get().device_id != 0) && (gyro.get().timestamp != 0);
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
calibration_valid = true;
is_calibration_valid = true;
} else {
calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0);
}
if (!calibration_valid) {
if (report_fail) {
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: Gyro Sensor %u missing", instance);
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("GYRO", gyro.get().device_id) >= 0);
}
}
return calibration_valid && valid;
if (report_fail && is_required) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro Sensor %u missing", instance);
report_fail = false;
} else if (!is_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Gyro %u", instance);
report_fail = false;
} else if (!is_calibration_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u uncalibrated", instance);
report_fail = false;
}
}
const bool is_sensor_ok = is_calibration_valid && is_valid;
return is_sensor_ok || !is_required;
}

70
src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
View File

@ -49,48 +49,40 @@ bool PreFlightCheck::isMagRequired(const uint8_t instance)
uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
const uint32_t device_id = static_cast<uint32_t>(magnetometer.get().device_id);
bool is_used_by_nav = false;
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().mag_device_id == device_id) {
return true;
is_used_by_nav = true;;
break;
}
}
return false;
return is_used_by_nav;
}
bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, const bool report_fail)
const bool is_mandatory, bool &report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
bool calibration_valid = false;
bool valid = false;
const bool is_required = is_mandatory || isMagRequired(instance);
bool is_valid = false;
bool is_calibration_valid = false;
bool is_mag_fault = false;
if (exists) {
uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0);
if (!valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass %u", instance);
}
}
is_valid = (magnetometer.get().device_id != 0) && (magnetometer.get().timestamp != 0);
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
calibration_valid = true;
is_calibration_valid = true;
} else {
calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", magnetometer.get().device_id) >= 0);
}
if (!calibration_valid) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass %u uncalibrated", instance);
}
is_calibration_valid = (calibration::FindCurrentCalibrationIndex("MAG", magnetometer.get().device_id) >= 0);
}
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
@ -103,25 +95,35 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st
}
}
}
if (is_mag_fault && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance);
}
} else {
if (!optional && report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance);
}
}
const bool success = calibration_valid && valid && !is_mag_fault;
const bool is_sensor_ok = is_valid && is_calibration_valid && !is_mag_fault;
if (instance == 0) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, is_required, is_sensor_ok, status);
} else if (instance == 1) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, is_required, is_sensor_ok, status);
}
return success;
if (report_fail && is_required) {
if (!exists) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensor %u missing", instance);
report_fail = false;
} else if (!is_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: no valid data from Compass %u", instance);
report_fail = false;
} else if (!is_calibration_valid) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass %u uncalibrated", instance);
report_fail = false;
} else if (is_mag_fault) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass #%u fault", instance);
report_fail = false;
}
}
return is_sensor_ok || !is_required;
}