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

commander: check magnetometers for inconsistency preflight

This commit is contained in:
Paul Riseborough 2017-10-02 10:57:37 +11:00 committed by Daniel Agar
parent 60a68d30c7
commit 0def4ace5f
2 changed files with 44 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -214,6 +214,32 @@ out:
return success;
}
// return false if the magnetomer measurements are inconsistent
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
// get the sensor preflight data
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
struct sensor_preflight_s sensors = {};
if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) {
// can happen if not advertised (yet)
return true;
}
orb_unsubscribe(sensors_sub);
// 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);
if (sensors.mag_inconsistency_ga > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
}
return false;
}
return true;
}
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool success = true;
@ -625,6 +651,12 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
}
failed = true;
}
/* fail if mag sensors are inconsistent */
if (!magConsistencyCheck(mavlink_log_pub, reportFailures)) {
failed = true;
}
}
/* ---- ACCEL ---- */

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

@ -590,6 +590,18 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_ACC, 0.7f);
*/
PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f);
/**
* Maximum magnetic field inconsistency between units that will allow arming
*
* @group Commander
* @unit Gauss
* @min 0.05
* @max 0.5
* @decimal 2
* @increment 0.05
*/
PARAM_DEFINE_FLOAT(COM_ARM_MAG, 0.15f);
/**
* Enable RC stick override of auto modes
*