fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-10 00:48:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

PreFlightCheck: mark sensor required if used by an EKF2 instance

Browse Source
This commit is contained in:
bresch
2022-03-16 11:46:01 +01:00
committed by Mathieu Bresciani
parent 6d0b11ca99
commit 5f1b577b6d
7 changed files with 86 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
+7 -9
View File
@@ -73,9 +73,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (sys_has_mag == 1) {
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_mag_count, max_optional_mag_count,
mavlink_log_pub, status, magnetometerCheck);
// TODO: highest priority mag
mavlink_log_pub, status, magnetometerCheck, isMagRequired);
/* mag consistency checks (need to be performed after the individual checks) */
if (!magConsistencyCheck(mavlink_log_pub, status, report_failures)) {
@@ -87,7 +85,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- ACCEL ---- */
{
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_accel_count, max_optional_accel_count,
mavlink_log_pub, status, accelerometerCheck);
mavlink_log_pub, status, accelerometerCheck, isAccelRequired);
// TODO: highest priority (from params)
}
@@ -95,7 +93,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
/* ---- GYRO ---- */
{
failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_gyro_count, max_optional_gyro_count,
mavlink_log_pub, status, gyroCheck);
mavlink_log_pub, status, gyroCheck, isGyroRequired);
// TODO: highest priority (from params)
}
@@ -107,7 +105,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (sys_has_baro == 1) {
static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count, max_optional_baro_count,
mavlink_log_pub, status, baroCheck));
mavlink_log_pub, status, baroCheck, isBaroRequired));
}
}
@@ -126,7 +124,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
if (sys_has_num_dist_sens > 0) {
static_cast<void>(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens, sys_has_num_dist_sens,
mavlink_log_pub, status, distSensCheck));
mavlink_log_pub, status, distSensCheck, isDistSensRequired));
}
}
@@ -240,14 +238,14 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
bool PreFlightCheck::sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub,
vehicle_status_s &status, sens_check_func_t sens_check)
vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired)
{
bool pass_check = true;
bool report_fail = report_failure;
/* check all sensors, but fail only for mandatory ones */
for (uint8_t i = 0u; i < max_optional_count; i++) {
const bool required = (i < max_mandatory_count);
const bool required = (i < max_mandatory_count) || isSensorRequired(i);
int32_t device_id = -1;
if (!sens_check(mavlink_log_pub, status, i, !required, device_id, report_fail)) {
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
+7 -1
View File
@@ -49,6 +49,7 @@
typedef bool (*sens_check_func_t)(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
typedef bool (*is_sens_req_func_t)(uint8_t instance);
class PreFlightCheck
{
@@ -101,16 +102,21 @@ public:
private:
static bool sensorAvailabilityCheck(const bool report_failure, const uint8_t max_mandatory_count,
const uint8_t max_optional_count, orb_advert_t *mavlink_log_pub,
vehicle_status_s &status, sens_check_func_t sens_check);
vehicle_status_s &status, sens_check_func_t sens_check, is_sens_req_func_t isSensorRequired);
static bool isMagRequired(uint8_t instance);
static bool magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
static bool isAccelRequired(uint8_t instance);
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
static bool isGyroRequired(uint8_t instance);
static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
static bool isBaroRequired(uint8_t instance);
static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
static bool isDistSensRequired(uint8_t instance);
static bool distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail);
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp
+17
View File
@@ -40,10 +40,27 @@
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_accel.h>
using namespace time_literals;
bool PreFlightCheck::isAccelRequired(const uint8_t instance)
{
uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
const uint32_t device_id = static_cast<uint32_t>(accel.get().device_id);
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().accel_device_id == device_id) {
return true;
}
}
return false;
}
bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail)
{
src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp
+17
View File
@@ -38,10 +38,27 @@
#include <px4_defines.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_baro.h>
using namespace time_literals;
bool PreFlightCheck::isBaroRequired(const uint8_t instance)
{
uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
const uint32_t device_id = static_cast<uint32_t>(baro.get().device_id);
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().baro_device_id == device_id) {
return true;
}
}
return false;
}
bool PreFlightCheck::baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail)
{
src/modules/commander/Arming/PreFlightCheck/checks/distanceSensorChecks.cpp
+5
View File
@@ -42,6 +42,11 @@
using namespace time_literals;
bool PreFlightCheck::isDistSensRequired(const uint8_t instance)
{
return false;
}
bool PreFlightCheck::distSensCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail)
{
src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp
+17
View File
@@ -39,10 +39,27 @@
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_gyro.h>
using namespace time_literals;
bool PreFlightCheck::isGyroRequired(const uint8_t instance)
{
uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
const uint32_t device_id = static_cast<uint32_t>(gyro.get().device_id);
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().gyro_device_id == device_id) {
return true;
}
}
return false;
}
bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail)
{
src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp
+16
View File
@@ -44,6 +44,22 @@
using namespace time_literals;
bool PreFlightCheck::isMagRequired(const uint8_t instance)
{
uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
const uint32_t device_id = static_cast<uint32_t>(magnetometer.get().device_id);
for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
uORB::SubscriptionData<estimator_status_s> estimator_status_sub{ORB_ID(estimator_status), i};
if (device_id > 0 && estimator_status_sub.get().mag_device_id == device_id) {
return true;
}
}
return false;
}
bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
const bool optional, int32_t &device_id, const bool report_fail)
{