diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
index 14b1c07a39a251bcecb88daee400aeaeb6b41ffd..9c0923a7e423778fee9f224c20290d3e7cdac828 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 25e8d5b2ffbdb9ffaf77e2c87b9e5eefd5fc08b1..26fdc8d5a1d55db13e64a5f40526258c10326b5b 100644
--- a/src/modules/commander/PreflightCheck.cpp
+++ b/src/modules/commander/PreflightCheck.cpp
@@ -40,51 +40,40 @@
* @author Johan Jansen <jnsn.johan@gmail.com>
*/
-#include <px4_config.h>
-#include <px4_posix.h>
+#include "PreflightCheck.h"
+#include "health_flag_helper.h"
+#include "rc_check.h"
#include <parameters/param.h>
#include <systemlib/mavlink_log.h>
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_mag.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_baro.h>
-#include <drivers/drv_airspeed.h>
-
+#include <uORB/Subscription.hpp>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_preflight.h>
-#include <uORB/topics/system_power.h>
-#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/system_power.h>
-#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<sensor_mag_s> 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<sensor_accel_s> 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<sensor_gyro_s> 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<sensor_baro_s> 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 c4fd55077a719224659dc9056ff0c3f15ad9f33e..77fda93d0d108d861a982480ba79d1dc5fd080dd 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 1d1e79378d96e9d9fe99f89159f7ef2b24e9ae06..5c274f5e225df4bac78708a4ac6e3dc25220839f 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 6807dd1e3f2a93c794cefb21eebb3e6eb1182145..c0137df5743241f751aaea53efd0e590cb2aeb0c 100644
--- a/src/modules/uORB/Subscription.hpp
+++ b/src/modules/uORB/Subscription.hpp
@@ -90,8 +90,10 @@ public:
protected:
const struct orb_metadata *_meta;
+
unsigned _instance;
- int _handle;
+
+ int _handle{-1};
};
/**
@@ -155,7 +157,9 @@ public:
List<SubscriptionNode *> *list = nullptr):
SubscriptionNode(meta, interval, instance, list),
_data() // initialize data structure to zero
- {}
+ {
+ forcedUpdate();
+ }
~Subscription() override = default;