mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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:
1 parent
ea63910683
commit
f05739035e
4 files changed
+154
-464
No files matched your search
@@ -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(¶m1_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
|
||||
@@ -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,
|
||||
|
||||
Reference in new issue
Block a user