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

Subsystem_info status flags & checks: 1) Set health flags in commander directly instead of publishing via uORB 2) move publish_subsystem_info into lib/ folder"

Browse Source
This commit is contained in:
Philipp Oettershagen 2018-05-26 01:06:14 +02:00 committed by Beat Küng
parent f5847a4a7b
commit 075009be2f
13 changed files with 211 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/subsystem_info.msg
View File

@ -32,4 +32,4 @@ bool enabled
bool ok
uint64 subsystem_type
uint32 ORB_QUEUE_LENGTH = 25
uint32 ORB_QUEUE_LENGTH = 8

1
src/lib/CMakeLists.txt
View File

@ -48,3 +48,4 @@ add_subdirectory(rc)
add_subdirectory(terrain_estimation)
add_subdirectory(tunes)
add_subdirectory(version)
add_subdirectory(subsystem_info_pub)

34
src/lib/subsystem_info_pub/CMakeLists.txt Normal file
View File

@ -0,0 +1,34 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(subsystem_info_pub subsystem_info_pub.cpp)

110
src/lib/subsystem_info_pub/subsystem_info_pub.cpp Normal file
View File

@ -0,0 +1,110 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file subsystem_info_pub.cpp
*
* Contains helper functions to efficiently publish the subsystem_info messages from various locations inside the code.
*
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
*/
#include "subsystem_info_pub.h"
#include <uORB/topics/subsystem_info.h>
orb_advert_t subsys_pub = nullptr;
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s *status_ptr)
{
PX4_DEBUG("publish_subsystem_info (method:%s): Type %llu pres=%u enabl=%u ok=%u",
status_ptr == nullptr ? "publish" : "local", subsystem_type, present, enabled, ok);
if (status_ptr == nullptr) {
/* Publish the subsystem_info message via uORB. This is the case when this
* function was called from a module outside of commander */
struct subsystem_info_s subsys_info = {};
subsys_info.present = present;
subsys_info.enabled = enabled;
subsys_info.ok = ok;
subsys_info.subsystem_type = subsystem_type;
if (subsys_pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), subsys_pub, &subsys_info);
} else {
subsys_pub = orb_advertise_queue(ORB_ID(subsystem_info), &subsys_info, subsystem_info_s::ORB_QUEUE_LENGTH);
}
} else {
/* Update locally, i.e. directly using the supplied status_ptr. This happens
* when this function was called from inside commander*/
if (present) {
status_ptr->onboard_control_sensors_present |= (uint32_t)subsystem_type;
} else {
status_ptr->onboard_control_sensors_present &= ~(uint32_t)subsystem_type;
}
if (enabled) {
status_ptr->onboard_control_sensors_enabled |= (uint32_t)subsystem_type;
} else {
status_ptr->onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type;
}
if (ok) {
status_ptr->onboard_control_sensors_health |= (uint32_t)subsystem_type;
} else {
status_ptr->onboard_control_sensors_health &= ~(uint32_t)subsystem_type;
}
}
}
void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy,
vehicle_status_s *status_ptr)
{
struct vehicle_status_s status;
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status);
publish_subsystem_info(subsystem_type, present, status.onboard_control_sensors_enabled & (uint32_t)subsystem_type,
healthy, status_ptr);
}
void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok, vehicle_status_s *status_ptr)
{
struct vehicle_status_s status;
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status);
publish_subsystem_info(subsystem_type, status.onboard_control_sensors_present & (uint32_t)subsystem_type,
status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, ok, status_ptr);
}

17
src/modules/systemlib/subsystem_info_pub.h → src/lib/subsystem_info_pub/subsystem_info_pub.h
View File

@ -45,15 +45,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok);
void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy);
void publish_subsystem_info_present_enabled(uint64_t subsystem_type, bool present, bool enabled);
void publish_subsystem_info_enabled_healthy(uint64_t subsystem_type, bool enabled, bool ok);
void publish_subsystem_info_enabled(uint64_t subsystem_type, bool enabled);
void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok);
//// Local helper functions
bool getPresent(uint64_t subsystem_type);
bool getEnabled(uint64_t subsystem_type);
bool getHealthy(uint64_t subsystem_type);
void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok,
vehicle_status_s *status_ptr = nullptr);
void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy,
vehicle_status_s *status_ptr = nullptr);
void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok, vehicle_status_s *status_ptr = nullptr);

1
src/modules/commander/CMakeLists.txt
View File

@ -55,4 +55,5 @@ px4_add_module(
df_driver_framework
git_ecl
ecl_geo
subsystem_info_pub
)

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

@ -55,7 +55,7 @@
#include <parameters/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/subsystem_info_pub.h>
#include <lib/subsystem_info_pub/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
@ -119,7 +119,7 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi
return !calibration_found;
}
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, 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, int &device_id, bool report_fail)
{
bool present = true;
bool success = true;
@ -164,14 +164,14 @@ 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);
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG, present, !optional, success, &status);
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, present, !optional, success, &status);
DevMgr::releaseHandle(h);
return success;
}
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, bool report_status)
{
bool success = true; // start with a pass and change to a fail if any test fails
float test_limit = 1.0f; // pass limit re-used for each test
@ -191,8 +191,8 @@ 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);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, &status);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, &status);
}
success = false;
goto out;
@ -209,8 +209,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);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, &status);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, &status);
}
success = false;
@ -228,7 +228,7 @@ out:
}
// return false if the magnetomer measurements are inconsistent
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, bool report_status)
{
// get the sensor preflight data
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
@ -250,15 +250,15 @@ 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);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, &status);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, &status);
return false;
}
return true;
}
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, 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, int &device_id, bool report_fail)
{
bool present = true;
bool success = true;
@ -337,14 +337,14 @@ 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);
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC, present, !optional, success, &status);
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, present, !optional, success, &status);
DevMgr::releaseHandle(h);
return success;
}
static bool gyroCheck(orb_advert_t *mavlink_log_pub, 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, int &device_id, bool report_fail)
{
bool present = true;
bool success = true;
@ -389,14 +389,14 @@ 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);
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, present, !optional, success, &status);
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, present, !optional, success, &status);
DevMgr::releaseHandle(h);
return success;
}
static bool baroCheck(orb_advert_t *mavlink_log_pub, 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, int &device_id, bool report_fail)
{
bool success = true;
@ -411,7 +411,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);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, !optional, false, &status);
return false;
}
@ -427,12 +427,12 @@ 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);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, true, !optional, success, &status);
DevMgr::releaseHandle(h);
return success;
}
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, 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;
@ -493,13 +493,13 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
}
out:
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, &status);
orb_unsubscribe(fd_airspeed);
orb_unsubscribe(fd_diffpres);
return success;
}
static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool prearm)
static bool powerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, bool report_fail, bool prearm)
{
bool success = true;
@ -547,7 +547,7 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool pre
return success;
}
static bool ekf2Check(orb_advert_t *mavlink_log_pub, 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;
@ -688,13 +688,13 @@ 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);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, &vehicle_status);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, &vehicle_status);
orb_unsubscribe(sub);
return success;
}
bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
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)
{
@ -767,7 +767,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
bool required = (i < max_mandatory_mag_count) && sys_has_mag == 1;
int device_id = -1;
if (!magnometerCheck(mavlink_log_pub, i, !required, device_id, (reportFailures && !failed && !mag_fail_reported)) && required) {
if (!magnometerCheck(mavlink_log_pub, status, i, !required, device_id, (reportFailures && !failed && !mag_fail_reported)) && required) {
failed = true;
mag_fail_reported = true;
}
@ -782,12 +782,12 @@ 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);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, &status);
failed = true;
}
/* fail if mag sensors are inconsistent */
if (!magConsistencyCheck(mavlink_log_pub, (reportFailures && !failed))) {
if (!magConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
failed = true;
}
}
@ -805,7 +805,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, (reportFailures && !failed && !accel_fail_reported)) && required) {
if (!accelerometerCheck(mavlink_log_pub, status, i, !required, checkDynamic, device_id, (reportFailures && !failed && !accel_fail_reported)) && required) {
failed = true;
accel_fail_reported = true;
}
@ -820,7 +820,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);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, &status);
failed = true;
}
}
@ -838,7 +838,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_log_pub, i, !required, device_id, (reportFailures && !failed && !gyro_fail_reported)) && required) {
if (!gyroCheck(mavlink_log_pub, status, i, !required, device_id, (reportFailures && !failed && !gyro_fail_reported)) && required) {
failed = true;
gyro_fail_reported = true;
}
@ -853,7 +853,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);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, &status);
failed = true;
}
}
@ -873,7 +873,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
bool required = (i < max_mandatory_baro_count) && sys_has_baro == 1;
int device_id = -1;
if (!baroCheck(mavlink_log_pub, i, !required, device_id, (reportFailures && !failed && !baro_fail_reported)) && required) {
if (!baroCheck(mavlink_log_pub, status, i, !required, device_id, (reportFailures && !failed && !baro_fail_reported)) && required) {
failed = true;
baro_fail_reported = true;
}
@ -889,14 +889,14 @@ 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);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, &status);
failed = true;
}
}
/* ---- IMU CONSISTENCY ---- */
if (checkSensors) {
if (!imuConsistencyCheck(mavlink_log_pub, (reportFailures && !failed))) {
if (!imuConsistencyCheck(mavlink_log_pub, status, (reportFailures && !failed))) {
failed = true;
}
}
@ -906,7 +906,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
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) {
if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
failed = true;
}
}
@ -920,18 +920,18 @@ 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);
publish_subsystem_info(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;
publish_subsystem_info_enabled_healthy(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, !status.rc_signal_lost);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, !status.rc_signal_lost, &status);
}
}
/* ---- SYSTEM POWER ---- */
if (checkPower) {
if (!powerCheck(mavlink_log_pub, (reportFailures && !failed), prearm)) {
if (!powerCheck(mavlink_log_pub, status, (reportFailures && !failed), prearm)) {
failed = true;
}
}
@ -949,7 +949,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
}
present = false;
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, present, true, present);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, present, true, present, &status);
orb_unsubscribe(fd_distance_sensor);
}
@ -961,7 +961,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, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) {
if (!ekf2Check(mavlink_log_pub, status, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) {
failed = true;
}
}

2
src/modules/commander/PreflightCheck.h
View File

@ -74,7 +74,7 @@ namespace Preflight
* @param checkPower
* true if the system power should be checked
**/
bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
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);

23
src/modules/commander/commander.cpp
View File

@ -79,7 +79,7 @@
#include <parameters/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/state_table.h>
#include <systemlib/subsystem_info_pub.h>
#include <lib/subsystem_info_pub/subsystem_info_pub.h>
#include <cmath>
#include <cfloat>
@ -1272,8 +1272,6 @@ Commander::run()
status_flags.condition_local_velocity_valid = false;
status_flags.condition_local_altitude_valid = false;
bool status_changed = true;
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@ -1314,6 +1312,7 @@ Commander::run()
bool emergency_battery_voltage_actions_done = false;
bool dangerous_battery_level_requests_poweroff = false;
bool status_changed = true;
bool param_init_forced = true;
bool updated = false;
@ -1830,8 +1829,8 @@ Commander::run()
if((_last_condition_global_position_valid != status_flags.condition_global_position_valid) && status_flags.condition_global_position_valid) {
// If global position state changed and is now valid, set respective health flags to true. For now also assume GPS is OK if global pos is OK, but not vice versa.
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true);
publish_subsystem_info_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, &status);
publish_subsystem_info_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, &status);
}
check_valid(_local_position_sub.get().timestamp, _failsafe_pos_delay.get() * 1_s, _local_position_sub.get().z_valid, &(status_flags.condition_local_altitude_valid), &status_changed);
@ -2002,7 +2001,7 @@ Commander::run()
}
}
/* update subsystem */
/* update subsystem info which arrives from outside of commander*/
do {
orb_check(subsys_sub, &updated);
if (updated) {
@ -2233,13 +2232,13 @@ Commander::run()
/* handle the case where RC signal was regained */
if (!status_flags.rc_signal_found_once) {
status_flags.rc_signal_found_once = true;
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid, &status);
status_changed = true;
} else {
if (status.rc_signal_lost) {
mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums", hrt_elapsed_time(&rc_signal_lost_timestamp) / 1000);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid, &status);
status_changed = true;
}
}
@ -2387,7 +2386,7 @@ Commander::run()
mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);
status.rc_signal_lost = true;
rc_signal_lost_timestamp = sp_man.timestamp;
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, &status);
status_changed = true;
}
}
@ -2425,7 +2424,7 @@ Commander::run()
status_changed = true;
PX4_ERR("Engine Failure");
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL,true,true,false);
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL,true,true,false, &status);
}
}
@ -4110,9 +4109,9 @@ void Commander::poll_telemetry_status()
(mission_result.instance_count > 0) && !mission_result.valid) {
mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false); // TODO
//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false, &status); // TODO
} else {
//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true); // TODO
//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true, &status); // TODO
}
}

1
src/modules/sensors/CMakeLists.txt
View File

@ -51,4 +51,5 @@ px4_add_module(
drivers__device
git_ecl
ecl_validation
subsystem_info_pub
)

4
src/modules/sensors/voted_sensors_update.cpp
View File

@ -40,7 +40,7 @@
#include "voted_sensors_update.h"
#include <systemlib/mavlink_log.h>
#include <systemlib/subsystem_info_pub.h>
#include <lib/subsystem_info_pub/subsystem_info_pub.h>
#include <conversion/rotation.h>
#include <ecl/geo/geo.h>
@ -929,7 +929,7 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n
PX4_WARN("FAILOVER event (idx=%u)! Sensor %s: Nr. %u Priority: %u", failover_index, sensor_name, i, sensor.priority[i]);
}
PX4_INFO("%s sensor switch from #%i", sensor_name, failover_index);
PX4_ERR("%s sensor switch from #%i", sensor_name, failover_index);
if (ctr_valid < 2) { // subsystem_info only contains flags for the first two sensors
uint64_t subsystem_type = 0;

1
src/modules/systemlib/CMakeLists.txt
View File

@ -44,7 +44,6 @@ set(SRCS
pid/pid.c
pwm_limit/pwm_limit.c
rc_check.c
subsystem_info_pub.cpp
)
if(${OS} STREQUAL "nuttx")

129
src/modules/systemlib/subsystem_info_pub.cpp
View File

@ -1,129 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file subsystem_info_pub.cpp
*
* Contains helper functions to efficiently publish the subsystem_info messages from various locations inside the code.
*
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
*/
#include "subsystem_info_pub.h"
#include <uORB/topics/subsystem_info.h>
static orb_advert_t pub = nullptr;
struct subsystem_info_s info = {};
struct vehicle_status_s status;
void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok)
{
PX4_INFO("publish_subsystem_info: Type %llu pres=%u enabl=%u ok=%u", subsystem_type, present, enabled, ok);
// Keep a local copy of the status flags. Because we use queuing, it could be that the flags in the vehicle_status topics are
// not up to date. When using those publish_subsystem_info_xxx functions that only write a subset of health flags but leave others
// unchanged, we'd write outdated health flags to vehicle_status. Having an up to date local copy resolves that issue.
if (present) {
status.onboard_control_sensors_present |= (uint32_t)subsystem_type;
} else {
status.onboard_control_sensors_present &= ~(uint32_t)subsystem_type;
}
if (enabled) {
status.onboard_control_sensors_enabled |= (uint32_t)subsystem_type;
} else {
status.onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type;
}
if (ok) {
status.onboard_control_sensors_health |= (uint32_t)subsystem_type;
} else {
status.onboard_control_sensors_health &= ~(uint32_t)subsystem_type;
}
/* Publish the subsystem_info message */
info.present = present;
info.enabled = enabled;
info.ok = ok;
info.subsystem_type = subsystem_type;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise_queue(ORB_ID(subsystem_info), &info, subsystem_info_s::ORB_QUEUE_LENGTH);
}
}
void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy)
{
publish_subsystem_info(subsystem_type, present, getEnabled(subsystem_type), healthy);
}
void publish_subsystem_info_present_enabled(uint64_t subsystem_type, bool present, bool enabled)
{
publish_subsystem_info(subsystem_type, present, enabled, getHealthy(subsystem_type));
}
void publish_subsystem_info_enabled_healthy(uint64_t subsystem_type, bool enabled, bool ok)
{
publish_subsystem_info(subsystem_type, getPresent(subsystem_type), enabled, ok);
}
void publish_subsystem_info_enabled(uint64_t subsystem_type, bool enabled)
{
publish_subsystem_info(subsystem_type, getPresent(subsystem_type), enabled, getHealthy(subsystem_type));
}
void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok)
{
publish_subsystem_info(subsystem_type, getPresent(subsystem_type), getEnabled(subsystem_type), ok);
}
// Local helper functions
bool getPresent(uint64_t subsystem_type)
{
return status.onboard_control_sensors_present & (uint32_t)subsystem_type;
}
bool getEnabled(uint64_t subsystem_type)
{
return status.onboard_control_sensors_enabled & (uint32_t)subsystem_type;
}
bool getHealthy(uint64_t subsystem_type)
{
return status.onboard_control_sensors_health & (uint32_t)subsystem_type;
}