diff --git a/msg/subsystem_info.msg b/msg/subsystem_info.msg index 56899ae4d8..a08d85abb3 100644 --- a/msg/subsystem_info.msg +++ b/msg/subsystem_info.msg @@ -25,7 +25,7 @@ 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 +uint64 SUBSYSTEM_TYPE_SATCOM = 134217728 bool present bool enabled diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index 74f73460a2..6730f75d13 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -300,6 +300,7 @@ 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; @@ -1061,6 +1062,29 @@ 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; + + if (_subsystem_pub == nullptr) { + _subsystem_pub = orb_advertise_queue(ORB_ID(subsystem_info), &_info, subsystem_info_s::ORB_QUEUE_LENGTH); + + } else { + orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &_info); + } + } +} + + int IridiumSBD::open_first(struct file *filep) { _cdev_used = true; diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 3e7eba69e3..98b6c7886c 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -41,6 +41,7 @@ #include #include +#include typedef enum { SATCOM_OK = 0, @@ -256,6 +257,8 @@ private: void publish_iridium_status(void); + void publish_subsystem_status(); + /** * Notification of the first open of CDev. * @@ -301,6 +304,7 @@ private: uint16_t _packet_length = 0; orb_advert_t _iridiumsbd_status_pub = nullptr; + orb_advert_t _subsystem_pub = nullptr; bool _test_pending = false; char _test_command[32]; @@ -338,4 +342,5 @@ private: bool _verbose = false; iridiumsbd_status_s _status = {}; + subsystem_info_s _info = {}; };