From 075009be2fde3f5dd3d3a57dd84967f94027bbca Mon Sep 17 00:00:00 2001 From: Philipp Oettershagen Date: Sat, 26 May 2018 01:06:14 +0200 Subject: [PATCH] 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" --- msg/subsystem_info.msg | 2 +- src/lib/CMakeLists.txt | 1 + src/lib/subsystem_info_pub/CMakeLists.txt | 34 +++++ .../subsystem_info_pub/subsystem_info_pub.cpp | 110 +++++++++++++++ .../subsystem_info_pub}/subsystem_info_pub.h | 17 +-- src/modules/commander/CMakeLists.txt | 1 + src/modules/commander/PreflightCheck.cpp | 88 ++++++------ src/modules/commander/PreflightCheck.h | 2 +- src/modules/commander/commander.cpp | 23 ++-- src/modules/sensors/CMakeLists.txt | 1 + src/modules/sensors/voted_sensors_update.cpp | 4 +- src/modules/systemlib/CMakeLists.txt | 1 - src/modules/systemlib/subsystem_info_pub.cpp | 129 ------------------ 13 files changed, 211 insertions(+), 202 deletions(-) create mode 100644 src/lib/subsystem_info_pub/CMakeLists.txt create mode 100644 src/lib/subsystem_info_pub/subsystem_info_pub.cpp rename src/{modules/systemlib => lib/subsystem_info_pub}/subsystem_info_pub.h (82%) delete mode 100644 src/modules/systemlib/subsystem_info_pub.cpp diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg index 1392b77e42..ad7681a2d9 100644 --- a/msg/subsystem_info.msg +++ b/msg/subsystem_info.msg @@ -32,4 +32,4 @@ bool enabled bool ok uint64 subsystem_type -uint32 ORB_QUEUE_LENGTH = 25 +uint32 ORB_QUEUE_LENGTH = 8 diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 93152ae534..f3349d035c 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -48,3 +48,4 @@ add_subdirectory(rc) add_subdirectory(terrain_estimation) add_subdirectory(tunes) add_subdirectory(version) +add_subdirectory(subsystem_info_pub) diff --git a/src/lib/subsystem_info_pub/CMakeLists.txt b/src/lib/subsystem_info_pub/CMakeLists.txt new file mode 100644 index 0000000000..52fc2846ed --- /dev/null +++ b/src/lib/subsystem_info_pub/CMakeLists.txt @@ -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) diff --git a/src/lib/subsystem_info_pub/subsystem_info_pub.cpp b/src/lib/subsystem_info_pub/subsystem_info_pub.cpp new file mode 100644 index 0000000000..0f92a8dd27 --- /dev/null +++ b/src/lib/subsystem_info_pub/subsystem_info_pub.cpp @@ -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 + +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); +} diff --git a/src/modules/systemlib/subsystem_info_pub.h b/src/lib/subsystem_info_pub/subsystem_info_pub.h similarity index 82% rename from src/modules/systemlib/subsystem_info_pub.h rename to src/lib/subsystem_info_pub/subsystem_info_pub.h index 29f263ccd7..e4c0b17f78 100644 --- a/src/modules/systemlib/subsystem_info_pub.h +++ b/src/lib/subsystem_info_pub/subsystem_info_pub.h @@ -45,15 +45,8 @@ #include #include -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); diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index b5e1c2f788..bf12985692 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -55,4 +55,5 @@ px4_add_module( df_driver_framework git_ecl ecl_geo + subsystem_info_pub ) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 33b9b7e6da..fe85cea882 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include @@ -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; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 3ed6c68f04..97c6c4fb03 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -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); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 66b3e8dd22..f57c88f5ee 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -79,7 +79,7 @@ #include #include #include -#include +#include #include #include @@ -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 } } diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 9cd9956916..660337bb43 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -51,4 +51,5 @@ px4_add_module( drivers__device git_ecl ecl_validation + subsystem_info_pub ) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 9cfeee6699..8a881d2dc7 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -40,7 +40,7 @@ #include "voted_sensors_update.h" #include -#include +#include #include #include @@ -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; diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt index 7f48f2fe63..675cdeb4de 100644 --- a/src/modules/systemlib/CMakeLists.txt +++ b/src/modules/systemlib/CMakeLists.txt @@ -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") diff --git a/src/modules/systemlib/subsystem_info_pub.cpp b/src/modules/systemlib/subsystem_info_pub.cpp deleted file mode 100644 index 94c81a27d0..0000000000 --- a/src/modules/systemlib/subsystem_info_pub.cpp +++ /dev/null @@ -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 - -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; -}