diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index 01e9eee837..a7d4678675 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -53,7 +53,7 @@ static constexpr const char *satcom_state_string[4] = {"STANDBY", "SIGNAL CHECK", "SBD SESSION", "TEST"}; -#define VERBOSE_INFO(...) if (verbose) { PX4_INFO(__VA_ARGS__); } +#define VERBOSE_INFO(...) if (_verbose) { PX4_INFO(__VA_ARGS__); } IridiumSBD *IridiumSBD::instance; int IridiumSBD::task_handle; @@ -91,14 +91,14 @@ int IridiumSBD::stop() return PX4_ERROR; } - if (IridiumSBD::instance->cdev_used) { + if (IridiumSBD::instance->_cdev_used) { PX4_WARN("device is used. Stop all users (MavLink)"); return PX4_ERROR; } PX4_WARN("stopping..."); - IridiumSBD::instance->task_should_exit = true; + IridiumSBD::instance->_task_should_exit = true; // give it enough time to stop //param_timeout_s = 10; @@ -130,15 +130,15 @@ void IridiumSBD::status() } PX4_INFO("started"); - PX4_INFO("state: %s", satcom_state_string[instance->state]); + PX4_INFO("state: %s", satcom_state_string[instance->_state]); - PX4_INFO("TX buf written: %d", instance->tx_buf_write_idx); - PX4_INFO("Signal quality: %d", instance->signal_quality); - PX4_INFO("TX session pending: %d", instance->tx_session_pending); - PX4_INFO("RX session pending: %d", instance->rx_session_pending); - PX4_INFO("RX read pending: %d", instance->rx_read_pending); - PX4_INFO("Time since last signal check: %lld", hrt_absolute_time() - instance->last_signal_check); - PX4_INFO("Last heartbeat: %lld", instance->last_heartbeat); + PX4_INFO("TX buf written: %d", instance->_tx_buf_write_idx); + PX4_INFO("Signal quality: %d", instance->_signal_quality); + PX4_INFO("TX session pending: %d", instance->_tx_session_pending); + PX4_INFO("RX session pending: %d", instance->_rx_session_pending); + PX4_INFO("RX read pending: %d", instance->_rx_read_pending); + PX4_INFO("Time since last signal check: %lld", hrt_absolute_time() - instance->_last_signal_check); + PX4_INFO("Last heartbeat: %lld", instance->_last_heartbeat); } void IridiumSBD::test(int argc, char *argv[]) @@ -148,17 +148,17 @@ void IridiumSBD::test(int argc, char *argv[]) return; } - if (instance->state != SATCOM_STATE_STANDBY || instance->test_pending) { + if (instance->_state != SATCOM_STATE_STANDBY || instance->_test_pending) { PX4_WARN("MODEM BUSY!"); return; } if (argc > 2) { - strncpy(instance->test_command, argv[2], sizeof(instance->test_command)); - instance->test_command[sizeof(instance->test_command) - 1] = 0; + strncpy(instance->_test_command, argv[2], sizeof(instance->_test_command)); + instance->_test_command[sizeof(instance->_test_command) - 1] = 0; } else { - instance->test_command[0] = 0; + instance->_test_command[0] = 0; } instance->schedule_test(); @@ -168,14 +168,14 @@ int IridiumSBD::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { case FIONREAD: { - int count = rx_msg_end_idx - rx_msg_read_idx; + int count = _rx_msg_end_idx - _rx_msg_read_idx; *(int *)arg = count; return OK; } case FIONSPACE: { - int count = SATCOM_TX_BUF_LEN - tx_buf_write_idx + SATCOM_MAX_MESSAGE_LENGTH; + int count = SATCOM_TX_BUF_LEN - _tx_buf_write_idx + SATCOM_MAX_MESSAGE_LENGTH; *(int *)arg = count; return OK; @@ -211,7 +211,7 @@ void IridiumSBD::main_loop(int argc, char *argv[]) { CDev::init(); - pthread_mutex_init(&tx_buf_mutex, NULL); + pthread_mutex_init(&_tx_buf_mutex, NULL); int arg_i = 3; int arg_uart_name = 0; @@ -223,7 +223,7 @@ void IridiumSBD::main_loop(int argc, char *argv[]) } else if (!strcmp(argv[arg_i], "-v")) { PX4_WARN("verbose mode ON"); - verbose = true; + _verbose = true; } arg_i++; @@ -231,13 +231,13 @@ void IridiumSBD::main_loop(int argc, char *argv[]) if (arg_uart_name == 0) { PX4_WARN("no Iridium SBD modem UART port provided!"); - task_should_exit = true; + _task_should_exit = true; return; } if (open_uart(argv[arg_uart_name]) != SATCOM_UART_OK) { PX4_WARN("failed to open UART port!"); - task_should_exit = true; + _task_should_exit = true; return; } @@ -246,7 +246,7 @@ void IridiumSBD::main_loop(int argc, char *argv[]) if (read_at_command() != SATCOM_RESULT_OK) { PX4_WARN("modem not responding"); - task_should_exit = true; + _task_should_exit = true; return; } @@ -255,35 +255,35 @@ void IridiumSBD::main_loop(int argc, char *argv[]) if (read_at_command() != SATCOM_RESULT_OK) { PX4_WARN("modem not responding"); - task_should_exit = true; + _task_should_exit = true; return; } param_t param_pointer; param_pointer = param_find("ISBD_READ_INT"); - param_get(param_pointer, ¶m_read_interval_s); + param_get(param_pointer, &_param_read_interval_s); param_pointer = param_find("ISBD_SBD_TIMEOUT"); - param_get(param_pointer, ¶m_session_timeout_s); + param_get(param_pointer, &_param_session_timeout_s); - if (param_session_timeout_s < 0) { - param_session_timeout_s = 60; + if (_param_session_timeout_s < 0) { + _param_session_timeout_s = 60; } param_pointer = param_find("ISBD_STACK_TIME"); - param_get(param_pointer, ¶m_stacking_time_ms); + param_get(param_pointer, &_param_stacking_time_ms); - if (param_stacking_time_ms < 0) { - param_stacking_time_ms = 0; + if (_param_stacking_time_ms < 0) { + _param_stacking_time_ms = 0; } - VERBOSE_INFO("read interval: %d s", param_read_interval_s); - VERBOSE_INFO("SBD session timeout: %d s", param_session_timeout_s); - VERBOSE_INFO("SBD stack time: %d ms", param_stacking_time_ms); + VERBOSE_INFO("read interval: %d s", _param_read_interval_s); + VERBOSE_INFO("SBD session timeout: %d s", _param_session_timeout_s); + VERBOSE_INFO("SBD stack time: %d ms", _param_stacking_time_ms); - while (!task_should_exit) { - switch (state) { + while (!_task_should_exit) { + switch (_state) { case SATCOM_STATE_STANDBY: standby_loop(); break; @@ -301,9 +301,9 @@ void IridiumSBD::main_loop(int argc, char *argv[]) break; } - if (new_state != state) { - VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[state], satcom_state_string[new_state]); - state = new_state; + if (_new_state != _state) { + VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]); + _state = _new_state; } else { usleep(100000); // 100ms @@ -313,17 +313,17 @@ void IridiumSBD::main_loop(int argc, char *argv[]) void IridiumSBD::standby_loop(void) { - if (test_pending) { - test_pending = false; + if (_test_pending) { + _test_pending = false; - if (!strcmp(test_command, "s")) { + if (!strcmp(_test_command, "s")) { write(0, "kreczmer", 8); - } else if (!strcmp(test_command, "read")) { - rx_session_pending = true; + } else if (!strcmp(_test_command, "read")) { + _rx_session_pending = true; } else { - test_timer = hrt_absolute_time(); + _test_timer = hrt_absolute_time(); start_test(); return; } @@ -332,20 +332,21 @@ void IridiumSBD::standby_loop(void) // check for incoming SBDRING, handled inside read_at_command() read_at_command(); - if (param_read_interval_s > 0 && ((hrt_absolute_time() - last_read_time) > (uint64_t)param_read_interval_s * 1000000)) { - rx_session_pending = true; + if (_param_read_interval_s > 0 + && ((hrt_absolute_time() - _last_read_time) > (uint64_t)_param_read_interval_s * 1000000)) { + _rx_session_pending = true; } // write the MO buffer when the message stacking time expires - if (tx_buf_write_pending && ((hrt_absolute_time() - last_write_time) > (uint64_t)param_stacking_time_ms * 1000)) { + if (_tx_buf_write_pending && ((hrt_absolute_time() - _last_write_time) > (uint64_t)_param_stacking_time_ms * 1000)) { write_tx_buf(); } // do not start an SBD session if there is still data in the MT buffer, or it will be lost - if ((tx_session_pending || rx_session_pending) && !rx_read_pending) { - if (signal_quality > 0) { + if ((_tx_session_pending || _rx_session_pending) && !_rx_read_pending) { + if (_signal_quality > 0) { // clear the MO buffer if we only want to read a message - if (rx_session_pending && !tx_session_pending) { + if (_rx_session_pending && !_tx_session_pending) { if (clear_mo_buffer()) { start_sbd_session(); } @@ -359,13 +360,14 @@ void IridiumSBD::standby_loop(void) } } - // start a signal check if requested - if ((hrt_absolute_time() - last_signal_check) > SATCOM_SIGNAL_REFRESH_DELAY) { + // start a signal check if requested and not a switch to another mode is scheduled + if (((hrt_absolute_time() - _last_signal_check) > SATCOM_SIGNAL_REFRESH_DELAY) + && (_new_state == SATCOM_STATE_STANDBY)) { start_csq(); } // only read the MT buffer if the higher layer (mavlink app) read the previous message - if (rx_read_pending && (rx_msg_read_idx == rx_msg_end_idx)) { + if (_rx_read_pending && (_rx_msg_read_idx == _rx_msg_end_idx)) { read_rx_buf(); } } @@ -381,23 +383,23 @@ void IridiumSBD::csq_loop(void) if (res != SATCOM_RESULT_OK) { VERBOSE_INFO("UPDATE SIGNAL QUALITY: ERROR"); - new_state = SATCOM_STATE_STANDBY; + _new_state = SATCOM_STATE_STANDBY; return; } - if (strncmp((const char *)rx_command_buf, "+CSQ:", 5)) { + if (strncmp((const char *)_rx_command_buf, "+CSQ:", 5)) { VERBOSE_INFO("UPDATE SIGNAL QUALITY: WRONG ANSWER:"); - VERBOSE_INFO("%s", rx_command_buf); + VERBOSE_INFO("%s", _rx_command_buf); - new_state = SATCOM_STATE_STANDBY; + _new_state = SATCOM_STATE_STANDBY; return; } - signal_quality = rx_command_buf[5] - 48; + _signal_quality = _rx_command_buf[5] - 48; - VERBOSE_INFO("SIGNAL QUALITY: %d", signal_quality); + VERBOSE_INFO("SIGNAL QUALITY: %d", _signal_quality); - new_state = SATCOM_STATE_STANDBY; + _new_state = SATCOM_STATE_STANDBY; publish_telemetry_status(); } @@ -407,13 +409,13 @@ void IridiumSBD::sbdsession_loop(void) int res = read_at_command(); if (res == SATCOM_RESULT_NA) { - if ((param_session_timeout_s > 0) - && (((hrt_absolute_time() - session_start_time)) - > (uint64_t)param_session_timeout_s * 1000000)) { + if ((_param_session_timeout_s > 0) + && (((hrt_absolute_time() - _session_start_time)) + > (uint64_t)_param_session_timeout_s * 1000000)) { PX4_WARN("SBD SESSION: TIMEOUT!"); - new_state = SATCOM_STATE_STANDBY; - pthread_mutex_unlock(&tx_buf_mutex); + _new_state = SATCOM_STATE_STANDBY; + pthread_mutex_unlock(&_tx_buf_mutex); } @@ -423,22 +425,22 @@ void IridiumSBD::sbdsession_loop(void) if (res != SATCOM_RESULT_OK) { VERBOSE_INFO("SBD SESSION: ERROR. RESULT: %d", res); - new_state = SATCOM_STATE_STANDBY; - pthread_mutex_unlock(&tx_buf_mutex); + _new_state = SATCOM_STATE_STANDBY; + pthread_mutex_unlock(&_tx_buf_mutex); return; } - if (strncmp((const char *)rx_command_buf, "+SBDIX:", 7)) { + if (strncmp((const char *)_rx_command_buf, "+SBDIX:", 7)) { - VERBOSE_INFO("SBD SESSION: WRONG ANSWER: %s", rx_command_buf); + VERBOSE_INFO("SBD SESSION: WRONG ANSWER: %s", _rx_command_buf); - new_state = SATCOM_STATE_STANDBY; - pthread_mutex_unlock(&tx_buf_mutex); + _new_state = SATCOM_STATE_STANDBY; + pthread_mutex_unlock(&_tx_buf_mutex); return; } int mo_status, mt_status, mt_len, mt_queued; - const char *p = (const char *)rx_command_buf + 7; + const char *p = (const char *)_rx_command_buf + 7; char **rx_buf_parse = (char **)&p; mo_status = strtol(*rx_buf_parse, rx_buf_parse, 10); @@ -454,7 +456,7 @@ void IridiumSBD::sbdsession_loop(void) mt_queued = strtol(*rx_buf_parse, rx_buf_parse, 10); VERBOSE_INFO("MO ST: %d, MT ST: %d, MT LEN: %d, MT QUEUED: %d", mo_status, mt_status, mt_len, mt_queued); - VERBOSE_INFO("SBD session duration: %.2f", (hrt_absolute_time() - session_start_time) / 1000000.0); + VERBOSE_INFO("SBD session duration: %.2f", (hrt_absolute_time() - _session_start_time) / 1000000.0); switch (mo_status) { case 0: @@ -463,18 +465,18 @@ void IridiumSBD::sbdsession_loop(void) case 4: VERBOSE_INFO("SBD SESSION: SUCCESS (%d)", mo_status); - ring_pending = false; - rx_session_pending = false; - tx_session_pending = false; - last_read_time = hrt_absolute_time(); - last_heartbeat = last_read_time; + _ring_pending = false; + _rx_session_pending = false; + _tx_session_pending = false; + _last_read_time = hrt_absolute_time(); + _last_heartbeat = _last_read_time; if (mt_len > 0) { - rx_read_pending = true; + _rx_read_pending = true; } // after a successful session reset the tx buffer - tx_buf_write_idx = 0; + _tx_buf_write_idx = 0; publish_telemetry_status(); @@ -482,19 +484,19 @@ void IridiumSBD::sbdsession_loop(void) case 1: VERBOSE_INFO("SBD SESSION: MO SUCCESS, MT FAIL"); - last_heartbeat = hrt_absolute_time(); + _last_heartbeat = hrt_absolute_time(); publish_telemetry_status(); // after a successful session reset the tx buffer - tx_buf_write_idx = 0; + _tx_buf_write_idx = 0; - tx_session_pending = false; + _tx_session_pending = false; break; case 32: VERBOSE_INFO("SBD SESSION: NO NETWORK SIGNAL"); - signal_quality = 0; + _signal_quality = 0; break; @@ -502,8 +504,8 @@ void IridiumSBD::sbdsession_loop(void) VERBOSE_INFO("SBD SESSION: FAILED (%d)", mo_status); } - new_state = SATCOM_STATE_STANDBY; - pthread_mutex_unlock(&tx_buf_mutex); + _new_state = SATCOM_STATE_STANDBY; + pthread_mutex_unlock(&_tx_buf_mutex); } void IridiumSBD::test_loop(void) @@ -511,23 +513,29 @@ void IridiumSBD::test_loop(void) int res = read_at_command(); if (res != SATCOM_RESULT_NA) { - PX4_INFO("TEST RESULT: %d, LENGTH %d\nDATA:\n%s", res, rx_command_len, rx_command_buf); - PX4_INFO("TEST DONE, TOOK %lld MS", (hrt_absolute_time() - test_timer) / 1000); - new_state = SATCOM_STATE_STANDBY; + PX4_INFO("TEST RESULT: %d, LENGTH %d\nDATA:\n%s", res, _rx_command_len, _rx_command_buf); + PX4_INFO("TEST DONE, TOOK %lld MS", (hrt_absolute_time() - _test_timer) / 1000); + _new_state = SATCOM_STATE_STANDBY; } // timeout after 60 s in the test state - if ((hrt_absolute_time() - test_timer) > 60000000) { - PX4_WARN("TEST TIMEOUT AFTER %lld S", (hrt_absolute_time() - test_timer) / 1000000); - new_state = SATCOM_STATE_STANDBY; + if ((hrt_absolute_time() - _test_timer) > 60000000) { + PX4_WARN("TEST TIMEOUT AFTER %lld S", (hrt_absolute_time() - _test_timer) / 1000000); + _new_state = SATCOM_STATE_STANDBY; } } void IridiumSBD::start_csq(void) { - VERBOSE_INFO("UPDATING SIGNAL QUALITY"); + if ((_state != SATCOM_STATE_STANDBY) || (_new_state != SATCOM_STATE_STANDBY)) { + VERBOSE_INFO("CANNOT ENTER CSQ STATE"); + return; - last_signal_check = hrt_absolute_time(); + } else { + VERBOSE_INFO("UPDATING SIGNAL QUALITY"); + } + + _last_signal_check = hrt_absolute_time(); if (!is_modem_ready()) { VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!"); @@ -535,40 +543,53 @@ void IridiumSBD::start_csq(void) } write_at("AT+CSQ"); - new_state = SATCOM_STATE_CSQ; + _new_state = SATCOM_STATE_CSQ; } void IridiumSBD::start_sbd_session(void) { - VERBOSE_INFO("STARTING SBD SESSION"); + if ((_state != SATCOM_STATE_STANDBY) || (_new_state != SATCOM_STATE_STANDBY)) { + VERBOSE_INFO("CANNOT ENTER SBD SESSION STATE"); + return; + + } else { + VERBOSE_INFO("STARTING SBD SESSION"); + } if (!is_modem_ready()) { VERBOSE_INFO("SBD SESSION: MODEM NOT READY!"); return; } - if (ring_pending) { + if (_ring_pending) { write_at("AT+SBDIXA"); } else { write_at("AT+SBDIX"); } - new_state = SATCOM_STATE_SBDSESSION; - pthread_mutex_lock(&tx_buf_mutex); - session_start_time = hrt_absolute_time(); + _new_state = SATCOM_STATE_SBDSESSION; + + pthread_mutex_lock(&_tx_buf_mutex); + + _session_start_time = hrt_absolute_time(); } void IridiumSBD::start_test(void) { + if ((_state != SATCOM_STATE_STANDBY) || (_new_state != SATCOM_STATE_STANDBY)) { + PX4_INFO("CANNOT ENTER TEST STATE"); + return; + } + int res = read_at_command(); if (res != SATCOM_RESULT_NA) { PX4_WARN("SOMETHING WAS IN BUFFER"); - printf("TEST RESULT: %d, LENGTH %d\nDATA:\n%s\nRAW DATA:\n", res, rx_command_len, rx_command_buf); + printf("TEST RESULT: %d, LENGTH %d\nDATA:\n%s\nRAW DATA:\n", res, _rx_command_len, _rx_command_buf); - for (int i = 0; i < rx_command_len; i++) { - printf("%d ", rx_command_buf[i]); + for (int i = 0; i < _rx_command_len; i++) { + printf("%d ", _rx_command_buf[i]); } printf("\n"); @@ -579,15 +600,15 @@ void IridiumSBD::start_test(void) return; } - if (strlen(test_command) != 0) { - if ((strstr(test_command, "AT") != nullptr) || (strstr(test_command, "at") != nullptr)) { - PX4_INFO("TEST %s", test_command); - write_at(test_command); - new_state = SATCOM_STATE_TEST; + if (strlen(_test_command) != 0) { + if ((strstr(_test_command, "AT") != nullptr) || (strstr(_test_command, "at") != nullptr)) { + PX4_INFO("TEST %s", _test_command); + write_at(_test_command); + _new_state = SATCOM_STATE_TEST; } else { - PX4_WARN("The test command does not include AT or at: %s, ignoring it.", test_command); - new_state = SATCOM_STATE_STANDBY; + PX4_WARN("The test command does not include AT or at: %s, ignoring it.", _test_command); + _new_state = SATCOM_STATE_STANDBY; } } else { @@ -602,51 +623,51 @@ ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen) return PX4_ERROR; } + pthread_mutex_lock(&_tx_buf_mutex); + // check if there is enough space for the incoming mavlink message if (buflen == 6) { if (*buffer == MAVLINK_PACKAGE_START) { - if (SATCOM_TX_BUF_LEN - tx_buf_write_idx - (*(buffer + 1) + 8) < 0) { - tx_buf_write_idx = 0; + if (SATCOM_TX_BUF_LEN - _tx_buf_write_idx - (*(buffer + 1) + 8) < 0) { + _tx_buf_write_idx = 0; mavlink_log_critical(&_mavlink_log_pub, "Deleting full TX buffer before writing new message"); } } } // check if there is enough space for the incoming non mavlink message - if ((ssize_t)buflen > SATCOM_TX_BUF_LEN - tx_buf_write_idx) { - tx_buf_write_idx = 0; + if ((ssize_t)buflen > SATCOM_TX_BUF_LEN - _tx_buf_write_idx) { + _tx_buf_write_idx = 0; mavlink_log_critical(&_mavlink_log_pub, "Deleting full TX buffer"); } - pthread_mutex_lock(&tx_buf_mutex); + VERBOSE_INFO("WRITE: LEN %d, TX WRITTEN: %d", buflen, _tx_buf_write_idx); - VERBOSE_INFO("WRITE: LEN %d, TX WRITTEN: %d", buflen, tx_buf_write_idx); + memcpy(_tx_buf + _tx_buf_write_idx, buffer, buflen); - memcpy(tx_buf + tx_buf_write_idx, buffer, buflen); + _tx_buf_write_idx += buflen; + _last_write_time = hrt_absolute_time(); + _tx_buf_write_pending = true; - tx_buf_write_idx += buflen; - last_write_time = hrt_absolute_time(); - tx_buf_write_pending = true; - - pthread_mutex_unlock(&tx_buf_mutex); + pthread_mutex_unlock(&_tx_buf_mutex); return buflen; } ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen) { - VERBOSE_INFO("READ: LEN %d, RX: %d RX END: %d", buflen, rx_msg_read_idx, rx_msg_end_idx); + VERBOSE_INFO("READ: LEN %d, RX: %d RX END: %d", buflen, _rx_msg_read_idx, _rx_msg_end_idx); - if (rx_msg_read_idx < rx_msg_end_idx) { - size_t bytes_to_copy = rx_msg_end_idx - rx_msg_read_idx; + if (_rx_msg_read_idx < _rx_msg_end_idx) { + size_t bytes_to_copy = _rx_msg_end_idx - _rx_msg_read_idx; if (bytes_to_copy > buflen) { bytes_to_copy = buflen; } - memcpy(buffer, &rx_msg_buf[rx_msg_read_idx], bytes_to_copy); + memcpy(buffer, &_rx_msg_buf[_rx_msg_read_idx], bytes_to_copy); - rx_msg_read_idx += bytes_to_copy; + _rx_msg_read_idx += bytes_to_copy; return bytes_to_copy; @@ -662,15 +683,15 @@ void IridiumSBD::write_tx_buf() return; } - pthread_mutex_lock(&tx_buf_mutex); + pthread_mutex_lock(&_tx_buf_mutex); char command[13]; - sprintf(command, "AT+SBDWB=%d", tx_buf_write_idx); + sprintf(command, "AT+SBDWB=%d", _tx_buf_write_idx); write_at(command); if (read_at_command() != SATCOM_RESULT_READY) { VERBOSE_INFO("WRITE SBD: MODEM NOT RESPONDING!"); - pthread_mutex_unlock(&tx_buf_mutex); + pthread_mutex_unlock(&_tx_buf_mutex); return; } @@ -678,12 +699,12 @@ void IridiumSBD::write_tx_buf() int written = 0; - while (written != tx_buf_write_idx) { - written += ::write(uart_fd, tx_buf + written, tx_buf_write_idx - written); + while (written != _tx_buf_write_idx) { + written += ::write(uart_fd, _tx_buf + written, _tx_buf_write_idx - written); } - for (int i = 0; i < tx_buf_write_idx; i++) { - sum += tx_buf[i]; + for (int i = 0; i < _tx_buf_write_idx; i++) { + sum += _tx_buf[i]; } uint8_t checksum[2] = {(uint8_t)(sum / 256), (uint8_t)(sum & 255)}; @@ -695,25 +716,25 @@ void IridiumSBD::write_tx_buf() if (read_at_command(250) != SATCOM_RESULT_OK) { VERBOSE_INFO("WRITE SBD: ERROR WHILE WRITING DATA TO MODEM!"); - pthread_mutex_unlock(&tx_buf_mutex); + pthread_mutex_unlock(&_tx_buf_mutex); return; } - if (rx_command_buf[0] != '0') { + if (_rx_command_buf[0] != '0') { - VERBOSE_INFO("WRITE SBD: ERROR WHILE WRITING DATA TO MODEM! (%d)", rx_command_buf[0] - '0'); + VERBOSE_INFO("WRITE SBD: ERROR WHILE WRITING DATA TO MODEM! (%d)", _rx_command_buf[0] - '0'); - pthread_mutex_unlock(&tx_buf_mutex); + pthread_mutex_unlock(&_tx_buf_mutex); return; } VERBOSE_INFO("WRITE SBD: DATA WRITTEN TO MODEM"); - tx_buf_write_pending = false; + _tx_buf_write_pending = false; - pthread_mutex_unlock(&tx_buf_mutex); + pthread_mutex_unlock(&_tx_buf_mutex); - tx_session_pending = true; + _tx_session_pending = true; } void IridiumSBD::read_rx_buf(void) @@ -730,10 +751,10 @@ void IridiumSBD::read_rx_buf(void) return; } - int data_len = (rx_msg_buf[0] << 8) + rx_msg_buf[1]; + int data_len = (_rx_msg_buf[0] << 8) + _rx_msg_buf[1]; // rx_buf contains 2 byte length, data, 2 byte checksum and /r/n delimiter - if (data_len != rx_msg_end_idx - 6) { + if (data_len != _rx_msg_end_idx - 6) { VERBOSE_INFO("READ SBD: WRONG DATA LENGTH"); return; } @@ -741,17 +762,17 @@ void IridiumSBD::read_rx_buf(void) int checksum = 0; for (int i = 2; i < data_len + 2; i++) { - checksum += rx_msg_buf[i]; + checksum += _rx_msg_buf[i]; } - if ((checksum / 256 != rx_msg_buf[rx_msg_end_idx - 4]) || ((checksum & 255) != rx_msg_buf[rx_msg_end_idx - 3])) { + if ((checksum / 256 != _rx_msg_buf[_rx_msg_end_idx - 4]) || ((checksum & 255) != _rx_msg_buf[_rx_msg_end_idx - 3])) { VERBOSE_INFO("READ SBD: WRONG DATA CHECKSUM"); return; } - rx_msg_read_idx = 2; // ignore the length - rx_msg_end_idx -= 4; // ignore the checksum and delimiter - rx_read_pending = false; + _rx_msg_read_idx = 2; // ignore the length + _rx_msg_end_idx -= 4; // ignore the checksum and delimiter + _rx_read_pending = false; VERBOSE_INFO("READ SBD: SUCCESS, LEN: %d", data_len); } @@ -766,12 +787,12 @@ void IridiumSBD::write_at(const char *command) satcom_result_code IridiumSBD::read_at_command(int16_t timeout) { - return read_at(rx_command_buf, &rx_command_len, timeout); + return read_at(_rx_command_buf, &_rx_command_len, timeout); } satcom_result_code IridiumSBD::read_at_msg(int16_t timeout) { - return read_at(rx_msg_buf, &rx_msg_end_idx, timeout); + return read_at(_rx_msg_buf, &_rx_msg_end_idx, timeout); } satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len, int16_t timeout) @@ -810,8 +831,8 @@ satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len, int16_t tim return SATCOM_RESULT_ERROR; } else if (!strncmp((const char *)&rx_buf[last_rn_idx], "SBDRING\r\n", 9)) { - ring_pending = true; - rx_session_pending = true; + _ring_pending = true; + _rx_session_pending = true; VERBOSE_INFO("GET SBDRING"); @@ -842,14 +863,14 @@ satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len, int16_t tim void IridiumSBD::schedule_test(void) { - test_pending = true; + _test_pending = true; } bool IridiumSBD::clear_mo_buffer() { write_at("AT+SBDD0"); - if (read_at_command() != SATCOM_RESULT_OK || rx_command_buf[0] != '0') { + if (read_at_command() != SATCOM_RESULT_OK || _rx_command_buf[0] != '0') { VERBOSE_INFO("CLEAR MO BUFFER: ERROR"); return false; } @@ -895,11 +916,11 @@ pollevent_t IridiumSBD::poll_state(struct file *filp) { pollevent_t pollstate = 0; - if (rx_msg_read_idx < rx_msg_end_idx) { + if (_rx_msg_read_idx < _rx_msg_end_idx) { pollstate |= POLLIN; } - if (SATCOM_TX_BUF_LEN - tx_buf_write_idx > 0) { + if (SATCOM_TX_BUF_LEN - _tx_buf_write_idx > 0) { pollstate |= POLLOUT; } @@ -914,28 +935,28 @@ void IridiumSBD::publish_telemetry_status() tstatus.timestamp = hrt_absolute_time(); tstatus.telem_time = tstatus.timestamp; tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM; - tstatus.rssi = signal_quality; - tstatus.txbuf = tx_buf_write_idx; - tstatus.heartbeat_time = last_heartbeat; + tstatus.rssi = _signal_quality; + tstatus.txbuf = _tx_buf_write_idx; + tstatus.heartbeat_time = _last_heartbeat; - if (telemetry_status_pub == nullptr) { + if (_telemetry_status_pub == nullptr) { int multi_instance; - telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_LOW); + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_LOW); } else { - orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus); + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); } } int IridiumSBD::open_first(struct file *filep) { - cdev_used = true; + _cdev_used = true; return CDev::open_first(filep); } int IridiumSBD::close_last(struct file *filep) { - cdev_used = false; + _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 2deaa6441c..c188897019 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -289,50 +289,50 @@ private: static IridiumSBD *instance; static int task_handle; - bool task_should_exit = false; + bool _task_should_exit = false; int uart_fd = -1; - int32_t param_read_interval_s = -1; - int32_t param_session_timeout_s = -1; - int32_t param_stacking_time_ms = -1; + int32_t _param_read_interval_s = -1; + int32_t _param_session_timeout_s = -1; + int32_t _param_stacking_time_ms = -1; - hrt_abstime last_signal_check = 0; - uint8_t signal_quality = 0; + hrt_abstime _last_signal_check = 0; + uint8_t _signal_quality = 0; - orb_advert_t telemetry_status_pub = nullptr; + orb_advert_t _telemetry_status_pub = nullptr; - bool test_pending = false; - char test_command[32]; - hrt_abstime test_timer = 0; + bool _test_pending = false; + char _test_command[32]; + hrt_abstime _test_timer = 0; - uint8_t rx_command_buf[SATCOM_RX_COMMAND_BUF_LEN] = {0}; - int rx_command_len = 0; + uint8_t _rx_command_buf[SATCOM_RX_COMMAND_BUF_LEN] = {0}; + int _rx_command_len = 0; - uint8_t rx_msg_buf[SATCOM_RX_MSG_BUF_LEN] = {0}; - int rx_msg_end_idx = 0; - int rx_msg_read_idx = 0; + uint8_t _rx_msg_buf[SATCOM_RX_MSG_BUF_LEN] = {0}; + int _rx_msg_end_idx = 0; + int _rx_msg_read_idx = 0; - uint8_t tx_buf[SATCOM_TX_BUF_LEN] = {0}; - int tx_buf_write_idx = 0; + uint8_t _tx_buf[SATCOM_TX_BUF_LEN] = {0}; + int _tx_buf_write_idx = 0; - bool tx_buf_write_pending = false; - bool ring_pending = false; - bool rx_session_pending = false; - bool rx_read_pending = false; - bool tx_session_pending = false; + bool _tx_buf_write_pending = false; + bool _ring_pending = false; + bool _rx_session_pending = false; + bool _rx_read_pending = false; + bool _tx_session_pending = false; - bool cdev_used = false; + bool _cdev_used = false; - hrt_abstime last_write_time = 0; - hrt_abstime last_read_time = 0; - hrt_abstime last_heartbeat = 0; - hrt_abstime session_start_time = 0; + hrt_abstime _last_write_time = 0; + hrt_abstime _last_read_time = 0; + hrt_abstime _last_heartbeat = 0; + hrt_abstime _session_start_time = 0; - satcom_state state = SATCOM_STATE_STANDBY; - satcom_state new_state = SATCOM_STATE_STANDBY; + satcom_state _state = SATCOM_STATE_STANDBY; + satcom_state _new_state = SATCOM_STATE_STANDBY; - pthread_mutex_t tx_buf_mutex = pthread_mutex_t(); - bool verbose = false; + pthread_mutex_t _tx_buf_mutex = pthread_mutex_t(); + bool _verbose = false; orb_advert_t _mavlink_log_pub{nullptr}; };