diff --git a/src/drivers/iridiumsbd/IridiumSBD.cpp b/src/drivers/iridiumsbd/IridiumSBD.cpp index d9ae99db67..dfd4e1306b 100644 --- a/src/drivers/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/iridiumsbd/IridiumSBD.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2018 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 @@ -174,9 +174,6 @@ void IridiumSBD::main_loop(int argc, char *argv[]) arg_i++; arg_uart_name = arg_i; - } else if (!strcmp(argv[arg_i], "-v")) { - PX4_WARN("verbose mode ON"); - verbose = true; } arg_i++; @@ -219,7 +216,7 @@ void IridiumSBD::main_loop(int argc, char *argv[]) param_read_interval_s = 10; } - if (verbose) { PX4_INFO("read interval %d", param_read_interval_s); } + PX4_DEBUG("read interval %d", param_read_interval_s); while (!task_should_exit) { switch (state) { @@ -241,7 +238,7 @@ void IridiumSBD::main_loop(int argc, char *argv[]) } if (new_state != state) { - if (verbose) { PX4_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[state], satcom_state_string[new_state]); } + PX4_DEBUG("SWITCHING STATE FROM %s TO %s", satcom_state_string[state], satcom_state_string[new_state]); state = new_state; @@ -257,54 +254,50 @@ void IridiumSBD::standby_loop(void) if (test_pending) { test_pending = false; - } else if (!strcmp(test_command, "read")) { - rx_session_pending = true; + if (!strcmp(test_command, "read")) { + rx_session_pending = true; - } else { - test_timer = hrt_absolute_time(); - start_test(); - return; + } else { + test_timer = hrt_absolute_time(); + start_test(); + return; + } } -} -// check for incoming SBDRING, handled inside read_at_command() -read_at_command(); + // check for incoming SBDRING, handled inside read_at_command() + read_at_command(); -if (param_read_interval_s != 0 && ((int64_t)(hrt_absolute_time() - last_read_time) > param_read_interval_s * 1000000)) -{ - rx_session_pending = true; -} + if (param_read_interval_s != 0 && ((int64_t)(hrt_absolute_time() - last_read_time) > param_read_interval_s * 1000000)) { + rx_session_pending = true; + } -// write the MO buffer when the message stacking time expires -if ((tx_buf_write_idx > 0) && (hrt_absolute_time() - last_write_time > SATCOM_TX_STACKING_TIME)) -{ - write_tx_buf(); -} + // write the MO buffer when the message stacking time expires + if ((tx_buf_write_idx > 0) && (hrt_absolute_time() - last_write_time > SATCOM_TX_STACKING_TIME)) { + 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 (hrt_absolute_time() - last_signal_check < SATCOM_SIGNAL_REFRESH_DELAY && signal_quality > 0) { - // clear the MO buffer if we only want to read a message - if (rx_session_pending && !tx_session_pending) { - if (clear_mo_buffer()) { + // 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 (hrt_absolute_time() - last_signal_check < SATCOM_SIGNAL_REFRESH_DELAY && signal_quality > 0) { + // clear the MO buffer if we only want to read a message + if (rx_session_pending && !tx_session_pending) { + if (clear_mo_buffer()) { + start_sbd_session(); + } + + } else { start_sbd_session(); } } else { - start_sbd_session(); + start_csq(); } - - } else { - 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)) -{ - read_rx_buf(); -} + // 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)) { + read_rx_buf(); + } } void IridiumSBD::csq_loop(void) @@ -316,16 +309,15 @@ void IridiumSBD::csq_loop(void) } if (res != SATCOM_RESULT_OK) { - if (verbose) { PX4_INFO("UPDATE SIGNAL QUALITY: ERROR"); } + PX4_DEBUG("UPDATE SIGNAL QUALITY: ERROR"); new_state = SATCOM_STATE_STANDBY; return; } if (strncmp((const char *)rx_command_buf, "+CSQ:", 5)) { - if (verbose) { PX4_INFO("UPDATE SIGNAL QUALITY: WRONG ANSWER:"); } - - if (verbose) { PX4_INFO("%s", rx_command_buf); } + PX4_DEBUG("UPDATE SIGNAL QUALITY: WRONG ANSWER:"); + PX4_DEBUG("%s", rx_command_buf); new_state = SATCOM_STATE_STANDBY; return; @@ -335,7 +327,7 @@ void IridiumSBD::csq_loop(void) //signal_check_pending = false; last_signal_check = hrt_absolute_time(); - if (verbose) { PX4_INFO("SIGNAL QUALITY: %d", signal_quality); } + PX4_DEBUG("SIGNAL QUALITY: %d", signal_quality); new_state = SATCOM_STATE_STANDBY; @@ -366,18 +358,16 @@ void IridiumSBD::sbdsession_loop(void) } if (res != SATCOM_RESULT_OK) { - if (verbose) { PX4_INFO("SBD SESSION: ERROR"); } - - if (verbose) { PX4_INFO("SBD SESSION: RESULT %d", res); } + PX4_DEBUG("SBD SESSION: ERROR"); + PX4_DEBUG("SBD SESSION: RESULT %d", res); new_state = SATCOM_STATE_STANDBY; return; } if (strncmp((const char *)rx_command_buf, "+SBDIX:", 7)) { - if (verbose) { PX4_INFO("SBD SESSION: WRONG ANSWER:"); } - - if (verbose) { PX4_INFO("%s", rx_command_buf); } + PX4_DEBUG("SBD SESSION: WRONG ANSWER:"); + PX4_DEBUG("%s", rx_command_buf); new_state = SATCOM_STATE_STANDBY; return; @@ -399,14 +389,14 @@ void IridiumSBD::sbdsession_loop(void) (*rx_buf_parse)++; mt_queued = strtol(*rx_buf_parse, rx_buf_parse, 10); - if (verbose) { PX4_INFO("MO ST: %d, MT ST: %d, MT LEN: %d, MT QUEUED: %d", mo_status, mt_status, mt_len, mt_queued); } + PX4_DEBUG("MO ST: %d, MT ST: %d, MT LEN: %d, MT QUEUED: %d", mo_status, mt_status, mt_len, mt_queued); switch (mo_status) { case 0: case 2: case 3: case 4: - if (verbose) { PX4_INFO("SBD SESSION: SUCCESS"); } + PX4_DEBUG("SBD SESSION: SUCCESS"); ring_pending = false; rx_session_pending = false; @@ -420,20 +410,20 @@ void IridiumSBD::sbdsession_loop(void) break; case 1: - if (verbose) { PX4_INFO("SBD SESSION: MO SUCCESS, MT FAIL"); } + PX4_DEBUG("SBD SESSION: MO SUCCESS, MT FAIL"); tx_session_pending = false; break; case 32: - if (verbose) { PX4_INFO("SBD SESSION: NO NETWORK SIGNAL"); } + PX4_DEBUG("SBD SESSION: NO NETWORK SIGNAL"); signal_quality = 0; break; default: - if (verbose) { PX4_INFO("SBD SESSION: FAILED (%d)", mo_status); } + PX4_DEBUG("SBD SESSION: FAILED (%d)", mo_status); } new_state = SATCOM_STATE_STANDBY; @@ -452,7 +442,7 @@ void IridiumSBD::test_loop(void) ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen) { - if (verbose) { PX4_INFO("WRITE: LEN %d, TX WRITTEN: %d", buflen, tx_buf_write_idx); } + PX4_DEBUG("WRITE: LEN %d, TX WRITTEN: %d", buflen, tx_buf_write_idx); if ((ssize_t)buflen > SATCOM_TX_BUF_LEN - tx_buf_write_idx) { return PX4_ERROR; @@ -472,7 +462,7 @@ ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen) ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen) { - if (verbose) { PX4_INFO("READ: LEN %d, RX: %d RX END: %d", buflen, rx_msg_read_idx, rx_msg_end_idx); } + PX4_DEBUG("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; @@ -535,7 +525,7 @@ pollevent_t IridiumSBD::poll_state(struct file *filp) void IridiumSBD::write_tx_buf() { if (!is_modem_ready()) { - if (verbose) { PX4_INFO("SEND SBD: MODEM NOT READY!"); } + PX4_DEBUG("SEND SBD: MODEM NOT READY!"); return; } @@ -547,7 +537,7 @@ void IridiumSBD::write_tx_buf() write_at(command); if (read_at_command() != SATCOM_RESULT_READY) { - if (verbose) { PX4_INFO("SEND SBD: MODEM NOT RESPONDING!"); } + PX4_DEBUG("SEND SBD: MODEM NOT RESPONDING!"); return; } @@ -567,23 +557,23 @@ void IridiumSBD::write_tx_buf() uint8_t checksum[2] = {(uint8_t)(sum / 256), (uint8_t)(sum & 255)}; ::write(uart_fd, checksum, 2); - if (verbose) { PX4_INFO("SEND SBD: CHECKSUM %d %d", checksum[0], checksum[1]); } + PX4_DEBUG("SEND SBD: CHECKSUM %d %d", checksum[0], checksum[1]); if (read_at_command() != SATCOM_RESULT_OK) { - if (verbose) { PX4_INFO("SEND SBD: ERROR WHILE WRITING DATA TO MODEM!"); } + PX4_DEBUG("SEND SBD: ERROR WHILE WRITING DATA TO MODEM!"); pthread_mutex_unlock(&tx_buf_mutex); return; } if (rx_command_buf[0] != '0') { - if (verbose) { PX4_INFO("SEND SBD: ERROR WHILE WRITING DATA TO MODEM! (%d)", rx_command_buf[0] - '0'); } + PX4_DEBUG("SEND SBD: ERROR WHILE WRITING DATA TO MODEM! (%d)", rx_command_buf[0] - '0'); pthread_mutex_unlock(&tx_buf_mutex); return; } - if (verbose) { PX4_INFO("SEND SBD: DATA WRITTEN TO MODEM"); } + PX4_DEBUG("SEND SBD: DATA WRITTEN TO MODEM"); tx_buf_write_idx = 0; @@ -595,7 +585,7 @@ void IridiumSBD::write_tx_buf() void IridiumSBD::read_rx_buf(void) { if (!is_modem_ready()) { - if (verbose) { PX4_INFO("READ SBD: MODEM NOT READY!"); } + PX4_DEBUG("READ SBD: MODEM NOT READY!"); return; } @@ -603,7 +593,7 @@ void IridiumSBD::read_rx_buf(void) write_at("AT+SBDRB"); if (read_at_msg() != SATCOM_RESULT_OK) { - if (verbose) { PX4_INFO("READ SBD: MODEM NOT RESPONDING!"); } + PX4_DEBUG("READ SBD: MODEM NOT RESPONDING!"); return; } @@ -612,7 +602,7 @@ void IridiumSBD::read_rx_buf(void) // rx_buf contains 2 byte length, data, 2 byte checksum and /r/n delimiter if (data_len != rx_msg_end_idx - 6) { - if (verbose) { PX4_INFO("READ SBD: WRONG DATA LENGTH"); } + PX4_DEBUG("READ SBD: WRONG DATA LENGTH"); return; } @@ -624,7 +614,7 @@ void IridiumSBD::read_rx_buf(void) } if ((checksum / 256 != rx_msg_buf[rx_msg_end_idx - 4]) || ((checksum & 255) != rx_msg_buf[rx_msg_end_idx - 3])) { - if (verbose) { PX4_INFO("READ SBD: WRONG DATA CHECKSUM"); } + PX4_DEBUG("READ SBD: WRONG DATA CHECKSUM"); return; } @@ -633,7 +623,7 @@ void IridiumSBD::read_rx_buf(void) rx_msg_end_idx -= 4; // ignore the checksum and delimiter rx_read_pending = false; - if (verbose) { PX4_INFO("READ SBD: SUCCESS, LEN: %d", data_len); } + PX4_DEBUG("READ SBD: SUCCESS, LEN: %d", data_len); } bool IridiumSBD::clear_mo_buffer() @@ -641,7 +631,7 @@ bool IridiumSBD::clear_mo_buffer() write_at("AT+SBDD0"); if (read_at_command() != SATCOM_RESULT_OK || rx_command_buf[0] != '0') { - if (verbose) { PX4_INFO("CLEAR MO BUFFER: ERROR"); } + PX4_DEBUG("CLEAR MO BUFFER: ERROR"); return false; } @@ -651,10 +641,10 @@ bool IridiumSBD::clear_mo_buffer() void IridiumSBD::start_csq(void) { - if (verbose) { PX4_INFO("UPDATING SIGNAL QUALITY"); } + PX4_DEBUG("UPDATING SIGNAL QUALITY"); if (!is_modem_ready()) { - if (verbose) { PX4_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!"); } + PX4_DEBUG("UPDATE SIGNAL QUALITY: MODEM NOT READY!"); return; } @@ -665,10 +655,10 @@ void IridiumSBD::start_csq(void) void IridiumSBD::start_sbd_session(void) { - if (verbose) { PX4_INFO("STARTING SBD SESSION"); } + PX4_DEBUG("STARTING SBD SESSION"); if (!is_modem_ready()) { - if (verbose) { PX4_INFO("SBD SESSION: MODEM NOT READY!"); } + PX4_DEBUG("SBD SESSION: MODEM NOT READY!"); return; } @@ -715,12 +705,12 @@ void IridiumSBD::start_test(void) satcom_uart_status IridiumSBD::open_uart(char *uart_name) { - if (verbose) { PX4_INFO("opening iridium sbd modem UART: %s", uart_name); } + PX4_DEBUG("opening iridium sbd modem UART: %s", uart_name); uart_fd = ::open(uart_name, O_RDWR | O_BINARY); if (uart_fd < 0) { - if (verbose) { PX4_WARN("UART open failed!"); } + PX4_DEBUG("UART open failed!"); return SATCOM_UART_OPEN_FAIL; } @@ -731,7 +721,7 @@ satcom_uart_status IridiumSBD::open_uart(char *uart_name) cfsetspeed(&uart_config, 19200); tcsetattr(uart_fd, TCSANOW, &uart_config); - if (verbose) { PX4_INFO("UART opened"); } + PX4_DEBUG("UART opened"); return SATCOM_UART_OK; } @@ -750,7 +740,7 @@ bool IridiumSBD::is_modem_ready(void) void IridiumSBD::write_at(const char *command) { - if (verbose) { PX4_INFO("WRITING AT COMMAND: %s", command); } + PX4_DEBUG("WRITING AT COMMAND: %s", command); ::write(uart_fd, command, strlen(command)); ::write(uart_fd, "\r", 1); @@ -804,7 +794,7 @@ satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len) ring_pending = true; rx_session_pending = true; - if (verbose) { PX4_INFO("GET SBDRING");} + PX4_DEBUG("GET SBDRING"); return SATCOM_RESULT_SBDRING; @@ -853,7 +843,7 @@ int iridiumsbd_main(int argc, char *argv[]) return OK; } - PX4_INFO("usage: iridiumsbd {start|stop|status|test} [-d uart_device | -v]"); + PX4_INFO("usage: iridiumsbd {start|stop|status|test} [-d uart_device]"); return PX4_ERROR; } diff --git a/src/drivers/iridiumsbd/IridiumSBD.h b/src/drivers/iridiumsbd/IridiumSBD.h index 3389a6603a..8ebed90a73 100644 --- a/src/drivers/iridiumsbd/IridiumSBD.h +++ b/src/drivers/iridiumsbd/IridiumSBD.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2018 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 @@ -132,7 +132,6 @@ public: satcom_state new_state = SATCOM_STATE_STANDBY; pthread_mutex_t tx_buf_mutex = pthread_mutex_t(); - bool verbose = false; /* * Constructor