From 945c17bc3f875a365f9ea9c9afada6333edf7815 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 25 Oct 2020 10:08:15 -0400 Subject: [PATCH] move subsystem_info entirely into commander and remove from uORB * HealthFlags: define bitfield using 1< --- msg/CMakeLists.txt | 1 - msg/subsystem_info.msg | 39 ------------------- msg/tools/uorb_rtps_message_ids.yaml | 2 - .../telemetry/iridiumsbd/IridiumSBD.cpp | 24 +----------- src/drivers/telemetry/iridiumsbd/IridiumSBD.h | 9 +---- .../Arming/HealthFlags/HealthFlags.cpp | 1 - .../Arming/HealthFlags/HealthFlags.h | 35 ++++++++++++++++- .../Arming/PreFlightCheck/PreFlightCheck.cpp | 1 - .../checks/accelerometerCheck.cpp | 1 - .../PreFlightCheck/checks/airspeedCheck.cpp | 1 - .../PreFlightCheck/checks/baroCheck.cpp | 1 - .../PreFlightCheck/checks/ekf2Check.cpp | 1 - .../PreFlightCheck/checks/gyroCheck.cpp | 1 - .../checks/imuConsistencyCheck.cpp | 1 - .../checks/magConsistencyCheck.cpp | 1 - .../checks/magnetometerCheck.cpp | 1 - src/modules/commander/Commander.cpp | 14 +++---- src/modules/commander/Commander.hpp | 2 - 18 files changed, 44 insertions(+), 92 deletions(-) delete mode 100644 msg/subsystem_info.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 1a78bc573b..263d12f8ec 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -119,7 +119,6 @@ set(msg_files sensor_preflight_mag.msg sensor_selection.msg sensors_status_imu.msg - subsystem_info.msg system_power.msg task_stack_info.msg tecs_status.msg diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg deleted file mode 100644 index d66e654a14..0000000000 --- a/msg/subsystem_info.msg +++ /dev/null @@ -1,39 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -uint64 SUBSYSTEM_TYPE_GYRO = 1 -uint64 SUBSYSTEM_TYPE_ACC = 2 -uint64 SUBSYSTEM_TYPE_MAG = 4 -uint64 SUBSYSTEM_TYPE_ABSPRESSURE = 8 -uint64 SUBSYSTEM_TYPE_DIFFPRESSURE = 16 -uint64 SUBSYSTEM_TYPE_GPS = 32 -uint64 SUBSYSTEM_TYPE_OPTICALFLOW = 64 -uint64 SUBSYSTEM_TYPE_CVPOSITION = 128 -uint64 SUBSYSTEM_TYPE_LASERPOSITION = 256 -uint64 SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512 -uint64 SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024 -uint64 SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048 -uint64 SUBSYSTEM_TYPE_YAWPOSITION = 4096 -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_SATCOM = 134217728 -uint64 SUBSYSTEM_TYPE_PREARM_CHECK = 268435456 -uint64 SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 536870912 - -bool present -bool enabled -bool ok -uint64 subsystem_type - -uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 5d23c05213..e64331aa0d 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -150,8 +150,6 @@ rtps: id: 70 - msg: px4io_status id: 71 - - msg: subsystem_info - id: 72 - msg: system_power id: 73 - msg: task_stack_info diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index 25c900bd36..4c1399955c 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -359,8 +359,6 @@ void IridiumSBD::main_loop(int argc, char *argv[]) break; } - publish_subsystem_status(); - if (_new_state != _state) { VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]); _state = _new_state; @@ -1116,31 +1114,13 @@ void IridiumSBD::publish_iridium_status() } } -void IridiumSBD::publish_subsystem_status() -{ - const bool present = true; - const bool enabled = true; - const bool ok = _status.last_heartbeat > 0; // maybe at some point here an additional check should be made - - if ((_info.present != present) || (_info.enabled != enabled) || (_info.ok != ok)) { - _info.timestamp = hrt_absolute_time(); - _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_SATCOM; - _info.present = present; - _info.enabled = enabled; - _info.ok = ok; - - _subsystem_pub.publish(_info); - } -} - - -int IridiumSBD::open_first(struct file *filep) +int IridiumSBD::open_first(struct file *filep) { _cdev_used = true; return CDev::open_first(filep); } -int IridiumSBD::close_last(struct file *filep) +int IridiumSBD::close_last(struct file *filep) { _cdev_used = false; return CDev::close_last(filep); diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 8ef474d40a..124fcecdc5 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -41,7 +41,6 @@ #include #include -#include typedef enum { SATCOM_OK = 0, @@ -255,9 +254,7 @@ private: */ pollevent_t poll_state(struct file *filp); - void publish_iridium_status(void); - - void publish_subsystem_status(); + void publish_iridium_status(); /** * Notification of the first open of CDev. @@ -305,7 +302,6 @@ private: uint16_t _packet_length = 0; uORB::Publication _iridiumsbd_status_pub{ORB_ID(iridiumsbd_status)}; - uORB::PublicationQueued _subsystem_pub{ORB_ID(subsystem_info)}; bool _test_pending = false; char _test_command[32]; @@ -342,6 +338,5 @@ private: bool _verbose = false; - iridiumsbd_status_s _status = {}; - subsystem_info_s _info = {}; + iridiumsbd_status_s _status{}; }; diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp index 8ec40c1db3..4a43e292b4 100644 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.cpp @@ -40,7 +40,6 @@ */ #include "HealthFlags.h" -#include void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status) { diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.h b/src/modules/commander/Arming/HealthFlags/HealthFlags.h index 470702aa91..e1cd5247fe 100644 --- a/src/modules/commander/Arming/HealthFlags/HealthFlags.h +++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.h @@ -43,7 +43,40 @@ #include #include -#include + +struct subsystem_info_s { + // keep in sync with mavlink MAV_SYS_STATUS_SENSOR + static constexpr uint64_t SUBSYSTEM_TYPE_GYRO = 1 << 0; + static constexpr uint64_t SUBSYSTEM_TYPE_ACC = 1 << 1; + static constexpr uint64_t SUBSYSTEM_TYPE_MAG = 1 << 2; + static constexpr uint64_t SUBSYSTEM_TYPE_ABSPRESSURE = 1 << 3; + static constexpr uint64_t SUBSYSTEM_TYPE_DIFFPRESSURE = 1 << 4; + static constexpr uint64_t SUBSYSTEM_TYPE_GPS = 1 << 5; + static constexpr uint64_t SUBSYSTEM_TYPE_OPTICALFLOW = 1 << 6; + static constexpr uint64_t SUBSYSTEM_TYPE_CVPOSITION = 1 << 7; + static constexpr uint64_t SUBSYSTEM_TYPE_LASERPOSITION = 1 << 8; + static constexpr uint64_t SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 1 << 9; + static constexpr uint64_t SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1 << 10; + static constexpr uint64_t SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 1 << 11; + static constexpr uint64_t SUBSYSTEM_TYPE_YAWPOSITION = 1 << 12; + static constexpr uint64_t SUBSYSTEM_TYPE_ALTITUDECONTROL = 1 << 13; + static constexpr uint64_t SUBSYSTEM_TYPE_POSITIONCONTROL = 1 << 14; + static constexpr uint64_t SUBSYSTEM_TYPE_MOTORCONTROL = 1 << 15; + static constexpr uint64_t SUBSYSTEM_TYPE_RCRECEIVER = 1 << 16; + static constexpr uint64_t SUBSYSTEM_TYPE_GYRO2 = 1 << 17; + static constexpr uint64_t SUBSYSTEM_TYPE_ACC2 = 1 << 18; + static constexpr uint64_t SUBSYSTEM_TYPE_MAG2 = 1 << 19; + static constexpr uint64_t SUBSYSTEM_TYPE_GEOFENCE = 1 << 20; + static constexpr uint64_t SUBSYSTEM_TYPE_AHRS = 1 << 21; + static constexpr uint64_t SUBSYSTEM_TYPE_TERRAIN = 1 << 22; + static constexpr uint64_t SUBSYSTEM_TYPE_REVERSEMOTOR = 1 << 23; + static constexpr uint64_t SUBSYSTEM_TYPE_LOGGING = 1 << 24; + static constexpr uint64_t SUBSYSTEM_TYPE_SENSORBATTERY = 1 << 25; + static constexpr uint64_t SUBSYSTEM_TYPE_SENSORPROXIMITY = 1 << 26; + static constexpr uint64_t SUBSYSTEM_TYPE_SATCOM = 1 << 27; + static constexpr uint64_t SUBSYSTEM_TYPE_PREARM_CHECK = 1 << 28; + static constexpr uint64_t SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 1 << 29; +}; void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status); void set_health_flags_present_healthy(uint64_t subsystem_type, bool present, bool healthy, vehicle_status_s &status); diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index c7dfeeba2a..5991effbde 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -42,7 +42,6 @@ #include #include #include -#include using namespace time_literals; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp index b9ea1827d0..b56dd3c26e 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/accelerometerCheck.cpp @@ -41,7 +41,6 @@ #include #include #include -#include using namespace time_literals; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp index 847a16b919..358bac25d6 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp @@ -39,7 +39,6 @@ #include #include #include -#include using namespace time_literals; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp index e5e16dd69c..ffc195a405 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/baroCheck.cpp @@ -39,7 +39,6 @@ #include #include #include -#include using namespace time_literals; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index 024d6b04d7..953b32f02e 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -40,7 +40,6 @@ #include #include #include -#include bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, const bool report_fail, const bool enforce_gps_required) diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp index 9387cca116..16823b8890 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/gyroCheck.cpp @@ -40,7 +40,6 @@ #include #include #include -#include using namespace time_literals; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp index 12379d4125..cad290ca44 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp @@ -39,7 +39,6 @@ #include #include #include -#include bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status) diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp index c5d8c97695..5c76650436 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magConsistencyCheck.cpp @@ -39,7 +39,6 @@ #include #include #include -#include #include // return false if the magnetomer measurements are inconsistent diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp index ea443702ac..d29ae269c9 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/magnetometerCheck.cpp @@ -40,7 +40,6 @@ #include #include #include -#include using namespace time_literals; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7897e45de9..12c1b2d66a 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1722,14 +1722,6 @@ Commander::run() battery_status_check(); - /* update subsystem info which arrives from outside of commander*/ - subsystem_info_s info; - - while (_subsys_sub.update(&info)) { - set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status); - _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) { @@ -3803,6 +3795,12 @@ void Commander::data_link_check() _status_changed = true; } } + + const bool present = true; + const bool enabled = true; + const bool ok = (iridium_status.last_heartbeat > 0); // maybe at some point here an additional check should be made + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SATCOM, present, enabled, ok, status); } break; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 48457f029b..e79c8ac03c 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -73,7 +73,6 @@ #include #include #include -#include #include #include #include @@ -391,7 +390,6 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _safety_sub{ORB_ID(safety)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _subsys_sub{ORB_ID(subsystem_info)}; uORB::Subscription _system_power_sub{ORB_ID(system_power)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};