zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander simplify sensors PreflightCheck

sbg
Daniel Agar 8 years ago
parent
391d103bfd
commit
9a9923c517
4 changed files with 80 additions and 90 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 148
      src/modules/commander/PreflightCheck.cpp
  2. 3
      src/modules/commander/PreflightCheck.h
  3. 17
      src/modules/commander/commander.cpp
  4. 2
      src/modules/commander/state_machine_helper.cpp

148
src/modules/commander/PreflightCheck.cpp
Unescape View File

@ -78,7 +78,7 @@ using namespace DriverFramework;
namespace Commander namespace Commander
{ {
static int check_calibration(DevHandle &h, const char* param_template, int &devid) static int check_calibration(DevHandle &h, const char *param_template, int &devid)
{ {
bool calibration_found; bool calibration_found;
@ -104,6 +104,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
/* if param get succeeds */ /* if param get succeeds */
int calibration_devid; int calibration_devid;
if (!param_get(parm, &(calibration_devid))) { if (!param_get(parm, &(calibration_devid))) {
/* if the devid matches, exit early */ /* if the devid matches, exit early */
@ -162,55 +163,54 @@ out:
return success; return success;
} }
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool checkAcc, bool checkGyro, bool report_status) static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{ {
// get the sensor preflight data bool success = true; // start with a pass and change to a fail if any test fails
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
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight)); int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
struct sensor_preflight_s sensors = {}; sensor_preflight_s sensors = {};
if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != 0) {
// can happen if not advertised (yet) if ((sensors_sub == -1) ||
return true; (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != PX4_OK)) {
goto out;
} }
orb_unsubscribe(sensors_sub);
// 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. // Use the difference between IMU's to detect a bad calibration.
bool success = true; // If a single IMU is fitted, the value being checked will be zero so this check will always pass.
float test_limit;
param_get(param_find("COM_ARM_IMU_ACC"), &test_limit); param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
if (checkAcc) { if (sensors.accel_inconsistency_m_s_s > test_limit) {
if (sensors.accel_inconsistency_m_s_s > test_limit) { if (report_status) {
if (report_status) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL"); }
}
success = false;
goto out;
} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) { success = false;
if (report_status) { goto out;
mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: ACCELS INCONSISTENT - CHECK CAL");
} } 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");
} }
} }
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec // 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); param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
if (checkGyro) { if (sensors.gyro_inconsistency_rad_s > test_limit) {
if (sensors.gyro_inconsistency_rad_s > test_limit) { if (report_status) {
if (report_status) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL"); }
} success = false;
success = false; goto out;
goto out;
} 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");
} } 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");
} }
} }
out: out:
orb_unsubscribe(sensors_sub);
return success; return success;
} }
@ -396,13 +396,14 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm) static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm)
{ {
bool success = true; bool success = true;
int ret;
int fd_airspeed = orb_subscribe(ORB_ID(airspeed)); int fd_airspeed = orb_subscribe(ORB_ID(airspeed));
int fd_diffpres = orb_subscribe(ORB_ID(differential_pressure)); airspeed_s airspeed = {};
struct differential_pressure_s differential_pressure; int fd_diffpres = orb_subscribe(ORB_ID(differential_pressure));
differential_pressure_s differential_pressure = {};
if ((ret = orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure)) || if ((orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure) != PX4_OK) ||
(hrt_elapsed_time(&differential_pressure.timestamp) > (500 * 1000))) { (hrt_elapsed_time(&differential_pressure.timestamp) > (500 * 1000))) {
if (report_fail) { if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
@ -411,9 +412,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
goto out; goto out;
} }
struct airspeed_s airspeed; if ((orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed) != PX4_OK) ||
if ((ret = orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) { (hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
if (report_fail) { if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
@ -441,8 +440,6 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
* Negative and positive offsets are considered. Do not check anymore while arming because pitot cover * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
* might have been removed. * might have been removed.
*/ */
if (fabsf(differential_pressure.differential_pressure_filtered_pa) > 15.0f && !prearm) { if (fabsf(differential_pressure.differential_pressure_filtered_pa) > 15.0f && !prearm) {
if (report_fail) { if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT"); mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT");
@ -467,12 +464,14 @@ static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
px4_pollfd_struct_t fds[1]; px4_pollfd_struct_t fds[1];
fds[0].fd = gpsSub; fds[0].fd = gpsSub;
fds[0].events = POLLIN; fds[0].events = POLLIN;
if(px4_poll(fds, 1, 2000) <= 0) {
if (px4_poll(fds, 1, 2000) <= 0) {
success = false; success = false;
}
else { } else {
struct vehicle_gps_position_s gps; struct vehicle_gps_position_s gps;
if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
if ((OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
(hrt_elapsed_time(&gps.timestamp) > 1000000)) { (hrt_elapsed_time(&gps.timestamp) > 1000000)) {
success = false; success = false;
} }
@ -491,16 +490,17 @@ static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required) static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required)
{ {
bool success = true; // start with a pass and change to a fail if any test fails
float test_limit = 1.0f; // pass limit re-used for each test
// Get estimator status data if available and exit with a fail recorded if not // Get estimator status data if available and exit with a fail recorded if not
int sub = orb_subscribe(ORB_ID(estimator_status)); int sub = orb_subscribe(ORB_ID(estimator_status));
bool updated; estimator_status_s status = {};
orb_check(sub,&updated);
struct estimator_status_s status;
orb_copy(ORB_ID(estimator_status), sub, &status);
orb_unsubscribe(sub);
bool success = true; // start with a pass and change to a fail if any test fails if ((sub == -1) ||
float test_limit; // pass limit re-used for each test (orb_copy(ORB_ID(estimator_status), sub, &status) != PX4_OK)) {
goto out;
}
// check vertical position innovation test ratio // check vertical position innovation test ratio
param_get(param_find("COM_ARM_EKF_HGT"), &test_limit); param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
@ -534,7 +534,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
// If GPS aiding is required, declare fault condition if the EKF is not using GPS // If GPS aiding is required, declare fault condition if the EKF is not using GPS
if (enforce_gps_required) { if (enforce_gps_required) {
if (!(status.control_mode_flags & (1<<2))) { if (!(status.control_mode_flags & (1 << 2))) {
if (report_fail) { if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS"); mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
} }
@ -569,7 +569,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
// check accelerometer delta velocity bias estimates // check accelerometer delta velocity bias estimates
param_get(param_find("COM_ARM_EKF_AB"), &test_limit); param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit || fabsf(status.states[15]) > test_limit) { if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit || fabsf(status.states[15]) > test_limit) {
if (report_fail) { if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU ACCEL BIAS"); mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU ACCEL BIAS");
} }
@ -579,7 +579,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
// check gyro delta angle bias estimates // check gyro delta angle bias estimates
param_get(param_find("COM_ARM_EKF_GB"), &test_limit); param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit || fabsf(status.states[12]) > test_limit) { if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit || fabsf(status.states[12]) > test_limit) {
if (report_fail) { if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU GYRO BIAS"); mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF HIGH IMU GYRO BIAS");
} }
@ -588,44 +588,34 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
} }
out: out:
orb_unsubscribe(sub);
return success; return success;
} }
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro, bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS,
bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot) bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot)
{ {
if (time_since_boot < 1e6) {
// the airspeed driver filter doesn't deliver the actual value yet
return true;
}
#ifdef __PX4_QURT #ifdef __PX4_QURT
// WARNING: Preflight checks are important and should be added back when // WARNING: Preflight checks are important and should be added back when
// all the sensors are supported // all the sensors are supported
PX4_WARN("Preflight checks always pass on Snapdragon."); PX4_WARN("Preflight checks always pass on Snapdragon.");
checkSensors = false;
return true; return true;
#elif defined(__PX4_POSIX_RPI) #elif defined(__PX4_POSIX_RPI)
if (reportFailures) { PX4_WARN("Preflight checks for mag, acc, gyro always pass on RPI");
PX4_WARN("Preflight checks for mag, acc, gyro always pass on RPI"); checkSensors = false;
}
checkMag = false;
checkAcc = false;
checkGyro = false;
#elif defined(__PX4_POSIX_BEBOP) #elif defined(__PX4_POSIX_BEBOP)
PX4_WARN("Preflight checks always pass on Bebop."); PX4_WARN("Preflight checks always pass on Bebop.");
return true; checkSensors = false;
#elif defined(__PX4_POSIX_OCPOC) #elif defined(__PX4_POSIX_OCPOC)
PX4_WARN("Preflight checks always pass on OcPoC."); PX4_WARN("Preflight checks always pass on OcPoC.");
return true; checkSensors = false;
#endif #endif
bool failed = false; bool failed = false;
/* ---- MAG ---- */ /* ---- MAG ---- */
if (checkMag) { if (checkSensors) {
bool prime_found = false; bool prime_found = false;
int32_t prime_id = 0; int32_t prime_id = 0;
param_get(param_find("CAL_MAG_PRIME"), &prime_id); param_get(param_find("CAL_MAG_PRIME"), &prime_id);
@ -660,7 +650,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
} }
/* ---- ACCEL ---- */ /* ---- ACCEL ---- */
if (checkAcc) { if (checkSensors) {
bool prime_found = false; bool prime_found = false;
int32_t prime_id = 0; int32_t prime_id = 0;
param_get(param_find("CAL_ACC_PRIME"), &prime_id); param_get(param_find("CAL_ACC_PRIME"), &prime_id);
@ -689,7 +679,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
} }
/* ---- GYRO ---- */ /* ---- GYRO ---- */
if (checkGyro) { if (checkSensors) {
bool prime_found = false; bool prime_found = false;
int32_t prime_id = 0; int32_t prime_id = 0;
param_get(param_find("CAL_GYRO_PRIME"), &prime_id); param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
@ -718,7 +708,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
} }
/* ---- BARO ---- */ /* ---- BARO ---- */
if (checkBaro) { if (checkSensors) {
bool prime_found = false; bool prime_found = false;
int32_t prime_id = 0; int32_t prime_id = 0;
param_get(param_find("CAL_BARO_PRIME"), &prime_id); param_get(param_find("CAL_BARO_PRIME"), &prime_id);
@ -748,7 +738,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
} }
/* ---- IMU CONSISTENCY ---- */ /* ---- IMU CONSISTENCY ---- */
imuConsistencyCheck(mavlink_log_pub, checkAcc, checkGyro, reportFailures); imuConsistencyCheck(mavlink_log_pub, reportFailures);
/* ---- AIRSPEED ---- */ /* ---- AIRSPEED ---- */
if (checkAirspeed) { if (checkAirspeed) {

3
src/modules/commander/PreflightCheck.h
Unescape View File

@ -68,8 +68,7 @@ namespace Commander
* @param checkGNSS * @param checkGNSS
* true if the GNSS receiver should be checked * true if the GNSS receiver should be checked
**/ **/
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkSensors, bool checkAirspeed, bool checkRC, bool checkGNSS,
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS,
bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot); bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot);
static constexpr unsigned max_mandatory_gyro_count = 1; static constexpr unsigned max_mandatory_gyro_count = 1;

17
src/modules/commander/commander.cpp
Unescape View File

@ -1719,9 +1719,10 @@ int commander_thread_main(int argc, char *argv[])
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} else { } else {
// sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet // sensor diagnostics done continuously, not just at boot so don't warn about any issues just yet
status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true,
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check, checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status_flags.circuit_breaker_engaged_gpsfailure_check,
/* checkDynamic */ false, is_vtol(&status), /* reportFailures */ false, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); false, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp));
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} }
@ -1986,14 +1987,14 @@ int commander_thread_main(int argc, char *argv[])
/* provide RC and sensor status feedback to the user */ /* provide RC and sensor status feedback to the user */
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
/* HITL configuration: check only RC input */ /* HITL configuration: check only RC input */
(void)Commander::preflightCheck(&mavlink_log_pub, false, false, false, false, false, (void)Commander::preflightCheck(&mavlink_log_pub, false, false,
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false,
/* checkDynamic */ true, is_vtol(&status), /* reportFailures */ false, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); true, is_vtol(&status), false, false, hrt_elapsed_time(&commander_boot_timestamp));
} else { } else {
/* check sensors also */ /* check sensors also */
(void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, (void)Commander::preflightCheck(&mavlink_log_pub, true, checkAirspeed,
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), arm_requirements & ARM_REQ_GPS_BIT,
/* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp));
} }
// Provide feedback on mission state // Provide feedback on mission state
@ -4315,9 +4316,9 @@ void *commander_low_prio_loop(void *arg)
checkAirspeed = true; checkAirspeed = true;
} }
status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed, status_flags.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, checkAirspeed,
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT, !(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), arm_requirements & ARM_REQ_GPS_BIT,
/* checkDynamic */ true, is_vtol(&status), /* reportFailures */ hotplug_timeout, /* prearm */ false, hrt_elapsed_time(&commander_boot_timestamp)); true, is_vtol(&status), hotplug_timeout, false, hrt_elapsed_time(&commander_boot_timestamp));
arming_state_transition(&status, arming_state_transition(&status,
&battery, &battery,

2
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -1106,7 +1106,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
bool sensor_checks = (status->hil_state == vehicle_status_s::HIL_STATE_OFF); bool sensor_checks = (status->hil_state == vehicle_status_s::HIL_STATE_OFF);
bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, sensor_checks, sensor_checks, sensor_checks, sensor_checks, bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, sensor_checks,
checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, reportFailures, prearm, time_since_boot); arm_requirements & ARM_REQ_GPS_BIT, true, status->is_vtol, reportFailures, prearm, time_since_boot);

Write Preview
Loading…
Cancel
Save