From d8cf0126417f6688b106b4e0597d7e458ee65ff5 Mon Sep 17 00:00:00 2001 From: acfloria Date: Wed, 18 Apr 2018 14:06:12 +0200 Subject: [PATCH] IridiumSBD: Add iridiumsbd_status uorb message --- msg/CMakeLists.txt | 1 + msg/iridiumsbd_status.msg | 14 +++ .../telemetry/iridiumsbd/IridiumSBD.cpp | 94 +++++++++++++++++++ src/drivers/telemetry/iridiumsbd/IridiumSBD.h | 8 ++ 4 files changed, 117 insertions(+) create mode 100644 msg/iridiumsbd_status.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 4091d2b718..5d0401a969 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -62,6 +62,7 @@ set(msg_files gps_inject_data.msg home_position.msg input_rc.msg + iridiumsbd_status.msg irlock_report.msg landing_target_innovations.msg landing_target_pose.msg diff --git a/msg/iridiumsbd_status.msg b/msg/iridiumsbd_status.msg new file mode 100644 index 0000000000..1483ce5a80 --- /dev/null +++ b/msg/iridiumsbd_status.msg @@ -0,0 +1,14 @@ +uint64 last_heartbeat # timestamp of the last successful sbd session +uint16 tx_buf_write_index # current size of the tx buffer +uint16 rx_buf_read_index # the rx buffer is parsed up to that index +uint16 rx_buf_end_index # current size of the rx buffer +uint16 failed_sbd_sessions # number of failed sbd sessions +uint16 successful_sbd_sessions # number of successfull sbd sessions +uint16 num_tx_buf_reset # number of times the tx buffer was reset +uint8 signal_quality # current signal quality, 0 is no signal, 5 the best +uint8 state # current state of the driver, see the satcom_state of IridiumSBD.h for the definition +bool ring_pending # indicates if a ring call is pending +bool tx_buf_write_pending # indicates if a tx buffer write is pending +bool tx_session_pending # indicates if a tx session is pending +bool rx_read_pending # indicates if a rx read is pending +bool rx_session_pending # indicates if a rx session is pending \ No newline at end of file diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index 41cae65c00..fd8ccc095e 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -303,8 +303,10 @@ void IridiumSBD::main_loop(int argc, char *argv[]) if (_new_state != _state) { VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]); _state = _new_state; + publish_iridium_status(); } else { + publish_iridium_status(); usleep(100000); // 100ms } } @@ -474,6 +476,8 @@ void IridiumSBD::sbdsession_loop(void) _tx_session_pending = false; _last_read_time = hrt_absolute_time(); _last_heartbeat = _last_read_time; + ++_successful_sbd_sessions; + if (mt_len > 0) { _rx_read_pending = true; @@ -489,6 +493,7 @@ void IridiumSBD::sbdsession_loop(void) // after a successful session reset the tx buffer _tx_buf_write_idx = 0; + ++_successful_sbd_sessions; _tx_session_pending = false; break; @@ -655,6 +660,7 @@ ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen) // check if there is enough space to write the message if (SATCOM_TX_BUF_LEN - _tx_buf_write_idx - _packet_length < 0) { _tx_buf_write_idx = 0; + ++_num_tx_buf_reset; publish_telemetry_status(); } @@ -974,6 +980,94 @@ void IridiumSBD::publish_telemetry_status() } } +void IridiumSBD::publish_iridium_status() +{ + bool need_to_publish = false; + + if (_status.last_heartbeat != _last_heartbeat) { + need_to_publish = true; + _status.last_heartbeat = _last_heartbeat; + } + + if (_status.tx_buf_write_index != _tx_buf_write_idx) { + need_to_publish = true; + _status.tx_buf_write_index = _tx_buf_write_idx; + } + + if (_status.rx_buf_read_index != _rx_msg_read_idx) { + need_to_publish = true; + _status.rx_buf_read_index = _rx_msg_read_idx; + } + + if (_status.rx_buf_end_index != _rx_msg_end_idx) { + need_to_publish = true; + _status.rx_buf_end_index = _rx_msg_end_idx; + } + + if (_status.failed_sbd_sessions != _failed_sbd_sessions) { + need_to_publish = true; + _status.failed_sbd_sessions = _failed_sbd_sessions; + } + + if (_status.successful_sbd_sessions != _successful_sbd_sessions) { + need_to_publish = true; + _status.successful_sbd_sessions = _successful_sbd_sessions; + } + + if (_status.num_tx_buf_reset != _num_tx_buf_reset) { + need_to_publish = true; + _status.num_tx_buf_reset = _num_tx_buf_reset; + } + + if (_status.signal_quality != _signal_quality) { + need_to_publish = true; + _status.signal_quality = _signal_quality; + } + + if (_status.state != _state) { + need_to_publish = true; + _status.state = _state; + } + + if (_status.ring_pending != _ring_pending) { + need_to_publish = true; + _status.ring_pending = _ring_pending; + } + + if (_status.tx_buf_write_pending != _tx_buf_write_pending) { + need_to_publish = true; + _status.tx_buf_write_pending = _tx_buf_write_pending; + } + + if (_status.tx_session_pending != _tx_session_pending) { + need_to_publish = true; + _status.tx_session_pending = _tx_session_pending; + } + + if (_status.rx_read_pending != _rx_read_pending) { + need_to_publish = true; + _status.rx_read_pending = _rx_read_pending; + } + + if (_status.rx_session_pending != _rx_session_pending) { + need_to_publish = true; + _status.rx_session_pending = _rx_session_pending; + } + + _status.timestamp = hrt_absolute_time(); + + // publish the status if it changed + if (need_to_publish) { + if (_iridiumsbd_status_pub == nullptr) { + _iridiumsbd_status_pub = orb_advertise(ORB_ID(iridiumsbd_status), &_status); + + } else { + orb_publish(ORB_ID(iridiumsbd_status), _iridiumsbd_status_pub, &_status); + } + } + +} + 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 613704a074..9f0c633738 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, @@ -259,6 +260,8 @@ private: */ void publish_telemetry_status(void); + void publish_iridium_status(void); + /** * Notification of the first open of CDev. * @@ -297,11 +300,14 @@ private: hrt_abstime _last_signal_check = 0; uint8_t _signal_quality = 0; uint16_t _failed_sbd_sessions = 0; + uint16_t _successful_sbd_sessions = 0; + uint16_t _num_tx_buf_reset = 0; bool _writing_mavlink_packet = false; uint16_t _packet_length = 0; orb_advert_t _telemetry_status_pub = nullptr; + orb_advert_t _iridiumsbd_status_pub = nullptr; bool _test_pending = false; char _test_command[32]; @@ -335,4 +341,6 @@ private: pthread_mutex_t _tx_buf_mutex = pthread_mutex_t(); bool _verbose = false; + + iridiumsbd_status_s _status = {}; };