diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg index bd0b9dd841..5784a3f589 100644 --- a/msg/subsystem_info.msg +++ b/msg/subsystem_info.msg @@ -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 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 9d72de0751..30514aa1a1 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -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 diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index b23923259e..515a49c87f 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -53,7 +53,6 @@ #include #include #include -#include #include #define BATT_SMBUS_ADDR_MIN 0x00 ///< lowest possible address diff --git a/src/drivers/differential_pressure/ets/ets_airspeed.cpp b/src/drivers/differential_pressure/ets/ets_airspeed.cpp index ca070b9fa6..ec4d58eff9 100644 --- a/src/drivers/differential_pressure/ets/ets_airspeed.cpp +++ b/src/drivers/differential_pressure/ets/ets_airspeed.cpp @@ -54,7 +54,6 @@ #include #include -#include #include /* I2C bus address */ diff --git a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp index e944555f03..4da9d37ac0 100644 --- a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp +++ b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp @@ -66,7 +66,6 @@ #include #include -#include #include #include diff --git a/src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp b/src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp index ddc84e6cc0..cda6cc328d 100644 --- a/src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp +++ b/src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp @@ -58,6 +58,7 @@ #include #include +#include #include #include @@ -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 diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index b4903dd406..6d6b0a9dd5 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp @@ -61,6 +61,7 @@ #include #include +#include #include #include @@ -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 diff --git a/src/drivers/distance_sensor/sf0x/sf0x.cpp b/src/drivers/distance_sensor/sf0x/sf0x.cpp index 2960f59b1a..685d9aa001 100644 --- a/src/drivers/distance_sensor/sf0x/sf0x.cpp +++ b/src/drivers/distance_sensor/sf0x/sf0x.cpp @@ -66,7 +66,6 @@ #include #include -#include #include #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 diff --git a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp index 1c1fd1eeac..480c32c511 100644 --- a/src/drivers/distance_sensor/sf1xx/sf1xx.cpp +++ b/src/drivers/distance_sensor/sf1xx/sf1xx.cpp @@ -64,12 +64,14 @@ #include #include +#include #include #include #include #include +#include #include #include @@ -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 diff --git a/src/drivers/distance_sensor/srf02/srf02.cpp b/src/drivers/distance_sensor/srf02/srf02.cpp index e73149971e..55dd2773f7 100644 --- a/src/drivers/distance_sensor/srf02/srf02.cpp +++ b/src/drivers/distance_sensor/srf02/srf02.cpp @@ -60,6 +60,7 @@ #include #include +#include #include #include @@ -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 diff --git a/src/drivers/distance_sensor/teraranger/teraranger.cpp b/src/drivers/distance_sensor/teraranger/teraranger.cpp index 1552fa801f..0343236f53 100644 --- a/src/drivers/distance_sensor/teraranger/teraranger.cpp +++ b/src/drivers/distance_sensor/teraranger/teraranger.cpp @@ -60,6 +60,7 @@ #include #include +#include #include #include @@ -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 diff --git a/src/drivers/distance_sensor/tfmini/tfmini.cpp b/src/drivers/distance_sensor/tfmini/tfmini.cpp index 383f0a8de5..c595dffeb2 100644 --- a/src/drivers/distance_sensor/tfmini/tfmini.cpp +++ b/src/drivers/distance_sensor/tfmini/tfmini.cpp @@ -61,6 +61,7 @@ #include #include +#include #include #include @@ -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 diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 903f5ec78a..052d3f02a9 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -73,7 +73,6 @@ #include #include -#include #include #include diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 6c0cbf94c3..f38af3739a 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -65,6 +65,7 @@ #include #include #include +#include #include @@ -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 diff --git a/src/lib/drivers/airspeed/airspeed.cpp b/src/lib/drivers/airspeed/airspeed.cpp index 0402b33b29..c65570a04f 100644 --- a/src/lib/drivers/airspeed/airspeed.cpp +++ b/src/lib/drivers/airspeed/airspeed.cpp @@ -54,20 +54,17 @@ #include #include -#include #include 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(); } diff --git a/src/lib/drivers/airspeed/airspeed.h b/src/lib/drivers/airspeed/airspeed.h index 4c1b396682..0498abb4ac 100644 --- a/src/lib/drivers/airspeed/airspeed.h +++ b/src/lib/drivers/airspeed/airspeed.h @@ -43,7 +43,6 @@ #include #include #include -#include #include /* 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; diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index fecc9176c9..7168181dec 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -69,6 +70,7 @@ #include #include #include +#include #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; } } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 4b708ea078..3ed6c68f04 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -43,6 +43,8 @@ #include #include +#include + #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; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 11bb5c91bf..42b033fa6b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -79,6 +79,7 @@ #include #include #include +#include #include #include @@ -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 } } diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index d47a828fb5..9cfeee6699 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -40,6 +40,7 @@ #include "voted_sensors_update.h" #include +#include #include #include @@ -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(); + int failover_index = sensor.voter.failover_index(); if (flags == DataValidator::ERROR_FLAG_NO_ERROR) { - int failover_index = sensor.voter.failover_index(); - 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); + } } } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 590118deca..00ffd18303 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -58,6 +58,7 @@ #include #include #include +#include #include diff --git a/src/modules/simulator/airspeedsim/airspeedsim.cpp b/src/modules/simulator/airspeedsim/airspeedsim.cpp index 48af005016..0d9f18e09f 100644 --- a/src/modules/simulator/airspeedsim/airspeedsim.cpp +++ b/src/modules/simulator/airspeedsim/airspeedsim.cpp @@ -65,7 +65,6 @@ #include #include -#include #include @@ -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 diff --git a/src/modules/simulator/airspeedsim/airspeedsim.h b/src/modules/simulator/airspeedsim/airspeedsim.h index 01e9286f1f..d065fbfcbc 100644 --- a/src/modules/simulator/airspeedsim/airspeedsim.h +++ b/src/modules/simulator/airspeedsim/airspeedsim.h @@ -69,7 +69,6 @@ #include #include -#include /* 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; diff --git a/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp b/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp index 31e2291da6..5de6c0c5b8 100644 --- a/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp +++ b/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp @@ -71,7 +71,6 @@ #include #include -#include #include #include "airspeedsim.h" diff --git a/src/modules/systemlib/CMakeLists.txt b/src/modules/systemlib/CMakeLists.txt index 675cdeb4de..7f48f2fe63 100644 --- a/src/modules/systemlib/CMakeLists.txt +++ b/src/modules/systemlib/CMakeLists.txt @@ -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") diff --git a/src/modules/systemlib/subsystem_info_pub.cpp b/src/modules/systemlib/subsystem_info_pub.cpp new file mode 100644 index 0000000000..8b12ab64de --- /dev/null +++ b/src/modules/systemlib/subsystem_info_pub.cpp @@ -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; +} diff --git a/src/modules/systemlib/subsystem_info_pub.h b/src/modules/systemlib/subsystem_info_pub.h new file mode 100644 index 0000000000..155eae8f70 --- /dev/null +++ b/src/modules/systemlib/subsystem_info_pub.h @@ -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 +#include +#include + +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); diff --git a/src/modules/uORB/uORBDevices.cpp b/src/modules/uORB/uORBDevices.cpp index 808c3110fb..d132b4828e 100644 --- a/src/modules/uORB/uORBDevices.cpp +++ b/src/modules/uORB/uORBDevices.cpp @@ -148,8 +148,13 @@ uORB::DeviceNode::open(device::file_t *filp) return -ENOMEM; } - /* default to no pending update */ - sd->generation = _generation; + /* 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); diff --git a/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp b/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp index 389b87ba57..4f171bdae4 100644 --- a/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp +++ b/src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp @@ -57,7 +57,6 @@ #include #include -#include #include #include diff --git a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp index 8c3989468c..5ab60782dd 100644 --- a/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp +++ b/src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp @@ -60,7 +60,6 @@ #include #include -#include #include #include diff --git a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp index fd9ce8259e..cdd2f5e682 100644 --- a/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp +++ b/src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp @@ -59,7 +59,6 @@ #include #include -#include #include #include