dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink USB auto start/stop on boards with VBUS 

- no longer start sercon or mavlink usb by default
 - on USB connection (VBUS) monitor serial USB at low rate and start Mavlink if there's a HEARTBEAT or nshterm on 3 consecutive carriage returns
 - the mavlink USB instance is automatically stopped and serdis executed if USB is disconnected
 - skipping Mavlink USB (and sercon) saves a considerable amount of memory on older boards
This commit is contained in:
Daniel Agar 2021-10-03 15:32:54 -04:00 committed by GitHub
parent 07e307b074
commit 6d78054f50
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
46 changed files with 447 additions and 453 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.ci/Jenkinsfile-hardware
View File

@ -867,8 +867,8 @@ void checkStatus() {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /etc"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status streams"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status streams" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status" || true'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show" || true'

5
ROMFS/cannode/init.d/rcS
View File

@ -16,11 +16,6 @@ set +e
#------------------------------------------------------------------------------
set R /
#
# Start CDC/ACM serial driver.
#
sercon
#
# Print full system version.
#

5
ROMFS/px4fmu_common/init.d/rcS
View File

@ -49,11 +49,6 @@ set STARTUP_TUNE 1
set USE_IO no
set VEHICLE_TYPE none
#
# Start CDC/ACM serial driver.
#
sercon
#
# Print full system version.
#

8
ROMFS/px4fmu_test/init.d/rcS
View File

@ -13,14 +13,6 @@ then
led_control on -c blue
fi
if sercon
then
echo "[i] USB interface connected"
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi
#
# Try to mount the microSD card.
#

4
Tools/mavlink_shell.py
View File

@ -45,6 +45,7 @@ class MavlinkSerialPort():
self.port = devnum
self.debug("Connecting with MAVLink to %s ..." % portname)
self.mav = mavutil.mavlink_connection(portname, autoreconnect=True, baud=baudrate)
self.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GENERIC, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
self.mav.wait_heartbeat()
self.debug("HEARTBEAT OK\n")
self.debug("Locked serial device\n")
@ -226,8 +227,7 @@ def main():
# handle heartbeat sending
heartbeat_time = timer()
if heartbeat_time > next_heartbeat_time:
mav_serialport.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, 0)
mav_serialport.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GENERIC, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
next_heartbeat_time = heartbeat_time + 1
except serial.serialutil.SerialException as e:

7
boards/airmind/mindpx-v2/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# Airmind Mindpx-v2 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

3
boards/atl/mantis-edu/init/rc.board_mavlink
View File

@ -14,6 +14,3 @@ mavlink stream -d ${GIMBAL_TTY} -s GIMBAL_DEVICE_SET_ATTITUDE -r 20
# optical flow
mavlink start -d /dev/ttyS2 -m custom -b 500000
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/bitcraze/crazyflie/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# Bitcraze Crazyflie specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/bitcraze/crazyflie21/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# Bitcraze Crazyflie specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/cuav/nora/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# board specific MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/cuav/x7pro/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# board specific MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

3
boards/cubepilot/cubeorange/init/rc.board_mavlink
View File

@ -3,9 +3,6 @@
# Board specific MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0
# Start ADS-B receiver mavlink connection if console not present
if [ ! -e /dev/console ]
then

7
boards/cubepilot/cubeyellow/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# Board specific MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/holybro/durandal-v1/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# Durandal specific specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/holybro/kakutef7/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# KakuteF7 specific specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/holybro/pix32v5/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# PX4 FMUv5 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/modalai/fc-v1/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# ModalAI FC-v1 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/modalai/fc-v2/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# ModalAI FC-v2 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/mro/ctrl-zero-f7-oem/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# mRo Control Zero specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/mro/ctrl-zero-f7/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# mRo Control Zero specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# board specific MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/mro/ctrl-zero-h7/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# board specific MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/mro/pixracerpro/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# board specific MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/mro/x21-777/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# Board specific MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/mro/x21/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# mRo x21 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/nxp/fmuk66-e/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# NXP fmuk66-e specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/nxp/fmuk66-v3/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# NXP fmuk66-v3 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/nxp/fmurt1062-v1/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# PX4 FMUv5 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/omnibus/f4sd/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# OmnibusF4SD specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/px4/fmu-v2/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# PX4 FMUv2 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/px4/fmu-v3/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# PX4 FMUv3 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

8
boards/px4/fmu-v4/init/rc.board_mavlink
View File

@ -1,8 +0,0 @@
#!/bin/sh
#
# PX4 FMUv4 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/px4/fmu-v4pro/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# PX4 FMUv4pro specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/px4/fmu-v5/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# PX4 FMUv5 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

13
boards/px4/fmu-v5x/init/rc.board_mavlink
View File

@ -1,13 +0,0 @@
#!/bin/sh
#
# PX4 FMUv5X specific board MAVLink startup script.
#------------------------------------------------------------------------------
if ver hwtypecmp V5Xa0 V5X91 V5Xa1
then
# Start MAVLink on the UART connected to the mission computer
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
else
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0
fi

7
boards/px4/fmu-v6u/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# PX4 FMUv6U specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/px4/fmu-v6x/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# PX4 FMUv6X specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/spracing/h7extreme/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# SP Racing H7 EXTREME specific specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

7
boards/uvify/core/init/rc.board_mavlink
View File

@ -1,7 +0,0 @@
#!/bin/sh
#
# UVify UVF4 specific board MAVLink startup script.
#------------------------------------------------------------------------------
# Start MAVLink on the USB port
mavlink start -d /dev/ttyACM0

212
platforms/nuttx/src/px4/common/px4_init.cpp
View File

@ -56,6 +56,214 @@
#include <px4_platform_common/crypto.h>
#endif
#if defined(CONFIG_SYSTEM_CDCACM)
__BEGIN_DECLS
#include <nuttx/wqueue.h>
#include <builtin/builtin.h>
#include <termios.h>
#include <sys/ioctl.h>
extern int sercon_main(int c, char **argv);
extern int serdis_main(int c, char **argv);
__END_DECLS
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#define USB_DEVICE_PATH "/dev/ttyACM0"
static struct work_s usb_serial_work;
static bool vbus_present_prev = false;
static int ttyacm_fd = -1;
enum class UsbAutoStartState {
disconnected,
connecting,
connected,
disconnecting,
} usb_auto_start_state{UsbAutoStartState::disconnected};
static void mavlink_usb_check(void *arg)
{
int rescheduled = -1;
uORB::SubscriptionData<actuator_armed_s> actuator_armed_sub{ORB_ID(actuator_armed)};
const bool armed = actuator_armed_sub.get().armed;
const bool vbus_present = (board_read_VBUS_state() == PX4_OK);
if (!armed) {
switch (usb_auto_start_state) {
case UsbAutoStartState::disconnected:
if (vbus_present && vbus_present_prev) {
if (sercon_main(0, nullptr) == EXIT_SUCCESS) {
usb_auto_start_state = UsbAutoStartState::connecting;
rescheduled = work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, USEC2TICK(100000));
}
} else if (vbus_present && !vbus_present_prev) {
// check again sooner if USB just connected
rescheduled = work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, USEC2TICK(100000));
}
break;
case UsbAutoStartState::connecting:
if (vbus_present && vbus_present_prev) {
if (ttyacm_fd < 0) {
ttyacm_fd = ::open(USB_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
}
if (ttyacm_fd >= 0) {
int bytes_available = 0;
int retval = ::ioctl(ttyacm_fd, FIONREAD, &bytes_available);
if ((retval == OK) && (bytes_available >= 3)) {
char buffer[80];
// non-blocking read
int nread = ::read(ttyacm_fd, buffer, sizeof(buffer));
#if defined(DEBUG_BUILD)
if (nread > 0) {
fprintf(stderr, "%d bytes\n", nread);
for (int i = 0; i < nread; i++) {
fprintf(stderr, "|%X", buffer[i]);
}
fprintf(stderr, "\n");
}
#endif // DEBUG_BUILD
if (nread > 0) {
bool launch_mavlink = false;
bool launch_nshterm = false;
static constexpr int MAVLINK_HEARTBEAT_MIN_LENGTH = 9;
if (nread >= MAVLINK_HEARTBEAT_MIN_LENGTH) {
// scan buffer for mavlink HEARTBEAT (v1 & v2)
for (int i = 0; i < nread - MAVLINK_HEARTBEAT_MIN_LENGTH; i++) {
if ((buffer[i] = 0xFE) && (buffer[i + 1] = 9) && (buffer[i + 5] == 0)) {
// mavlink v1 HEARTBEAT
// buffer[0]: start byte (0xFE for mavlink v1)
// buffer[1]: length (9 for HEARTBEAT)
// buffer[3]: SYSID
// buffer[4]: COMPID
// buffer[5]: mavlink message id (0 for HEARTBEAT)
syslog(LOG_INFO, "%s: launching mavlink (HEARTBEAT v1 from SYSID:%d COMPID:%d)\n",
USB_DEVICE_PATH, buffer[i + 3], buffer[i + 4]);
launch_mavlink = true;
break;
} else if ((buffer[i] = 0xFD) && (buffer[i + 1] = 9)
&& (buffer[i + 7] == 0) && (buffer[i + 8] == 0) && (buffer[i + 9] == 0)) {
// mavlink v2 HEARTBEAT
// buffer[0]: start byte (0xFD for mavlink v2)
// buffer[1]: length (9 for HEARTBEAT)
// buffer[5]: SYSID
// buffer[6]: COMPID
// buffer[7:9]: mavlink message id (0 for HEARTBEAT)
syslog(LOG_INFO, "%s: launching mavlink (HEARTBEAT v2 from SYSID:%d COMPID:%d)\n",
USB_DEVICE_PATH, buffer[i + 5], buffer[i + 6]);
launch_mavlink = true;
break;
}
}
}
if (!launch_mavlink && (nread >= 3)) {
// nshterm (3 carriage returns)
// scan buffer looking for 3 consecutive carriage returns (0xD)
for (int i = 1; i < nread - 1; i++) {
if (buffer[i - 1] == 0xD && buffer[i] == 0xD && buffer[i + 1] == 0xD) {
syslog(LOG_INFO, "%s: launching nshterm\n", USB_DEVICE_PATH);
launch_nshterm = true;
break;
}
}
}
if (launch_mavlink || launch_nshterm) {
// cleanup serial port
close(ttyacm_fd);
ttyacm_fd = -1;
static const char *mavlink_argv[] {"mavlink", "start", "-d", USB_DEVICE_PATH, nullptr};
static const char *nshterm_argv[] {"nshterm", USB_DEVICE_PATH, nullptr};
char **exec_argv = nullptr;
if (launch_nshterm) {
exec_argv = (char **)nshterm_argv;
} else if (launch_mavlink) {
exec_argv = (char **)mavlink_argv;
}
sched_lock();
if (exec_builtin(exec_argv[0], exec_argv, nullptr, 0) > 0) {
usb_auto_start_state = UsbAutoStartState::connected;
} else {
usb_auto_start_state = UsbAutoStartState::disconnecting;
}
sched_unlock();
}
}
}
}
} else {
// cleanup
if (ttyacm_fd >= 0) {
close(ttyacm_fd);
ttyacm_fd = -1;
}
usb_auto_start_state = UsbAutoStartState::disconnecting;
}
break;
case UsbAutoStartState::connected:
if (!vbus_present && !vbus_present_prev) {
sched_lock();
static const char app[] {"mavlink"};
static const char *stop_argv[] {"mavlink", "stop", "-d", USB_DEVICE_PATH, NULL};
exec_builtin(app, (char **)stop_argv, NULL, 0);
sched_unlock();
usb_auto_start_state = UsbAutoStartState::disconnecting;
}
break;
case UsbAutoStartState::disconnecting:
// serial disconnect if unused
serdis_main(0, NULL);
usb_auto_start_state = UsbAutoStartState::disconnected;
break;
}
}
vbus_present_prev = vbus_present;
if (rescheduled != PX4_OK) {
work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, NULL, USEC2TICK(1000000));
}
}
#endif // CONFIG_SYSTEM_CDCACM
int px4_platform_init()
{
@ -140,6 +348,10 @@ int px4_platform_init()
px4_log_initialize();
#if defined(CONFIG_SYSTEM_CDCACM)
work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, 0);
#endif // CONFIG_SYSTEM_CDCACM
return PX4_OK;
}

25
src/modules/commander/Commander.cpp
View File

@ -1911,31 +1911,6 @@ Commander::run()
_status_flags.condition_power_input_valid = true;
}
#if defined(CONFIG_BOARDCTL_RESET)
if (!_status_flags.circuit_breaker_engaged_usb_check && _status_flags.usb_connected) {
/* if the USB hardware connection went away, reboot */
if (_system_power_usb_connected && !system_power.usb_connected) {
/*
* Apparently the USB cable went away but we are still powered,
* so we bring the system back to a nominal state for flight.
* This is important to unload the USB stack of the OS which is
* a relatively complex piece of software that is non-essential
* for flight and continuing to run it would add a software risk
* without a need. The clean approach to unload it is to reboot.
*/
if (shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
mavlink_log_critical(&_mavlink_log_pub, "USB disconnected, rebooting for flight safety\t");
events::send(events::ID("commander_reboot_usb_disconnect"), {events::Log::Critical, events::LogInternal::Info},
"USB disconnected, rebooting for flight safety");
while (1) { px4_usleep(1); }
}
}
}
#endif // CONFIG_BOARDCTL_RESET
_system_power_usb_connected = system_power.usb_connected;
}
}

311
src/modules/mavlink/mavlink_main.cpp
View File

@ -127,34 +127,53 @@ Mavlink::Mavlink() :
}
_vehicle_command_sub.subscribe();
if (orb_exists(ORB_ID(event), 0) == PX4_ERROR) {
orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH);
}
_event_sub.subscribe();
}
Mavlink::~Mavlink()
{
if (_task_running) {
if (running()) {
/* task wakes up every 10ms or so at the longest */
_task_should_exit = true;
request_stop();
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
px4_usleep(20000);
/* wait at least 1 second (10ms * 10) */
px4_usleep(10000);
/* if we have given up, kill it */
if (++i > 50) {
//TODO store main task handle in Mavlink instance to allow killing task
//task_delete(_mavlink_task);
if (++i > 100) {
PX4_ERR("mavlink didn't stop, killing task %d", _task_id);
px4_task_delete(_task_id);
break;
}
} while (_task_running);
} while (running());
}
if (_instance_id >= 0) {
mavlink_module_instances[_instance_id] = nullptr;
}
// if this instance was responsible for checking events then select a new mavlink instance
if (check_events()) {
check_events_disable();
// select next available instance
for (Mavlink *inst : mavlink_module_instances) {
if (inst) {
inst->check_events_enable();
break;
}
}
}
perf_free(_loop_perf);
perf_free(_loop_interval_perf);
perf_free(_send_byte_error_perf);
@ -230,6 +249,20 @@ Mavlink::set_instance_id()
{
LockGuard lg{mavlink_module_mutex};
// instance count
size_t inst_count = 0;
for (Mavlink *inst : mavlink_module_instances) {
if (inst != nullptr) {
inst_count++;
}
}
// if this is the first instance use it to check events
if (inst_count == 0) {
check_events_enable();
}
for (int instance_id = 0; instance_id < MAVLINK_COMM_NUM_BUFFERS; instance_id++) {
if (mavlink_module_instances[instance_id] == nullptr) {
mavlink_module_instances[instance_id] = this;
@ -312,9 +345,9 @@ Mavlink::destroy_all_instances()
for (Mavlink *inst_to_del : mavlink_module_instances) {
if (inst_to_del != nullptr) {
/* set flag to stop thread and wait for all threads to finish */
inst_to_del->_task_should_exit = true;
inst_to_del->request_stop();
while (inst_to_del->_task_running) {
while (inst_to_del->running()) {
printf(".");
fflush(stdout);
px4_usleep(10000);
@ -527,44 +560,9 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
return -EINVAL;
}
/* back off 1800 ms to avoid running into the USB setup timing */
while (_is_usb_uart && hrt_absolute_time() < 1800U * 1000U) {
px4_usleep(50000);
}
/* open uart */
_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
/* if this is a config link, stay here and wait for it to open */
if (_uart_fd < 0 && _is_usb_uart) {
uORB::SubscriptionData<actuator_armed_s> armed_sub{ORB_ID(actuator_armed)};
/* get the system arming state and abort on arming */
while (_uart_fd < 0 && !_task_should_exit) {
/* another task might have requested subscriptions: make sure we handle it */
check_requested_subscriptions();
/* abort if an arming topic is published and system is armed */
armed_sub.update();
/* the system is now providing arming status feedback.
* instead of timing out, we resort to abort bringing
* up the terminal.
*/
if (armed_sub.get().armed) {
/* this is not an error, but we are done */
return -1;
}
int errcode = errno;
/* ENOTCONN means that the USB device is not yet connected */
px4_usleep(errcode == ENOTCONN ? 1000000 : 100000);
_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
}
}
/*
* Return here in the iridium mode since the iridium driver does not
* support the subsequent function calls.
@ -587,21 +585,11 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
/* Clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
if (!_is_usb_uart) {
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state);
::close(_uart_fd);
return -1;
}
} else {
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000;
set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state);
::close(_uart_fd);
return -1;
}
#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
@ -626,12 +614,6 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
int
Mavlink::setup_flow_control(enum FLOW_CONTROL_MODE mode)
{
// We can't do this on USB - skip
if (_is_usb_uart) {
_flow_control_mode = FLOW_CONTROL_OFF;
return OK;
}
struct termios uart_config;
int ret = tcgetattr(_uart_fd, &uart_config);
@ -1251,7 +1233,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
/* orb subscription must be done from the main thread,
* set _subscribe_to_stream and _subscribe_to_stream_rate fields
* which polled in mavlink main loop */
if (!_task_should_exit) {
if (!should_exit()) {
/* wait for previous subscription completion */
while (_subscribe_to_stream != nullptr) {
px4_usleep(MAIN_LOOP_DELAY / 2);
@ -2127,17 +2109,23 @@ Mavlink::task_main(int argc, char *argv[])
}
/* USB serial is indicated by /dev/ttyACMx */
if (strcmp(_device_name, "/dev/ttyACM0") == OK || strcmp(_device_name, "/dev/ttyACM1") == OK) {
if (strncmp(_device_name, "/dev/ttyACM", 11) == 0) {
if (_datarate == 0) {
_datarate = 800000;
}
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000;
if (_mode == MAVLINK_MODE_COUNT) {
_mode = MAVLINK_MODE_CONFIG;
}
_ftp_on = true;
_is_usb_uart = true;
_flow_control_mode = FLOW_CONTROL_OFF;
set_telemetry_status_type(telemetry_status_s::LINK_TYPE_USB);
}
if (_mode == MAVLINK_MODE_COUNT) {
@ -2183,6 +2171,12 @@ Mavlink::task_main(int argc, char *argv[])
if (!set_instance_id()) {
PX4_ERR("no instances available");
return PX4_ERROR;
} else {
// set thread name
char thread_name[13];
snprintf(thread_name, sizeof(thread_name), "mavlink_if%d", get_instance_id());
px4_prctl(PR_SET_NAME, thread_name, px4_getpid());
}
set_channel();
@ -2249,13 +2243,9 @@ Mavlink::task_main(int argc, char *argv[])
if (get_protocol() == Protocol::SERIAL) {
_uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control);
if (_uart_fd < 0 && !_is_usb_uart) {
if (_uart_fd < 0) {
PX4_ERR("could not open %s", _device_name);
return PX4_ERROR;
} else if (_uart_fd < 0 && _is_usb_uart) {
/* the config link is optional */
return PX4_OK;
}
}
@ -2268,6 +2258,8 @@ Mavlink::task_main(int argc, char *argv[])
#endif // MAVLINK_UDP
_task_id = px4_getpid();
/* if the protocol is serial, we send the system version blindly */
if (get_protocol() == Protocol::SERIAL) {
send_autopilot_capabilities();
@ -2275,17 +2267,11 @@ Mavlink::task_main(int argc, char *argv[])
_receiver.start();
/* Events subscription: only the first MAVLink instance should check */
uORB::Subscription event_sub{ORB_ID(event)};
const bool should_check_events = _instance_id == 0;
uint16_t event_sequence_offset = 0; // offset to account for skipped events, not sent via MAVLink
// ensure topic exists, otherwise we might lose first queued events
orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH);
event_sub.subscribe();
_mavlink_start_time = hrt_absolute_time();
while (!_task_should_exit) {
while (!should_exit()) {
/* main loop */
px4_usleep(_main_loop_delay);
@ -2504,23 +2490,27 @@ Mavlink::task_main(int argc, char *argv[])
}
/* handle new events */
if (should_check_events) {
event_s orb_event;
if (check_events()) {
if (_event_sub.updated()) {
LockGuard lg{mavlink_module_mutex};
while (event_sub.update(&orb_event)) {
if (events::externalLogLevel(orb_event.log_levels) == events::LogLevel::Disabled) {
++event_sequence_offset; // skip this event
event_s orb_event;
} else {
events::Event e;
e.id = orb_event.id;
e.timestamp_ms = orb_event.timestamp / 1000;
e.sequence = orb_event.event_sequence - event_sequence_offset;
e.log_levels = orb_event.log_levels;
static_assert(sizeof(e.arguments) == sizeof(orb_event.arguments),
"uorb message event: arguments size mismatch");
memcpy(e.arguments, orb_event.arguments, sizeof(orb_event.arguments));
_event_buffer->insert_event(e);
while (_event_sub.update(&orb_event)) {
if (events::externalLogLevel(orb_event.log_levels) == events::LogLevel::Disabled) {
++event_sequence_offset; // skip this event
} else {
events::Event e;
e.id = orb_event.id;
e.timestamp_ms = orb_event.timestamp / 1000;
e.sequence = orb_event.event_sequence - event_sequence_offset;
e.log_levels = orb_event.log_levels;
static_assert(sizeof(e.arguments) == sizeof(orb_event.arguments),
"uorb message event: arguments size mismatch");
memcpy(e.arguments, orb_event.arguments, sizeof(orb_event.arguments));
_event_buffer->insert_event(e);
}
}
}
}
@ -2608,7 +2598,7 @@ Mavlink::task_main(int argc, char *argv[])
/* delete streams */
_streams.clear();
if (_uart_fd >= 0 && !_is_usb_uart) {
if (_uart_fd >= 0) {
/* discard all pending data, as close() might block otherwise on NuttX with flow control enabled */
tcflush(_uart_fd, TCIOFLUSH);
/* close UART */
@ -2633,8 +2623,6 @@ Mavlink::task_main(int argc, char *argv[])
pthread_mutex_destroy(&_send_mutex);
pthread_mutex_destroy(&_radio_status_mutex);
_task_running = false;
PX4_INFO("exiting channel %i", (int)_channel);
return OK;
@ -2791,16 +2779,15 @@ int Mavlink::start_helper(int argc, char *argv[])
int res;
if (!instance) {
/* out of memory */
res = -ENOMEM;
PX4_ERR("OUT OF MEM");
} else {
/* this will actually only return once MAVLink exits */
instance->_task_running.store(true);
res = instance->task_main(argc, argv);
instance->_task_running = false;
instance->_task_running.store(false);
}
return res;
@ -2838,14 +2825,12 @@ Mavlink::start(int argc, char *argv[])
}
// Instantiate thread
char buf[24];
sprintf(buf, "mavlink_if%d", ic);
// This is where the control flow splits
// between the starting task and the spawned
// task - start_helper() only returns
// when the started task exits.
px4_task_spawn_cmd(buf,
px4_task_spawn_cmd("mavlink_main",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
PX4_STACK_ADJUSTED(2896) + MAVLINK_NET_ADDED_STACK,
@ -2907,7 +2892,7 @@ Mavlink::display_status()
printf("\t rx errors:\t%" PRIu16 "\n", _rstatus.rxerrors);
printf("\t fixed:\t%" PRIu16 "\n", _rstatus.fix);
} else if (_is_usb_uart) {
} else if (_tstatus.type == telemetry_status_s::LINK_TYPE_USB) {
printf("USB CDC\n");
} else {
@ -3014,6 +2999,107 @@ Mavlink::display_status_streams()
}
}
int
Mavlink::stop_command(int argc, char *argv[])
{
const char *device_name = nullptr;
#if defined(MAVLINK_UDP)
char *eptr;
int temp_int_arg;
unsigned short network_port = 0;
#endif // MAVLINK_UDP
bool provided_device = false;
bool provided_network_port = false;
/*
* Called via main with original argv
* mavlink start
*
* Remove 2
*/
argc -= 2;
argv += 2;
/* don't exit from getopt loop to leave getopt global variables in consistent state,
* set error flag instead */
bool err_flag = false;
int i = 0;
while (i < argc) {
if (0 == strcmp(argv[i], "-d") && i < argc - 1) {
provided_device = true;
device_name = argv[i + 1];
i++;
#if defined(MAVLINK_UDP)
} else if (0 == strcmp(argv[i], "-u") && i < argc - 1) {
provided_network_port = true;
temp_int_arg = strtoul(argv[i + 1], &eptr, 10);
if (*eptr == '\0') {
network_port = temp_int_arg;
} else {
err_flag = true;
}
i++;
#endif // MAVLINK_UDP
} else {
err_flag = true;
}
i++;
}
if (!err_flag) {
Mavlink *inst = nullptr;
if (provided_device && !provided_network_port) {
inst = get_instance_for_device(device_name);
#if defined(MAVLINK_UDP)
} else if (provided_network_port && !provided_device) {
inst = get_instance_for_network_port(network_port);
#endif // MAVLINK_UDP
} else if (provided_device && provided_network_port) {
PX4_WARN("please provide either a device name or a network port");
return PX4_ERROR;
}
if (inst != nullptr) {
/* set flag to stop thread and wait for all threads to finish */
if (inst->running() && !inst->should_exit()) {
inst->request_stop();
LockGuard lg{mavlink_module_mutex};
for (int mavlink_instance = 0; mavlink_instance < MAVLINK_COMM_NUM_BUFFERS; mavlink_instance++) {
if (mavlink_module_instances[mavlink_instance] == inst) {
delete mavlink_module_instances[mavlink_instance];
mavlink_module_instances[mavlink_instance] = nullptr;
return PX4_OK;
}
}
}
return PX4_ERROR;
}
} else {
usage();
}
return PX4_ERROR;
}
int
Mavlink::stream_command(int argc, char *argv[])
{
@ -3215,6 +3301,13 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
PRINT_MODULE_USAGE_COMMAND_DESCR("stop-all", "Stop all instances");
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop a running instance");
#if defined(CONFIG_NET) || defined(__PX4_POSIX)
PRINT_MODULE_USAGE_PARAM_INT('u', -1, 0, 65536, "Select Mavlink instance via local Network Port", true);
#endif
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Select Mavlink instance via Serial Device", true);
PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status for all instances");
PRINT_MODULE_USAGE_ARG("streams", "Print all enabled streams", true);
@ -3241,11 +3334,6 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
return Mavlink::start(argc, argv);
} else if (!strcmp(argv[1], "stop")) {
PX4_WARN("mavlink stop is deprecated, use stop-all instead");
usage();
return 1;
} else if (!strcmp(argv[1], "stop-all")) {
return Mavlink::destroy_all_instances();
@ -3253,6 +3341,9 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[])
bool show_streams_status = argc > 2 && strcmp(argv[2], "streams") == 0;
return Mavlink::get_status_all_instances(show_streams_status);
} else if (!strcmp(argv[1], "stop")) {
return Mavlink::stop_command(argc, argv);
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream_command(argc, argv);

24
src/modules/mavlink/mavlink_main.h
View File

@ -125,6 +125,14 @@ public:
*/
static int start(int argc, char *argv[]);
bool running() const { return _task_running.load(); }
bool should_exit() const { return _task_should_exit.load(); }
void request_stop()
{
_task_should_exit.store(true);
_receiver.request_stop();
}
/**
* Display the mavlink status.
*/
@ -135,6 +143,7 @@ public:
*/
void display_status_streams();
static int stop_command(int argc, char *argv[]);
static int stream_command(int argc, char *argv[]);
static int instance_count();
@ -166,6 +175,10 @@ public:
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
bool check_events() const { return _should_check_events.load(); }
void check_events_enable() { _should_check_events.store(true); }
void check_events_disable() { _should_check_events.store(false); }
int get_uart_fd() const { return _uart_fd; }
/**
@ -455,8 +468,6 @@ public:
int get_socket_fd() { return _socket_fd; };
bool _task_should_exit{false}; /**< Mavlink task should exit iff true. */
#if defined(MAVLINK_UDP)
unsigned short get_network_port() { return _network_port; }
@ -529,7 +540,12 @@ public:
private:
MavlinkReceiver _receiver;
int _instance_id{-1};
int _task_id{-1};
px4::atomic_bool _task_should_exit{false};
px4::atomic_bool _task_running{false};
bool _transmitting_enabled{true};
bool _transmitting_enabled_commanded{false};
@ -540,12 +556,12 @@ private:
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
uORB::PublicationMulti<telemetry_status_s> _telemetry_status_pub{ORB_ID(telemetry_status)};
uORB::Subscription _event_sub{ORB_ID(event)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
bool _task_running{true};
static bool _boot_complete;
static constexpr int MAVLINK_MIN_INTERVAL{1500};
@ -562,6 +578,8 @@ private:
bool _wait_to_transmit{false}; /**< Wait to transmit until received messages. */
bool _received_messages{false}; /**< Whether we've received valid mavlink messages. */
px4::atomic_bool _should_check_events{false}; /**< Events subscription: only one MAVLink instance should check */
unsigned _main_loop_delay{1000}; /**< mainloop delay, depends on data rate */
List<MavlinkStream *> _streams;

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -3179,7 +3179,7 @@ MavlinkReceiver::run()
ssize_t nread = 0;
hrt_abstime last_send_update = 0;
while (!_mavlink->_task_should_exit) {
while (!_mavlink->should_exit()) {
// check for parameter updates
if (_parameter_update_sub.updated()) {

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -131,6 +131,8 @@ public:
void enable_message_statistics() { _message_statistics_enabled = true; }
void print_detailed_rx_stats() const;
void request_stop() { _should_exit.store(true); }
private:
static void *start_trampoline(void *context);
void run();

61
src/systemcmds/nshterm/nshterm.c
View File

@ -50,11 +50,8 @@
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/actuator_armed.h>
__EXPORT int nshterm_main(int argc, char *argv[]);
static void print_usage(void)
{
PRINT_MODULE_DESCRIPTION("Start an NSH shell on a given port.\n"
@ -67,63 +64,15 @@ static void print_usage(void)
PRINT_MODULE_USAGE_ARG("<file:dev>", "Device on which to start the shell (eg. /dev/ttyACM0)", false);
}
int
nshterm_main(int argc, char *argv[])
int nshterm_main(int argc, char *argv[])
{
if (argc < 2) {
print_usage();
return 1;
}
unsigned retries = 0;
int fd = -1;
int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
struct actuator_armed_s armed;
/* back off 1800 ms to avoid running into the USB setup timing */
while (hrt_absolute_time() < 1800U * 1000U) {
usleep(50000);
}
/* try to bring up the console - stop doing so if the system gets armed */
while (true) {
/* abort if an arming topic is published and system is armed */
bool updated = false;
orb_check(armed_fd, &updated);
if (updated) {
/* the system is now providing arming status feedback.
* instead of timing out, we resort to abort bringing
* up the terminal.
*/
orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
if (armed.armed) {
/* this is not an error, but we are done */
return 0;
}
}
/* the retries are to cope with the behaviour of /dev/ttyACM0 */
/* which may not be ready immediately. */
fd = open(argv[1], O_RDWR);
if (fd != -1) {
close(armed_fd);
break;
}
usleep(100000);
retries++;
}
if (fd == -1) {
perror(argv[1]);
return 1;
}
/* set up the serial port with output processing */
int fd = open(argv[1], O_RDWR);
/* Try to set baud rate */
struct termios uart_config;
@ -131,7 +80,7 @@ nshterm_main(int argc, char *argv[])
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
warnx("ERR get config %s: %d\n", argv[1], termios_state);
PX4_ERR("get config %s: %d\n", argv[1], termios_state);
close(fd);
return -1;
}
@ -140,7 +89,7 @@ nshterm_main(int argc, char *argv[])
uart_config.c_oflag |= (ONLCR | OPOST);
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR set config %s\n", argv[1]);
PX4_ERR("set config %s\n", argv[1]);
close(fd);
return -1;
}
@ -157,5 +106,7 @@ nshterm_main(int argc, char *argv[])
close(fd);
PX4_INFO("exiting");
return 0;
}