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

sensors: move mag aggregation to new VehicleMagnetometer WorkItem

Browse Source 
- purge all reminaing magnetometer IOCTL usage
 - mag calibration add off diagonal (soft iron) scale factors
This commit is contained in:
Daniel Agar
2020-08-21 10:12:13 -04:00
parent ad148af2fd
commit 971b1e4b4d
72 changed files with 870 additions and 1134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
+6 -62
View File
@@ -87,11 +87,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (sys_has_mag == 1) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1);
@@ -99,29 +94,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t device_id = -1;
if (magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (!magnetometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
if (required) {
failed = true;
}
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status);
failed = true;
}
// TODO: highest priority mag
/* mag consistency checks (need to be performed after the individual checks) */
if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures))) {
@@ -132,10 +112,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- ACCEL ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_ACC_PRIME"), &prime_id);
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
const bool required = (i < max_mandatory_accel_count);
@@ -143,63 +119,31 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
int32_t device_id = -1;
if (accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (!accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, report_fail)) {
if (required) {
failed = true;
}
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status);
failed = true;
}
// TODO: highest priority (from params)
}
/* ---- GYRO ---- */
if (checkSensors) {
bool prime_found = false;
int32_t prime_id = -1;
param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
/* check all sensors individually, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
const bool required = (i < max_mandatory_gyro_count);
int32_t device_id = -1;
if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
if ((prime_id > 0) && (device_id == prime_id)) {
prime_found = true;
}
} else {
if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, reportFailures)) {
if (required) {
failed = true;
}
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status);
failed = true;
}
// TODO: highest priority (from params)
}
/* ---- BARO ---- */
src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp
+3 -3
View File
@@ -38,7 +38,7 @@
#include <lib/parameters/param.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_imu.h>
#include <uORB/topics/subsystem_info.h>
bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
@@ -47,9 +47,9 @@ bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_
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_s> sensors_sub{ORB_ID(sensor_preflight)};
uORB::SubscriptionData<sensor_preflight_imu_s> sensors_sub{ORB_ID(sensor_preflight_imu)};
sensors_sub.update();
const sensor_preflight_s &sensors = sensors_sub.get();
const sensor_preflight_imu_s &sensors = sensors_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.
src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp
+3 -3
View File
@@ -40,7 +40,7 @@
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/sensor_preflight_mag.h>
// return false if the magnetomer measurements are inconsistent
bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
@@ -49,9 +49,9 @@ bool PreFlightCheck::magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_
bool pass = false; // flag for result of checks
// get the sensor preflight data
uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
uORB::SubscriptionData<sensor_preflight_mag_s> sensors_sub{ORB_ID(sensor_preflight_mag)};
sensors_sub.update();
const sensor_preflight_s &sensors = sensors_sub.get();
const sensor_preflight_mag_s &sensors = sensors_sub.get();
if (sensors.timestamp == 0) {
// can happen if not advertised (yet)