fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 01:23:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Subsystem_info status flags & checks: Separate the functionality to a) set the health flags inside commander and b) to publish them from external modules

Browse Source
This commit is contained in:
Philipp Oettershagen
2018-06-06 18:54:24 +02:00
committed by Beat Küng
parent a807d34a7a
commit e4d863b95f
13 changed files with 148 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/CMakeLists.txt
+1 -1
View File
@@ -55,5 +55,5 @@ px4_add_module(
df_driver_framework
git_ecl
ecl_geo
subsystem_info_pub
health_flags
)
src/modules/commander/PreflightCheck.cpp
+25 -26
View File
@@ -55,7 +55,7 @@
#include <parameters/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/mavlink_log.h>
#include <lib/subsystem_info_pub/subsystem_info_pub.h>
#include <lib/health_flags/health_flags.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
@@ -71,7 +71,6 @@
#include <uORB/topics/system_power.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#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);
}
}
src/modules/commander/PreflightCheck.h
-2
View File
@@ -43,8 +43,6 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vehicle_status_flags.h>
#pragma once
namespace Preflight
src/modules/commander/commander.cpp
+10 -36
View File
@@ -64,6 +64,7 @@
#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>
@@ -79,7 +80,6 @@
#include <parameters/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/state_table.h>
#include <lib/subsystem_info_pub/subsystem_info_pub.h>
#include <cmath>
#include <cfloat>
@@ -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
}
}