From e4d863b95f940e2e454491bb86e42c0814bf83e0 Mon Sep 17 00:00:00 2001 From: Philipp Oettershagen Date: Mon, 28 May 2018 12:49:51 +0200 Subject: [PATCH] Subsystem_info status flags & checks: Separate the functionality to a) set the health flags inside commander and b) to publish them from external modules --- msg/subsystem_info.msg | 2 +- src/lib/CMakeLists.txt | 2 +- .../CMakeLists.txt | 2 +- src/lib/health_flags/health_flags.cpp | 80 +++++++++++++ .../health_flags.h} | 13 +-- .../subsystem_info_pub/subsystem_info_pub.cpp | 110 ------------------ src/modules/commander/CMakeLists.txt | 2 +- src/modules/commander/PreflightCheck.cpp | 51 ++++---- src/modules/commander/PreflightCheck.h | 2 - src/modules/commander/commander.cpp | 46 ++------ src/modules/sensors/CMakeLists.txt | 2 +- src/modules/sensors/voted_sensors_update.cpp | 55 +++------ src/modules/sensors/voted_sensors_update.h | 4 + 13 files changed, 148 insertions(+), 223 deletions(-) rename src/lib/{subsystem_info_pub => health_flags}/CMakeLists.txt (96%) create mode 100644 src/lib/health_flags/health_flags.cpp rename src/lib/{subsystem_info_pub/subsystem_info_pub.h => health_flags/health_flags.h} (77%) delete mode 100644 src/lib/subsystem_info_pub/subsystem_info_pub.cpp diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg index ad7681a2d9..56899ae4d8 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 = 8 +uint32 ORB_QUEUE_LENGTH = 5 diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f3349d035c..e17d750f57 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -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) diff --git a/src/lib/subsystem_info_pub/CMakeLists.txt b/src/lib/health_flags/CMakeLists.txt similarity index 96% rename from src/lib/subsystem_info_pub/CMakeLists.txt rename to src/lib/health_flags/CMakeLists.txt index 52fc2846ed..e24b814592 100644 --- a/src/lib/subsystem_info_pub/CMakeLists.txt +++ b/src/lib/health_flags/CMakeLists.txt @@ -31,4 +31,4 @@ # ############################################################################ -px4_add_library(subsystem_info_pub subsystem_info_pub.cpp) +px4_add_library(health_flags health_flags.cpp) diff --git a/src/lib/health_flags/health_flags.cpp b/src/lib/health_flags/health_flags.cpp new file mode 100644 index 0000000000..c57662a10d --- /dev/null +++ b/src/lib/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); +} diff --git a/src/lib/subsystem_info_pub/subsystem_info_pub.h b/src/lib/health_flags/health_flags.h similarity index 77% rename from src/lib/subsystem_info_pub/subsystem_info_pub.h rename to src/lib/health_flags/health_flags.h index e4c0b17f78..2aa5eb7908 100644 --- a/src/lib/subsystem_info_pub/subsystem_info_pub.h +++ b/src/lib/health_flags/health_flags.h @@ -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 -#include #include -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); diff --git a/src/lib/subsystem_info_pub/subsystem_info_pub.cpp b/src/lib/subsystem_info_pub/subsystem_info_pub.cpp deleted file mode 100644 index 0f92a8dd27..0000000000 --- a/src/lib/subsystem_info_pub/subsystem_info_pub.cpp +++ /dev/null @@ -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 - -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/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index bf12985692..1446bed4a9 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -55,5 +55,5 @@ px4_add_module( df_driver_framework git_ecl ecl_geo - subsystem_info_pub + health_flags ) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index be08357df5..6a626d2864 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include @@ -71,7 +71,6 @@ #include #include #include -#include #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); } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 97c6c4fb03..c792d042d6 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -43,8 +43,6 @@ #include #include -#include - #pragma once namespace Preflight diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f57c88f5ee..9b3fea4d2f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -64,6 +64,7 @@ #include #include #include +#include #include #include #include @@ -79,7 +80,6 @@ #include #include #include -#include #include #include @@ -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 } } diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 660337bb43..f793069b8c 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -51,5 +51,5 @@ px4_add_module( drivers__device git_ecl ecl_validation - subsystem_info_pub + health_flags ) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 8a881d2dc7..19991465d3 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -40,7 +40,6 @@ #include "voted_sensors_update.h" #include -#include #include #include @@ -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); - } } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 00ffd18303..2b0833196b 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -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] = {};