fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 09:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: sensor_preflight_imu -> sensors_status_imu and run continuously

Browse Source 
- inconsistency checks now run continuously instead of only preflight
 - keep inconsistencies for all sensors
 - add per sensor data validator state as overall health flag
This commit is contained in:
Daniel Agar
2020-09-06 22:06:13 -04:00
committed by GitHub
parent d4ecf24bf2
commit 60d613ea04
12 changed files with 165 additions and 204 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp
+63 -28
View File
@@ -38,53 +38,88 @@
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_preflight_imu.h>
#include <uORB/topics/sensors_status_imu.h>
#include <uORB/topics/subsystem_info.h>
bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
const bool report_status)
{
float test_limit = 1.0f; // pass limit re-used for each test
// Get sensor_preflight data if available and exit with a fail recorded if not
uORB::SubscriptionData<sensor_preflight_imu_s> sensors_sub{ORB_ID(sensor_preflight_imu)};
sensors_sub.update();
const sensor_preflight_imu_s &sensors = sensors_sub.get();
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.
param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
float accel_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit);
if (sensors.accel_inconsistency_m_s_s > test_limit) {
if (report_status) {
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);
}
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) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, imu.accel_healthy[i], status);
return false;
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, imu.accel_healthy[i], status);
}
} 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");
const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i];
if (accel_inconsistency_m_s_s > accel_test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Accel %u inconsistent - Check Cal", i);
if (imu.accel_device_ids[i] == imu.accel_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status);
}
}
return false;
} else if (accel_inconsistency_m_s_s > accel_test_limit * 0.8f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Accel %u inconsistent - Check Cal", i);
}
}
}
}
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
float gyro_test_limit = 1.f;
param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit);
if (sensors.gyro_inconsistency_rad_s > test_limit) {
if (report_status) {
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);
}
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) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, imu.accel_healthy[i], status);
return false;
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, imu.accel_healthy[i], status);
}
} 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");
const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i];
if (gyro_inconsistency_rad_s > gyro_test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Gyro %u inconsistent - Check Cal", i);
if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
} else {
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
}
}
return false;
} else if (gyro_inconsistency_rad_s > gyro_test_limit * 0.5f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "Preflight Advice: Gyro %u inconsistent - Check Cal", i);
}
}
}
}