Commit fcf3bb5a authored by Beat Küng's avatar Beat Küng Committed by Lorenz Meier
Browse files

fix iridiumsbd: use ModuleBase

fixes hardfaults, e.g. when device not connected
parent 9b717055
......@@ -53,189 +53,67 @@ static constexpr const char *satcom_state_string[4] = {"STANDBY", "SIGNAL CHECK"
#define IRIDIUMSBD_DEVICE_PATH "/dev/iridium"
IridiumSBD *IridiumSBD::instance;
int IridiumSBD::task_handle;
IridiumSBD::IridiumSBD()
: CDev(IRIDIUMSBD_DEVICE_PATH)
{
}
///////////////////////////////////////////////////////////////////////
// public functions //
///////////////////////////////////////////////////////////////////////
int IridiumSBD::start(int argc, char *argv[])
IridiumSBD::~IridiumSBD()
{
PX4_INFO("starting");
if (IridiumSBD::instance != nullptr) {
PX4_WARN("already started");
return PX4_ERROR;
}
IridiumSBD::instance = new IridiumSBD();
IridiumSBD::task_handle = px4_task_spawn_cmd("iridiumsbd", SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER, 1350, (main_t)&IridiumSBD::main_loop_helper, argv);
int counter = 0;
IridiumSBD::instance->_start_completed = false;
IridiumSBD::instance->_task_should_exit = false;
// give the driver 6 seconds to start up
while (!IridiumSBD::instance->_start_completed && (IridiumSBD::task_handle != -1) && counter < 60) {
counter++;
usleep(100000);
}
if (IridiumSBD::instance->_start_completed && (IridiumSBD::task_handle != -1)) {
return PX4_OK;
} else {
// the driver failed to start so make sure it is shut down before exiting
IridiumSBD::instance->_task_should_exit = true;
for (int i = 0; (i < 10 + 1) && (IridiumSBD::task_handle != -1); i++) {
sleep(1);
}
return PX4_ERROR;
}
::close(_uart_fd);
}
int IridiumSBD::stop()
int IridiumSBD::task_spawn(int argc, char *argv[])
{
if (IridiumSBD::instance == nullptr) {
PX4_WARN("not started");
return PX4_ERROR;
}
_task_id = px4_task_spawn_cmd("iridiumsbd",
SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER,
1350,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (IridiumSBD::instance->_cdev_used) {
PX4_WARN("device is used. Stop all users (MavLink)");
return PX4_ERROR;
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
PX4_WARN("stopping...");
IridiumSBD::instance->_task_should_exit = true;
// give it enough time to stop
//param_timeout_s = 10;
// TODO
for (int i = 0; (i < 10 + 1) && (IridiumSBD::task_handle != -1); i++) {
sleep(1);
// wait until task is up & running (max 6 seconds)
if (wait_until_running(6000) < 0) {
_task_id = -1;
return -1;
}
// well, kill it anyway, though this may crash
if (IridiumSBD::task_handle != -1) {
PX4_WARN("killing task forcefully");
::close(IridiumSBD::instance->uart_fd);
task_delete(IridiumSBD::task_handle);
IridiumSBD::task_handle = -1;
delete IridiumSBD::instance;
IridiumSBD::instance = nullptr;
}
return OK;
}
void IridiumSBD::status()
{
if (IridiumSBD::instance == nullptr) {
PX4_WARN("not started");
return;
}
PX4_INFO("started");
PX4_INFO("state: %s", satcom_state_string[instance->_state]);
PX4_INFO("TX buf written: %d", instance->_tx_buf_write_idx);
PX4_INFO("Signal quality: %" PRId8, 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: %" PRId64, hrt_absolute_time() - instance->_last_signal_check);
PX4_INFO("Last heartbeat: %" PRId64, instance->_last_heartbeat);
return 0;
}
void IridiumSBD::test(int argc, char *argv[])
IridiumSBD *IridiumSBD::instantiate(int argc, char *argv[])
{
if (instance == nullptr) {
PX4_WARN("not started");
return;
}
IridiumSBD *instance = new IridiumSBD();
if (instance->_state != SATCOM_STATE_STANDBY || instance->_test_pending) {
PX4_WARN("MODEM BUSY!");
return;
}
if (instance) {
int ret = instance->init(argc, argv);
if (argc > 2) {
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;
if (ret != PX4_OK) {
delete instance;
return nullptr;
}
}
instance->schedule_test();
return instance;
}
int IridiumSBD::ioctl(struct file *filp, int cmd, unsigned long arg)
int IridiumSBD::init(int argc, char *argv[])
{
switch (cmd) {
case FIONREAD: {
int count = _rx_msg_end_idx - _rx_msg_read_idx;
*(int *)arg = count;
int ret = CDev::init();
return OK;
}
case FIONSPACE: {
int count = SATCOM_TX_BUF_LEN - _tx_buf_write_idx + SATCOM_MAX_MESSAGE_LENGTH;
*(int *)arg = count;
return OK;
}
default: {
/* see if the parent class can make any use of it */
return CDev::ioctl(filp, cmd, arg);
}
if (ret != PX4_OK) {
return ret;
}
}
///////////////////////////////////////////////////////////////////////
// private functions //
///////////////////////////////////////////////////////////////////////
int IridiumSBD::main_loop_helper(int argc, char *argv[])
{
// start the main loop and stay in it
IridiumSBD::instance->main_loop(argc, argv);
// tear down everything after the main loop exits
::close(IridiumSBD::instance->uart_fd);
IridiumSBD::task_handle = -1;
delete IridiumSBD::instance;
IridiumSBD::instance = nullptr;
PX4_WARN("stopped");
return 0;
}
void IridiumSBD::main_loop(int argc, char *argv[])
{
CDev::init();
pthread_mutex_init(&_tx_buf_mutex, NULL);
pthread_mutex_init(&_rx_buf_mutex, NULL);
int arg_i = 3;
int arg_i = 1;
int arg_uart_name = 0;
while (arg_i < argc) {
......@@ -244,7 +122,7 @@ void IridiumSBD::main_loop(int argc, char *argv[])
arg_uart_name = arg_i;
} else if (!strcmp(argv[arg_i], "-v")) {
PX4_WARN("verbose mode ON");
PX4_INFO("verbose mode ON");
_verbose = true;
}
......@@ -253,8 +131,7 @@ void IridiumSBD::main_loop(int argc, char *argv[])
if (arg_uart_name == 0) {
PX4_ERR("no Iridium SBD modem UART port provided!");
_task_should_exit = true;
return;
return -EINVAL;
}
......@@ -271,8 +148,7 @@ void IridiumSBD::main_loop(int argc, char *argv[])
if (!command_executed) {
PX4_ERR("failed to open UART port!");
_task_should_exit = true;
return;
return -EIO;
}
// disable flow control
......@@ -291,8 +167,7 @@ void IridiumSBD::main_loop(int argc, char *argv[])
if (!command_executed) {
PX4_ERR("modem not responding");
_task_should_exit = true;
return;
return -EIO;
}
// disable command echo
......@@ -311,8 +186,7 @@ void IridiumSBD::main_loop(int argc, char *argv[])
if (!command_executed) {
PX4_ERR("modem not responding");
_task_should_exit = true;
return;
return -EIO;
}
param_t param_pointer;
......@@ -337,10 +211,70 @@ void IridiumSBD::main_loop(int argc, char *argv[])
VERBOSE_INFO("read interval: %" PRId32 " s", _param_read_interval_s);
VERBOSE_INFO("SBD session timeout: %" PRId32 " s", _param_session_timeout_s);
VERBOSE_INFO("SBD stack time: %" PRId32 " ms", _param_stacking_time_ms);
return PX4_OK;
}
int IridiumSBD::print_status()
{
PX4_INFO("started");
PX4_INFO("state: %s", satcom_state_string[_state]);
PX4_INFO("TX buf written: %d", _tx_buf_write_idx);
PX4_INFO("Signal quality: %" PRId8, _signal_quality);
PX4_INFO("TX session pending: %d", _tx_session_pending);
PX4_INFO("RX session pending: %d", _rx_session_pending);
PX4_INFO("RX read pending: %d", _rx_read_pending);
PX4_INFO("Time since last signal check: %" PRId64, hrt_absolute_time() - _last_signal_check);
PX4_INFO("Last heartbeat: %" PRId64, _last_heartbeat);
return 0;
}
void IridiumSBD::test(int argc, char *argv[])
{
if (_state != SATCOM_STATE_STANDBY || _test_pending.load()) {
PX4_WARN("MODEM BUSY!");
return;
}
if (argc > 1) {
strncpy(_test_command, argv[1], sizeof(_test_command));
_test_command[sizeof(_test_command) - 1] = 0;
} else {
_test_command[0] = 0;
}
schedule_test();
}
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 *)arg = count;
_start_completed = true;
return OK;
}
case FIONSPACE: {
int count = SATCOM_TX_BUF_LEN - _tx_buf_write_idx + SATCOM_MAX_MESSAGE_LENGTH;
*(int *)arg = count;
while (!_task_should_exit) {
return OK;
}
default: {
/* see if the parent class can make any use of it */
return CDev::ioctl(filp, cmd, arg);
}
}
}
void IridiumSBD::run()
{
while (!should_exit()) {
switch (_state) {
case SATCOM_STATE_STANDBY:
standby_loop();
......@@ -373,8 +307,8 @@ void IridiumSBD::main_loop(int argc, char *argv[])
void IridiumSBD::standby_loop(void)
{
if (_test_pending) {
_test_pending = false;
if (_test_pending.load()) {
_test_pending.store(false);
if (!strcmp(_test_command, "s")) {
write(0, "kreczmer", 8);
......@@ -796,7 +730,7 @@ 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);
written += ::write(_uart_fd, _tx_buf + written, _tx_buf_write_idx - written);
}
for (int i = 0; i < _tx_buf_write_idx; i++) {
......@@ -804,7 +738,7 @@ void IridiumSBD::write_tx_buf()
}
uint8_t checksum[2] = {(uint8_t)(sum / 256), (uint8_t)(sum & 255)};
::write(uart_fd, checksum, 2);
::write(_uart_fd, checksum, 2);
VERBOSE_INFO("SEND SBD: CHECKSUM %" PRId8 " %" PRId8, checksum[0], checksum[1]);
......@@ -887,8 +821,8 @@ void IridiumSBD::write_at(const char *command)
{
VERBOSE_INFO("WRITING AT COMMAND: %s", command);
::write(uart_fd, command, strlen(command));
::write(uart_fd, "\r", 1);
::write(_uart_fd, command, strlen(command));
::write(_uart_fd, "\r", 1);
}
satcom_result_code IridiumSBD::read_at_command(int16_t timeout)
......@@ -904,7 +838,7 @@ satcom_result_code IridiumSBD::read_at_msg(int16_t timeout)
satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len, int16_t timeout)
{
struct pollfd fds[1];
fds[0].fd = uart_fd;
fds[0].fd = _uart_fd;
fds[0].events = POLLIN;
uint8_t buf = 0;
......@@ -914,7 +848,7 @@ satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len, int16_t tim
while (1) {
if (::poll(&fds[0], 1, timeout) > 0) {
if (::read(uart_fd, &buf, 1) > 0) {
if (::read(_uart_fd, &buf, 1) > 0) {
if (rx_buf_pos == 0 && (buf == '\r' || buf == '\n')) {
// ignore the leading \r\n
continue;
......@@ -968,7 +902,7 @@ 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.store(true);
}
bool IridiumSBD::clear_mo_buffer()
......@@ -987,18 +921,18 @@ satcom_uart_status IridiumSBD::open_uart(char *uart_name)
{
VERBOSE_INFO("opening Iridium SBD modem UART: %s", uart_name);
uart_fd = ::open(uart_name, O_RDWR | O_BINARY);
_uart_fd = ::open(uart_name, O_RDWR | O_BINARY);
if (uart_fd < 0) {
if (_uart_fd < 0) {
VERBOSE_INFO("UART open failed!");
return SATCOM_UART_OPEN_FAIL;
}
// set the UART speed to 115200
struct termios uart_config;
tcgetattr(uart_fd, &uart_config);
tcgetattr(_uart_fd, &uart_config);
cfsetspeed(&uart_config, 115200);
tcsetattr(uart_fd, TCSANOW, &uart_config);
tcsetattr(_uart_fd, TCSANOW, &uart_config);
VERBOSE_INFO("UART opened");
......@@ -1116,39 +1050,64 @@ void IridiumSBD::publish_iridium_status()
int IridiumSBD::open_first(struct file *filep)
{
_cdev_used = true;
_cdev_used.store(true);
return CDev::open_first(filep);
}
int IridiumSBD::close_last(struct file *filep)
{
_cdev_used = false;
_cdev_used.store(false);
return CDev::close_last(filep);
}
int iridiumsbd_main(int argc, char *argv[])
int IridiumSBD::print_usage(const char *reason)
{
if (argc < 2) {
goto out_error;
if (reason) {
PX4_INFO("%s", reason);
}
if (!strcmp(argv[1], "start")) {
return IridiumSBD::start(argc, argv);
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
IridiumSBD driver.
Creates a virtual serial port that another module can use for communication (e.g. mavlink).
)DESCR_STR");
} else if (!strcmp(argv[1], "stop")) {
return IridiumSBD::stop();
PRINT_MODULE_USAGE_NAME("iridiumsbd", "driver");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Serial device", false);
PRINT_MODULE_USAGE_PARAM_FLAG('v', "Enable verbose output", true);
PRINT_MODULE_USAGE_COMMAND("test");
PRINT_MODULE_USAGE_ARG("s|read|AT <cmd>", "Test command", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 1;
}
} else if (!strcmp(argv[1], "status")) {
IridiumSBD::status();
return OK;
int IridiumSBD::custom_command(int argc, char *argv[])
{
if (!is_running()) {
print_usage("not running");
return 1;
}
} else if (!strcmp(argv[1], "test")) {
IridiumSBD::test(argc, argv);
return OK;
if (!strcmp(argv[0], "test")) {
get_instance()->test(argc, argv);
return 0;
}
out_error:
PX4_INFO("usage: iridiumsbd {start|stop|status|test} [-d uart_device]");
return print_usage("unknown command");
}
int iridiumsbd_main(int argc, char *argv[])
{
if (argc >= 2 && !strcmp(argv[1], "stop") && IridiumSBD::is_running()) {
if (!IridiumSBD::can_stop()) {
PX4_ERR("Device is used. Stop all users first (mavlink stop-all)");
return -1;
}
}
return PX4_ERROR;
return IridiumSBD::main(argc, argv);
}
......@@ -42,6 +42,9 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/iridiumsbd_status.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/module.h>
typedef enum {
SATCOM_OK = 0,
SATCOM_NO_MSG = -1,
......@@ -97,28 +100,29 @@ extern "C" __EXPORT int iridiumsbd_main(int argc, char *argv[]);
* - Improve TX buffer handling:
* - Do not reset the full TX buffer but delete the oldest HIGH_LATENCY2 message if one is in the buffer or delete the oldest message in general
*/
class IridiumSBD : public cdev::CDev
class IridiumSBD : public cdev::CDev, public ModuleBase<IridiumSBD>
{
public:
/*
* Constructor
*/
IridiumSBD();
~IridiumSBD();
/*
* Start the driver
*/
static int start(int argc, char *argv[]);
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/*
* Stop the driver
*/
static int stop();
/** @see ModuleBase */
static IridiumSBD *instantiate(int argc, char *argv[]);
/*
* Display driver status
*/
static void status();
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
/** @see ModuleBase::print_status() */
int print_status() override;
/*
* Run a driver test based on the input
......@@ -126,23 +130,17 @@ public:
* - `read`: Start a sbd read session
* - else: Is assumed to be a valid AT command and written to the modem
*/
static void test(int argc, char *argv[]);
void test(int argc, char *argv[]);
/*
* Passes everything to CDev
*/
int ioctl(struct file *filp, int cmd, unsigned long arg);
private:
/*
* Entry point of the task, has to be a static function
*/
static int main_loop_helper(int argc, char *argv[]);
static bool can_stop() { return !get_instance()->_cdev_used.load(); }
/*
* Main driver loop
*/
void main_loop(int argc, char *argv[]);
private:
int init(int argc, char *argv[]);
/*
* Loop executed while in SATCOM_STATE_STANDBY
......@@ -282,11 +280,7 @@ private:
*/
virtual int close_last(struct file *filep) override;
static IridiumSBD *instance;
static int task_handle;
bool _task_should_exit = false;
bool _start_completed = false;
int uart_fd = -1;
int _uart_fd = -1;
int32_t _param_read_interval_s = -1;