forked from Archive/PX4-Autopilot
Iridium driver and support
Mavlink module implement HIGH_LATENCY (Iridium)
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
77e482a30e
commit
bce7ecb0f6
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@ set(config_module_list
|
|||
drivers/bst
|
||||
#drivers/snapdragon_rc_pwm
|
||||
drivers/lis3mdl
|
||||
drivers/iridiumsbd
|
||||
|
||||
#
|
||||
# System commands
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -50,6 +50,7 @@ set(config_module_list
|
|||
drivers/bma180
|
||||
drivers/bmi160
|
||||
drivers/tap_esc
|
||||
drivers/iridiumsbd
|
||||
|
||||
#
|
||||
# System commands
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "board_config.h"
|
||||
|
||||
#define IRIDIUMSBD_DEVICE_PATH "/dev/iridium"
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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 <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <termios.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/scheduling_priorities.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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 <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
|
||||
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);
|
||||
};
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* Satellite radio read interval
|
||||
*
|
||||
* @unit s
|
||||
* @min 0
|
||||
* @max 300
|
||||
* @group Iridium SBD
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ISBD_READINT, 10);
|
||||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
*
|
||||
|
|
|
|||
|
|
@ -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
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue