forked from Archive/PX4-Autopilot
Subsystem_info status flags & checks: Separate the functionality to a) set the health flags inside commander and b) to publish them from external modules
This commit is contained in:
Philipp Oettershagen
Beat Küng
parent
a807d34a7a
commit
e4d863b95f
|
|
@ -32,4 +32,4 @@ bool enabled
|
|||
bool ok
|
||||
uint64 subsystem_type
|
||||
|
||||
uint32 ORB_QUEUE_LENGTH = 8
|
||||
uint32 ORB_QUEUE_LENGTH = 5
|
||||
|
|
|
|||
|
|
@ -48,4 +48,4 @@ add_subdirectory(rc)
|
|||
add_subdirectory(terrain_estimation)
|
||||
add_subdirectory(tunes)
|
||||
add_subdirectory(version)
|
||||
add_subdirectory(subsystem_info_pub)
|
||||
add_subdirectory(health_flags)
|
||||
|
|
|
|||
|
|
@ -31,4 +31,4 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(subsystem_info_pub subsystem_info_pub.cpp)
|
||||
px4_add_library(health_flags health_flags.cpp)
|
||||
|
|
@ -0,0 +1,80 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 health_flags.cpp
|
||||
*
|
||||
* Contains helper functions to efficiently set the system health flags from commander and preflight check.
|
||||
*
|
||||
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
|
||||
*/
|
||||
|
||||
#include "health_flags.h"
|
||||
|
||||
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
|
||||
{
|
||||
PX4_INFO("set_health_flags: Type %llu pres=%u enabl=%u ok=%u", subsystem_type, present, enabled, ok);
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status)
|
||||
{
|
||||
set_health_flags(subsystem_type, present, status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy,
|
||||
status);
|
||||
}
|
||||
|
||||
void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status)
|
||||
{
|
||||
set_health_flags(subsystem_type, status.onboard_control_sensors_present & (uint32_t)subsystem_type,
|
||||
status.onboard_control_sensors_enabled & (uint32_t)subsystem_type, healthy, status);
|
||||
}
|
||||
|
|
@ -32,9 +32,9 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file subsystem_info_pub.h
|
||||
* @file health_flags.h
|
||||
*
|
||||
* Contains helper functions to efficiently publish the subsystem_info topic from various locations inside the code.
|
||||
* Contains helper functions to efficiently set the system health flags from commander and preflight check.
|
||||
*
|
||||
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
|
||||
*/
|
||||
|
|
@ -42,11 +42,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <px4_log.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
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);
|
||||
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status);
|
||||
void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status);
|
||||
void set_health_flags_healthy(uint64_t subsystem_type, bool healthy, vehicle_status_s &status);
|
||||
|
|
@ -1,110 +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>
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
@ -55,5 +55,5 @@ px4_add_module(
|
|||
df_driver_framework
|
||||
git_ecl
|
||||
ecl_geo
|
||||
subsystem_info_pub
|
||||
health_flags
|
||||
)
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@
|
|||
#include <parameters/param.h>
|
||||
#include <systemlib/rc_check.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <lib/subsystem_info_pub/subsystem_info_pub.h>
|
||||
#include <lib/health_flags/health_flags.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
|
|
@ -71,7 +71,6 @@
|
|||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
|
||||
#include "PreflightCheck.h"
|
||||
|
||||
|
|
@ -164,8 +163,8 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &sta
|
|||
}
|
||||
|
||||
out:
|
||||
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);
|
||||
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);
|
||||
|
||||
DevMgr::releaseHandle(h);
|
||||
return success;
|
||||
|
|
@ -191,8 +190,8 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s
|
|||
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, &status);
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false, &status);
|
||||
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;
|
||||
|
|
@ -209,8 +208,8 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s
|
|||
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, &status);
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, &status);
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, status);
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false, status);
|
||||
}
|
||||
|
||||
success = false;
|
||||
|
|
@ -250,8 +249,8 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s
|
|||
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, &status);
|
||||
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false, &status);
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -337,8 +336,8 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &
|
|||
#endif
|
||||
|
||||
out:
|
||||
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);
|
||||
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);
|
||||
|
||||
DevMgr::releaseHandle(h);
|
||||
return success;
|
||||
|
|
@ -389,8 +388,8 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, u
|
|||
}
|
||||
|
||||
out:
|
||||
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);
|
||||
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);
|
||||
|
||||
DevMgr::releaseHandle(h);
|
||||
return success;
|
||||
|
|
@ -411,7 +410,7 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, u
|
|||
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, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, !optional, false, status);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -427,7 +426,7 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, u
|
|||
// }
|
||||
|
||||
//out:
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, true, !optional, success, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, true, !optional, success, status);
|
||||
DevMgr::releaseHandle(h);
|
||||
return success;
|
||||
}
|
||||
|
|
@ -493,7 +492,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &statu
|
|||
}
|
||||
|
||||
out:
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status);
|
||||
orb_unsubscribe(fd_airspeed);
|
||||
orb_unsubscribe(fd_diffpres);
|
||||
return success;
|
||||
|
|
@ -688,8 +687,8 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
|
|||
}
|
||||
|
||||
out:
|
||||
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);
|
||||
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;
|
||||
}
|
||||
|
|
@ -768,7 +767,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
|||
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, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false, status);
|
||||
failed = true;
|
||||
}
|
||||
|
||||
|
|
@ -806,7 +805,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
|||
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, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false, status);
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -839,7 +838,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
|||
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, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false, status);
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -875,7 +874,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
|||
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, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false, status);
|
||||
failed = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -889,7 +888,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
|||
|
||||
/* ---- AIRSPEED ---- */
|
||||
if (checkAirspeed) {
|
||||
int32_t optional = 0; //Given checkAirspeed==true, assume the airspeed sensor is also required by default
|
||||
int32_t optional = 0;
|
||||
param_get(param_find("FW_ARSP_MODE"), &optional);
|
||||
|
||||
if (!airspeedCheck(mavlink_log_pub, status, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
|
||||
|
|
@ -906,12 +905,12 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
|||
|
||||
failed = true;
|
||||
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, false, &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;
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, !status.rc_signal_lost, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, !status.rc_signal_lost, status);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -43,8 +43,6 @@
|
|||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace Preflight
|
||||
|
|
|
|||
|
|
@ -64,6 +64,7 @@
|
|||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/health_flags/health_flags.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <navigator/navigation.h>
|
||||
#include <px4_config.h>
|
||||
|
|
@ -79,7 +80,6 @@
|
|||
#include <parameters/param.h>
|
||||
#include <systemlib/rc_check.h>
|
||||
#include <systemlib/state_table.h>
|
||||
#include <lib/subsystem_info_pub/subsystem_info_pub.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <cfloat>
|
||||
|
|
@ -1829,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, &status);
|
||||
publish_subsystem_info_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, &status);
|
||||
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, status);
|
||||
set_health_flags_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);
|
||||
|
|
@ -2006,33 +2006,7 @@ Commander::run()
|
|||
orb_check(subsys_sub, &updated);
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
|
||||
|
||||
//warnx("subsystem changed: %d\n", (int)info.subsystem_type);
|
||||
|
||||
/* mark / unmark as present */
|
||||
if (info.present) {
|
||||
status.onboard_control_sensors_present |= info.subsystem_type;
|
||||
|
||||
} else {
|
||||
status.onboard_control_sensors_present &= ~info.subsystem_type;
|
||||
}
|
||||
|
||||
/* mark / unmark as enabled */
|
||||
if (info.enabled) {
|
||||
status.onboard_control_sensors_enabled |= info.subsystem_type;
|
||||
|
||||
} else {
|
||||
status.onboard_control_sensors_enabled &= ~info.subsystem_type;
|
||||
}
|
||||
|
||||
/* mark / unmark as ok */
|
||||
if (info.ok) {
|
||||
status.onboard_control_sensors_health |= info.subsystem_type;
|
||||
|
||||
} else {
|
||||
status.onboard_control_sensors_health &= ~info.subsystem_type;
|
||||
}
|
||||
|
||||
set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
|
||||
status_changed = true;
|
||||
}
|
||||
} while(updated);
|
||||
|
|
@ -2232,13 +2206,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, &status);
|
||||
set_health_flags(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, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid, status);
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -2386,7 +2360,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, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status);
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -2424,7 +2398,7 @@ Commander::run()
|
|||
status_changed = true;
|
||||
|
||||
PX4_ERR("Engine Failure");
|
||||
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL,true,true,false, &status);
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, status);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -4109,9 +4083,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, &status); // TODO
|
||||
//set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false, status); // TODO
|
||||
} else {
|
||||
//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true, &status); // TODO
|
||||
//set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true, status); // TODO
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -51,5 +51,5 @@ px4_add_module(
|
|||
drivers__device
|
||||
git_ecl
|
||||
ecl_validation
|
||||
subsystem_info_pub
|
||||
health_flags
|
||||
)
|
||||
|
|
|
|||
|
|
@ -40,7 +40,6 @@
|
|||
#include "voted_sensors_update.h"
|
||||
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <lib/subsystem_info_pub/subsystem_info_pub.h>
|
||||
|
||||
#include <conversion/rotation.h>
|
||||
#include <ecl/geo/geo.h>
|
||||
|
|
@ -60,8 +59,9 @@ VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_en
|
|||
memset(&_gyro_diff, 0, sizeof(_gyro_diff));
|
||||
memset(&_mag_diff, 0, sizeof(_mag_diff));
|
||||
|
||||
// initialise the corrections
|
||||
// initialise the publication variables
|
||||
memset(&_corrections, 0, sizeof(_corrections));
|
||||
memset(&_info, 0, sizeof(_info));
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
_corrections.gyro_scale_0[i] = 1.0f;
|
||||
|
|
@ -932,26 +932,33 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n
|
|||
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;
|
||||
|
||||
if (ctr_valid == 0) { // There are no valid sensors left!
|
||||
if (strcmp(sensor_name, "Gyro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO; }
|
||||
if (strcmp(sensor_name, "Gyro") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO; }
|
||||
|
||||
if (strcmp(sensor_name, "Accel") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC; }
|
||||
if (strcmp(sensor_name, "Accel") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC; }
|
||||
|
||||
if (strcmp(sensor_name, "Mag") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG; }
|
||||
if (strcmp(sensor_name, "Mag") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG; }
|
||||
|
||||
if (strcmp(sensor_name, "Baro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE; }
|
||||
if (strcmp(sensor_name, "Baro") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE; }
|
||||
|
||||
} else if (ctr_valid == 1) { // A single valid sensor remains, set secondary sensor health to false
|
||||
if (strcmp(sensor_name, "Gyro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
|
||||
if (strcmp(sensor_name, "Gyro") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
|
||||
|
||||
if (strcmp(sensor_name, "Accel") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
|
||||
if (strcmp(sensor_name, "Accel") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
|
||||
|
||||
if (strcmp(sensor_name, "Mag") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; }
|
||||
if (strcmp(sensor_name, "Mag") == 0) { _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; }
|
||||
}
|
||||
|
||||
publish_subsystem_info_healthy(subsystem_type, false);
|
||||
_info.present = true;
|
||||
_info.enabled = true;
|
||||
_info.ok = false;
|
||||
|
||||
if (_info_pub == nullptr) {
|
||||
_info_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(subsystem_info), _info_pub, &_info);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -983,30 +990,6 @@ void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, Sens
|
|||
PX4_ERR("failed to add validator for sensor %s %i", meta->o_name, i);
|
||||
}
|
||||
}
|
||||
|
||||
// Update the subsystem_info uORB to indicate the amount of valid sensors
|
||||
if (i < 2) { // subsystem_info only contains flags for the first two sensors
|
||||
uint64_t subsystem_type = 0;
|
||||
|
||||
if (i == 0) { // First sensor valid
|
||||
if (strcmp(meta->o_name, "sensor_gyro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO; }
|
||||
|
||||
if (strcmp(meta->o_name, "sensor_accel") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC; }
|
||||
|
||||
if (strcmp(meta->o_name, "sensor_mag") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG; }
|
||||
|
||||
if (strcmp(meta->o_name, "sensor_baro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE; }
|
||||
|
||||
} else if (i == 1) { // We also have a second sensor
|
||||
if (strcmp(meta->o_name, "sensor_gyro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
|
||||
|
||||
if (strcmp(meta->o_name, "sensor_accel") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
|
||||
|
||||
if (strcmp(meta->o_name, "sensor_mag") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; }
|
||||
}
|
||||
|
||||
publish_subsystem_info(subsystem_type, true, true, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -273,6 +273,10 @@ private:
|
|||
orb_advert_t _sensor_selection_pub = nullptr; /**< handle to the sensor selection uORB topic */
|
||||
bool _selection_changed = false; /**< true when a sensor selection has changed and not been published */
|
||||
|
||||
/* subsystem info publication */
|
||||
struct subsystem_info_s _info;
|
||||
orb_advert_t _info_pub = nullptr;
|
||||
|
||||
uint32_t _accel_device_id[SENSOR_COUNT_MAX] = {}; /**< accel driver device id for each uorb instance */
|
||||
uint32_t _baro_device_id[SENSOR_COUNT_MAX] = {};
|
||||
uint32_t _gyro_device_id[SENSOR_COUNT_MAX] = {};
|
||||
|
|
|
|||
Loading…
Reference in New Issue