yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Iridium SBD: Make debug messages a compile-time flag

Browse Source 
This ensures that users do not accidentally enable this in an airborne system and saves flash space.
This commit is contained in:
Lorenz Meier
2018-01-13 19:03:42 +01:00
committed by Daniel Agar
parent bc487c7c36
commit f1a3fd09a3
2 changed files with 72 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files

152
src/drivers/iridiumsbd/IridiumSBD.cpp
View File

@@ -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;
}

3
src/drivers/iridiumsbd/IridiumSBD.h
View File

@@ -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