mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-19 19:09:06 +08:00
commander preflight check sensors via uORB instead of IOCTL
- remove all platform defines
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
f72e9e4385
commit
c7e572d2c2
@ -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
|
||||
|
||||
|
||||
@ -40,51 +40,40 @@
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_posix.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/topics/airspeed.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/estimator_status.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 "PreflightCheck.h"
|
||||
#include "health_flag_helper.h"
|
||||
#include "rc_check.h"
|
||||
|
||||
#include "DevMgr.hpp"
|
||||
#include <parameters/param.h>
|
||||
#include <systemlib/mavlink_log.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/subsystem_info.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
|
||||
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;
|
||||
|
||||
char s[30];
|
||||
sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
|
||||
DevHandle h;
|
||||
DevMgr::getHandle(s, h);
|
||||
if (exists) {
|
||||
|
||||
if (!h.isValid()) {
|
||||
if (!optional) {
|
||||
uORB::Subscription<sensor_accel_s> accel{ORB_ID(sensor_accel), 0, instance};
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
|
||||
if (dynamic) {
|
||||
/* check measurement result range */
|
||||
struct accel_report acc;
|
||||
ret = h.read(&acc, sizeof(acc));
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
/* this is frickin' fatal */
|
||||
success = false;
|
||||
goto out;
|
||||
if (!calibration_valid) {
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
|
||||
}
|
||||
|
||||
} else {
|
||||
if (report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ");
|
||||
}
|
||||
|
||||
/* this is frickin' fatal */
|
||||
success = false;
|
||||
goto out;
|
||||
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 (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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} 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;
|
||||
|
||||
char s[30];
|
||||
sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
|
||||
DevHandle h;
|
||||
DevMgr::getHandle(s, h);
|
||||
if (exists) {
|
||||
|
||||
if (!h.isValid()) {
|
||||
if (!optional) {
|
||||
uORB::Subscription<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), 0, instance};
|
||||
|
||||
gyro_valid = (hrt_elapsed_time(&gyro.get().timestamp) < 1_s);
|
||||
|
||||
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;
|
||||
|
||||
|
||||
} else {
|
||||
if (!optional && report_fail) {
|
||||
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
|
||||
}
|
||||
}
|
||||
|
||||
device_id = -1000;
|
||||
if (instance == 0) {
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, exists, !optional, baro_valid, status);
|
||||
}
|
||||
|
||||
// TODO: There is no baro calibration yet, since no external baros exist
|
||||
// int ret = check_calibration(fd, "CAL_BARO%u_ID");
|
||||
|
||||
// if (ret) {
|
||||
// mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
|
||||
// success = false;
|
||||
// goto out;
|
||||
// }
|
||||
|
||||
//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 (device_id == prime_id) {
|
||||
prime_found = true;
|
||||
if (gyroCheck(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;
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user