yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Subsystem_info status flags & checks : Initial commit, updating the health flags in a centralized way mostly in commander and the votedSensorsUpdate function.

Browse Source
This commit is contained in:
Philipp Oettershagen
2018-04-03 16:36:33 +02:00
committed by Beat Küng
parent 40e6a5a39b
commit 6f1f414b49
31 changed files with 427 additions and 266 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
src/modules/commander/PreflightCheck.cpp
View File

@@ -55,6 +55,7 @@
#include <parameters/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
@@ -69,6 +70,7 @@
#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"
@@ -118,7 +120,9 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool present = true;
bool success = true;
int ret = 0;
char s[30];
sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
@@ -131,11 +135,12 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
}
}
return false;
present = false;
success = false;
goto out;
}
int ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
if (ret) {
if (report_fail) {
@@ -158,6 +163,9 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
}
out:
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG, present, !optional, success);
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, present, !optional, success);
DevMgr::releaseHandle(h);
return success;
}
@@ -182,8 +190,9 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
if (sensors.accel_inconsistency_m_s_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false);
}
success = false;
goto out;
@@ -199,6 +208,8 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
if (sensors.gyro_inconsistency_rad_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false);
}
success = false;
@@ -238,7 +249,8 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
}
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false);
return false;
}
@@ -247,7 +259,9 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool present = true;
bool success = true;
int ret = 0;
char s[30];
sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
@@ -260,11 +274,12 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
}
return false;
present = false;
success = false;
goto out;
}
int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
if (ret) {
if (report_fail) {
@@ -321,13 +336,18 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
#endif
out:
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC, present, !optional, success);
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, present, !optional, success);
DevMgr::releaseHandle(h);
return success;
}
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool present = true;
bool success = true;
int ret = 0;
char s[30];
sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
@@ -340,11 +360,12 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
}
return false;
present = false;
success = false;
goto out;
}
int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
if (ret) {
if (report_fail) {
@@ -367,6 +388,9 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
}
out:
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, present, !optional, success);
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, present, !optional, success);
DevMgr::releaseHandle(h);
return success;
}
@@ -386,7 +410,7 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
}
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, !optional, false);
return false;
}
@@ -402,13 +426,14 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
// }
//out:
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, true, !optional, success);
DevMgr::releaseHandle(h);
return success;
}
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm)
{
bool present = true;
bool success = true;
int fd_airspeed = orb_subscribe(ORB_ID(airspeed));
@@ -419,20 +444,20 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if ((orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure) != PX4_OK) ||
(hrt_elapsed_time(&differential_pressure.timestamp) > 1000000)) {
if (report_fail) {
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)) {
if (report_fail) {
if (report_fail && !optional) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
present = false;
success = false;
goto out;
}
@@ -447,7 +472,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR STUCK");
}
present = true;
success = false;
goto out;
}
@@ -461,12 +486,13 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT");
}
present = true;
success = false;
goto out;
}
out:
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success);
orb_unsubscribe(fd_airspeed);
orb_unsubscribe(fd_diffpres);
return success;
@@ -523,6 +549,7 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool pre
static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required)
{
bool success = true; // start with a pass and change to a fail if any test fails
bool present = true;
float test_limit = 1.0f; // pass limit re-used for each test
// Get estimator status data if available and exit with a fail recorded if not
@@ -530,6 +557,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
estimator_status_s status = {};
if (orb_copy(ORB_ID(estimator_status), sub, &status) != PX4_OK) {
present = false;
goto out;
}
@@ -626,12 +654,13 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
// The EKF is not using GPS
if (report_fail) {
if (ekf_gps_check_fail) {
// Poor GPS qulaity is the likely cause
// Poor GPS quality is the likely cause
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
} else {
// Likely cause unknown
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
}
}
@@ -649,6 +678,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
if (gps_quality_fail) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
}
success = false;
@@ -658,15 +688,15 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
}
out:
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present);
orb_unsubscribe(sub);
return success;
}
bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
const hrt_abstime &time_since_boot)
{
if (time_since_boot < 2000000) {
// the airspeed driver filter doesn't deliver the actual value yet
reportFailures = false;
@@ -737,7 +767,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
if ((reportFailures && !failed)) {
mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false);
failed = true;
}
@@ -775,7 +805,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
if ((reportFailures && !failed)) {
mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false);
failed = true;
}
}
@@ -808,7 +838,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
if ((reportFailures && !failed)) {
mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false);
failed = true;
}
}
@@ -844,7 +874,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
if (reportFailures && !failed) {
mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false);
failed = true;
}
}
@@ -858,7 +888,10 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures && !failed, prearm)) {
int32_t optional = 0; //Given checkAirspeed==true, assume the airspeed sensor is also required by default
param_get(param_find("FW_ARSP_MODE"), &optional);
if (!airspeedCheck(mavlink_log_pub, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
failed = true;
}
}
@@ -871,6 +904,13 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
}
failed = true;
publish_subsystem_info_enabled_healthy(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, false);
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;
publish_subsystem_info_enabled_healthy(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, !status.rc_signal_lost);
}
}
@@ -881,6 +921,29 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
}
}
/* ---- GPS ---- */
if (checkGNSS) {
int fd_gps = orb_subscribe(ORB_ID(vehicle_gps_position));
vehicle_gps_position_s gps = {};
bool present = true;
if ((orb_copy(ORB_ID(vehicle_gps_position), fd_gps, &gps) != PX4_OK) ||
(hrt_elapsed_time(&gps.timestamp) > 2000000)) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS MODULE MISSING");
}
present = false;
}
// Mark GPS as required (given that checkGNSS=true) and indicate whether it is present. For now also assume it is healthy
// if there is a lock ... EKF2 will then set the healthy=false if its more extensive GPS checks fail in the next step.
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GPS, present, true, present && gps.fix_type>=3);
}
// TODO: Add rangefinder here. We have the SENS_EN_XXX params that tell us what we should have. This is currently completely done inside the driver.
// TODO: Add optical flow check here? This is currently completely done inside the driver.
/* ---- Navigation EKF ---- */
// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
int32_t estimator_type;
@@ -889,8 +952,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
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);
if (!ekf2Check(mavlink_log_pub, true, (reportFailures && report_ekf_fail && !failed), checkGNSS)) {
if (!ekf2Check(mavlink_log_pub, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) {
failed = true;
}
}