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

commander: PreFlightCheck param_find all parameters immediately 

- this ensures the relevant parameters are marked active immediately
before parameter sync
 - fixes https://github.com/PX4/Firmware/issues/15872
This commit is contained in:
Daniel Agar 2020-10-03 11:25:59 -04:00
parent 57da61dc17
commit eabbd19c1c
3 changed files with 37 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
View File

@ -47,11 +47,22 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
{
bool success = true; // start with a pass and change to a fail if any test fails
bool ahrs_present = true;
float test_limit = 1.0f; // pass limit re-used for each test
int32_t mag_strength_check_enabled = 1;
param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
float hgt_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
float vel_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_VEL"), &vel_test_ratio_limit);
float pos_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_POS"), &pos_test_ratio_limit);
float mag_test_ratio_limit = 1.f;
param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit);
bool gps_success = true;
bool gps_present = true;
@ -99,9 +110,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
}
// check vertical position innovation test ratio
param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
if (status.hgt_test_ratio > test_limit) {
if (status.hgt_test_ratio > hgt_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
}
@ -111,9 +120,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
}
// check velocity innovation test ratio
param_get(param_find("COM_ARM_EKF_VEL"), &test_limit);
if (status.vel_test_ratio > test_limit) {
if (status.vel_test_ratio > vel_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error");
}
@ -123,9 +130,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
}
// check horizontal position innovation test ratio
param_get(param_find("COM_ARM_EKF_POS"), &test_limit);
if (status.pos_test_ratio > test_limit) {
if (status.pos_test_ratio > pos_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
}
@ -135,9 +140,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
}
// check magnetometer innovation test ratio
param_get(param_find("COM_ARM_EKF_YAW"), &test_limit);
if (status.mag_test_ratio > test_limit) {
if (status.mag_test_ratio > mag_test_ratio_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
}
@ -229,6 +232,12 @@ out:
bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail)
{
float ekf_ab_test_limit = 1.0f; // pass limit re-used for each test
param_get(param_find("COM_ARM_EKF_AB"), &ekf_ab_test_limit);
float ekf_gb_test_limit = 1.0f; // pass limit re-used for each test
param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit);
// Get estimator states data if available and exit with a fail recorded if not
uORB::Subscription states_sub{ORB_ID(estimator_states)};
estimator_states_s states;
@ -236,18 +245,15 @@ bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool r
if (states_sub.copy(&states)) {
// check accelerometer delta velocity bias estimates
float test_limit = 1.0f; // pass limit re-used for each test
param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
for (uint8_t index = 13; index < 16; index++) {
// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
// adjust test threshold by 3-sigma
float test_uncertainty = 3.0f * sqrtf(fmaxf(states.covariances[index], 0.0f));
if (fabsf(states.states[index]) > test_limit + test_uncertainty) {
if (fabsf(states.states[index]) > ekf_ab_test_limit + test_uncertainty) {
if (report_fail) {
PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)states.states[index], (double)test_limit,
PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)states.states[index], (double)ekf_ab_test_limit,
(double)test_uncertainty);
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
}
@ -257,11 +263,11 @@ bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool r
}
// check gyro delta angle bias estimates
param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
if (fabsf(states.states[10]) > test_limit
|| fabsf(states.states[11]) > test_limit
|| fabsf(states.states[12]) > test_limit) {
if (fabsf(states.states[10]) > ekf_gb_test_limit
|| fabsf(states.states[11]) > ekf_gb_test_limit
|| fabsf(states.states[12]) > ekf_gb_test_limit) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");

12
src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp
View File

@ -44,15 +44,18 @@
bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
const bool report_status)
{
float accel_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit);
float gyro_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit);
// Get sensor_preflight data if available and exit with a fail recorded if not
uORB::SubscriptionData<sensors_status_imu_s> sensors_status_imu_sub{ORB_ID(sensors_status_imu)};
const sensors_status_imu_s &imu = sensors_status_imu_sub.get();
// Use the difference between IMU's to detect a bad calibration.
// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
float accel_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit);
for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) {
if (imu.accel_device_ids[i] != 0) {
if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
@ -87,9 +90,6 @@ bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_
}
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
float gyro_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit);
for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) {
if (imu.gyro_device_ids[i] != 0) {
if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {

7
src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp
View File

@ -68,6 +68,9 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta
const system_power_s &system_power = system_power_sub.get();
if (system_power.timestamp != 0) {
int32_t required_power_module_count = 0;
param_get(param_find("COM_POWER_COUNT"), &required_power_module_count);
// Check avionics rail voltages (if USB isn't connected)
if (!system_power.usb_connected) {
float avionics_power_rail_voltage = system_power.voltage5v_v;
@ -94,9 +97,6 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta
const int power_module_count = countSetBits(system_power.brick_valid);
int32_t required_power_module_count = 0;
param_get(param_find("COM_POWER_COUNT"), &required_power_module_count);
if (power_module_count < required_power_module_count) {
success = false;
@ -105,7 +105,6 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta
power_module_count, required_power_module_count);
}
}
}
} else {