forked from Archive/PX4-Autopilot
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:
parent
07e307b074
commit
6d78054f50
|
@ -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'
|
||||
|
|
|
@ -16,11 +16,6 @@ set +e
|
|||
#------------------------------------------------------------------------------
|
||||
set R /
|
||||
|
||||
#
|
||||
# Start CDC/ACM serial driver.
|
||||
#
|
||||
sercon
|
||||
|
||||
#
|
||||
# Print full system version.
|
||||
#
|
||||
|
|
|
@ -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.
|
||||
#
|
||||
|
|
|
@ -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.
|
||||
#
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# Bitcraze Crazyflie specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# Bitcraze Crazyflie specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -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
|
||||
|
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# Board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# Durandal specific specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# KakuteF7 specific specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv5 specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# Board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# mRo x21 specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -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
|
|
@ -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
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv5 specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# OmnibusF4SD specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv2 specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv3 specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,8 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv4 specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
||||
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv4pro specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv5 specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -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
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv6U specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# PX4 FMUv6X specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -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
|
|
@ -1,7 +0,0 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# UVify UVF4 specific board MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
# Start MAVLink on the USB port
|
||||
mavlink start -d /dev/ttyACM0
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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()) {
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue