IridiumSBD.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   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
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include "IridiumSBD.h"

#include <px4_platform_common/tasks.h>

#include <errno.h>
#include <fcntl.h>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <pthread.h>

#include <systemlib/err.h>
#include <parameters/param.h>

static constexpr const char *satcom_state_string[4] = {"STANDBY", "SIGNAL CHECK", "SBD SESSION", "TEST"};

#define VERBOSE_INFO(...) if (_verbose) { PX4_INFO(__VA_ARGS__); }

#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[])
{
    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;
    }
}

int IridiumSBD::stop()
{
    if (IridiumSBD::instance == nullptr) {
        PX4_WARN("not started");
        return PX4_ERROR;
    }

    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;

    // 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);
    }

    // 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:               %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[])
{
    if (instance == nullptr) {
        PX4_WARN("not started");
        return;
    }

    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;

    } else {
        instance->_test_command[0] = 0;
    }

    instance->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;

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

///////////////////////////////////////////////////////////////////////
// private functions                                                 //
///////////////////////////////////////////////////////////////////////

void 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");
}

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_uart_name = 0;

    while (arg_i < argc) {
        if (!strcmp(argv[arg_i], "-d")) {
            arg_i++;
            arg_uart_name = arg_i;

        } else if (!strcmp(argv[arg_i], "-v")) {
            PX4_WARN("verbose mode ON");
            _verbose = true;
        }

        arg_i++;
    }

    if (arg_uart_name == 0) {
        PX4_ERR("no Iridium SBD modem UART port provided!");
        _task_should_exit = true;
        return;
    }


    bool command_executed = false;

    for (int counter = 0; (counter < 20) && !command_executed; counter++) {
        if (open_uart(argv[arg_uart_name]) == SATCOM_UART_OK) {
            command_executed = true;

        } else {
            usleep(100000);
        }
    }

    if (!command_executed) {
        PX4_ERR("failed to open UART port!");
        _task_should_exit = true;
        return;
    }

    // disable flow control
    command_executed = false;

    for (int counter = 0; (counter < 20) && !command_executed; counter++) {
        write_at("AT&K0");

        if (read_at_command() != SATCOM_RESULT_OK) {
            usleep(100000);

        } else {
            command_executed = true;
        }
    }

    if (!command_executed) {
        PX4_ERR("modem not responding");
        _task_should_exit = true;
        return;
    }

    // disable command echo
    command_executed = false;

    for (int counter = 0; (counter < 10) && !command_executed; counter++) {
        write_at("ATE0");

        if (read_at_command() != SATCOM_RESULT_OK) {
            usleep(100000);

        } else {
            command_executed = true;
        }
    }

    if (!command_executed) {
        PX4_ERR("modem not responding");
        _task_should_exit = true;
        return;
    }

    param_t param_pointer;

    param_pointer = param_find("ISBD_READ_INT");
    param_get(param_pointer, &_param_read_interval_s);

    param_pointer = param_find("ISBD_SBD_TIMEOUT");
    param_get(param_pointer, &_param_session_timeout_s);

    if (_param_session_timeout_s < 0) {
        _param_session_timeout_s = 60;
    }

    param_pointer = param_find("ISBD_STACK_TIME");
    param_get(param_pointer, &_param_stacking_time_ms);

    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);

    _start_completed = true;

    while (!_task_should_exit) {
        switch (_state) {
        case SATCOM_STATE_STANDBY:
            standby_loop();
            break;

        case SATCOM_STATE_CSQ:
            csq_loop();
            break;

        case SATCOM_STATE_SBDSESSION:
            sbdsession_loop();
            break;

        case SATCOM_STATE_TEST:
            test_loop();
            break;
        }

        publish_subsystem_status();

        if (_new_state != _state) {
            VERBOSE_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[_state], satcom_state_string[_new_state]);
            _state = _new_state;
            publish_iridium_status();

        } else {
            publish_iridium_status();
            usleep(100000); // 100ms
        }
    }
}

void IridiumSBD::standby_loop(void)
{
    if (_test_pending) {
        _test_pending = false;

        if (!strcmp(_test_command, "s")) {
            write(0, "kreczmer", 8);

        } else if (!strcmp(_test_command, "read")) {
            _rx_session_pending = true;

        } else {
            _test_timer = hrt_absolute_time();
            start_test();
            return;
        }
    }

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

    // 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)) {
        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) {
            // 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();
                    return;
                }

            } else {
                start_sbd_session();
                return;
            }

        } else {
            start_csq();
            return;
        }
    }

    // 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();
        return;
    }

    // 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) && (_new_state == SATCOM_STATE_STANDBY)) {
        read_rx_buf();
        return;
    }
}

void IridiumSBD::csq_loop(void)
{
    int res = read_at_command();

    if (res == SATCOM_RESULT_NA) {
        return;
    }

    if (res != SATCOM_RESULT_OK) {
        VERBOSE_INFO("UPDATE SIGNAL QUALITY: ERROR");

        _new_state = SATCOM_STATE_STANDBY;
        return;
    }

    if (strncmp((const char *)_rx_command_buf, "+CSQ:", 5)) {
        VERBOSE_INFO("UPDATE SIGNAL QUALITY: WRONG ANSWER:");
        VERBOSE_INFO("%s", _rx_command_buf);

        _new_state = SATCOM_STATE_STANDBY;
        return;
    }

    _signal_quality = _rx_command_buf[5] - 48;

    VERBOSE_INFO("SIGNAL QUALITY: %d", _signal_quality);

    _new_state = SATCOM_STATE_STANDBY;
}

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)) {

            PX4_WARN("SBD SESSION: TIMEOUT!");
            ++_failed_sbd_sessions;
            _new_state = SATCOM_STATE_STANDBY;
            pthread_mutex_unlock(&_tx_buf_mutex);
        }

        return;
    }

    if (res != SATCOM_RESULT_OK) {
        VERBOSE_INFO("SBD SESSION: ERROR. RESULT: %d", res);

        ++_failed_sbd_sessions;
        _new_state = SATCOM_STATE_STANDBY;
        pthread_mutex_unlock(&_tx_buf_mutex);
        return;
    }

    if (strncmp((const char *)_rx_command_buf, "+SBDIX:", 7)) {

        VERBOSE_INFO("SBD SESSION: WRONG ANSWER: %s", _rx_command_buf);

        _new_state = SATCOM_STATE_STANDBY;
        ++_failed_sbd_sessions;
        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;
    char **rx_buf_parse = (char **)&p;

    mo_status = strtol(*rx_buf_parse, rx_buf_parse, 10);
    (*rx_buf_parse)++;
    strtol(*rx_buf_parse, rx_buf_parse, 10); // MOMSN, ignore it
    (*rx_buf_parse)++;
    mt_status = strtol(*rx_buf_parse, rx_buf_parse, 10);
    (*rx_buf_parse)++;
    strtol(*rx_buf_parse, rx_buf_parse, 10); // MTMSN, ignore it
    (*rx_buf_parse)++;
    mt_len = strtol(*rx_buf_parse, rx_buf_parse, 10);
    (*rx_buf_parse)++;
    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);

    switch (mo_status) {
    case 0:
    case 2:
    case 3:
    case 4:
        VERBOSE_INFO("SBD SESSION: SUCCESS (%d)", mo_status);

        _ring_pending = false;
        _tx_session_pending = false;
        _last_read_time = hrt_absolute_time();
        _last_heartbeat = _last_read_time;
        ++_successful_sbd_sessions;

        if (mt_queued > 0) {
            _rx_session_pending = true;

        } else {
            _rx_session_pending = false;

        }

        if (mt_len > 0) {
            _rx_read_pending = true;
        }

        // after a successful session reset the tx buffer
        _tx_buf_write_idx = 0;
        break;

    case 1:
        VERBOSE_INFO("SBD SESSION: MO SUCCESS, MT FAIL");
        _last_heartbeat = hrt_absolute_time();

        // after a successful session reset the tx buffer
        _tx_buf_write_idx = 0;
        ++_successful_sbd_sessions;

        _tx_session_pending = false;
        break;

    case 32:
        VERBOSE_INFO("SBD SESSION: NO NETWORK SIGNAL");

        ++_failed_sbd_sessions;
        _signal_quality = 0;
        break;

    default:
        ++_failed_sbd_sessions;
        VERBOSE_INFO("SBD SESSION: FAILED (%d)", mo_status);
    }

    _new_state = SATCOM_STATE_STANDBY;
    pthread_mutex_unlock(&_tx_buf_mutex);
}

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

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

void IridiumSBD::start_csq(void)
{
    if ((_state != SATCOM_STATE_STANDBY) || (_new_state != SATCOM_STATE_STANDBY)) {
        VERBOSE_INFO("CANNOT ENTER CSQ STATE");
        return;

    } else {
        VERBOSE_INFO("UPDATING SIGNAL QUALITY");
    }

    _last_signal_check = hrt_absolute_time();

    if (!is_modem_ready()) {
        VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!");
        return;
    }

    write_at("AT+CSQ");
    _new_state = SATCOM_STATE_CSQ;
}

void IridiumSBD::start_sbd_session(void)
{
    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) {
        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();
}

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);

        for (int i = 0; i < _rx_command_len; i++) {
            printf("%d ", _rx_command_buf[i]);
        }

        printf("\n");
    }

    if (!is_modem_ready()) {
        PX4_WARN("MODEM NOT READY!");
        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;

        } else {
            PX4_WARN("The test command does not include AT or at: %s, ignoring it.", _test_command);
            _new_state = SATCOM_STATE_STANDBY;
        }

    } else {
        PX4_INFO("TEST DONE");
    }
}

ssize_t IridiumSBD::write(struct file *filp, const char *buffer, size_t buflen)
{
    // general check if the incoming message would be too large (the buffer should not reset in that case)
    if ((ssize_t)buflen > SATCOM_TX_BUF_LEN) {
        return PX4_ERROR;
    }

    pthread_mutex_lock(&_tx_buf_mutex);

    // parsing the size of the message to write
    if (!_writing_mavlink_packet) {
        if (buflen < 3) {
            _packet_length = buflen;

        } else if ((unsigned char)buffer[0] == 253 && (buflen == 10)) { // mavlink 2
            const uint8_t payload_len = buffer[1];
            const uint8_t incompat_flags = buffer[2];
            _packet_length = payload_len + 12;
            _writing_mavlink_packet = true;

            if (incompat_flags & 0x1) { //signing
                _packet_length += 13;
            }

        } else if ((unsigned char)buffer[0] == 254 && (buflen == 6)) { // mavlink 1
            const uint8_t payload_len = buffer[1];
            _packet_length = payload_len + 8;
            _writing_mavlink_packet = true;

        } else {
            _packet_length = buflen;
        }
    }

    // check if there is enough space to write the message
    if (SATCOM_TX_BUF_LEN - _tx_buf_write_idx - _packet_length < 0) {
        _tx_buf_write_idx = 0;
        ++_num_tx_buf_reset;
    }

    // keep track of the remaining packet length and if the full message is written
    _packet_length -= buflen;

    if (_packet_length == 0) {
        _writing_mavlink_packet = false;
    }

    VERBOSE_INFO("WRITE: LEN %d, TX WRITTEN: %d", buflen, _tx_buf_write_idx);

    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;

    pthread_mutex_unlock(&_tx_buf_mutex);

    return buflen;
}

ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen)
{
    pthread_mutex_lock(&_rx_buf_mutex);
    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 (bytes_to_copy > buflen) {
            bytes_to_copy = buflen;
        }

        memcpy(buffer, &_rx_msg_buf[_rx_msg_read_idx], bytes_to_copy);

        _rx_msg_read_idx += bytes_to_copy;

        pthread_mutex_unlock(&_rx_buf_mutex);
        return bytes_to_copy;

    } else {
        pthread_mutex_unlock(&_rx_buf_mutex);
        return -EAGAIN;
    }
}

void IridiumSBD::write_tx_buf()
{
    if (!is_modem_ready()) {
        VERBOSE_INFO("WRITE SBD: MODEM NOT READY!");
        return;
    }

    pthread_mutex_lock(&_tx_buf_mutex);

    char command[13];
    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);
        return;
    }

    int sum = 0;

    int written = 0;

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

    uint8_t checksum[2] = {(uint8_t)(sum / 256), (uint8_t)(sum & 255)};
    ::write(uart_fd, checksum, 2);


    VERBOSE_INFO("SEND SBD: CHECKSUM %d %d", checksum[0], checksum[1]);

    if (read_at_command(250) != SATCOM_RESULT_OK) {
        VERBOSE_INFO("WRITE SBD: ERROR WHILE WRITING DATA TO MODEM!");

        pthread_mutex_unlock(&_tx_buf_mutex);
        return;
    }

    if (_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);
        return;
    }

    VERBOSE_INFO("WRITE SBD: DATA WRITTEN TO MODEM");

    _tx_buf_write_pending = false;

    pthread_mutex_unlock(&_tx_buf_mutex);

    _tx_session_pending = true;
}

void IridiumSBD::read_rx_buf(void)
{
    if (!is_modem_ready()) {
        VERBOSE_INFO("READ SBD: MODEM NOT READY!");
        return;
    }

    pthread_mutex_lock(&_rx_buf_mutex);


    write_at("AT+SBDRB");

    if (read_at_msg() != SATCOM_RESULT_OK) {
        VERBOSE_INFO("READ SBD: MODEM NOT RESPONDING!");
        _rx_msg_read_idx = _rx_msg_end_idx;
        pthread_mutex_unlock(&_rx_buf_mutex);
        return;
    }

    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) {
        PX4_ERR("READ SBD: WRONG DATA LENGTH");
        _rx_msg_read_idx = _rx_msg_end_idx;
        pthread_mutex_unlock(&_rx_buf_mutex);
        return;
    }

    int checksum = 0;

    for (int i = 2; i < data_len + 2; 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])) {
        PX4_ERR("READ SBD: WRONG DATA CHECKSUM");
        _rx_msg_read_idx = _rx_msg_end_idx;
        pthread_mutex_unlock(&_rx_buf_mutex);
        return;
    }

    _rx_msg_read_idx = 2;   // ignore the length
    _rx_msg_end_idx -= 4;   // ignore the checksum and delimiter
    _rx_read_pending = false;

    pthread_mutex_unlock(&_rx_buf_mutex);
    VERBOSE_INFO("READ SBD: SUCCESS, LEN: %d", data_len);
}

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

satcom_result_code IridiumSBD::read_at_command(int16_t 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);
}

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].events = POLLIN;

    uint8_t buf = 0;
    int last_rn_idx = 0;
    int rx_buf_pos = 0;
    *rx_len = 0;

    while (1) {
        if (::poll(&fds[0], 1, timeout) > 0) {
            if (::read(uart_fd, &buf, 1) > 0) {
                if (rx_buf_pos == 0 && (buf == '\r' || buf == '\n')) {
                    // ignore the leading \r\n
                    continue;
                }

                rx_buf[rx_buf_pos++] = buf;

                if (rx_buf[rx_buf_pos - 1] == '\n' && rx_buf[rx_buf_pos - 2] == '\r') {
                    // found the \r\n delimiter
                    if (rx_buf_pos == last_rn_idx + 2) {
                        //if (verbose) { PX4_INFO("second in a row, ignore it");}
                        ; // second in a row, ignore it

                    } else if (!strncmp((const char *)&rx_buf[last_rn_idx], "OK\r\n", 4)) {
                        rx_buf[*rx_len] = 0;    // null terminator after the information response for printing purposes
                        return SATCOM_RESULT_OK;

                    } else if (!strncmp((const char *)&rx_buf[last_rn_idx], "ERROR\r\n", 7)) {
                        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;

                        VERBOSE_INFO("GET SBDRING");

                        return SATCOM_RESULT_SBDRING;

                    } else if (!strncmp((const char *)&rx_buf[last_rn_idx], "READY\r\n", 7)) {
                        return SATCOM_RESULT_READY;

                    } else if (!strncmp((const char *)&rx_buf[last_rn_idx], "HARDWARE FAILURE", 16)) {
                        PX4_WARN("HARDWARE FAILURE!");
                        return SATCOM_RESULT_HWFAIL;

                    } else {
                        *rx_len = rx_buf_pos;   // that was the information response, result code incoming
                    }

                    last_rn_idx = rx_buf_pos;
                }
            }

        } else {
            break;
        }
    }

    return SATCOM_RESULT_NA;
}

void IridiumSBD::schedule_test(void)
{
    _test_pending = true;
}

bool IridiumSBD::clear_mo_buffer()
{
    write_at("AT+SBDD0");

    if (read_at_command() != SATCOM_RESULT_OK || _rx_command_buf[0] != '0') {
        VERBOSE_INFO("CLEAR MO BUFFER: ERROR");
        return false;
    }

    return true;
}

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);

    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);
    cfsetspeed(&uart_config, 115200);
    tcsetattr(uart_fd, TCSANOW, &uart_config);

    VERBOSE_INFO("UART opened");

    return SATCOM_UART_OK;
}

bool IridiumSBD::is_modem_ready(void)
{
    write_at("AT");

    if (read_at_command() == SATCOM_RESULT_OK) {
        return true;

    } else {
        return false;
    }
}

pollevent_t IridiumSBD::poll_state(struct file *filp)
{
    pollevent_t pollstate = 0;

    if (_rx_msg_read_idx < _rx_msg_end_idx) {
        pollstate |= POLLIN;
    }

    if (SATCOM_TX_BUF_LEN - _tx_buf_write_idx > 0) {
        pollstate |= POLLOUT;
    }

    return pollstate;
}

void IridiumSBD::publish_iridium_status()
{
    bool need_to_publish = false;

    if (_status.last_heartbeat != _last_heartbeat) {
        need_to_publish = true;
        _status.last_heartbeat = _last_heartbeat;
    }

    if (_status.tx_buf_write_index != _tx_buf_write_idx) {
        need_to_publish = true;
        _status.tx_buf_write_index = _tx_buf_write_idx;
    }

    if (_status.rx_buf_read_index != _rx_msg_read_idx) {
        need_to_publish = true;
        _status.rx_buf_read_index = _rx_msg_read_idx;
    }

    if (_status.rx_buf_end_index != _rx_msg_end_idx) {
        need_to_publish = true;
        _status.rx_buf_end_index = _rx_msg_end_idx;
    }

    if (_status.failed_sbd_sessions != _failed_sbd_sessions) {
        need_to_publish = true;
        _status.failed_sbd_sessions = _failed_sbd_sessions;
    }

    if (_status.successful_sbd_sessions != _successful_sbd_sessions) {
        need_to_publish = true;
        _status.successful_sbd_sessions = _successful_sbd_sessions;
    }

    if (_status.num_tx_buf_reset != _num_tx_buf_reset) {
        need_to_publish = true;
        _status.num_tx_buf_reset = _num_tx_buf_reset;
    }

    if (_status.signal_quality != _signal_quality) {
        need_to_publish = true;
        _status.signal_quality = _signal_quality;
    }

    if (_status.state != _state) {
        need_to_publish = true;
        _status.state = _state;
    }

    if (_status.ring_pending != _ring_pending) {
        need_to_publish = true;
        _status.ring_pending = _ring_pending;
    }

    if (_status.tx_buf_write_pending != _tx_buf_write_pending) {
        need_to_publish = true;
        _status.tx_buf_write_pending = _tx_buf_write_pending;
    }

    if (_status.tx_session_pending != _tx_session_pending) {
        need_to_publish = true;
        _status.tx_session_pending = _tx_session_pending;
    }

    if (_status.rx_read_pending != _rx_read_pending) {
        need_to_publish = true;
        _status.rx_read_pending = _rx_read_pending;
    }

    if (_status.rx_session_pending != _rx_session_pending) {
        need_to_publish = true;
        _status.rx_session_pending = _rx_session_pending;
    }

    _status.timestamp = hrt_absolute_time();

    // publish the status if it changed
    if (need_to_publish) {
        _iridiumsbd_status_pub.publish(_status);
    }
}

void IridiumSBD::publish_subsystem_status()
{
    const bool present = true;
    const bool enabled = true;
    const bool ok = _status.last_heartbeat > 0; // maybe at some point here an additional check should be made

    if ((_info.present != present) || (_info.enabled != enabled) || (_info.ok != ok)) {
        _info.timestamp = hrt_absolute_time();
        _info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_SATCOM;
        _info.present = present;
        _info.enabled = enabled;
        _info.ok = ok;

        _subsystem_pub.publish(_info);
    }
}


int IridiumSBD::open_first(struct file *filep)
{
    _cdev_used = true;
    return CDev::open_first(filep);
}

int IridiumSBD::close_last(struct file *filep)
{
    _cdev_used = false;
    return CDev::close_last(filep);
}

int iridiumsbd_main(int argc, char *argv[])
{
    if (argc < 2) {
        goto out_error;
    }

    if (!strcmp(argv[1], "start")) {
        return IridiumSBD::start(argc, argv);

    } else if (!strcmp(argv[1], "stop")) {
        return IridiumSBD::stop();

    } else if (!strcmp(argv[1], "status")) {
        IridiumSBD::status();
        return OK;

    } else if (!strcmp(argv[1], "test")) {
        IridiumSBD::test(argc, argv);
        return OK;
    }

out_error:
    PX4_INFO("usage: iridiumsbd {start|stop|status|test} [-d uart_device]");

    return PX4_ERROR;
}