fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 18:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Subsystem_info status flags & checks: Moved the set_health_flags helper functions out of the /lib/ folder and into the module/commander folder because they are actually only needed there

Browse Source
This commit is contained in:
Philipp Oettershagen
2018-05-29 13:31:19 +02:00
committed by Beat Küng
parent bd2af289f5
commit 302cb0a285
9 changed files with 10 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/CMakeLists.txt
+1 -1
View File
@@ -51,9 +51,9 @@ px4_add_module(
esc_calibration.cpp
PreflightCheck.cpp
arm_auth.cpp
health_flag_helper.cpp
DEPENDS
df_driver_framework
git_ecl
ecl_geo
health_flags
)
src/modules/commander/PreflightCheck.cpp
+1 -1
View File
@@ -55,7 +55,6 @@
#include <parameters/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/mavlink_log.h>
#include <lib/health_flags/health_flags.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
@@ -73,6 +72,7 @@
#include <uORB/topics/subsystem_info.h>
#include "PreflightCheck.h"
#include "health_flag_helper.h"
#include "DevMgr.hpp"
src/modules/commander/commander.cpp
+1 -1
View File
@@ -58,13 +58,13 @@
#include "px4_custom_mode.h"
#include "rc_calibration.h"
#include "state_machine_helper.h"
#include "health_flag_helper.h"
/* PX4 headers */
#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <lib/ecl/geo/geo.h>
#include <lib/health_flags/health_flags.h>
#include <mathlib/mathlib.h>
#include <navigator/navigation.h>
#include <px4_config.h>
src/modules/commander/health_flag_helper.cpp
+80
View File
@@ -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_flag_helper.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_flag_helper.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);
}
src/modules/commander/health_flag_helper.h
+49
View File
@@ -0,0 +1,49 @@
/****************************************************************************
*
* 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_flag_helper.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_log.h>
#include <uORB/topics/vehicle_status.h>
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);
src/modules/sensors/CMakeLists.txt
-1
View File
@@ -51,5 +51,4 @@ px4_add_module(
drivers__device
git_ecl
ecl_validation
health_flags
)
src/modules/sensors/voted_sensors_update.cpp
+4 -4
View File
@@ -920,17 +920,17 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n
// reduce priority of failed sensor to the minimum
sensor.priority[failover_index] = 1;
// Update the subsystem_info uORB given that a sensor failed
PX4_ERR("Sensor %s #%i failed. Reconfiguring sensor priorities.", sensor_name, failover_index);
int ctr_valid = 0;
for (uint8_t i = 0; i < sensor.subscription_count; i++) {
if (sensor.priority[i] > 1) { ctr_valid++; }
PX4_WARN("FAILOVER event (idx=%u)! Sensor %s: Nr. %u Priority: %u", failover_index, sensor_name, i, sensor.priority[i]);
PX4_WARN("Remaining sensors after failover event %u: %s #%u priority: %u", failover_index, sensor_name, i,
sensor.priority[i]);
}
PX4_ERR("%s sensor switch from #%i", sensor_name, failover_index);
if (ctr_valid < 2) {
if (ctr_valid == 0) {
// Zero valid sensors remain! Set even the primary sensor health to false