mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
IridiumSBD: Robustify state machine and fix format
- Check if changing to a non standby state if the current state is the standy state and no change is already scheduled. - Add prefix _ to all class variables
This commit is contained in:
acfloria
Beat Küng
parent
63cb895a1d
commit
b66b997c66
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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};
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user