From c7e572d2c2bb21ae3dfdbff637022946d9f9b72c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 2 Aug 2018 21:57:24 -0400 Subject: [PATCH] commander preflight check sensors via uORB instead of IOCTL - remove all platform defines --- ROMFS/px4fmu_common/init.d-posix/rcS | 14 +- src/modules/commander/PreflightCheck.cpp | 469 ++++++++++---------- src/modules/simulator/accelsim/accelsim.cpp | 3 + src/modules/simulator/gyrosim/gyrosim.cpp | 5 +- src/modules/uORB/Subscription.hpp | 8 +- 5 files changed, 260 insertions(+), 239 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 14b1c07a39a2..9c0923a7e423 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -94,18 +94,14 @@ then param set BAT_N_CELLS 3 param set CAL_ACC0_ID 1376264 - param set CAL_ACC0_XOFF 0.01 - param set CAL_ACC0_XSCALE 1.01 - param set CAL_ACC0_YOFF -0.01 - param set CAL_ACC0_YSCALE 1.01 - param set CAL_ACC0_ZOFF 0.01 - param set CAL_ACC0_ZSCALE 1.01 param set CAL_ACC1_ID 1310728 - param set CAL_ACC1_XOFF 0.01 + param set CAL_ACC_PRIME 1376264 + param set CAL_GYRO0_ID 2293768 - param set CAL_GYRO0_XOFF 0.01 + param set CAL_GYRO_PRIME 2293768 + param set CAL_MAG0_ID 196616 - param set CAL_MAG0_XOFF 0.01 + param set CAL_MAG_PRIME 196616 param set CBRK_AIRSPD_CHK 0 diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 25e8d5b2ffbd..26fdc8d5a1d5 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -40,51 +40,40 @@ * @author Johan Jansen */ -#include -#include +#include "PreflightCheck.h" +#include "health_flag_helper.h" +#include "rc_check.h" #include #include - -#include -#include -#include -#include -#include -#include - +#include #include #include #include +#include +#include +#include +#include #include -#include -#include #include +#include -#include "PreflightCheck.h" -#include "health_flag_helper.h" -#include "rc_check.h" - -#include "DevMgr.hpp" - -using namespace DriverFramework; +using namespace time_literals; namespace Preflight { -static int check_calibration(DevHandle &h, const char *param_template, int &devid) +static bool check_calibration(const char *param_template, int32_t device_id) { bool calibration_found = false; - devid = h.ioctl(DEVIOCGDEVICEID, 0); - char s[20]; int instance = 0; /* old style transition: check param values */ while (!calibration_found) { sprintf(s, param_template, instance); - param_t parm = param_find(s); + const param_t parm = param_find_no_notification(s); /* if the calibration param is not present, abort */ if (parm == PARAM_INVALID) { @@ -92,12 +81,12 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi } /* if param get succeeds */ - int32_t calibration_devid; + int32_t calibration_devid = -1; - if (!param_get(parm, &(calibration_devid))) { + if (param_get(parm, &calibration_devid) == PX4_OK) { /* if the devid matches, exit early */ - if (devid == calibration_devid) { + if (device_id == calibration_devid) { calibration_found = true; break; } @@ -106,47 +95,53 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi instance++; } - return !calibration_found; + return calibration_found; } -static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional, int &device_id, bool report_fail) +static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional, + int32_t &device_id, bool report_fail) { - bool present = true; - bool success = true; - int ret = 0; + const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK); + bool calibration_valid = false; + bool mag_valid = false; - char s[30]; - sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance); - DevHandle h; - DevMgr::getHandle(s, h); + if (exists) { - if (!h.isValid()) { - if (!optional) { + uORB::Subscription magnetometer{ORB_ID(sensor_mag), 0, instance}; + + mag_valid = (hrt_elapsed_time(&magnetometer.get().timestamp) < 1_s); + + if (!mag_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u invalid", instance); } } - present = false; - success = false; - goto out; - } - ret = check_calibration(h, "CAL_MAG%u_ID", device_id); + device_id = magnetometer.get().device_id; - if (ret) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); + calibration_valid = check_calibration("CAL_MAG%u_ID", device_id); + + if (!calibration_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); + } } - success = false; - goto out; + } else { + if (!optional && report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance); + } } -out: - if (instance==0) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, present, !optional, success, status); - if (instance==1) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, present, !optional, success, status); + const bool success = calibration_valid && mag_valid; + + if (instance == 0) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, exists, !optional, success, status); + + } else if (instance == 1) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, exists, !optional, success, status); + } - DevMgr::releaseHandle(h); return success; } @@ -173,6 +168,7 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, status); set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, status); } + success = false; goto out; @@ -231,165 +227,155 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s 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 false; } return true; } -static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail) +static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, + bool optional, bool dynamic, int32_t &device_id, bool report_fail) { - bool present = true; - bool success = true; - int ret = 0; + const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK); + bool calibration_valid = false; + bool accel_valid = true; + + if (exists) { - char s[30]; - sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance); - DevHandle h; - DevMgr::getHandle(s, h); + uORB::Subscription accel{ORB_ID(sensor_accel), 0, instance}; - if (!h.isValid()) { - if (!optional) { + accel_valid = (hrt_elapsed_time(&accel.get().timestamp) < 1_s); + + if (!accel_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u invalid", instance); } } - present = false; - success = false; - goto out; - } - ret = check_calibration(h, "CAL_ACC%u_ID", device_id); + device_id = accel.get().device_id; - if (ret) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); - } + calibration_valid = check_calibration("CAL_ACC%u_ID", device_id); - success = false; - goto out; - } + if (!calibration_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); + } -#ifdef __PX4_NUTTX + } else { - if (dynamic) { - /* check measurement result range */ - struct accel_report acc; - ret = h.read(&acc, sizeof(acc)); + if (dynamic) { + const float accel_magnitude = sqrtf(accel.get().x * accel.get().x + + accel.get().y * accel.get().y + + accel.get().z * accel.get().z); - if (ret == sizeof(acc)) { - /* evaluate values */ - float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + } - if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming"); + /* this is frickin' fatal */ + accel_valid = false; } - - /* this is frickin' fatal */ - success = false; - goto out; - } - - } else { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ"); } + } - /* this is frickin' fatal */ - success = false; - goto out; + } else { + if (!optional && report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance); } } -#endif + const bool success = calibration_valid && accel_valid; -out: - if (instance==0) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, present, !optional, success, status); - if (instance==1) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, present, !optional, success, status); + if (instance == 0) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, exists, !optional, success, status); + + } else if (instance == 1) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, exists, !optional, success, status); + } - DevMgr::releaseHandle(h); return success; } -static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional, int &device_id, bool report_fail) +static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional, + int32_t &device_id, bool report_fail) { - bool present = true; - bool success = true; - int ret = 0; + const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK); + bool calibration_valid = false; + bool gyro_valid = false; + + if (exists) { + + uORB::Subscription gyro{ORB_ID(sensor_gyro), 0, instance}; - char s[30]; - sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance); - DevHandle h; - DevMgr::getHandle(s, h); + gyro_valid = (hrt_elapsed_time(&gyro.get().timestamp) < 1_s); - if (!h.isValid()) { - if (!optional) { + if (!gyro_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u invalid", instance); } } - present = false; - success = false; - goto out; - } - ret = check_calibration(h, "CAL_GYRO%u_ID", device_id); + device_id = gyro.get().device_id; - if (ret) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); + calibration_valid = check_calibration("CAL_GYRO%u_ID", device_id); + + if (!calibration_valid) { + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); + } } - success = false; - goto out; + } else { + if (!optional && report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance); + } } -out: - if (instance==0) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, present, !optional, success, status); - if (instance==1) set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, present, !optional, success, status); + if (instance == 0) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, exists, !optional, calibration_valid && gyro_valid, status); - DevMgr::releaseHandle(h); - return success; + } else if (instance == 1) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, exists, !optional, calibration_valid && gyro_valid, status); + } + + return calibration_valid && gyro_valid; } -static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional, int &device_id, bool report_fail) +static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional, + int32_t &device_id, bool report_fail) { - bool success = true; + const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK); + bool baro_valid = false; - char s[30]; - sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance); - DevHandle h; - DevMgr::getHandle(s, h); + if (exists) { + uORB::Subscription baro{ORB_ID(sensor_baro), 0, instance}; - if (!h.isValid()) { - if (!optional) { + baro_valid = (hrt_elapsed_time(&baro.get().timestamp) < 1_s); + + if (!baro_valid) { if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u invalid", instance); } } - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, !optional, false, status); - return false; - } - device_id = -1000; - // TODO: There is no baro calibration yet, since no external baros exist - // int ret = check_calibration(fd, "CAL_BARO%u_ID"); + } else { + if (!optional && report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance); + } + } - // if (ret) { - // mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance); - // success = false; - // goto out; - // } + if (instance == 0) { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, baro_valid, status); + } -//out: - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, true, !optional, success, status); - DevMgr::releaseHandle(h); - return success; + return baro_valid; } -static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, bool optional, bool report_fail, bool prearm) +static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, bool optional, bool report_fail, + bool prearm) { bool present = true; bool success = true; @@ -401,20 +387,22 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu differential_pressure_s differential_pressure = {}; if ((orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure) != PX4_OK) || - (hrt_elapsed_time(&differential_pressure.timestamp) > 1000000)) { + (hrt_elapsed_time(&differential_pressure.timestamp) > 1_s)) { if (report_fail && !optional) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); } + present = false; success = false; goto out; } if ((orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed) != PX4_OK) || - (hrt_elapsed_time(&airspeed.timestamp) > 1000000)) { + (hrt_elapsed_time(&airspeed.timestamp) > 1_s)) { if (report_fail && !optional) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING"); } + present = false; success = false; goto out; @@ -430,6 +418,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR STUCK"); } + present = true; success = false; goto out; @@ -444,6 +433,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu if (report_fail) { mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT"); } + present = true; success = false; goto out; @@ -451,8 +441,10 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu out: set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status); + orb_unsubscribe(fd_airspeed); orb_unsubscribe(fd_diffpres); + return success; } @@ -471,7 +463,7 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, if (orb_copy(ORB_ID(system_power), system_power_sub, &system_power) == PX4_OK) { - if (hrt_elapsed_time(&system_power.timestamp) < 200000) { + if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) { /* copy avionics voltage */ float avionics_power_rail_voltage = system_power.voltage5v_v; @@ -504,7 +496,8 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, return success; } -static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, bool optional, bool report_fail, bool enforce_gps_required) +static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, 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 bool present = true; @@ -615,11 +608,18 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s // The EKF is not using GPS if (ekf_gps_check_fail) { // Poor GPS quality is the likely cause - if (report_fail) mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); + } + gps_success = false; + } else { // Likely cause unknown - if (report_fail) mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS"); + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS"); + } + gps_success = false; gps_present = false; } @@ -636,7 +636,10 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0); if (gps_quality_fail) { - if (report_fail) mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); + if (report_fail) { + mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR"); + } + gps_success = false; success = false; goto out; @@ -647,7 +650,9 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s out: set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, vehicle_status); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status); + orb_unsubscribe(sub); + return success; } @@ -655,7 +660,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm, const hrt_abstime &time_since_boot) { - if (time_since_boot < 2000000) { + if (time_since_boot < 2_s) { // the airspeed driver filter doesn't deliver the actual value yet reportFailures = false; } @@ -677,32 +682,15 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout && !status_flags.condition_calibration_enabled); -#ifdef __PX4_QURT - // WARNING: Preflight checks are important and should be added back when - // all the sensors are supported - PX4_WARN("Preflight checks always pass on Snapdragon."); - checkSensors = false; -#elif defined(__PX4_POSIX_RPI) - PX4_WARN("Preflight checks for mag, acc, gyro always pass on RPI"); - checkSensors = false; -#elif defined(__PX4_POSIX_BBBLUE) - PX4_WARN("Preflight checks for mag, acc, gyro always pass on BBBLUE"); - checkSensors = false; -#elif defined(__PX4_POSIX_BEBOP) - PX4_WARN("Preflight checks always pass on Bebop."); - checkSensors = false; -#elif defined(__PX4_POSIX_OCPOC) - PX4_WARN("Preflight checks always pass on OcPoC."); - checkSensors = false; -#endif - bool failed = false; /* ---- MAG ---- */ if (checkSensors) { bool prime_found = false; - int32_t prime_id = 0; + + int32_t prime_id = -1; param_get(param_find("CAL_MAG_PRIME"), &prime_id); + int32_t sys_has_mag = 1; param_get(param_find("SYS_HAS_MAG"), &sys_has_mag); @@ -710,24 +698,31 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, /* check all sensors individually, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { - bool required = (i < max_mandatory_mag_count) && sys_has_mag == 1; - int device_id = -1; + const bool required = (i < max_mandatory_mag_count) && (sys_has_mag == 1); + const bool report_fail = (reportFailures && !failed && !mag_fail_reported); - if (!magnometerCheck(mavlink_log_pub, status, i, !required, device_id, (reportFailures && !failed && !mag_fail_reported)) && required) { - failed = true; - mag_fail_reported = true; - } + int32_t device_id = -1; - if (device_id == prime_id) { - prime_found = true; + if (magnometerCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + + if ((prime_id > 0) && (device_id == prime_id)) { + prime_found = true; + } + + } else { + if (required) { + failed = true; + mag_fail_reported = true; + } } } /* check if the primary device is present */ - if (!prime_found && prime_id != 0) { - if ((reportFailures && !failed)) { + if (!prime_found) { + if (reportFailures && !failed) { 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; } @@ -741,31 +736,38 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, /* ---- ACCEL ---- */ if (checkSensors) { bool prime_found = false; - int32_t prime_id = 0; + int32_t prime_id = -1; param_get(param_find("CAL_ACC_PRIME"), &prime_id); bool accel_fail_reported = false; /* check all sensors individually, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { - bool required = (i < max_mandatory_accel_count); - int device_id = -1; + const bool required = (i < max_mandatory_accel_count); + const bool report_fail = (reportFailures && !failed && !accel_fail_reported); - if (!accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, (reportFailures && !failed && !accel_fail_reported)) && required) { - failed = true; - accel_fail_reported = true; - } + int32_t device_id = -1; - if (device_id == prime_id) { - prime_found = true; + 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 (required) { + failed = true; + accel_fail_reported = true; + } } } /* check if the primary device is present */ - if (!prime_found && prime_id != 0) { - if ((reportFailures && !failed)) { + if (!prime_found) { + if (reportFailures && !failed) { 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; } @@ -774,31 +776,38 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, /* ---- GYRO ---- */ if (checkSensors) { bool prime_found = false; - int32_t prime_id = 0; + int32_t prime_id = -1; param_get(param_find("CAL_GYRO_PRIME"), &prime_id); bool gyro_fail_reported = false; /* check all sensors individually, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { - bool required = (i < max_mandatory_gyro_count); - int device_id = -1; + const bool required = (i < max_mandatory_gyro_count); + const bool report_fail = (reportFailures && !failed && !gyro_fail_reported); - if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, (reportFailures && !failed && !gyro_fail_reported)) && required) { - failed = true; - gyro_fail_reported = true; - } + int32_t device_id = -1; + + if (gyroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + + if ((prime_id > 0) && (device_id == prime_id)) { + prime_found = true; + } - if (device_id == prime_id) { - prime_found = true; + } else { + if (required) { + failed = true; + gyro_fail_reported = true; + } } } /* check if the primary device is present */ - if (!prime_found && prime_id != 0) { - if ((reportFailures && !failed)) { + if (!prime_found) { + if (reportFailures && !failed) { 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; } @@ -807,8 +816,10 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, /* ---- BARO ---- */ if (checkSensors) { bool prime_found = false; - int32_t prime_id = 0; + + int32_t prime_id = -1; param_get(param_find("CAL_BARO_PRIME"), &prime_id); + int32_t sys_has_baro = 1; param_get(param_find("SYS_HAS_BARO"), &sys_has_baro); @@ -816,25 +827,31 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { - bool required = (i < max_mandatory_baro_count) && sys_has_baro == 1; - int device_id = -1; + const bool required = (i < max_mandatory_baro_count) && (sys_has_baro == 1); + const bool report_fail = (reportFailures && !failed && !baro_fail_reported); - if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, (reportFailures && !failed && !baro_fail_reported)) && required) { - failed = true; - baro_fail_reported = true; - } + int32_t device_id = -1; - if (device_id == prime_id) { - prime_found = true; + if (baroCheck(mavlink_log_pub, status, i, !required, device_id, report_fail)) { + if ((prime_id > 0) && (device_id == prime_id)) { + prime_found = true; + } + + } else { + if (required) { + failed = true; + baro_fail_reported = true; + } } } // TODO there is no logic in place to calibrate the primary baro yet // // check if the primary device is present - if (!prime_found && prime_id != 0) { + if (!prime_found && false) { if (reportFailures && !failed) { mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational"); } + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status); failed = true; } @@ -869,6 +886,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, status); status_flags.rc_calibration_valid = false; + } else { // The calibration is fine, but only set the overall health state to true if the signal is not currently lost status_flags.rc_calibration_valid = true; @@ -890,7 +908,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, if (estimator_type == 2) { // don't report ekf failures for the first 10 seconds to allow time for the filter to start - bool report_ekf_fail = (time_since_boot > 10 * 1000000); + bool report_ekf_fail = (time_since_boot > 10_s); + if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) { failed = true; } diff --git a/src/modules/simulator/accelsim/accelsim.cpp b/src/modules/simulator/accelsim/accelsim.cpp index c4fd55077a71..77fda93d0d10 100644 --- a/src/modules/simulator/accelsim/accelsim.cpp +++ b/src/modules/simulator/accelsim/accelsim.cpp @@ -837,6 +837,8 @@ ACCELSIM::_measure() accel_report.timestamp = hrt_absolute_time(); + accel_report.device_id = 1310728; + // use the temperature from the last mag reading accel_report.temperature = _last_temperature; @@ -919,6 +921,7 @@ ACCELSIM::mag_measure() mag_report.timestamp = hrt_absolute_time(); + mag_report.device_id = 196616; mag_report.is_external = false; mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale); diff --git a/src/modules/simulator/gyrosim/gyrosim.cpp b/src/modules/simulator/gyrosim/gyrosim.cpp index 1d1e79378d96..5c274f5e225d 100644 --- a/src/modules/simulator/gyrosim/gyrosim.cpp +++ b/src/modules/simulator/gyrosim/gyrosim.cpp @@ -968,7 +968,7 @@ GYROSIM::_measure() arb.z_integral = aval_integrated(2); /* fake device ID */ - arb.device_id = 6789478; + arb.device_id = 1376264; grb.x_raw = (int16_t)(mpu_report.gyro_x / _gyro_range_scale); grb.y_raw = (int16_t)(mpu_report.gyro_y / _gyro_range_scale); @@ -991,12 +991,11 @@ GYROSIM::_measure() grb.z_integral = gval_integrated(2); /* fake device ID */ - grb.device_id = 3467548; + grb.device_id = 2293768; _accel_reports->force(&arb); _gyro_reports->force(&grb); - if (accel_notify) { if (!(_pub_blocked)) { /* publish it */ diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 6807dd1e3f2a..c0137df57432 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -90,8 +90,10 @@ class __EXPORT SubscriptionBase protected: const struct orb_metadata *_meta; + unsigned _instance; - int _handle; + + int _handle{-1}; }; /** @@ -155,7 +157,9 @@ class __EXPORT Subscription : public SubscriptionNode List *list = nullptr): SubscriptionNode(meta, interval, instance, list), _data() // initialize data structure to zero - {} + { + forcedUpdate(); + } ~Subscription() override = default;