mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-01 08:20:34 +08:00
Subsystem_info status flags & checks: Separate the functionality to a) set the health flags inside commander and b) to publish them from external modules
This commit is contained in:
Philipp Oettershagen
committed by
Beat Küng
Beat Küng
parent
a807d34a7a
commit
e4d863b95f
@@ -55,5 +55,5 @@ px4_add_module(
|
||||
df_driver_framework
|
||||
git_ecl
|
||||
ecl_geo
|
||||
subsystem_info_pub
|
||||
health_flags
|
||||
)
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -51,5 +51,5 @@ px4_add_module(
|
||||
drivers__device
|
||||
git_ecl
|
||||
ecl_validation
|
||||
subsystem_info_pub
|
||||
health_flags
|
||||
)
|
||||
|
||||
@@ -40,7 +40,6 @@
|
||||
#include "voted_sensors_update.h"
|
||||
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <lib/subsystem_info_pub/subsystem_info_pub.h>
|
||||
|
||||
#include <conversion/rotation.h>
|
||||
#include <ecl/geo/geo.h>
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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] = {};
|
||||
|
||||
Reference in New Issue
Block a user