From bce7ecb0f611f554306a0374bb4d03b5fc0d9bb5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 18 Nov 2016 14:59:07 -0500 Subject: [PATCH] Iridium driver and support Mavlink module implement HIGH_LATENCY (Iridium) --- ROMFS/px4fmu_common/init.d/rcS | 5 + cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v3_default.cmake | 1 + cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + msg/telemetry_status.msg | 1 + src/drivers/drv_iridiumsbd.h | 8 + src/drivers/iridiumsbd/CMakeLists.txt | 45 + src/drivers/iridiumsbd/IridiumSBD.cpp | 863 ++++++++++++++++++++ src/drivers/iridiumsbd/IridiumSBD.h | 283 +++++++ src/drivers/iridiumsbd/iridiumsbd_params.c | 11 + src/modules/mavlink/mavlink_main.cpp | 27 +- src/modules/mavlink/mavlink_main.h | 14 +- src/modules/mavlink/mavlink_messages.cpp | 205 +++++ src/modules/systemlib/system_params.c | 1 + 14 files changed, 1450 insertions(+), 16 deletions(-) create mode 100644 src/drivers/drv_iridiumsbd.h create mode 100644 src/drivers/iridiumsbd/CMakeLists.txt create mode 100644 src/drivers/iridiumsbd/IridiumSBD.cpp create mode 100644 src/drivers/iridiumsbd/IridiumSBD.h create mode 100644 src/drivers/iridiumsbd/iridiumsbd_params.c diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 726b198b22..bfa903f465 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -600,6 +600,11 @@ then then mavlink start -d $MAVLINK_COMPANION_DEVICE -b 57600 -r 1000 fi + if param compare SYS_COMPANION 419200 + then + iridiumsbd start -d /dev/ttyS2 + mavlink start -d /dev/iridium -b 19200 -m iridium -r 10 + fi if param compare SYS_COMPANION 1921600 then mavlink start -d $MAVLINK_COMPANION_DEVICE -b 921600 -r 20000 diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index e9e20a50d9..c3df2d9113 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -48,6 +48,7 @@ set(config_module_list drivers/bst #drivers/snapdragon_rc_pwm drivers/lis3mdl + drivers/iridiumsbd # # System commands diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake index a02f8bacd0..9836dee539 100644 --- a/cmake/configs/nuttx_px4fmu-v3_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake @@ -23,6 +23,7 @@ set(config_module_list drivers/hott drivers/hott/hott_sensors drivers/hott/hott_telemetry + drivers/iridiumsbd drivers/l3gd20 drivers/led drivers/lis3mdl diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index faf19aa8ff..65bb456ad4 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -50,6 +50,7 @@ set(config_module_list drivers/bma180 drivers/bmi160 drivers/tap_esc + drivers/iridiumsbd # # System commands diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index e7c653c6f5..7232102410 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -3,6 +3,7 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1 uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2 uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3 uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4 +uint8 TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM = 5 uint64 heartbeat_time # Time of last received heartbeat from remote system uint64 telem_time # Time of last received telemetry status packet, 0 for none diff --git a/src/drivers/drv_iridiumsbd.h b/src/drivers/drv_iridiumsbd.h new file mode 100644 index 0000000000..95c7e11d03 --- /dev/null +++ b/src/drivers/drv_iridiumsbd.h @@ -0,0 +1,8 @@ +#pragma once + +#include +#include + +#include "board_config.h" + +#define IRIDIUMSBD_DEVICE_PATH "/dev/iridium" diff --git a/src/drivers/iridiumsbd/CMakeLists.txt b/src/drivers/iridiumsbd/CMakeLists.txt new file mode 100644 index 0000000000..25dba0502b --- /dev/null +++ b/src/drivers/iridiumsbd/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ +px4_add_module( + MODULE drivers__iridiumsbd + MAIN iridiumsbd + STACK 1024 + STACK_MAIN 1024 + COMPILE_FLAGS + -Os + SRCS + IridiumSBD.cpp + iridiumsbd_params.c + DEPENDS + platforms__common + ) diff --git a/src/drivers/iridiumsbd/IridiumSBD.cpp b/src/drivers/iridiumsbd/IridiumSBD.cpp new file mode 100644 index 0000000000..3748bce327 --- /dev/null +++ b/src/drivers/iridiumsbd/IridiumSBD.cpp @@ -0,0 +1,863 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "drivers/drv_iridiumsbd.h" + +IridiumSBD *IridiumSBD::instance; +int IridiumSBD::task_handle; + +IridiumSBD::IridiumSBD() + : CDev("iridiumsbd", IRIDIUMSBD_DEVICE_PATH) +{ +} + +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, 1024, (main_t)&IridiumSBD::main_loop_helper, argv); + + return OK; +} + +int IridiumSBD::stop() +{ + if (IridiumSBD::instance == nullptr) { + PX4_WARN("not started"); + 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); +} + +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) { + strcpy(instance->test_command, argv[2]); + + } else { + instance->test_command[0] = 0; + } + + instance->schedule_test(); +} + +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); + + 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_WARN("no iridium sbd modem UART port provided!"); + 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; + return; + } + + // disable flow control + write_at("AT&K0"); + + if (read_at_command() != SATCOM_RESULT_OK) { + PX4_WARN("modem not responding"); + return; + } + + // disable command echo + write_at("ATE0"); + + if (read_at_command() != SATCOM_RESULT_OK) { + PX4_WARN("modem not responding"); + return; + } + + param_t param_pointer; + + param_pointer = param_find("ISBD_READINT"); + param_get(param_pointer, ¶m_read_interval_s); + + if (param_read_interval_s == -1) { + param_read_interval_s = 10; + } + + if (verbose) { PX4_INFO("read interval %d", param_read_interval_s); } + + 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; + } + + if (new_state != state) { + if (verbose) { PX4_INFO("SWITCHING STATE FROM %s TO %s", satcom_state_string[state], satcom_state_string[new_state]); } + + state = new_state; + + } else { + usleep(100000); // 100ms + } + } +} + +void IridiumSBD::standby_loop(void) +{ + // TODO probably remove this later + if (test_pending) { + test_pending = false; + + if (!strcmp(test_command, "s")) { + write(0, "kreczmer", 8); + + } else if (!strcmp(test_command, "ss")) { + write(0, "kreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmerkreczmer!!!!!!", 100); + + } 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) / 1000000 > param_read_interval_s) { + 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(); + } + + // 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_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(); + } +} + +void IridiumSBD::csq_loop(void) +{ + int res = read_at_command(); + + if (res == SATCOM_RESULT_NA) { + return; + } + + if (res != SATCOM_RESULT_OK) { + if (verbose) { PX4_INFO("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); } + + new_state = SATCOM_STATE_STANDBY; + return; + } + + signal_quality = rx_command_buf[5] - 48; + //signal_check_pending = false; + last_signal_check = hrt_absolute_time(); + + if (verbose) { PX4_INFO("SIGNAL QUALITY: %d", signal_quality); } + + new_state = SATCOM_STATE_STANDBY; + + // publish telemetry status for logger + struct telemetry_status_s tstatus = {}; + + 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; + + if (telemetry_status_pub == nullptr) { + int multi_instance; + 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); + } +} + +void IridiumSBD::sbdsession_loop(void) +{ + int res = read_at_command(); + + if (res == SATCOM_RESULT_NA) { + return; + } + + if (res != SATCOM_RESULT_OK) { + if (verbose) { PX4_INFO("SBD SESSION: ERROR"); } + + if (verbose) { PX4_INFO("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); } + + new_state = SATCOM_STATE_STANDBY; + 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); + + if (verbose) { PX4_INFO("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"); } + + ring_pending = false; + rx_session_pending = false; + tx_session_pending = false; + last_read_time = hrt_absolute_time(); + + if (mt_len > 0) { + rx_read_pending = true; + } + + break; + + case 1: + if (verbose) { PX4_INFO("SBD SESSION: MO SUCCESS, MT FAIL"); } + + tx_session_pending = false; + break; + + case 32: + if (verbose) { PX4_INFO("SBD SESSION: NO NETWORK SIGNAL"); } + + signal_quality = 0; + + break; + + default: + if (verbose) { PX4_INFO("SBD SESSION: FAILED (%d)", mo_status); } + } + + new_state = SATCOM_STATE_STANDBY; +} + +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; + } +} + +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); } + + if (buflen > SATCOM_TX_BUF_LEN - tx_buf_write_idx) { + return PX4_ERROR; + } + + pthread_mutex_lock(&tx_buf_mutex); + + memcpy(tx_buf + tx_buf_write_idx, buffer, buflen); + + tx_buf_write_idx += buflen; + last_write_time = hrt_absolute_time(); + + pthread_mutex_unlock(&tx_buf_mutex); + + return 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); } + + 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; + + return bytes_to_copy; + + } else { + return -EAGAIN; + } +} + +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 FIONWRITE: { + int count = SATCOM_TX_BUF_LEN - tx_buf_write_idx; + *(int *)arg = count; + + return OK; + } + + default: { + + /* see if the parent class can make any use of it */ +#ifdef __PX4_NUTTX + return CDev::ioctl(filp, cmd, arg); +#else + return VDev::ioctl(filp, cmd, arg); +#endif + } + } +} + +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::write_tx_buf() +{ + if (!is_modem_ready()) { + if (verbose) { PX4_INFO("SEND 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) { + if (verbose) { PX4_INFO("SEND SBD: MODEM NOT RESPONDING!"); } + + 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); + + if (verbose) { PX4_INFO("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!"); } + + 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'); } + + pthread_mutex_unlock(&tx_buf_mutex); + return; + } + + if (verbose) { PX4_INFO("SEND SBD: DATA WRITTEN TO MODEM"); } + + tx_buf_write_idx = 0; + + pthread_mutex_unlock(&tx_buf_mutex); + + tx_session_pending = true; +} + +void IridiumSBD::read_rx_buf(void) +{ + if (!is_modem_ready()) { + if (verbose) { PX4_INFO("READ SBD: MODEM NOT READY!"); } + + return; + } + + write_at("AT+SBDRB"); + + if (read_at_msg() != SATCOM_RESULT_OK) { + if (verbose) { PX4_INFO("READ SBD: MODEM NOT RESPONDING!"); } + + 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) { + if (verbose) { PX4_INFO("READ SBD: WRONG DATA LENGTH"); } + + 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])) { + if (verbose) { PX4_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; + + if (verbose) { PX4_INFO("READ SBD: SUCCESS, LEN: %d", data_len); } +} + +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"); } + + return false; + } + + return true; +} + +void IridiumSBD::start_csq(void) +{ + if (verbose) { PX4_INFO("UPDATING SIGNAL QUALITY"); } + + if (!is_modem_ready()) { + if (verbose) { PX4_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!"); } + + return; + } + + write_at("AT+CSQ"); + new_state = SATCOM_STATE_CSQ; +} + +void IridiumSBD::start_sbd_session(void) +{ + if (verbose) { PX4_INFO("STARTING SBD SESSION"); } + + if (!is_modem_ready()) { + if (verbose) { PX4_INFO("SBD SESSION: MODEM NOT READY!"); } + + return; + } + + if (ring_pending) { + write_at("AT+SBDIXA"); + + } else { + write_at("AT+SBDIX"); + } + + new_state = SATCOM_STATE_SBDSESSION; +} + +void IridiumSBD::start_test(void) +{ + 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) { + PX4_INFO("TEST %s", test_command); + write_at(test_command); + new_state = SATCOM_STATE_TEST; + + } else { + PX4_INFO("TEST DONE"); + } +} + +satcom_uart_status IridiumSBD::open_uart(char *uart_name) +{ + if (verbose) { PX4_INFO("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!"); } + + return SATCOM_UART_OPEN_FAIL; + } + + // set the UART speed to 19200 + struct termios uart_config; + tcgetattr(uart_fd, &uart_config); + cfsetspeed(&uart_config, 19200); + tcsetattr(uart_fd, TCSANOW, &uart_config); + + if (verbose) { PX4_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; + } +} + +void IridiumSBD::write_at(const char *command) +{ + if (verbose) { PX4_INFO("WRITING AT COMMAND: %s", command); } + + ::write(uart_fd, command, strlen(command)); + ::write(uart_fd, "\r", 1); +} + +satcom_result_code IridiumSBD::read_at_command() +{ + return read_at(rx_command_buf, &rx_command_len); +} + +satcom_result_code IridiumSBD::read_at_msg() +{ + return read_at(rx_msg_buf, &rx_msg_end_idx); +} + +satcom_result_code IridiumSBD::read_at(uint8_t *rx_buf, int *rx_len) +{ + 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, 100) > 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; + + if (verbose) { PX4_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; +} + +int iridiumsbd_main(int argc, char *argv[]) +{ + 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; + } + + PX4_INFO("usage: iridiumsbd {start|stop|status|test} [-d uart_device | -v]"); + + return PX4_ERROR; +} diff --git a/src/drivers/iridiumsbd/IridiumSBD.h b/src/drivers/iridiumsbd/IridiumSBD.h new file mode 100644 index 0000000000..809280c46b --- /dev/null +++ b/src/drivers/iridiumsbd/IridiumSBD.h @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include +#include + +#include +#include + +typedef enum { + SATCOM_OK = 0, + SATCOM_NO_MSG = -1, + SATCOM_ERROR = -255, +} satcom_status; + +typedef enum { + SATCOM_UART_OK = 0, + SATCOM_UART_OPEN_FAIL = -1, +} satcom_uart_status; + +typedef enum { + SATCOM_READ_OK = 0, + SATCOM_READ_TIMEOUT = -1, + SATCOM_READ_PARSING_FAIL = -2, +} satcom_read_status; + +typedef enum { + SATCOM_RESULT_OK, + SATCOM_RESULT_ERROR, + SATCOM_RESULT_SBDRING, + SATCOM_RESULT_READY, + SATCOM_RESULT_HWFAIL, + SATCOM_RESULT_NA, +} satcom_result_code; + +//typedef struct +//{ +// uint8_t info; +// uint8_t result_code; +//} satcom_at_msg; + +typedef enum { + SATCOM_STATE_STANDBY, + SATCOM_STATE_CSQ, + SATCOM_STATE_SBDSESSION, + SATCOM_STATE_TEST, +} satcom_state; + +const char *satcom_state_string[4] = {"STANDBY", "SIGNAL CHECK", "SBD SESSION", "TEST"}; + +extern "C" __EXPORT int iridiumsbd_main(int argc, char *argv[]); + +#define SATCOM_TX_BUF_LEN 50 // TX buffer size - maximum for a SBD MO message is 340, but billed per 50 +#define SATCOM_RX_MSG_BUF_LEN 300 // RX buffer size for MT messages +#define SATCOM_RX_COMMAND_BUF_LEN 50 // RX buffer size for other commands +#define SATCOM_TX_STACKING_TIME 3000000 // time to wait for additional mavlink messages, TODO make this a param +#define SATCOM_SIGNAL_REFRESH_DELAY 5000000 // update signal quality every 5s + +class IridiumSBD : public device::CDev +{ +public: + static IridiumSBD *instance; + static int task_handle; + bool task_should_exit = false; + int uart_fd = -1; + + int param_read_interval_s; + + hrt_abstime last_signal_check = 0; + uint8_t signal_quality = 0; + + orb_advert_t telemetry_status_pub = nullptr; + + 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_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; + + bool ring_pending = false; + bool rx_session_pending = false; + bool rx_read_pending = false; + bool tx_session_pending = false; + + hrt_abstime last_write_time = 0; + hrt_abstime last_read_time = 0; + + 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; + + /* + * Constructor + */ + IridiumSBD(); + + /* + * Start the driver + */ + static int start(int argc, char *argv[]); + + /* + * Stop the driver + */ + static int stop(); + + /* + * Display driver status + */ + static void status(); + + /* + * Run a basic driver test + */ + static void test(int argc, char *argv[]); + + /* + * Entry point of the task, has to be a static function + */ + static void main_loop_helper(int argc, char *argv[]); + + /* + * Main driver loop + */ + void main_loop(int argc, char *argv[]); + + /* + * Use to send mavlink messages directly + */ + ssize_t write(struct file *filp, const char *buffer, size_t buflen); + + /* + * Use to read received mavlink messages directly + */ + ssize_t read(struct file *filp, char *buffer, size_t buflen); + + /* + * Passes everything to CDev + */ + int ioctl(struct file *filp, int cmd, unsigned long arg); + + /* + * Get the poll state + */ + pollevent_t poll_state(struct file *filp); + + /* + * Open and configure the given UART port + */ + satcom_uart_status open_uart(char *uart_name); + + /* + * + */ + void write_tx_buf(); + + /* + * + */ + void read_rx_buf(); + + /* + * + */ + bool clear_mo_buffer(); + + /* + * Perform a SBD session, sending the message from the MO buffer (if previously written) + * and retrieving a MT message from the Iridium system (if there is one waiting) + * This will also update the registration needed for SBD RING + */ + int sbd_session(void); + + /* + * Get the network signal strength + */ + void start_csq(void); + + /* + * + */ + satcom_result_code read_at_command(); + + /* + * + */ + satcom_result_code read_at_msg(); + + /* + * + */ + satcom_result_code read_at(uint8_t *rx_buf, int *rx_len); + + /* + * + */ + void schedule_test(void); + + /* + * + */ + void standby_loop(void); + + /* + * + */ + void csq_loop(void); + + /* + * + */ + void sbdsession_loop(void); + + /* + * + */ + void test_loop(void); + + /* + * TEST + */ + void start_test(void); + + /* + * + */ + void start_sbd_session(void); + + /* + * Checks if the modem responds to the "AT" command + */ + bool is_modem_ready(void); + + /* + * Send a AT command to the modem + */ + void write_at(const char *command); +}; diff --git a/src/drivers/iridiumsbd/iridiumsbd_params.c b/src/drivers/iridiumsbd/iridiumsbd_params.c new file mode 100644 index 0000000000..5fd05b9705 --- /dev/null +++ b/src/drivers/iridiumsbd/iridiumsbd_params.c @@ -0,0 +1,11 @@ +#include + +/** + * Satellite radio read interval + * + * @unit s + * @min 0 + * @max 300 + * @group Iridium SBD + */ +PARAM_DEFINE_INT32(ISBD_READINT, 10); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a39d2a780..9b2b14c2b6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1818,6 +1818,10 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "config") == 0) { _mode = MAVLINK_MODE_CONFIG; + + } else if (strcmp(myoptarg, "iridium") == 0) { + _mode = MAVLINK_MODE_IRIDIUM; + _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM; } break; @@ -1935,14 +1939,18 @@ Mavlink::task_main(int argc, char *argv[]) /* add default streams depending on mode */ - /* HEARTBEAT is constant rate stream, rate never adjusted */ - configure_stream("HEARTBEAT", 1.0f); + if (_mode != MAVLINK_MODE_IRIDIUM) { - /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ - configure_stream("STATUSTEXT", 20.0f); + /* HEARTBEAT is constant rate stream, rate never adjusted */ + configure_stream("HEARTBEAT", 1.0f); - /* COMMAND_LONG stream: use high rate to avoid commands skipping */ - configure_stream("COMMAND_LONG", 100.0f); + /* STATUSTEXT stream is like normal stream but gets messages from logbuffer instead of uORB */ + configure_stream("STATUSTEXT", 20.0f); + + /* COMMAND_LONG stream: use high rate to avoid commands skipping */ + configure_stream("COMMAND_LONG", 100.0f); + + } /* PARAM_VALUE stream */ _parameters_manager = (MavlinkParametersManager *) MavlinkParametersManager::new_instance(this); @@ -2078,6 +2086,11 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("MISSION_ITEM", 50.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f); configure_stream("MANUAL_CONTROL", 5.0f); + break; + + case MAVLINK_MODE_IRIDIUM: + configure_stream("HIGH_LATENCY", 0.1f); + break; default: break; @@ -2441,7 +2454,7 @@ Mavlink::start(int argc, char *argv[]) px4_task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2400, + 2500, (px4_main_t)&Mavlink::start_helper, (char *const *)argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e4b206ad4e..d9e51bc7b4 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -163,7 +163,8 @@ public: MAVLINK_MODE_ONBOARD, MAVLINK_MODE_OSD, MAVLINK_MODE_MAGIC, - MAVLINK_MODE_CONFIG + MAVLINK_MODE_CONFIG, + MAVLINK_MODE_IRIDIUM }; enum BROADCAST_MODE { @@ -191,6 +192,9 @@ public: case MAVLINK_MODE_CONFIG: return "Config"; + case MAVLINK_MODE_IRIDIUM: + return "Iridium"; + default: return "Unknown"; } @@ -232,14 +236,6 @@ public: static int start_helper(int argc, char *argv[]); - /** - * Handle parameter related messages. - */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - - void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); - /** * Enable / disable Hardware in the Loop simulation mode. * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b0c3aecc30..777ffef6f3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -3592,6 +3592,210 @@ protected: } }; +class MavlinkStreamHighLatency : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamHighLatency::get_name_static(); + } + + static const char *get_name_static() + { + return "HIGH_LATENCY"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_HIGH_LATENCY; + } + + uint16_t get_id() + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamHighLatency(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_HIGH_LATENCY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_actuator_sub; + uint64_t _actuator_time; + + MavlinkOrbSubscription *_airspeed_sub; + uint64_t _airspeed_time; + + MavlinkOrbSubscription *_attitude_sp_sub; + uint64_t _attitude_sp_time; + + MavlinkOrbSubscription *_attitude_sub; + uint64_t _attitude_time; + + MavlinkOrbSubscription *_battery_sub; + uint64_t _battery_time; + + MavlinkOrbSubscription *_fw_pos_ctrl_status_sub; + uint64_t _fw_pos_ctrl_status_time; + + MavlinkOrbSubscription *_global_pos_sub; + uint64_t _global_pos_time; + + MavlinkOrbSubscription *_gps_sub; + uint64_t _gps_time; + + MavlinkOrbSubscription *_home_sub; + uint64_t _home_time; + + MavlinkOrbSubscription *_landed_sub; + uint64_t _landed_time; + + MavlinkOrbSubscription *_mission_result_sub; + uint64_t _mission_result_time; + + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; + + MavlinkOrbSubscription *_status_sub; + uint64_t _status_time; + + MavlinkOrbSubscription *_tecs_status_sub; + uint64_t _tecs_time; + + MavlinkOrbSubscription *_wind_sub; + uint64_t _wind_time; + + /* do not allow top copying this class */ + MavlinkStreamHighLatency(MavlinkStreamHighLatency &); + MavlinkStreamHighLatency& operator = (const MavlinkStreamHighLatency &); + +protected: + explicit MavlinkStreamHighLatency(Mavlink *mavlink) : MavlinkStream(mavlink), + _actuator_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), + _actuator_time(0), + _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), + _airspeed_time(0), + _attitude_sp_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))), + _attitude_sp_time(0), + _attitude_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), + _attitude_time(0), + _battery_sub(_mavlink->add_orb_subscription(ORB_ID(battery_status))), + _battery_time(0), + _fw_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(fw_pos_ctrl_status))), + _fw_pos_ctrl_status_time(0), + _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), + _global_pos_time(0), + _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), + _gps_time(0), + _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), + _home_time(0), + _landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))), + _landed_time(0), + _mission_result_sub(_mavlink->add_orb_subscription(ORB_ID(mission_result))), + _mission_result_time(0), + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0), + _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), + _status_time(0), + _tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))), + _tecs_time(0) + {} + + void send(const hrt_abstime t) + { + struct actuator_controls_s actuator = {}; + struct airspeed_s airspeed = {}; + struct battery_status_s battery = {}; + struct fw_pos_ctrl_status_s fw_pos_ctrl_status = {}; + struct home_position_s home = {}; + struct mission_result_s mission_result = {}; + struct sensor_combined_s sensor = {}; + struct tecs_status_s tecs_status = {}; + struct vehicle_attitude_s attitude = {}; + struct vehicle_attitude_setpoint_s attitude_sp = {}; + struct vehicle_global_position_s global_pos = {}; + struct vehicle_gps_position_s gps = {}; + struct vehicle_land_detected_s land_detected = {}; + struct vehicle_status_s status = {}; + + bool updated = _status_sub->update(&_status_time, &status); + updated |= _actuator_sub->update(&_actuator_time, &actuator); + updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); + updated |= _attitude_sp_sub->update(&_attitude_sp_time, &attitude_sp); + updated |= _attitude_sub->update(&_attitude_time, &attitude); + updated |= _battery_sub->update(&_battery_time, &battery); + updated |= _fw_pos_ctrl_status_sub->update(&_fw_pos_ctrl_status_time, &fw_pos_ctrl_status); + updated |= _global_pos_sub->update(&_global_pos_time, &global_pos); + updated |= _gps_sub->update(&_gps_time, &gps); + updated |= _home_sub->update(&_home_time, &home); + updated |= _landed_sub->update(&_landed_time, &land_detected); + updated |= _mission_result_sub->update(&_mission_result_time, &mission_result); + updated |= _sensor_sub->update(&_sensor_time, &sensor); + updated |= _tecs_status_sub->update(&_tecs_time, &tecs_status); + + if (updated) { + mavlink_high_latency_t msg = {}; + + //timespec tv; + //px4_clock_gettime(CLOCK_REALTIME, &tv); + //msg.time_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + + msg.base_mode = 0; + msg.custom_mode = 0; + uint8_t sys_status; + get_mavlink_mode_state(&status, &sys_status, &msg.base_mode, &msg.custom_mode); + + matrix::Eulerf euler = matrix::Quatf(attitude.q); + msg.roll = math::degrees(euler.phi()) * 100; + msg.pitch = math::degrees(euler.theta()) * 100; + msg.heading = math::degrees(_wrap_2pi(euler.psi())) * 100; + + //msg.roll_sp = math::degrees(attitude_sp.roll_body) * 100; + //msg.pitch_sp = math::degrees(attitude_sp.pitch_body) * 100; + msg.heading_sp = math::degrees(_wrap_2pi(attitude_sp.yaw_body)) * 100; + + if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + msg.throttle = actuator.control[actuator_controls_s::INDEX_THROTTLE] * 100; + } else { + msg.throttle = 0; + } + + msg.latitude = global_pos.lat * 1e7; + msg.longitude = global_pos.lon * 1e7; + + //msg.altitude_home = (_home_time > 0) ? (global_pos.alt - home.alt) : NAN; + msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : NAN; + + msg.altitude_sp = (_tecs_time > 0) ? (tecs_status.altitudeSp - home.alt) : NAN; + + msg.airspeed = airspeed.indicated_airspeed_m_s * 100.0f; + msg.groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) * 100.0f; + msg.climb_rate = -global_pos.vel_d; + + msg.gps_nsat = gps.satellites_used; + msg.gps_fix_type = gps.fix_type; + + msg.landed_state = land_detected.landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR; + + msg.battery_remaining = (battery.connected) ? battery.remaining * 100.0f : -1; + + msg.temperature = sensor.baro_temp_celcius; + msg.temperature_air = airspeed.air_temperature_celsius; + + msg.wp_num = mission_result.seq_current; + msg.wp_distance = fw_pos_ctrl_status.wp_dist; + + mavlink_msg_high_latency_send_struct(_mavlink->get_channel(), &msg); + } + } +}; + const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static), new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static), @@ -3638,5 +3842,6 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamCollision::new_instance, &MavlinkStreamCollision::get_name_static, &MavlinkStreamCollision::get_id_static), new StreamListItem(&MavlinkStreamWind::new_instance, &MavlinkStreamWind::get_name_static, &MavlinkStreamWind::get_id_static), new StreamListItem(&MavlinkStreamMountOrientation::new_instance, &MavlinkStreamMountOrientation::get_name_static, &MavlinkStreamMountOrientation::get_id_static), + new StreamListItem(&MavlinkStreamHighLatency::new_instance, &MavlinkStreamHighLatency::get_name_static, &MavlinkStreamWind::get_id_static), nullptr }; diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 1a51f4551d..2211ef17f9 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -121,6 +121,7 @@ PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 2); * @value 319200 Normal Telemetry (19200 baud, 8N1) * @value 338400 Normal Telemetry (38400 baud, 8N1) * @value 357600 Normal Telemetry (57600 baud, 8N1) + * @value 419200 Iridium Telemetry (19200 baud, 8N1) * @value 1921600 ESP8266 (921600 baud, 8N1) * * @min 0