mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-24 07:09:48 +08:00
commander: replace health flags with health_report from arming checks
This commit is contained in:
Beat Küng
Daniel Agar
@@ -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
|
||||
|
||||
@@ -33,4 +33,3 @@
|
||||
|
||||
add_subdirectory(ArmAuthorization)
|
||||
add_subdirectory(ArmStateMachine)
|
||||
add_subdirectory(HealthFlags)
|
||||
|
||||
@@ -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})
|
||||
@@ -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
|
||||
}
|
||||
@@ -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 <px4_platform_common/log.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
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);
|
||||
@@ -65,7 +65,6 @@ px4_add_module(
|
||||
hysteresis
|
||||
ArmAuthorization
|
||||
ArmStateMachine
|
||||
HealthFlags
|
||||
sensor_calibration
|
||||
world_magnetic_model
|
||||
mode_util
|
||||
|
||||
@@ -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_s> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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) */
|
||||
|
||||
@@ -55,6 +55,9 @@
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
#include <uORB/topics/health_report.h>
|
||||
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
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;
|
||||
|
||||
@@ -37,6 +37,8 @@
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/cpuload.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/health_report.h>
|
||||
#include <px4_platform_common/events.h>
|
||||
|
||||
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_s, battery_status_s::MAX_INSTANCES> _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<uint32_t>(status.onboard_control_sensors_present & 0xFFFFFFFF) |
|
||||
MAV_SYS_STATUS_EXTENSION_USED;
|
||||
msg.onboard_control_sensors_enabled = static_cast<uint32_t>(status.onboard_control_sensors_enabled & 0xFFFFFFFF) |
|
||||
MAV_SYS_STATUS_EXTENSION_USED;
|
||||
msg.onboard_control_sensors_health = static_cast<uint32_t>(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<uint32_t>((status.onboard_control_sensors_present >> 32u) &
|
||||
0xFFFFFFFF);
|
||||
msg.onboard_control_sensors_enabled_extended = static_cast<uint32_t>((status.onboard_control_sensors_enabled >> 32u) &
|
||||
0xFFFFFFFF);
|
||||
msg.onboard_control_sensors_health_extended = static_cast<uint32_t>((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;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user