dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Subsystem_info status flags & checks : Initial commit, updating the health flags in a centralized way mostly in commander and the votedSensorsUpdate function.

This commit is contained in:
Philipp Oettershagen 2018-04-03 16:36:33 +02:00 committed by Beat Küng
parent 40e6a5a39b
commit 6f1f414b49
31 changed files with 427 additions and 266 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
msg/subsystem_info.msg
View File

@ -11,10 +11,21 @@ uint64 SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512
uint64 SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024
uint64 SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048
uint64 SUBSYSTEM_TYPE_YAWPOSITION = 4096
uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384
uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 32768
uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 65536
uint64 SUBSYSTEM_TYPE_RANGEFINDER = 131072
uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 8192
uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 16384
uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 32768
uint64 SUBSYSTEM_TYPE_RCRECEIVER = 65536
uint64 SUBSYSTEM_TYPE_GYRO2 = 131072
uint64 SUBSYSTEM_TYPE_ACC2 = 262144
uint64 SUBSYSTEM_TYPE_MAG2 = 524288
uint64 SUBSYSTEM_TYPE_GEOFENCE = 1048576
uint64 SUBSYSTEM_TYPE_AHRS = 2097152
uint64 SUBSYSTEM_TYPE_TERRAIN = 4194304
uint64 SUBSYSTEM_TYPE_REVERSEMOTOR = 8388608
uint64 SUBSYSTEM_TYPE_LOGGING = 16777216
uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432
uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864
uint64 SUBSYSTEM_TYPE_MISSION = 134217728
bool present
bool enabled

1
msg/vehicle_status_flags.msg
View File

@ -27,5 +27,6 @@ bool offboard_control_loss_timeout # true if offboard is lost for
bool rc_signal_found_once
bool rc_input_blocked # set if RC input should be ignored temporarily
bool rc_calibration_valid # set if RC calibration is valid
bool vtol_transition_failure # Set to true if vtol transition failed
bool usb_connected # status of the USB power supply

1
src/drivers/batt_smbus/batt_smbus.cpp
View File

@ -53,7 +53,6 @@
#include <px4_workqueue.h>
#include <perf/perf_counter.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/uORB.h>
#define BATT_SMBUS_ADDR_MIN 0x00 ///< lowest possible address

1
src/drivers/differential_pressure/ets/ets_airspeed.cpp
View File

@ -54,7 +54,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <drivers/airspeed/airspeed.h>
/* I2C bus address */

1
src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp
View File

@ -66,7 +66,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include <drivers/airspeed/airspeed.h>

19
src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp
View File

@ -58,6 +58,7 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
@ -73,7 +74,6 @@
/* Configuration Constants */
#define SR04_DEVICE_PATH "/dev/hc_sr04"
#define SUBSYSTEM_TYPE_RANGEFINDER 131072
/* Device limits */
#define SR04_MIN_DISTANCE (0.10f)
#define SR04_MAX_DISTANCE (4.00f)
@ -626,22 +626,7 @@ HC_SR04::start()
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = true;
info.subsystem_type = SUBSYSTEM_TYPE_RANGEFINDER;
static orb_advert_t pub = nullptr;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
}
void

18
src/drivers/distance_sensor/mb12xx/mb12xx.cpp
View File

@ -61,6 +61,7 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
@ -590,22 +591,7 @@ MB12XX::start()
work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = true;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
static orb_advert_t pub = nullptr;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
}
void

16
src/drivers/distance_sensor/sf0x/sf0x.cpp
View File

@ -66,7 +66,6 @@
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include "sf0x_parser.h"
@ -645,20 +644,7 @@ SF0X::start()
work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
// /* notify about state change */
// struct subsystem_info_s info = {
// true,
// true,
// true,
// SUBSYSTEM_TYPE_RANGEFINDER
// };
// static orb_advert_t pub = -1;
// if (pub > 0) {
// orb_publish(ORB_ID(subsystem_info), pub, &info);
// } else {
// pub = orb_advertise(ORB_ID(subsystem_info), &info);
// }
// publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
}
void

5
src/drivers/distance_sensor/sf1xx/sf1xx.cpp
View File

@ -64,12 +64,14 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>
@ -584,6 +586,9 @@ SF1XX::start()
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, USEC2TICK(_conversion_interval));
/* notify about state change */
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
}
void

18
src/drivers/distance_sensor/srf02/srf02.cpp
View File

@ -60,6 +60,7 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
@ -593,22 +594,7 @@ SRF02::start()
work_queue(HPWORK, &_work, (worker_t)&SRF02::cycle_trampoline, this, 5);
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = true;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
static orb_advert_t pub = nullptr;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
}
void

16
src/drivers/distance_sensor/teraranger/teraranger.cpp
View File

@ -60,6 +60,7 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
@ -670,20 +671,7 @@ TERARANGER::start()
work_queue(HPWORK, &_work, (worker_t)&TERARANGER::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = true;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
static orb_advert_t pub = nullptr;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
}
void

18
src/drivers/distance_sensor/tfmini/tfmini.cpp
View File

@ -61,6 +61,7 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
@ -609,22 +610,7 @@ TFMINI::start()
work_queue(HPWORK, &_work, (worker_t)&TFMINI::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = true;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
static orb_advert_t pub = nullptr;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
}
void

1
src/drivers/pwm_input/pwm_input.cpp
View File

@ -73,7 +73,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/pwm_input.h>
#include <uORB/topics/subsystem_info.h>
#include <drivers/drv_device.h>
#include <drivers/device/device.h>

16
src/drivers/px4flow/px4flow.cpp
View File

@ -65,6 +65,7 @@
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <systemlib/subsystem_info_pub.h>
#include <conversion/rotation.h>
@ -589,20 +590,7 @@ PX4FLOW::start()
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = true;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW;
static orb_advert_t pub = nullptr;
if (pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW, true, true, true);
}
void

27
src/lib/drivers/airspeed/airspeed.cpp
View File

@ -54,20 +54,17 @@
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <drivers/airspeed/airspeed.h>
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
I2C("Airspeed", path, bus, address, 100000),
_sensor_ok(false),
_last_published_sensor_ok(true), /* initialize differently to force publication */
_measure_ticks(0),
_collect_phase(false),
_diff_pres_offset(0.0f),
_airspeed_pub(nullptr),
_airspeed_orb_class_instance(-1),
_subsys_pub(nullptr),
_class_instance(-1),
_conversion_interval(conversion_interval),
_sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")),
@ -248,33 +245,9 @@ Airspeed::stop()
work_cancel(HPWORK, &_work);
}
void
Airspeed::update_status()
{
if (_sensor_ok != _last_published_sensor_ok) {
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = _sensor_ok;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE;
if (_subsys_pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
} else {
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
_last_published_sensor_ok = _sensor_ok;
}
}
void
Airspeed::cycle_trampoline(void *arg)
{
Airspeed *dev = (Airspeed *)arg;
dev->cycle();
dev->update_status();
}

9
src/lib/drivers/airspeed/airspeed.h
View File

@ -43,7 +43,6 @@
#include <systemlib/airspeed.h>
#include <perf/perf_counter.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/uORB.h>
/* Default I2C bus */
@ -75,14 +74,8 @@ protected:
virtual int measure() = 0;
virtual int collect() = 0;
/**
* Update the subsystem status
*/
void update_status();
work_s _work;
bool _sensor_ok;
bool _last_published_sensor_ok;
uint32_t _measure_ticks;
bool _collect_phase;
float _diff_pres_offset;
@ -90,8 +83,6 @@ protected:
orb_advert_t _airspeed_pub;
int _airspeed_orb_class_instance;
orb_advert_t _subsys_pub;
int _class_instance;
unsigned _conversion_interval;

122
src/modules/commander/PreflightCheck.cpp
View File

@ -55,6 +55,7 @@
#include <parameters/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/subsystem_info_pub.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
@ -69,6 +70,7 @@
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/subsystem_info.h>
#include "PreflightCheck.h"
@ -118,7 +120,9 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool present = true;
bool success = true;
int ret = 0;
char s[30];
sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
@ -131,11 +135,12 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
}
}
return false;
present = false;
success = false;
goto out;
}
int ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
if (ret) {
if (report_fail) {
@ -158,6 +163,9 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
}
out:
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG, present, !optional, success);
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, present, !optional, success);
DevMgr::releaseHandle(h);
return success;
}
@ -182,8 +190,9 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
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);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false);
}
success = false;
goto out;
@ -199,6 +208,8 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
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);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false);
}
success = false;
@ -238,7 +249,8 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
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);
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false);
return false;
}
@ -247,7 +259,9 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool present = true;
bool success = true;
int ret = 0;
char s[30];
sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
@ -260,11 +274,12 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
}
return false;
present = false;
success = false;
goto out;
}
int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
if (ret) {
if (report_fail) {
@ -321,13 +336,18 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
#endif
out:
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC, present, !optional, success);
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, present, !optional, success);
DevMgr::releaseHandle(h);
return success;
}
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool present = true;
bool success = true;
int ret = 0;
char s[30];
sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
@ -340,11 +360,12 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
}
return false;
present = false;
success = false;
goto out;
}
int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
if (ret) {
if (report_fail) {
@ -367,6 +388,9 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
}
out:
if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, present, !optional, success);
if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, present, !optional, success);
DevMgr::releaseHandle(h);
return success;
}
@ -386,7 +410,7 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
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);
return false;
}
@ -402,13 +426,14 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
// }
//out:
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, true, !optional, success);
DevMgr::releaseHandle(h);
return success;
}
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm)
{
bool present = true;
bool success = true;
int fd_airspeed = orb_subscribe(ORB_ID(airspeed));
@ -419,20 +444,20 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if ((orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure) != PX4_OK) ||
(hrt_elapsed_time(&differential_pressure.timestamp) > 1000000)) {
if (report_fail) {
if (report_fail && !optional) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
present = false;
success = false;
goto out;
}
if ((orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed) != PX4_OK) ||
(hrt_elapsed_time(&airspeed.timestamp) > 1000000)) {
if (report_fail) {
if (report_fail && !optional) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
present = false;
success = false;
goto out;
}
@ -447,7 +472,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR STUCK");
}
present = true;
success = false;
goto out;
}
@ -461,12 +486,13 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT");
}
present = true;
success = false;
goto out;
}
out:
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success);
orb_unsubscribe(fd_airspeed);
orb_unsubscribe(fd_diffpres);
return success;
@ -523,6 +549,7 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool pre
static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required)
{
bool success = true; // start with a pass and change to a fail if any test fails
bool present = true;
float test_limit = 1.0f; // pass limit re-used for each test
// Get estimator status data if available and exit with a fail recorded if not
@ -530,6 +557,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
estimator_status_s status = {};
if (orb_copy(ORB_ID(estimator_status), sub, &status) != PX4_OK) {
present = false;
goto out;
}
@ -626,12 +654,13 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
// The EKF is not using GPS
if (report_fail) {
if (ekf_gps_check_fail) {
// Poor GPS qulaity is the likely cause
// Poor GPS quality is the likely cause
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
} else {
// Likely cause unknown
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
}
}
@ -649,6 +678,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
if (gps_quality_fail) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
}
success = false;
@ -658,15 +688,15 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
}
out:
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present);
orb_unsubscribe(sub);
return success;
}
bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
const hrt_abstime &time_since_boot)
{
if (time_since_boot < 2000000) {
// the airspeed driver filter doesn't deliver the actual value yet
reportFailures = false;
@ -737,7 +767,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
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);
failed = true;
}
@ -775,7 +805,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
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);
failed = true;
}
}
@ -808,7 +838,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
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);
failed = true;
}
}
@ -844,7 +874,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
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);
failed = true;
}
}
@ -858,7 +888,10 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures && !failed, prearm)) {
int32_t optional = 0; //Given checkAirspeed==true, assume the airspeed sensor is also required by default
param_get(param_find("FW_ARSP_MODE"), &optional);
if (!airspeedCheck(mavlink_log_pub, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
failed = true;
}
}
@ -871,6 +904,13 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
}
failed = true;
publish_subsystem_info_enabled_healthy(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, false);
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_enabled_healthy(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, !status.rc_signal_lost);
}
}
@ -881,6 +921,29 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
}
}
/* ---- GPS ---- */
if (checkGNSS) {
int fd_gps = orb_subscribe(ORB_ID(vehicle_gps_position));
vehicle_gps_position_s gps = {};
bool present = true;
if ((orb_copy(ORB_ID(vehicle_gps_position), fd_gps, &gps) != PX4_OK) ||
(hrt_elapsed_time(&gps.timestamp) > 2000000)) {
if (reportFailures) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS MODULE MISSING");
}
present = false;
}
// Mark GPS as required (given that checkGNSS=true) and indicate whether it is present. For now also assume it is healthy
// if there is a lock ... EKF2 will then set the healthy=false if its more extensive GPS checks fail in the next step.
publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GPS, present, true, present && gps.fix_type>=3);
}
// TODO: Add rangefinder here. We have the SENS_EN_XXX params that tell us what we should have. This is currently completely done inside the driver.
// TODO: Add optical flow check here? This is currently completely done inside the driver.
/* ---- Navigation EKF ---- */
// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
int32_t estimator_type;
@ -889,8 +952,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
if (estimator_type == 2) {
// don't report ekf failures for the first 10 seconds to allow time for the filter to start
bool report_ekf_fail = (time_since_boot > 10 * 1000000);
if (!ekf2Check(mavlink_log_pub, true, (reportFailures && report_ekf_fail && !failed), checkGNSS)) {
if (!ekf2Check(mavlink_log_pub, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) {
failed = true;
}
}

4
src/modules/commander/PreflightCheck.h
View File

@ -43,6 +43,8 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <uORB/topics/vehicle_status_flags.h>
#pragma once
namespace Preflight
@ -73,7 +75,7 @@ namespace Preflight
* true if the system power should be checked
**/
bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
const hrt_abstime &time_since_boot);
static constexpr unsigned max_mandatory_gyro_count = 1;

55
src/modules/commander/commander.cpp
View File

@ -79,6 +79,7 @@
#include <parameters/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/state_table.h>
#include <systemlib/subsystem_info_pub.h>
#include <cmath>
#include <cfloat>
@ -1256,6 +1257,7 @@ Commander::run()
status_flags.condition_power_input_valid = true;
status_flags.usb_connected = false;
status_flags.rc_calibration_valid = true;
// CIRCUIT BREAKERS
status_flags.circuit_breaker_engaged_power_check = false;
@ -1270,6 +1272,11 @@ Commander::run()
status_flags.condition_local_velocity_valid = false;
status_flags.condition_local_altitude_valid = false;
bool status_changed = true;
/* initialize the vehicle status flag helper functions. This also initializes the sensor health flags*/
publish_subsystem_info_init(&status, &status_changed);
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@ -1310,7 +1317,6 @@ Commander::run()
bool emergency_battery_voltage_actions_done = false;
bool dangerous_battery_level_requests_poweroff = false;
bool status_changed = true;
bool param_init_forced = true;
bool updated = false;
@ -1825,6 +1831,12 @@ 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);
publish_subsystem_info_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true);
}
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);
/* Update land detector */
@ -1993,41 +2005,6 @@ Commander::run()
}
}
/* update subsystem */
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;
}
status_changed = true;
}
/* If in INIT state, try to proceed to STANDBY state */
if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
@ -2223,11 +2200,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_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_changed = true;
}
}
@ -2375,6 +2354,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_changed = true;
}
}
@ -4096,6 +4076,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); // TODO
} else {
//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true); // TODO
}
}

64
src/modules/sensors/voted_sensors_update.cpp
View File

@ -40,6 +40,7 @@
#include "voted_sensors_update.h"
#include <systemlib/mavlink_log.h>
#include <systemlib/subsystem_info_pub.h>
#include <conversion/rotation.h>
#include <ecl/geo/geo.h>
@ -897,18 +898,15 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n
if (sensor.last_failover_count != sensor.voter.failover_count()) {
uint32_t flags = sensor.voter.failover_state();
if (flags == DataValidator::ERROR_FLAG_NO_ERROR) {
int failover_index = sensor.voter.failover_index();
if (flags == DataValidator::ERROR_FLAG_NO_ERROR) {
if (failover_index != -1) {
//we switched due to a non-critical reason. No need to panic.
PX4_INFO("%s sensor switch from #%i", sensor_name, failover_index);
}
} else {
int failover_index = sensor.voter.failover_index();
if (failover_index != -1) {
mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!",
sensor_name,
@ -921,6 +919,40 @@ 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
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_INFO("%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, "Accel") == 0) { 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, "Baro") == 0) { 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, "Accel") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
if (strcmp(sensor_name, "Mag") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; }
}
publish_subsystem_info_healthy(subsystem_type, false);
}
}
}
@ -951,6 +983,30 @@ 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);
}
}
}

1
src/modules/sensors/voted_sensors_update.h
View File

@ -58,6 +58,7 @@
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/subsystem_info.h>
#include <DevMgr.hpp>

29
src/modules/simulator/airspeedsim/airspeedsim.cpp
View File

@ -65,7 +65,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <simulator/simulator.h>
@ -76,12 +75,10 @@ AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, con
_reports(nullptr),
_retries(0),
_sensor_ok(false),
_last_published_sensor_ok(true), /* initialize differently to force publication */
_measure_ticks(0),
_collect_phase(false),
_diff_pres_offset(0.0f),
_airspeed_pub(nullptr),
_subsys_pub(nullptr),
_class_instance(-1),
_conversion_interval(conversion_interval),
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
@ -360,38 +357,12 @@ AirspeedSim::stop()
work_cancel(HPWORK, &_work);
}
void
AirspeedSim::update_status()
{
if (_sensor_ok != _last_published_sensor_ok) {
/* notify about state change */
struct subsystem_info_s info = {};
info.present = true;
info.enabled = true;
info.ok = _sensor_ok;
info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE;
if (_subsys_pub != nullptr) {
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
} else {
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
_last_published_sensor_ok = _sensor_ok;
}
}
void
AirspeedSim::cycle_trampoline(void *arg)
{
AirspeedSim *dev = (AirspeedSim *)arg;
dev->cycle();
// XXX we do not know if this is
// really helping - do not update the
// subsys state right now
//dev->update_status();
}
void

8
src/modules/simulator/airspeedsim/airspeedsim.h
View File

@ -69,7 +69,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
/* Default I2C bus */
#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION
@ -117,20 +116,13 @@ protected:
virtual int transfer(const uint8_t *send, unsigned send_len,
uint8_t *recv, unsigned recv_len);
/**
* Update the subsystem status
*/
void update_status();
struct work_s _work;
bool _sensor_ok;
bool _last_published_sensor_ok;
unsigned _measure_ticks;
bool _collect_phase;
float _diff_pres_offset;
orb_advert_t _airspeed_pub;
orb_advert_t _subsys_pub;
int _class_instance;

1
src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp
View File

@ -71,7 +71,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/system_power.h>
#include "airspeedsim.h"

1
src/modules/systemlib/CMakeLists.txt
View File

@ -44,6 +44,7 @@ set(SRCS
pid/pid.c
pwm_limit/pwm_limit.c
rc_check.c
subsystem_info_pub.cpp
)
if(${OS} STREQUAL "nuttx")

148
src/modules/systemlib/subsystem_info_pub.cpp Normal file
View File

@ -0,0 +1,148 @@
/****************************************************************************
*
* 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 subsystem_info_pub.cpp
*
* Contains helper functions to efficiently publish the subsystem_info messages from various locations inside the code. It is basically a
* helper function for commander. Approach:
* - Before commander starts (which happens after some of the drivers have already published the respective subsystem_info), this helper
* code stores all requests for a publish_subsystem_info in the internal_status variable
* - When commander starts up, it calls the publish_subsystem_info_init function. This 1) copies the internal_status into commander's
* vehicle status variable and 2) assigns the status pointer to commanders vehicle status
* - After that, all requests to publish_subsystem_info are directly written to commander's vehicle status such that it is always up
* to date. Commander then publishes the vehicle_status uORB (and is in fact the only app that does that, which is why this approach works)
*
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
*/
#include "subsystem_info_pub.h"
vehicle_status_s internal_status = {};
vehicle_status_s *status = &internal_status;
bool *status_changed = nullptr;
/* initialize pointer to commander's vehicle status variable */
void publish_subsystem_info_init(vehicle_status_s *commander_vehicle_status_ptr, bool *commander_status_changed_ptr)
{
status = commander_vehicle_status_ptr;
status_changed = commander_status_changed_ptr;
status->onboard_control_sensors_present = internal_status.onboard_control_sensors_present;
status->onboard_control_sensors_enabled = internal_status.onboard_control_sensors_enabled;
status->onboard_control_sensors_health = internal_status.onboard_control_sensors_health;
*status_changed = true;
}
/* Writes the full state information for a specific subsystem type, either directly into commander's vehicle
* status variable or into an internal variable that is later copied to commander's vehicle status variable*/
void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok)
{
PX4_DEBUG("publish_subsystem_info (ext:%u): Type %llu pres=%u enabl=%u ok=%u", status != &internal_status,
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;
}
if (status != &internal_status) { *status_changed = true; }
}
void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy)
{
publish_subsystem_info(subsystem_type, present, getEnabled(subsystem_type), healthy);
}
void publish_subsystem_info_present_enabled(uint64_t subsystem_type, bool present, bool enabled)
{
publish_subsystem_info(subsystem_type, present, enabled, getHealthy(subsystem_type));
}
void publish_subsystem_info_enabled_healthy(uint64_t subsystem_type, bool enabled, bool ok)
{
publish_subsystem_info(subsystem_type, getPresent(subsystem_type), enabled, ok);
}
void publish_subsystem_info_enabled(uint64_t subsystem_type, bool enabled)
{
publish_subsystem_info(subsystem_type, getPresent(subsystem_type), enabled, getHealthy(subsystem_type));
}
void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok)
{
publish_subsystem_info(subsystem_type, getPresent(subsystem_type), getEnabled(subsystem_type), ok);
}
void publish_subsystem_info_print()
{
uint64_t type = 1;
for (int i = 1; i < 31; i++) {
PX4_DEBUG("subsystem_info: Type %llu pres=%u enabl=%u ok=%u", type,
(status->onboard_control_sensors_present & (uint32_t)type) > 0,
(status->onboard_control_sensors_enabled & (uint32_t)type) > 0,
(status->onboard_control_sensors_health & (uint32_t)type) > 0);
type = type * 2;
}
}
// Local helper functions
bool getPresent(uint64_t subsystem_type)
{
return status->onboard_control_sensors_present & (uint32_t)subsystem_type;
}
bool getEnabled(uint64_t subsystem_type)
{
return status->onboard_control_sensors_enabled & (uint32_t)subsystem_type;
}
bool getHealthy(uint64_t subsystem_type)
{
return status->onboard_control_sensors_health & (uint32_t)subsystem_type;
}

62
src/modules/systemlib/subsystem_info_pub.h Normal file
View File

@ -0,0 +1,62 @@
/****************************************************************************
*
* 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 subsystem_info_pub.h
*
* Contains helper functions to efficiently publish the subsystem_info topic from various locations inside the code.
*
* @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
*/
#pragma once
#include <px4_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
void publish_subsystem_info_init(vehicle_status_s *commander_vehicle_status_ptr, bool *commander_status_changed_ptr);
void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok);
void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy);
void publish_subsystem_info_present_enabled(uint64_t subsystem_type, bool present, bool enabled);
void publish_subsystem_info_enabled_healthy(uint64_t subsystem_type, bool enabled, bool ok);
void publish_subsystem_info_enabled(uint64_t subsystem_type, bool enabled);
void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok);
void publish_subsystem_info_print();
// Local helper functions
bool getPresent(uint64_t subsystem_type);
bool getEnabled(uint64_t subsystem_type);
bool getHealthy(uint64_t subsystem_type);

7
src/modules/uORB/uORBDevices.cpp
View File

@ -148,9 +148,14 @@ uORB::DeviceNode::open(device::file_t *filp)
return -ENOMEM;
}
/* default to no pending update */
/* If queue size >1, allow the subscriber to read the data in the queue. Otherwise, assume subscriber is up to date.*/
if (_queue_size <= 1) {
sd->generation = _generation;
} else {
sd->generation = _generation - (_queue_size < _generation ? _queue_size : _generation);
}
/* set priority */
sd->set_priority(_priority);

1
src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp
View File

@ -57,7 +57,6 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>

1
src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp
View File

@ -60,7 +60,6 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>

1
src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp
View File

@ -59,7 +59,6 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/distance_sensor.h>
#include <board_config.h>