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

Improve magnetometer inconsistency check 

To check directional difference between the magnetometer field vectors
instead of vector component difference.
This commit is contained in:
Matthias Grob 2019-06-26 13:54:03 +01:00 committed by Lorenz Meier
parent 83e532d339
commit 45187e1aa8
7 changed files with 43 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/airframes/4250_teal
View File

@ -47,7 +47,7 @@ then
param set CAL_MAG0_ROT 0
param set CAL_MAG_SIDES 63
param set SENS_BOARD_ROT 0
param set COM_ARM_MAG 1.5
param set COM_ARM_MAG_ANG -1
param set COM_ARM_EKF_AB 0.0032
# circuit breakers

8
msg/sensor_preflight.msg
View File

@ -2,7 +2,7 @@
# Pre-flight sensor check metrics. These will be zero if the vehicle only has one sensor.
# The topic will not be updated when the vehicle is armed
#
uint64 timestamp # time since system start (microseconds)
float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s).
float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s).
float32 mag_inconsistency_ga # magnitude of maximum difference between Mag instances in (Gauss).
uint64 timestamp # time since system start (microseconds)
float32 accel_inconsistency_m_s_s # magnitude of maximum acceleration difference between IMU instances in (m/s/s).
float32 gyro_inconsistency_rad_s # magnitude of maximum angular rate difference between IMU instances in (rad/s).
float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians.

24
src/modules/commander/PreflightCheck.cpp
View File

@ -45,6 +45,7 @@
#include "rc_check.h"
#include <math.h>
#include <mathlib/mathlib.h>
#include <parameters/param.h>
#include <systemlib/mavlink_log.h>
@ -203,6 +204,8 @@ out:
// return false if the magnetomer measurements are inconsistent
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, bool report_status)
{
bool pass = false; // flag for result of checks
// get the sensor preflight data
uORB::SubscriptionData<sensor_preflight_s> sensors_sub{ORB_ID(sensor_preflight)};
sensors_sub.update();
@ -210,25 +213,24 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s
if (sensors.timestamp == 0) {
// can happen if not advertised (yet)
return true;
pass = true;
}
// Use the difference between sensors to detect a bad calibration, orientation or magnetic interference.
// If a single sensor is fitted, the value being checked will be zero so this check will always pass.
float test_limit;
param_get(param_find("COM_ARM_MAG"), &test_limit);
int32_t angle_difference_limit_deg;
param_get(param_find("COM_ARM_MAG_ANG"), &angle_difference_limit_deg);
if (sensors.mag_inconsistency_ga > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensors inconsistent");
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status);
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status);
}
pass = pass || angle_difference_limit_deg < 0; // disabled, pass check
pass = pass || sensors.mag_inconsistency_angle < math::radians<float>(angle_difference_limit_deg);
return false;
if (!pass && report_status) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Compass Sensors inconsistent");
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, status);
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, status);
}
return true;
return pass;
}
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, uint8_t instance,

11
src/modules/commander/commander_params.c
View File

@ -600,15 +600,14 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f);
/**
* Maximum magnetic field inconsistency between units that will allow arming
* Set -1 to disable the check.
*
* @group Commander
* @unit Gauss
* @min 0.05
* @max 0.5
* @decimal 2
* @increment 0.05
* @unit deg
* @min 3
* @max 180
*/
PARAM_DEFINE_FLOAT(COM_ARM_MAG, 0.15f);
PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 30);
/**
* Enable RC stick override of auto modes

2
src/modules/sensors/sensors.cpp
View File

@ -673,7 +673,7 @@ Sensors::run()
preflt.timestamp = hrt_absolute_time();
_voted_sensors_update.calc_accel_inconsistency(preflt);
_voted_sensors_update.calc_gyro_inconsistency(preflt);
_voted_sensors_update.calc_mag_inconsistency(preflt);
_voted_sensors_update.calcMagInconsistency(preflt);
orb_publish(ORB_ID(sensor_preflight), _sensor_preflight, &preflt);
}
}

48
src/modules/sensors/voted_sensors_update.cpp
View File

@ -50,6 +50,7 @@
using namespace sensors;
using namespace DriverFramework;
using namespace matrix;
VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_enabled)
: _parameters(parameters), _hil_enabled(hil_enabled)
@ -60,7 +61,7 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters &parameters, bool hil_en
memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp));
memset(&_accel_diff, 0, sizeof(_accel_diff));
memset(&_gyro_diff, 0, sizeof(_gyro_diff));
memset(&_mag_diff, 0, sizeof(_mag_diff));
memset(&_mag_angle_diff, 0, sizeof(_mag_angle_diff));
// initialise the publication variables
memset(&_corrections, 0, sizeof(_corrections));
@ -1205,34 +1206,25 @@ void VotedSensorsUpdate::calc_gyro_inconsistency(sensor_preflight_s &preflt)
}
}
void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt)
void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt)
{
float mag_diff_sum_max_sq = 0.0f; // the maximum sum of axis differences squared
Vector3f primary_mag(_last_magnetometer[_mag.last_best_vote].magnetometer_ga); // primary mag field vector
float mag_angle_diff_max = 0.0f; // the maximum angle difference
unsigned check_index = 0; // the number of sensors the primary has been checked against
// Check each sensor against the primary
for (int sensor_index = 0; sensor_index < _mag.subscription_count; sensor_index++) {
for (int i = 0; i < _mag.subscription_count; i++) {
// check that the sensor we are checking against is not the same as the primary
if ((_mag.priority[sensor_index] > 0) && (sensor_index != _mag.last_best_vote)) {
if ((_mag.priority[i] > 0) && (i != _mag.last_best_vote)) {
// calculate angle to 3D magnetic field vector of the primary sensor
Vector3f current_mag(_last_magnetometer[i].magnetometer_ga);
float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();
float mag_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary
// complementary filter to not fail/pass on single outliers
_mag_angle_diff[check_index] *= 0.95f;
_mag_angle_diff[check_index] += 0.05f * angle_error;
// calculate mag_diff_sum_sq for the specified sensor against the primary
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
_mag_diff[axis_index][check_index] = 0.95f * _mag_diff[axis_index][check_index] + 0.05f *
(_last_magnetometer[_mag.last_best_vote].magnetometer_ga[axis_index] -
_last_magnetometer[sensor_index].magnetometer_ga[axis_index]);
mag_diff_sum_sq += _mag_diff[axis_index][check_index] * _mag_diff[axis_index][check_index];
}
// capture the largest sum value
if (mag_diff_sum_sq > mag_diff_sum_max_sq) {
mag_diff_sum_max_sq = mag_diff_sum_sq;
}
mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]);
// increment the check index
check_index++;
@ -1241,16 +1233,10 @@ void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt)
// check to see if the maximum number of checks has been reached and break
if (check_index >= 2) {
break;
}
}
// skip check if less than 2 sensors
if (check_index < 1) {
preflt.mag_inconsistency_ga = 0.0f;
} else {
// get the vector length of the largest difference and write to the combined sensor struct
preflt.mag_inconsistency_ga = sqrtf(mag_diff_sum_max_sq);
}
// get the vector length of the largest difference and write to the combined sensor struct
// will be zero if there is only one magnetometer and hence nothing to compare
preflt.mag_inconsistency_angle = mag_angle_diff_max;
}

4
src/modules/sensors/voted_sensors_update.h
View File

@ -141,7 +141,7 @@ public:
/**
* Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
*/
void calc_mag_inconsistency(sensor_preflight_s &preflt);
void calcMagInconsistency(sensor_preflight_s &preflt);
private:
@ -261,7 +261,7 @@ private:
float _accel_diff[3][2]; /**< filtered accel differences between IMU units (m/s/s) */
float _gyro_diff[3][2]; /**< filtered gyro differences between IMU uinits (rad/s) */
float _mag_diff[3][2]; /**< filtered mag differences between sensor instances (Ga) */
float _mag_angle_diff[2]; /**< filtered mag angle differences between sensor instances (Ga) */
/* sensor thermal compensation */
TemperatureCompensation _temperature_compensation;