fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-15 08:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: PreFlightCheck param_find all parameters immediately

Browse Source 
- 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
+28 -22
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");