diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 0b1d61a985..716fcc9d23 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -105,12 +105,6 @@ uint8 system_type # system type, contains mavlink MAV_TYPE uint8 system_id # system id, contains MAVLink's system ID field uint8 component_id # subsystem / component id, contains MAVLink's component ID field -# see SYS_STATUS mavlink message for the following -# lower 32 bits are for the base flags, higher 32 bits are or the extended flags -uint64 onboard_control_sensors_present -uint64 onboard_control_sensors_enabled -uint64 onboard_control_sensors_health - bool safety_button_available # Set to true if a safety button is connected bool safety_off # Set to true if safety is off bool auto_mission_available diff --git a/src/modules/commander/Arming/CMakeLists.txt b/src/modules/commander/Arming/CMakeLists.txt index b97cc35bc0..37aa7473d9 100644 --- a/src/modules/commander/Arming/CMakeLists.txt +++ b/src/modules/commander/Arming/CMakeLists.txt @@ -33,4 +33,3 @@ add_subdirectory(ArmAuthorization) add_subdirectory(ArmStateMachine) -add_subdirectory(HealthFlags) diff --git a/src/modules/commander/Arming/HealthFlags/CMakeLists.txt b/src/modules/commander/Arming/HealthFlags/CMakeLists.txt deleted file mode 100644 index 4b8dc20cef..0000000000 --- a/src/modules/commander/Arming/HealthFlags/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 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(HealthFlags - HealthFlags.cpp -) -target_include_directories(HealthFlags PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp deleted file mode 100644 index 449d966fed..0000000000 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2020 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 HealthFlags.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 "HealthFlags.h" - -void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status) -{ - PX4_DEBUG("set_health_flags: Type %llu pres=%u enabl=%u ok=%u", (long long unsigned int)subsystem_type, present, - enabled, ok); - - if (present) { - status.onboard_control_sensors_present |= subsystem_type; - - } else { - status.onboard_control_sensors_present &= ~subsystem_type; - } - - if (enabled) { - status.onboard_control_sensors_enabled |= subsystem_type; - - } else { - status.onboard_control_sensors_enabled &= ~subsystem_type; - } - - if (ok) { - status.onboard_control_sensors_health |= subsystem_type; - - } else { - status.onboard_control_sensors_health &= ~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 & 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 & subsystem_type, - status.onboard_control_sensors_enabled & subsystem_type, healthy, status); -} - -void _print_sub(const char *name, const vehicle_status_s &status, uint64_t bit) -{ - PX4_INFO("%s:\t\t%s\t%s", name, - (status.onboard_control_sensors_enabled & bit) ? "EN" : " ", - (status.onboard_control_sensors_present & bit) ? - ((status.onboard_control_sensors_health & bit) ? "OK" : "ERR") : - (status.onboard_control_sensors_enabled & bit) ? "OFF" : ""); -} - -void print_health_flags(const vehicle_status_s &status) -{ -#ifndef CONSTRAINED_FLASH - PX4_INFO("DEVICE\t\tSTATUS"); - PX4_INFO("----------------------------------"); - _print_sub("GYRO", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO); - _print_sub("ACC", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC); - _print_sub("MAG", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG); - _print_sub("PRESS", status, subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE); - _print_sub("AIRSP", status, subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE); - _print_sub("GPS", status, subsystem_info_s::SUBSYSTEM_TYPE_GPS); - _print_sub("OPT", status, subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW); - _print_sub("VIO", status, subsystem_info_s::SUBSYSTEM_TYPE_CVPOSITION); - _print_sub("LASER", status, subsystem_info_s::SUBSYSTEM_TYPE_LASERPOSITION); - _print_sub("GTRUTH", status, subsystem_info_s::SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH); - _print_sub("RATES", status, subsystem_info_s::SUBSYSTEM_TYPE_ANGULARRATECONTROL); - _print_sub("ATT", status, subsystem_info_s::SUBSYSTEM_TYPE_ATTITUDESTABILIZATION); - _print_sub("YAW", status, subsystem_info_s::SUBSYSTEM_TYPE_YAWPOSITION); - _print_sub("ALTCTL", status, subsystem_info_s::SUBSYSTEM_TYPE_ALTITUDECONTROL); - _print_sub("POS", status, subsystem_info_s::SUBSYSTEM_TYPE_POSITIONCONTROL); - _print_sub("MOT", status, subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL); - _print_sub("RC ", status, subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER); - _print_sub("GYRO2", status, subsystem_info_s::SUBSYSTEM_TYPE_GYRO2); - _print_sub("ACC2", status, subsystem_info_s::SUBSYSTEM_TYPE_ACC2); - _print_sub("MAG2", status, subsystem_info_s::SUBSYSTEM_TYPE_MAG2); - _print_sub("GEOFENCE", status, subsystem_info_s::SUBSYSTEM_TYPE_GEOFENCE); - _print_sub("AHRS", status, subsystem_info_s::SUBSYSTEM_TYPE_AHRS); - _print_sub("TERRAIN", status, subsystem_info_s::SUBSYSTEM_TYPE_TERRAIN); - _print_sub("REVMOT", status, subsystem_info_s::SUBSYSTEM_TYPE_REVERSEMOTOR); - _print_sub("LOGGIN", status, subsystem_info_s::SUBSYSTEM_TYPE_LOGGING); - _print_sub("BATT", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY); - _print_sub("PROX", status, subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY); - _print_sub("SATCOM", status, subsystem_info_s::SUBSYSTEM_TYPE_SATCOM); - _print_sub("PREARM", status, subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK); - _print_sub("OBSAVD", status, subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE); -#endif -} diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.h b/src/modules/commander/Arming/HealthFlags/HealthFlags.h deleted file mode 100644 index 04e5db1c38..0000000000 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.h +++ /dev/null @@ -1,85 +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 HealthFlags.h - * - * Contains helper functions to efficiently set the system health flags from commander and preflight check. - * - * @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch) - */ - -#pragma once - -#include -#include - -struct subsystem_info_s { - // keep in sync with mavlink MAV_SYS_STATUS_SENSOR - static constexpr uint64_t SUBSYSTEM_TYPE_GYRO = 1 << 0; - static constexpr uint64_t SUBSYSTEM_TYPE_ACC = 1 << 1; - static constexpr uint64_t SUBSYSTEM_TYPE_MAG = 1 << 2; - static constexpr uint64_t SUBSYSTEM_TYPE_ABSPRESSURE = 1 << 3; - static constexpr uint64_t SUBSYSTEM_TYPE_DIFFPRESSURE = 1 << 4; - static constexpr uint64_t SUBSYSTEM_TYPE_GPS = 1 << 5; - static constexpr uint64_t SUBSYSTEM_TYPE_OPTICALFLOW = 1 << 6; - static constexpr uint64_t SUBSYSTEM_TYPE_CVPOSITION = 1 << 7; - static constexpr uint64_t SUBSYSTEM_TYPE_LASERPOSITION = 1 << 8; - static constexpr uint64_t SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 1 << 9; - static constexpr uint64_t SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1 << 10; - static constexpr uint64_t SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 1 << 11; - static constexpr uint64_t SUBSYSTEM_TYPE_YAWPOSITION = 1 << 12; - static constexpr uint64_t SUBSYSTEM_TYPE_ALTITUDECONTROL = 1 << 13; - static constexpr uint64_t SUBSYSTEM_TYPE_POSITIONCONTROL = 1 << 14; - static constexpr uint64_t SUBSYSTEM_TYPE_MOTORCONTROL = 1 << 15; - static constexpr uint64_t SUBSYSTEM_TYPE_RCRECEIVER = 1 << 16; - static constexpr uint64_t SUBSYSTEM_TYPE_GYRO2 = 1 << 17; - static constexpr uint64_t SUBSYSTEM_TYPE_ACC2 = 1 << 18; - static constexpr uint64_t SUBSYSTEM_TYPE_MAG2 = 1 << 19; - static constexpr uint64_t SUBSYSTEM_TYPE_GEOFENCE = 1 << 20; - static constexpr uint64_t SUBSYSTEM_TYPE_AHRS = 1 << 21; - static constexpr uint64_t SUBSYSTEM_TYPE_TERRAIN = 1 << 22; - static constexpr uint64_t SUBSYSTEM_TYPE_REVERSEMOTOR = 1 << 23; - static constexpr uint64_t SUBSYSTEM_TYPE_LOGGING = 1 << 24; - static constexpr uint64_t SUBSYSTEM_TYPE_SENSORBATTERY = 1 << 25; - static constexpr uint64_t SUBSYSTEM_TYPE_SENSORPROXIMITY = 1 << 26; - static constexpr uint64_t SUBSYSTEM_TYPE_SATCOM = 1 << 27; - static constexpr uint64_t SUBSYSTEM_TYPE_PREARM_CHECK = 1 << 28; - static constexpr uint64_t SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 1 << 29; - static constexpr uint64_t SUBSYSTEM_TYPE_PARACHUTE = 1ULL << 32; -}; - -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); -void print_health_flags(const vehicle_status_s &status); diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 5bada2bf11..b59ec05fed 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -65,7 +65,6 @@ px4_add_module( hysteresis ArmAuthorization ArmStateMachine - HealthFlags sensor_calibration world_magnetic_model mode_util diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c48b7665b0..8e782c1ac9 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -46,7 +46,6 @@ /* commander module headers */ #include "Arming/ArmAuthorization/ArmAuthorization.h" -#include "Arming/HealthFlags/HealthFlags.h" #include "commander_helper.h" #include "esc_calibration.h" #include "px4_custom_mode.h" @@ -305,8 +304,6 @@ int Commander::custom_command(int argc, char *argv[]) uORB::SubscriptionData vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; PX4_INFO("Preflight check: %s", vehicle_status_flags_sub.get().pre_flight_checks_pass ? "OK" : "FAILED"); - print_health_flags(vehicle_status_sub.get()); - return 0; } @@ -2274,9 +2271,6 @@ Commander::run() _vehicle_status.safety_off = _safety.isSafetyOff(); if (safety_changed) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, _safety.isButtonAvailable(), _safety.isSafetyOff(), - _safety.isButtonAvailable(), _vehicle_status); - // Notify the user if the status of the safety button changes if (!_safety.isSafetyDisabled()) { if (_safety.isSafetyOff()) { @@ -2644,8 +2638,6 @@ Commander::run() // data link checks which update the status data_link_check(); - avoidance_check(); - /* check if we are disarmed and there is a better mode to wait in */ if (!_arm_state_machine.isArmed()) { /* if there is no radio control but GPS lock the user might want to fly using @@ -2764,13 +2756,11 @@ Commander::run() mavlink_log_critical(&_mavlink_log_pub, "Motor failure detected\t"); events::send(events::ID("commander_motor_failure"), events::Log::Emergency, "Motor failure! Land immediately"); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, false, _vehicle_status); } else { mavlink_log_critical(&_mavlink_log_pub, "Motor recovered\t"); events::send(events::ID("commander_motor_recovered"), events::Log::Warning, "Motor recovered, landing still advised"); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL, true, true, true, _vehicle_status); } } @@ -2977,9 +2967,6 @@ Commander::run() _vehicle_status_flags.pre_flight_checks_pass = _health_and_arming_checks.canArm( _vehicle_status.nav_state); perf_end(_preflight_check_perf); - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, - _vehicle_status_flags.pre_flight_checks_pass, _vehicle_status); } // publish actuator_armed first (used by output modules) @@ -3574,12 +3561,6 @@ void Commander::data_link_check() _status_changed = true; } } - - const bool present = true; - const bool enabled = true; - const bool ok = (iridium_status.last_heartbeat > 0); // maybe at some point here an additional check should be made - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SATCOM, present, enabled, ok, _vehicle_status); } break; @@ -3630,7 +3611,6 @@ void Commander::data_link_check() _datalink_last_heartbeat_parachute_system = telemetry.timestamp; _vehicle_status.parachute_system_present = true; _vehicle_status.parachute_system_healthy = healthy; - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, true, true, healthy, _vehicle_status); } if (telemetry.heartbeat_component_obstacle_avoidance) { @@ -3681,7 +3661,6 @@ void Commander::data_link_check() _vehicle_status.parachute_system_healthy = false; _parachute_system_lost = true; _status_changed = true; - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, false, true, false, _vehicle_status); } // AVOIDANCE SYSTEM state check (only if it is enabled) @@ -3709,39 +3688,6 @@ void Commander::data_link_check() } } -void Commander::avoidance_check() -{ - for (auto &dist_sens_sub : _distance_sensor_subs) { - distance_sensor_s distance_sensor; - - if (dist_sens_sub.update(&distance_sensor)) { - if ((distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && - (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { - - _valid_distance_sensor_time_us = distance_sensor.timestamp; - } - } - } - - const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms; - const bool cp_healthy = _vehicle_status.avoidance_system_valid || distance_sensor_valid; - - const bool sensor_oa_present = cp_healthy || _vehicle_status.avoidance_system_required - || _collision_prevention_enabled; - - const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled; - const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled - && _vehicle_control_mode.flag_control_position_enabled); - - const bool sensor_oa_enabled = ((auto_mode && _vehicle_status.avoidance_system_required) || (pos_ctl_mode - && _collision_prevention_enabled)); - const bool sensor_oa_healthy = ((auto_mode && _vehicle_status.avoidance_system_valid) || (pos_ctl_mode - && cp_healthy)); - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled, - sensor_oa_healthy, _vehicle_status); -} - void Commander::battery_status_check() { size_t battery_required_count = 0; @@ -4012,14 +3958,12 @@ void Commander::estimator_check() mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t"); events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical, "Stopping compass use! Land now and calibrate the compass"); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MAG, true, true, false, _vehicle_status); } if (!gnss_heading_fault_prev && estimator_status_flags.cs_gps_yaw_fault) { mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!\t"); events::send(events::ID("commander_stopping_gnss_heading_use"), events::Log::Critical, "GNSS heading not reliable. Land now!"); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, false, _vehicle_status); } } @@ -4264,17 +4208,6 @@ void Commander::manual_control_check() _is_throttle_above_center = (manual_control_setpoint.z > 0.6f); _is_throttle_low = (manual_control_setpoint.z < 0.1f); - const bool is_mavlink = (manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC); - - if (is_mavlink) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true, _vehicle_status); - - } else { - // if not mavlink also report valid RC calibration for health - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true, - _vehicle_status); - } - if (_arm_state_machine.isArmed()) { // Abort autonomous mode and switch to position mode if sticks are moved significantly // but only if actually in air. @@ -4327,6 +4260,8 @@ void Commander::manual_control_check() } } else { + const bool is_mavlink = (manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC); + // disarmed // if there's never been a mode change force position control as initial state if (_commander_state.main_state_changes == 0) { @@ -4348,8 +4283,6 @@ void Commander::manual_control_check() mavlink_log_critical(&_mavlink_log_pub, "Manual control lost\t"); events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info}, "Manual control lost"); - - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _vehicle_status); } } } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3f7d7b0cff..0c573ed48b 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -146,8 +146,6 @@ private: */ void data_link_check(); - void avoidance_check(); - void esc_status_check(); void estimator_check(); @@ -294,8 +292,6 @@ private: ArmStateMachine _arm_state_machine{}; - hrt_abstime _valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */ - hrt_abstime _last_gpos_fail_time_us{0}; /**< Last time that the global position validity recovery check failed (usec) */ hrt_abstime _last_lpos_fail_time_us{0}; /**< Last time that the local position validity recovery check failed (usec) */ hrt_abstime _last_lvel_fail_time_us{0}; /**< Last time that the local velocity validity recovery check failed (usec) */ diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index ec3bf49923..a92073d1ea 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -55,6 +55,9 @@ #include #include #include +#include + +#include class MavlinkStreamHighLatency2 : public MavlinkStream { @@ -402,35 +405,28 @@ private: vehicle_status_s status; if (_status_sub.update(&status)) { - if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) - && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)) { - msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; - } + health_report_s health_report; - if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_ACCEL) - && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_ACCEL)) || - ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_ACCEL2) && - !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_ACCEL2))) { - msg->failure_flags |= HL_FAILURE_FLAG_3D_ACCEL; - } + if (_health_report_sub.copy(&health_report)) { + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) + events::px4::enums::health_component_t::absolute_pressure) { + msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE; + } - if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_GYRO) - && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_GYRO)) || - ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_GYRO2) && - !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_GYRO2))) { - msg->failure_flags |= HL_FAILURE_FLAG_3D_GYRO; - } + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) + events::px4::enums::health_component_t::accel) { + msg->failure_flags |= HL_FAILURE_FLAG_3D_ACCEL; + } - if (((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_MAG) - && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG)) || - ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_3D_MAG2) && - !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_3D_MAG2))) { - msg->failure_flags |= HL_FAILURE_FLAG_3D_MAG; - } + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) + events::px4::enums::health_component_t::gyro) { + msg->failure_flags |= HL_FAILURE_FLAG_3D_GYRO; + } - if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_TERRAIN) - && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_TERRAIN)) { - msg->failure_flags |= HL_FAILURE_FLAG_TERRAIN; + if ((health_report.arming_check_error_flags | health_report.health_error_flags) & (uint64_t) + events::px4::enums::health_component_t::magnetometer) { + msg->failure_flags |= HL_FAILURE_FLAG_3D_MAG; + } } if (status.rc_signal_lost) { @@ -640,6 +636,7 @@ private: uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)}; uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; + uORB::Subscription _health_report_sub{ORB_ID(health_report)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 5d81e6d859..6859ea7f4b 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -37,6 +37,8 @@ #include #include #include +#include +#include class MavlinkStreamSysStatus : public MavlinkStream { @@ -59,8 +61,48 @@ private: uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; + uORB::Subscription _health_report{ORB_ID(health_report)}; uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; + using health_component_t = events::px4::enums::health_component_t; + + void fillOutComponent(const health_report_s &health_report, MAV_SYS_STATUS_SENSOR mav_sensor, + health_component_t health_component, mavlink_sys_status_t &msg) + { + if (health_report.health_is_present_flags & (uint64_t)health_component) { + msg.onboard_control_sensors_present |= mav_sensor; + } + + if (((health_report.arming_check_error_flags | health_report.arming_check_warning_flags | + health_report.health_error_flags | health_report.health_warning_flags) & (uint64_t)health_component) == 0) { + msg.onboard_control_sensors_health |= mav_sensor; + } + + // Set as enabled if present or unhealthy (required, but missing) + if ((msg.onboard_control_sensors_present & mav_sensor) || (msg.onboard_control_sensors_health & mav_sensor) == 0) { + msg.onboard_control_sensors_enabled |= mav_sensor; + } + } + + void fillOutComponent(const health_report_s &health_report, MAV_SYS_STATUS_SENSOR_EXTENDED mav_sensor, + health_component_t health_component, mavlink_sys_status_t &msg) + { + if (health_report.health_is_present_flags & (uint64_t)health_component) { + msg.onboard_control_sensors_present_extended |= mav_sensor; + } + + if (((health_report.arming_check_error_flags | health_report.arming_check_warning_flags | + health_report.health_error_flags | health_report.health_warning_flags) & (uint64_t)health_component) == 0) { + msg.onboard_control_sensors_health_extended |= mav_sensor; + } + + // Set as enabled if present or unhealthy (required, but missing) + if ((msg.onboard_control_sensors_present_extended & mav_sensor) + || (msg.onboard_control_sensors_health_extended & mav_sensor) == 0) { + msg.onboard_control_sensors_enabled_extended |= mav_sensor; + } + } + bool send() override { if (_status_sub.updated() || _cpuload_sub.updated() || _battery_status_subs.updated()) { @@ -70,6 +112,9 @@ private: cpuload_s cpuload{}; _cpuload_sub.copy(&cpuload); + health_report_s health_report{}; + _health_report.copy(&health_report); + battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {}; for (int i = 0; i < _battery_status_subs.size(); i++) { @@ -84,26 +129,31 @@ private: // the current battery status is replaced with the cached value for (int i = 0; i < _battery_status_subs.size(); i++) { if (battery_status[i].connected && ((!battery_status[lowest_battery_index].connected) - || (battery_status[i].remaining < battery_status[lowest_battery_index].remaining))) { + || (battery_status[i].remaining < + battery_status[lowest_battery_index].remaining))) { lowest_battery_index = i; } } mavlink_sys_status_t msg{}; - msg.onboard_control_sensors_present = static_cast(status.onboard_control_sensors_present & 0xFFFFFFFF) | - MAV_SYS_STATUS_EXTENSION_USED; - msg.onboard_control_sensors_enabled = static_cast(status.onboard_control_sensors_enabled & 0xFFFFFFFF) | - MAV_SYS_STATUS_EXTENSION_USED; - msg.onboard_control_sensors_health = static_cast(status.onboard_control_sensors_health & 0xFFFFFFFF) | - MAV_SYS_STATUS_EXTENSION_USED; + if (health_report.can_arm_mode_flags & (1u << status.nav_state)) { + msg.onboard_control_sensors_health |= MAV_SYS_STATUS_PREARM_CHECK; + } - msg.onboard_control_sensors_present_extended = static_cast((status.onboard_control_sensors_present >> 32u) & - 0xFFFFFFFF); - msg.onboard_control_sensors_enabled_extended = static_cast((status.onboard_control_sensors_enabled >> 32u) & - 0xFFFFFFFF); - msg.onboard_control_sensors_health_extended = static_cast((status.onboard_control_sensors_health >> 32u) & - 0xFFFFFFFF); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_BATTERY, health_component_t::battery, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, health_component_t::motors_escs, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_RECOVERY_SYSTEM, health_component_t::parachute, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_OBSTACLE_AVOIDANCE, health_component_t::avoidance, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_ACCEL, health_component_t::accel, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_GYRO, health_component_t::gyro, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_MAG, health_component_t::magnetometer, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_GPS, health_component_t::gps, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_RC_RECEIVER, health_component_t::remote_control, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_AHRS, health_component_t::local_position_estimate, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, health_component_t::absolute_pressure, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, health_component_t::differential_pressure, + msg); msg.load = cpuload.load * 1000.0f;