fix(cdcacm_autostart): start MAVLink without holding the USB port (#28185)

* fix(cdcacm_autostart): start MAVLink without holding the USB port

SYS_USB_AUTO=2 opened /dev/ttyACM0 O_RDONLY and kept it for the life of
the link, then treated a successful mavlink spawn as permanent success.
If mavlink later failed its UART open retries and exited, the driver
stayed "connected" and never restarted — listen-first hosts (production
USB MAVLink benches) saw no heartbeats until a VBUS cycle.

Start mavlink without a probe open, track its PID and restart if it
dies while VBUS is present, close the autodetect fd before handing the
device to a protocol, and drop the unused legacy cdc_acm_check path.

Assisted-by: Grok:grok-4.5
Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

* fix(cdcacm_autostart): initialise actuator_armed before the armed gate

copy() leaves the destination untouched when a topic has never been advertised,
so with commander not running `report` is whatever was on the stack. When that
garbage has armed set, run_state_machine() takes the "do not reconfigure USB
while flying" branch every cycle and never advances the state machine, so
MAVLink is never started on the USB CDC — on a vehicle that is definitionally
not flying, because commander is not running.

Seen on an ARK FMU v6X production test bench: a `commander stop` early in the
sequence leaves `listener actuator_armed` reporting "never published", and from
then on `mavlink status` shows no ttyACM instance at all while a host that opens
the port waits out its timeout against silence.

Zero-initialising restores the correct default for that case: not armed, so the
state machine runs.

* Update src/drivers/cdcacm_autostart/cdcacm_autostart.cpp

* Update src/drivers/cdcacm_autostart/cdcacm_autostart.cpp

* Update src/drivers/cdcacm_autostart/cdcacm_autostart.cpp

* Update src/drivers/cdcacm_autostart/cdcacm_autostart.cpp

---------

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
This commit is contained in:
Jacob Dahl authored and GitHub committed 2026-08-07 19:34:51 -06:00
1 parent ea63910683
commit f05739035e
4 files changed
+154 -464

No files matched your search

+2 -2
View File
@@ -190,8 +190,8 @@
* GPIO_OTGFS_VBUS unconditionally:
* - bootloader common main.c (skipped via BOARD_USB_VBUS_SENSE_DISABLED)
* - platforms/nuttx/src/px4/common/board_ctrl.c (board_read_VBUS_state())
* - platforms/nuttx/src/px4/common/cdc_acm_check.cpp gates sercon_main()
* on board_read_VBUS_state() returning PX4_OK
* - src/drivers/cdcacm_autostart gates sercon_main() on
* board_read_VBUS_state() returning PX4_OK
*
* So GPIO_OTGFS_VBUS must read HIGH for the application's CDC/ACM auto-start
* logic to ever fire and register /dev/ttyACM0. We point it at PE3 (an
@@ -1,376 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019-2022 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 <board_config.h>
#if defined(CONFIG_SYSTEM_CDCACM)
__BEGIN_DECLS
#include <board_config.h>
#include <arch/board/board.h>
#include <syslog.h>
#include <nuttx/wqueue.h>
#include <builtin/builtin.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <fcntl.h>
extern int sercon_main(int c, char **argv);
extern int serdis_main(int c, char **argv);
__END_DECLS
#include <px4_platform_common/shutdown.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#define USB_DEVICE_PATH "/dev/ttyACM0"
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
# undef SERIAL_PASSTHRU_UBLOX_DEV
# if defined(CONFIG_SERIAL_PASSTHRU_GPS1) && defined(CONFIG_BOARD_SERIAL_GPS1)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS1
# elif defined(CONFIG_SERIAL_PASSTHRU_GPS2)&& defined(CONFIG_BOARD_SERIAL_GPS2)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS2
# elif defined(CONFIG_SERIAL_PASSTHRU_GPS3)&& defined(CONFIG_BOARD_SERIAL_GPS3)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS3
# elif defined(CONFIG_SERIAL_PASSTHRU_GPS4)&& defined(CONFIG_BOARD_SERIAL_GPS4)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS4
# elif defined(CONFIG_SERIAL_PASSTHRU_GPS5) && defined(CONFIG_BOARD_SERIAL_GPS5)
# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS5
# endif
# if !defined(SERIAL_PASSTHRU_UBLOX_DEV)
# error "CONFIG_SERIAL_PASSTHRU_GPSn and CONFIG_BOARD_SERIAL_GPSn must be defined"
# endif
#endif
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;
bool vbus_present = (board_read_VBUS_state() == PX4_OK);
bool locked_out = false;
// If the hardware support RESET lockout that has nArmed ANDed with VBUS
// vbus_sense may drop during a param save which uses
// BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE to prevent external resets
// while writing the params. If we are not armed and nARMRED is low
// we are in such a lock out so ignore changes on VBUS_SENSE during this
// time.
#if defined(BOARD_GET_EXTERNAL_LOCKOUT_STATE)
locked_out = BOARD_GET_EXTERNAL_LOCKOUT_STATE() == 0;
if (locked_out) {
vbus_present = vbus_present_prev;
}
#endif
if (!armed && !locked_out) {
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) {
// Mavlink reboot/shutdown command
// COMMAND_LONG (#76) with command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246)
static constexpr int MAVLINK_COMMAND_LONG_MIN_LENGTH = 41;
if (nread >= MAVLINK_COMMAND_LONG_MIN_LENGTH) {
// scan buffer for mavlink COMMAND_LONG
for (int i = 0; i < nread - MAVLINK_COMMAND_LONG_MIN_LENGTH; i++) {
if ((buffer[i] == 0xFE) // Mavlink v1 start byte
&& (buffer[i + 5] == 76) // 76=0x4C COMMAND_LONG
&& (buffer[i + 34] == 246) // 246=0xF6 MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN
) {
// mavlink v1 COMMAND_LONG
// buffer[0]: start byte (0xFE for mavlink v1)
// buffer[3]: SYSID
// buffer[4]: COMPID
// buffer[5]: message id (COMMAND_LONG 76=0x4C)
// buffer[6-10]: COMMAND_LONG param 1 (little endian float)
// buffer[34]: COMMAND_LONG command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246/0xF6)
float param1_raw = 0;
memcpy(&param1_raw, &buffer[i + 6], 4);
int param1 = roundf(param1_raw);
syslog(LOG_INFO, "%s: Mavlink MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN param 1: %d (SYSID:%d COMPID:%d)\n",
USB_DEVICE_PATH, param1, buffer[i + 3], buffer[i + 4]);
if (param1 == 1) {
// 1: Reboot autopilot
px4_reboot_request(REBOOT_REQUEST, 0);
} else if (param1 == 2) {
// 2: Shutdown autopilot
#if defined(BOARD_HAS_POWER_CONTROL)
px4_shutdown_request(0);
#endif // BOARD_HAS_POWER_CONTROL
} else if (param1 == 3) {
// 3: Reboot autopilot and keep it in the bootloader until upgraded.
px4_reboot_request(REBOOT_TO_BOOTLOADER, 0);
}
}
}
}
bool launch_mavlink = false;
bool launch_nshterm = false;
bool launch_passthru = false;
struct termios uart_config;
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 defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
if (!launch_mavlink && !launch_nshterm && (nread >= 4)) {
// passthru Ublox
// scan buffer looking for 0xb5 0x62
for (int i = 0; i < nread; i++) {
bool ub = buffer[i] == 0xb5 && buffer[i + 1] == 0x62;
if (ub && ((buffer[i + 2 ] == 0x6 && (buffer[i + 3 ] == 0xb8 || buffer[i + 3 ] == 0x13)) ||
(buffer[i + 2 ] == 0xa && buffer[i + 3 ] == 0x4))) {
syslog(LOG_INFO, "%s: launching serial_passthru\n", USB_DEVICE_PATH);
launch_passthru = true;
break;
}
}
}
#endif
if (launch_mavlink || launch_nshterm || launch_passthru) {
// Get the current settings
tcgetattr(ttyacm_fd, &uart_config);
// 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};
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
speed_t baudrate = cfgetspeed(&uart_config);
char baudstring[16];
snprintf(baudstring, sizeof(baudstring), "%ld", baudrate);
static const char *gps_argv[] {"gps", "stop", nullptr};
static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", SERIAL_PASSTHRU_UBLOX_DEV, nullptr};
#endif
char **exec_argv = nullptr;
if (launch_nshterm) {
exec_argv = (char **)nshterm_argv;
} else if (launch_mavlink) {
exec_argv = (char **)mavlink_argv;
}
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
else if (launch_passthru) {
sched_lock();
exec_argv = (char **)gps_argv;
exec_builtin(exec_argv[0], exec_argv, nullptr, 0);
sched_unlock();
exec_argv = (char **)passthru_argv;
}
#endif
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));
}
}
void cdcacm_init(void)
{
work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, 0);
}
#endif // CONFIG_SYSTEM_CDCACM
+139 -78
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2023-2026 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
@@ -46,6 +46,9 @@ __END_DECLS
#include <px4_platform_common/shutdown.h>
#include <px4_platform_common/posix.h>
#include <errno.h>
#include <signal.h>
ModuleBase::Descriptor CdcAcmAutostart::desc{task_spawn, custom_command, print_usage};
#define USB_DEVICE_PATH "/dev/ttyACM0"
@@ -77,11 +80,11 @@ CdcAcmAutostart::~CdcAcmAutostart()
{
PX4_INFO("Stopping CDC/ACM autostart");
if (_ttyacm_fd >= 0) {
px4_close(_ttyacm_fd);
_ttyacm_fd = -1;
if (_active_protocol == UsbProtocol::mavlink) {
stop_mavlink();
}
close_ttyacm();
ScheduleClear();
}
@@ -129,7 +132,7 @@ void CdcAcmAutostart::run_state_machine()
#endif
// Do not reconfigure USB while flying
actuator_armed_s report;
actuator_armed_s report{};
_actuator_armed_sub.copy(&report);
if (report.armed) {
@@ -160,16 +163,50 @@ void CdcAcmAutostart::run_state_machine()
ScheduleDelayed(_reschedule_time);
}
void CdcAcmAutostart::close_ttyacm()
{
if (_ttyacm_fd >= 0) {
px4_close(_ttyacm_fd);
_ttyacm_fd = -1;
}
}
bool CdcAcmAutostart::process_running(int pid) const
{
if (pid <= 0) {
return false;
}
// kill(pid, 0) succeeds when the task exists (NuttX/POSIX).
return kill(pid, 0) == 0;
}
void CdcAcmAutostart::state_connected()
{
if (!_vbus_present && !_vbus_present_prev && (_active_protocol == UsbProtocol::mavlink)) {
PX4_DEBUG("lost vbus!");
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();
// Lost VBUS for two consecutive samples: tear down.
if (!_vbus_present && !_vbus_present_prev) {
PX4_DEBUG("lost vbus");
if (_active_protocol == UsbProtocol::mavlink) {
stop_mavlink();
}
_state = UsbAutoStartState::disconnecting;
return;
}
// SYS_USB_AUTO=2 (and autodetect mavlink): if the mavlink task exited after a
// successful spawn (e.g. failed to open the UART after retries), restart it.
if (_active_protocol == UsbProtocol::mavlink && !process_running(_mavlink_pid)) {
PX4_WARN("mavlink on %s exited, restarting", USB_DEVICE_PATH);
_mavlink_pid = -1;
if (start_mavlink()) {
// Stay connected; next tick re-checks the new PID.
} else {
_state = UsbAutoStartState::disconnecting;
_reschedule_time = 100_ms;
}
}
}
@@ -181,6 +218,7 @@ void CdcAcmAutostart::state_disconnected()
if (sercon_main(0, nullptr) == EXIT_SUCCESS) {
_state = UsbAutoStartState::connecting;
PX4_DEBUG("state connecting");
// Give CDC/ACM time to create /dev/ttyACM0 before mavlink open retries.
_reschedule_time = 1_s;
}
@@ -203,18 +241,11 @@ void CdcAcmAutostart::state_connecting()
goto fail;
}
if (_ttyacm_fd < 0) {
PX4_DEBUG("opening port");
_ttyacm_fd = px4_open(USB_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
}
if (_ttyacm_fd < 0) {
PX4_DEBUG("can't open port");
// fail silently and keep trying to open the port
return;
}
// SYS_USB_AUTO=2: always start MAVLink. Do not open/hold the port here —
// mavlink opens it itself (with open retries). Holding O_RDONLY previously
// was unnecessary and left a second open on the device for the life of the link.
if (_sys_usb_auto.get() == 2) {
close_ttyacm();
PX4_INFO("Starting mavlink on %s (SYS_USB_AUTO=2)", USB_DEVICE_PATH);
if (start_mavlink()) {
@@ -227,20 +258,31 @@ void CdcAcmAutostart::state_connecting()
}
return;
}
} else if (_sys_usb_auto.get() == 0) {
// Do nothing
// SYS_USB_AUTO=0: sercon only, no protocol.
if (_sys_usb_auto.get() == 0) {
close_ttyacm();
_state = UsbAutoStartState::connected;
_active_protocol = UsbProtocol::none;
return;
}
// Otherwise autodetect
// SYS_USB_AUTO=1: autodetect from host bytes.
if (_ttyacm_fd < 0) {
PX4_DEBUG("opening port");
_ttyacm_fd = px4_open(USB_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
}
if (_ttyacm_fd < 0) {
PX4_DEBUG("can't open port");
// Port not ready yet (e.g. USB power-only / not fully enumerated). Keep trying.
return;
}
if ((px4_ioctl(_ttyacm_fd, FIONREAD, &bytes_available) != PX4_OK) ||
(bytes_available < 3)) {
PX4_DEBUG("bytes_available: %d", bytes_available);
// Return back to connecting state to check again soon
return;
}
@@ -263,7 +305,6 @@ void CdcAcmAutostart::state_connecting()
if (_bytes_read <= 0) {
PX4_DEBUG("no _bytes_read");
// Return back to connecting state to check again soon
return;
}
@@ -273,8 +314,9 @@ void CdcAcmAutostart::state_connecting()
baudrate = cfgetspeed(&uart_config);
#endif
PX4_DEBUG("_bytes_read %d", _bytes_read);
px4_close(_ttyacm_fd);
_ttyacm_fd = -1;
// Release the probe open before starting any protocol that needs the device.
close_ttyacm();
// Parse for mavlink reboot command
if (scan_buffer_for_mavlink_reboot()) {
@@ -332,13 +374,7 @@ void CdcAcmAutostart::state_connecting()
fail:
PX4_DEBUG("fail...");
// VBUS not present, open failed
if (_ttyacm_fd >= 0) {
px4_close(_ttyacm_fd);
_ttyacm_fd = -1;
}
close_ttyacm();
_state = UsbAutoStartState::disconnecting;
}
@@ -346,10 +382,8 @@ void CdcAcmAutostart::state_disconnecting()
{
PX4_DEBUG("state_disconnecting");
if (_ttyacm_fd > 0) {
px4_close(_ttyacm_fd);
_ttyacm_fd = -1;
}
close_ttyacm();
_mavlink_pid = -1;
// Disconnect serial
serdis_main(0, NULL);
@@ -415,10 +449,10 @@ bool CdcAcmAutostart::scan_buffer_for_mavlink_reboot()
bool CdcAcmAutostart::scan_buffer_for_mavlink_heartbeat()
{
static constexpr int MAVLINK_HEARTBEAT_MIN_LENGTH = 9;
bool start_mavlink = false;
bool launch_mavlink = false;
if (_bytes_read < MAVLINK_HEARTBEAT_MIN_LENGTH) {
return start_mavlink;
return launch_mavlink;
}
// scan buffer for mavlink HEARTBEAT (v1 & v2)
@@ -432,7 +466,8 @@ bool CdcAcmAutostart::scan_buffer_for_mavlink_heartbeat()
// buffer[5]: mavlink message id (0 for HEARTBEAT)
PX4_INFO("%s: launching mavlink (HEARTBEAT v1 from SYSID:%d COMPID:%d)",
USB_DEVICE_PATH, _buffer[i + 3], _buffer[i + 4]);
start_mavlink = true;
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)) {
@@ -444,11 +479,12 @@ bool CdcAcmAutostart::scan_buffer_for_mavlink_heartbeat()
// buffer[7:9]: mavlink message id (0 for HEARTBEAT)
PX4_INFO("%s: launching mavlink (HEARTBEAT v2 from SYSID:%d COMPID:%d)",
USB_DEVICE_PATH, _buffer[i + 5], _buffer[i + 6]);
start_mavlink = true;
launch_mavlink = true;
break;
}
}
return start_mavlink;
return launch_mavlink;
}
bool CdcAcmAutostart::scan_buffer_for_carriage_returns()
@@ -481,11 +517,12 @@ bool CdcAcmAutostart::scan_buffer_for_ublox_bytes()
}
// scan buffer looking for 0xb5 0x62 which indicates the start of a packet
for (int i = 0; i < _bytes_read; i++) {
for (int i = 0; i < _bytes_read - 1; i++) {
bool ub = _buffer[i] == 0xb5 && _buffer[i + 1] == 0x62;
if (ub && ((_buffer[i + 2 ] == 0x6 && (_buffer[i + 3 ] == 0xb8 || _buffer[i + 3 ] == 0x13)) ||
(_buffer[i + 2 ] == 0xa && _buffer[i + 3 ] == 0x4))) {
if (ub && (i + 3) < _bytes_read &&
((_buffer[i + 2] == 0x6 && (_buffer[i + 3] == 0xb8 || _buffer[i + 3] == 0x13)) ||
(_buffer[i + 2] == 0xa && _buffer[i + 3] == 0x4))) {
PX4_INFO("%s: launching ublox serial passthru", USB_DEVICE_PATH);
success = true;
break;
@@ -497,52 +534,67 @@ bool CdcAcmAutostart::scan_buffer_for_ublox_bytes()
bool CdcAcmAutostart::start_mavlink()
{
bool success = false;
char mavlink_mode_string[3];
snprintf(mavlink_mode_string, sizeof(mavlink_mode_string), "%ld", _usb_mav_mode.get());
static const char *argv[] {"mavlink", "start", "-d", USB_DEVICE_PATH, "-m", mavlink_mode_string, nullptr};
if (execute_process((char **)argv) > 0) {
success = true;
// Do not double-start if a prior instance is still alive.
if (process_running(_mavlink_pid)) {
return true;
}
return success;
char mavlink_mode_string[16];
snprintf(mavlink_mode_string, sizeof(mavlink_mode_string), "%ld", _usb_mav_mode.get());
// Non-static argv: NuttX task_spawn copies argv into the new task stack before return.
char *argv[] { (char *)"mavlink", (char *)"start", (char *)"-d", (char *)USB_DEVICE_PATH,
(char *)"-m", mavlink_mode_string, nullptr
};
const int pid = execute_process(argv);
if (pid > 0) {
_mavlink_pid = pid;
return true;
}
_mavlink_pid = -1;
return false;
}
void CdcAcmAutostart::stop_mavlink()
{
char *stop_argv[] { (char *)"mavlink", (char *)"stop", (char *)"-d", (char *)USB_DEVICE_PATH, nullptr };
execute_process(stop_argv);
_mavlink_pid = -1;
}
bool CdcAcmAutostart::start_nsh()
{
bool success = false;
static const char *argv[] {"nshterm", USB_DEVICE_PATH, nullptr};
if (execute_process((char **)argv) > 0) {
success = true;
}
return success;
char *argv[] { (char *)"nshterm", (char *)USB_DEVICE_PATH, nullptr };
return execute_process(argv) > 0;
}
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
bool CdcAcmAutostart::start_ublox_serial_passthru(speed_t baudrate)
{
bool success = false;
char baudstring[16];
snprintf(baudstring, sizeof(baudstring), "%ld", baudrate);
// Stop the GPS driver first
static const char *gps_argv[] {"gps", "stop", nullptr};
static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", SERIAL_PASSTHRU_UBLOX_DEV, nullptr};
char *gps_argv[] { (char *)"gps", (char *)"stop", nullptr };
char *passthru_argv[] {
(char *)"serial_passthru", (char *)"start", (char *)"-t", (char *)"-b", baudstring,
(char *)"-e", (char *)USB_DEVICE_PATH, (char *)"-d", (char *)SERIAL_PASSTHRU_UBLOX_DEV, nullptr
};
if (execute_process((char **)gps_argv) > 0) {
if (execute_process((char **)passthru_argv) > 0) {
success = true;
if (execute_process(gps_argv) > 0) {
if (execute_process(passthru_argv) > 0) {
return true;
}
}
return success;
return false;
}
#endif
int CdcAcmAutostart::execute_process(char **argv)
int CdcAcmAutostart::execute_process(char *const *argv)
{
int pid = -1;
sched_lock();
@@ -630,9 +682,15 @@ int CdcAcmAutostart::print_status()
break;
}
PX4_INFO("Running");
PX4_INFO("State: %s", state);
PX4_INFO("Protocol: %s", protocol);
PX4_INFO("VBUS: %s", _vbus_present ? "present" : "absent");
PX4_INFO("SYS_USB_AUTO: %ld", _sys_usb_auto.get());
if (_active_protocol == UsbProtocol::mavlink) {
PX4_INFO("mavlink pid: %d (%s)", _mavlink_pid, process_running(_mavlink_pid) ? "running" : "not running");
}
return PX4_OK;
}
@@ -645,10 +703,13 @@ int CdcAcmAutostart::print_usage(const char *reason)
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
This module listens on USB and auto-configures the protocol depending on the bytes received.
The supported protocols are: MAVLink, nsh, and ublox serial passthrough. If the parameter SYS_USB_AUTO=2
the module will only try to start mavlink as long as the USB VBUS is detected. Otherwise it will spin
and continue to check for VBUS and start mavlink once it is detected.
Manages the USB CDC/ACM serial device (`/dev/ttyACM0`).
`SYS_USB_AUTO` selects the protocol policy once USB VBUS is detected:
- `0` Disabled: bring up the USB serial device only.
- `1` Auto-detect: wait for host bytes and start MAVLink, nsh, or u-blox passthrough.
- `2` MAVLink (default): start MAVLink immediately so the autopilot transmits first
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("cdcacm_autostart", "system");
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2023-2026 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
@@ -93,31 +93,36 @@ private:
void state_connected();
void state_disconnecting();
void close_ttyacm();
bool process_running(int pid) const;
bool scan_buffer_for_mavlink_reboot();
bool scan_buffer_for_mavlink_heartbeat();
bool scan_buffer_for_carriage_returns();
bool scan_buffer_for_ublox_bytes();
bool start_mavlink();
void stop_mavlink();
bool start_nsh();
#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX)
bool start_ublox_serial_passthru(speed_t baudrate);
#endif
int execute_process(char **argv);
int execute_process(char *const *argv);
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 500_ms};
UsbAutoStartState _state{UsbAutoStartState::disconnected};
UsbProtocol _active_protocol{UsbProtocol::none};
bool _vbus_present = false;
bool _vbus_present_prev = false;
int _ttyacm_fd = -1;
bool _vbus_present{false};
bool _vbus_present_prev{false};
int _ttyacm_fd{-1};
int _mavlink_pid{-1};
char _buffer[80] = {};
int _bytes_read = 0;
char _buffer[80] {};
int _bytes_read{0};
uint32_t _reschedule_time = 0;
uint32_t _reschedule_time{0};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_USB_AUTO>) _sys_usb_auto,