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 /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 /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 "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 streams" || true'
|
||||||
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" || 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 "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 "mtd status"'
|
||||||
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show" || true'
|
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 /
|
set R /
|
||||||
|
|
||||||
#
|
|
||||||
# Start CDC/ACM serial driver.
|
|
||||||
#
|
|
||||||
sercon
|
|
||||||
|
|
||||||
#
|
#
|
||||||
# Print full system version.
|
# Print full system version.
|
||||||
#
|
#
|
||||||
|
|
|
@ -49,11 +49,6 @@ set STARTUP_TUNE 1
|
||||||
set USE_IO no
|
set USE_IO no
|
||||||
set VEHICLE_TYPE none
|
set VEHICLE_TYPE none
|
||||||
|
|
||||||
#
|
|
||||||
# Start CDC/ACM serial driver.
|
|
||||||
#
|
|
||||||
sercon
|
|
||||||
|
|
||||||
#
|
#
|
||||||
# Print full system version.
|
# Print full system version.
|
||||||
#
|
#
|
||||||
|
|
|
@ -13,14 +13,6 @@ then
|
||||||
led_control on -c blue
|
led_control on -c blue
|
||||||
fi
|
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.
|
# Try to mount the microSD card.
|
||||||
#
|
#
|
||||||
|
|
|
@ -45,6 +45,7 @@ class MavlinkSerialPort():
|
||||||
self.port = devnum
|
self.port = devnum
|
||||||
self.debug("Connecting with MAVLink to %s ..." % portname)
|
self.debug("Connecting with MAVLink to %s ..." % portname)
|
||||||
self.mav = mavutil.mavlink_connection(portname, autoreconnect=True, baud=baudrate)
|
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.mav.wait_heartbeat()
|
||||||
self.debug("HEARTBEAT OK\n")
|
self.debug("HEARTBEAT OK\n")
|
||||||
self.debug("Locked serial device\n")
|
self.debug("Locked serial device\n")
|
||||||
|
@ -226,8 +227,7 @@ def main():
|
||||||
# handle heartbeat sending
|
# handle heartbeat sending
|
||||||
heartbeat_time = timer()
|
heartbeat_time = timer()
|
||||||
if heartbeat_time > next_heartbeat_time:
|
if heartbeat_time > next_heartbeat_time:
|
||||||
mav_serialport.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS,
|
mav_serialport.mav.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GENERIC, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 0, 0, 0)
|
||||||
mavutil.mavlink.MAV_AUTOPILOT_GENERIC, 0, 0, 0)
|
|
||||||
next_heartbeat_time = heartbeat_time + 1
|
next_heartbeat_time = heartbeat_time + 1
|
||||||
|
|
||||||
except serial.serialutil.SerialException as e:
|
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
|
# optical flow
|
||||||
mavlink start -d /dev/ttyS2 -m custom -b 500000
|
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.
|
# 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
|
# Start ADS-B receiver mavlink connection if console not present
|
||||||
if [ ! -e /dev/console ]
|
if [ ! -e /dev/console ]
|
||||||
then
|
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>
|
#include <px4_platform_common/crypto.h>
|
||||||
#endif
|
#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()
|
int px4_platform_init()
|
||||||
{
|
{
|
||||||
|
|
||||||
|
@ -140,6 +348,10 @@ int px4_platform_init()
|
||||||
|
|
||||||
px4_log_initialize();
|
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;
|
return PX4_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1911,31 +1911,6 @@ Commander::run()
|
||||||
_status_flags.condition_power_input_valid = true;
|
_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;
|
_system_power_usb_connected = system_power.usb_connected;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -127,34 +127,53 @@ Mavlink::Mavlink() :
|
||||||
}
|
}
|
||||||
|
|
||||||
_vehicle_command_sub.subscribe();
|
_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()
|
Mavlink::~Mavlink()
|
||||||
{
|
{
|
||||||
if (_task_running) {
|
if (running()) {
|
||||||
/* task wakes up every 10ms or so at the longest */
|
/* 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 */
|
/* wait for a second for the task to quit at our request */
|
||||||
unsigned i = 0;
|
unsigned i = 0;
|
||||||
|
|
||||||
do {
|
do {
|
||||||
/* wait 20ms */
|
/* wait at least 1 second (10ms * 10) */
|
||||||
px4_usleep(20000);
|
px4_usleep(10000);
|
||||||
|
|
||||||
/* if we have given up, kill it */
|
/* if we have given up, kill it */
|
||||||
if (++i > 50) {
|
if (++i > 100) {
|
||||||
//TODO store main task handle in Mavlink instance to allow killing task
|
PX4_ERR("mavlink didn't stop, killing task %d", _task_id);
|
||||||
//task_delete(_mavlink_task);
|
px4_task_delete(_task_id);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
} while (_task_running);
|
} while (running());
|
||||||
}
|
}
|
||||||
|
|
||||||
if (_instance_id >= 0) {
|
if (_instance_id >= 0) {
|
||||||
mavlink_module_instances[_instance_id] = nullptr;
|
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_perf);
|
||||||
perf_free(_loop_interval_perf);
|
perf_free(_loop_interval_perf);
|
||||||
perf_free(_send_byte_error_perf);
|
perf_free(_send_byte_error_perf);
|
||||||
|
@ -230,6 +249,20 @@ Mavlink::set_instance_id()
|
||||||
{
|
{
|
||||||
LockGuard lg{mavlink_module_mutex};
|
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++) {
|
for (int instance_id = 0; instance_id < MAVLINK_COMM_NUM_BUFFERS; instance_id++) {
|
||||||
if (mavlink_module_instances[instance_id] == nullptr) {
|
if (mavlink_module_instances[instance_id] == nullptr) {
|
||||||
mavlink_module_instances[instance_id] = this;
|
mavlink_module_instances[instance_id] = this;
|
||||||
|
@ -312,9 +345,9 @@ Mavlink::destroy_all_instances()
|
||||||
for (Mavlink *inst_to_del : mavlink_module_instances) {
|
for (Mavlink *inst_to_del : mavlink_module_instances) {
|
||||||
if (inst_to_del != nullptr) {
|
if (inst_to_del != nullptr) {
|
||||||
/* set flag to stop thread and wait for all threads to finish */
|
/* 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(".");
|
printf(".");
|
||||||
fflush(stdout);
|
fflush(stdout);
|
||||||
px4_usleep(10000);
|
px4_usleep(10000);
|
||||||
|
@ -527,44 +560,9 @@ Mavlink::mavlink_open_uart(const int baud, const char *uart_name, const FLOW_CON
|
||||||
return -EINVAL;
|
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 */
|
/* open uart */
|
||||||
_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
|
_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
|
* Return here in the iridium mode since the iridium driver does not
|
||||||
* support the subsequent function calls.
|
* 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) */
|
/* Clear ONLCR flag (which appends a CR for every LF) */
|
||||||
uart_config.c_oflag &= ~ONLCR;
|
uart_config.c_oflag &= ~ONLCR;
|
||||||
|
|
||||||
if (!_is_usb_uart) {
|
/* Set baud rate */
|
||||||
|
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
||||||
/* Set baud rate */
|
PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state);
|
||||||
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
|
::close(_uart_fd);
|
||||||
PX4_ERR("ERR SET BAUD %s: %d\n", uart_name, termios_state);
|
return -1;
|
||||||
::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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
|
#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
|
int
|
||||||
Mavlink::setup_flow_control(enum FLOW_CONTROL_MODE mode)
|
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;
|
struct termios uart_config;
|
||||||
|
|
||||||
int ret = tcgetattr(_uart_fd, &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,
|
/* orb subscription must be done from the main thread,
|
||||||
* set _subscribe_to_stream and _subscribe_to_stream_rate fields
|
* set _subscribe_to_stream and _subscribe_to_stream_rate fields
|
||||||
* which polled in mavlink main loop */
|
* which polled in mavlink main loop */
|
||||||
if (!_task_should_exit) {
|
if (!should_exit()) {
|
||||||
/* wait for previous subscription completion */
|
/* wait for previous subscription completion */
|
||||||
while (_subscribe_to_stream != nullptr) {
|
while (_subscribe_to_stream != nullptr) {
|
||||||
px4_usleep(MAIN_LOOP_DELAY / 2);
|
px4_usleep(MAIN_LOOP_DELAY / 2);
|
||||||
|
@ -2127,17 +2109,23 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
/* USB serial is indicated by /dev/ttyACMx */
|
/* 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) {
|
if (_datarate == 0) {
|
||||||
_datarate = 800000;
|
_datarate = 800000;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* USB has no baudrate, but use a magic number for 'fast' */
|
||||||
|
_baudrate = 2000000;
|
||||||
|
|
||||||
if (_mode == MAVLINK_MODE_COUNT) {
|
if (_mode == MAVLINK_MODE_COUNT) {
|
||||||
_mode = MAVLINK_MODE_CONFIG;
|
_mode = MAVLINK_MODE_CONFIG;
|
||||||
}
|
}
|
||||||
|
|
||||||
_ftp_on = true;
|
_ftp_on = true;
|
||||||
_is_usb_uart = 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) {
|
if (_mode == MAVLINK_MODE_COUNT) {
|
||||||
|
@ -2183,6 +2171,12 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
if (!set_instance_id()) {
|
if (!set_instance_id()) {
|
||||||
PX4_ERR("no instances available");
|
PX4_ERR("no instances available");
|
||||||
return PX4_ERROR;
|
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();
|
set_channel();
|
||||||
|
@ -2249,13 +2243,9 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
if (get_protocol() == Protocol::SERIAL) {
|
if (get_protocol() == Protocol::SERIAL) {
|
||||||
_uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control);
|
_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);
|
PX4_ERR("could not open %s", _device_name);
|
||||||
return PX4_ERROR;
|
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
|
#endif // MAVLINK_UDP
|
||||||
|
|
||||||
|
_task_id = px4_getpid();
|
||||||
|
|
||||||
/* if the protocol is serial, we send the system version blindly */
|
/* if the protocol is serial, we send the system version blindly */
|
||||||
if (get_protocol() == Protocol::SERIAL) {
|
if (get_protocol() == Protocol::SERIAL) {
|
||||||
send_autopilot_capabilities();
|
send_autopilot_capabilities();
|
||||||
|
@ -2275,17 +2267,11 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
|
|
||||||
_receiver.start();
|
_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
|
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();
|
_mavlink_start_time = hrt_absolute_time();
|
||||||
|
|
||||||
while (!_task_should_exit) {
|
while (!should_exit()) {
|
||||||
/* main loop */
|
/* main loop */
|
||||||
px4_usleep(_main_loop_delay);
|
px4_usleep(_main_loop_delay);
|
||||||
|
|
||||||
|
@ -2504,23 +2490,27 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
/* handle new events */
|
/* handle new events */
|
||||||
if (should_check_events) {
|
if (check_events()) {
|
||||||
event_s orb_event;
|
if (_event_sub.updated()) {
|
||||||
|
LockGuard lg{mavlink_module_mutex};
|
||||||
|
|
||||||
while (event_sub.update(&orb_event)) {
|
event_s orb_event;
|
||||||
if (events::externalLogLevel(orb_event.log_levels) == events::LogLevel::Disabled) {
|
|
||||||
++event_sequence_offset; // skip this event
|
|
||||||
|
|
||||||
} else {
|
while (_event_sub.update(&orb_event)) {
|
||||||
events::Event e;
|
if (events::externalLogLevel(orb_event.log_levels) == events::LogLevel::Disabled) {
|
||||||
e.id = orb_event.id;
|
++event_sequence_offset; // skip this event
|
||||||
e.timestamp_ms = orb_event.timestamp / 1000;
|
|
||||||
e.sequence = orb_event.event_sequence - event_sequence_offset;
|
} else {
|
||||||
e.log_levels = orb_event.log_levels;
|
events::Event e;
|
||||||
static_assert(sizeof(e.arguments) == sizeof(orb_event.arguments),
|
e.id = orb_event.id;
|
||||||
"uorb message event: arguments size mismatch");
|
e.timestamp_ms = orb_event.timestamp / 1000;
|
||||||
memcpy(e.arguments, orb_event.arguments, sizeof(orb_event.arguments));
|
e.sequence = orb_event.event_sequence - event_sequence_offset;
|
||||||
_event_buffer->insert_event(e);
|
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 */
|
/* delete streams */
|
||||||
_streams.clear();
|
_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 */
|
/* discard all pending data, as close() might block otherwise on NuttX with flow control enabled */
|
||||||
tcflush(_uart_fd, TCIOFLUSH);
|
tcflush(_uart_fd, TCIOFLUSH);
|
||||||
/* close UART */
|
/* close UART */
|
||||||
|
@ -2633,8 +2623,6 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
pthread_mutex_destroy(&_send_mutex);
|
pthread_mutex_destroy(&_send_mutex);
|
||||||
pthread_mutex_destroy(&_radio_status_mutex);
|
pthread_mutex_destroy(&_radio_status_mutex);
|
||||||
|
|
||||||
_task_running = false;
|
|
||||||
|
|
||||||
PX4_INFO("exiting channel %i", (int)_channel);
|
PX4_INFO("exiting channel %i", (int)_channel);
|
||||||
|
|
||||||
return OK;
|
return OK;
|
||||||
|
@ -2791,16 +2779,15 @@ int Mavlink::start_helper(int argc, char *argv[])
|
||||||
int res;
|
int res;
|
||||||
|
|
||||||
if (!instance) {
|
if (!instance) {
|
||||||
|
|
||||||
/* out of memory */
|
/* out of memory */
|
||||||
res = -ENOMEM;
|
res = -ENOMEM;
|
||||||
PX4_ERR("OUT OF MEM");
|
PX4_ERR("OUT OF MEM");
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* this will actually only return once MAVLink exits */
|
/* this will actually only return once MAVLink exits */
|
||||||
|
instance->_task_running.store(true);
|
||||||
res = instance->task_main(argc, argv);
|
res = instance->task_main(argc, argv);
|
||||||
instance->_task_running = false;
|
instance->_task_running.store(false);
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
|
@ -2838,14 +2825,12 @@ Mavlink::start(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
// Instantiate thread
|
// Instantiate thread
|
||||||
char buf[24];
|
|
||||||
sprintf(buf, "mavlink_if%d", ic);
|
|
||||||
|
|
||||||
// This is where the control flow splits
|
// This is where the control flow splits
|
||||||
// between the starting task and the spawned
|
// between the starting task and the spawned
|
||||||
// task - start_helper() only returns
|
// task - start_helper() only returns
|
||||||
// when the started task exits.
|
// when the started task exits.
|
||||||
px4_task_spawn_cmd(buf,
|
px4_task_spawn_cmd("mavlink_main",
|
||||||
SCHED_DEFAULT,
|
SCHED_DEFAULT,
|
||||||
SCHED_PRIORITY_DEFAULT,
|
SCHED_PRIORITY_DEFAULT,
|
||||||
PX4_STACK_ADJUSTED(2896) + MAVLINK_NET_ADDED_STACK,
|
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 rx errors:\t%" PRIu16 "\n", _rstatus.rxerrors);
|
||||||
printf("\t fixed:\t%" PRIu16 "\n", _rstatus.fix);
|
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");
|
printf("USB CDC\n");
|
||||||
|
|
||||||
} else {
|
} 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
|
int
|
||||||
Mavlink::stream_command(int argc, char *argv[])
|
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-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_COMMAND_DESCR("status", "Print status for all instances");
|
||||||
PRINT_MODULE_USAGE_ARG("streams", "Print all enabled streams", true);
|
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")) {
|
if (!strcmp(argv[1], "start")) {
|
||||||
return Mavlink::start(argc, argv);
|
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")) {
|
} else if (!strcmp(argv[1], "stop-all")) {
|
||||||
return Mavlink::destroy_all_instances();
|
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;
|
bool show_streams_status = argc > 2 && strcmp(argv[2], "streams") == 0;
|
||||||
return Mavlink::get_status_all_instances(show_streams_status);
|
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")) {
|
} else if (!strcmp(argv[1], "stream")) {
|
||||||
return Mavlink::stream_command(argc, argv);
|
return Mavlink::stream_command(argc, argv);
|
||||||
|
|
||||||
|
|
|
@ -125,6 +125,14 @@ public:
|
||||||
*/
|
*/
|
||||||
static int start(int argc, char *argv[]);
|
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.
|
* Display the mavlink status.
|
||||||
*/
|
*/
|
||||||
|
@ -135,6 +143,7 @@ public:
|
||||||
*/
|
*/
|
||||||
void display_status_streams();
|
void display_status_streams();
|
||||||
|
|
||||||
|
static int stop_command(int argc, char *argv[]);
|
||||||
static int stream_command(int argc, char *argv[]);
|
static int stream_command(int argc, char *argv[]);
|
||||||
|
|
||||||
static int instance_count();
|
static int instance_count();
|
||||||
|
@ -166,6 +175,10 @@ public:
|
||||||
|
|
||||||
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
|
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; }
|
int get_uart_fd() const { return _uart_fd; }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -455,8 +468,6 @@ public:
|
||||||
|
|
||||||
int get_socket_fd() { return _socket_fd; };
|
int get_socket_fd() { return _socket_fd; };
|
||||||
|
|
||||||
bool _task_should_exit{false}; /**< Mavlink task should exit iff true. */
|
|
||||||
|
|
||||||
#if defined(MAVLINK_UDP)
|
#if defined(MAVLINK_UDP)
|
||||||
unsigned short get_network_port() { return _network_port; }
|
unsigned short get_network_port() { return _network_port; }
|
||||||
|
|
||||||
|
@ -529,7 +540,12 @@ public:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
MavlinkReceiver _receiver;
|
MavlinkReceiver _receiver;
|
||||||
|
|
||||||
int _instance_id{-1};
|
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{true};
|
||||||
bool _transmitting_enabled_commanded{false};
|
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::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::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::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||||
uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)};
|
uORB::Subscription _vehicle_command_ack_sub{ORB_ID(vehicle_command_ack)};
|
||||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||||
|
|
||||||
bool _task_running{true};
|
|
||||||
static bool _boot_complete;
|
static bool _boot_complete;
|
||||||
|
|
||||||
static constexpr int MAVLINK_MIN_INTERVAL{1500};
|
static constexpr int MAVLINK_MIN_INTERVAL{1500};
|
||||||
|
@ -562,6 +578,8 @@ private:
|
||||||
bool _wait_to_transmit{false}; /**< Wait to transmit until received messages. */
|
bool _wait_to_transmit{false}; /**< Wait to transmit until received messages. */
|
||||||
bool _received_messages{false}; /**< Whether we've received valid mavlink 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 */
|
unsigned _main_loop_delay{1000}; /**< mainloop delay, depends on data rate */
|
||||||
|
|
||||||
List<MavlinkStream *> _streams;
|
List<MavlinkStream *> _streams;
|
||||||
|
|
|
@ -3179,7 +3179,7 @@ MavlinkReceiver::run()
|
||||||
ssize_t nread = 0;
|
ssize_t nread = 0;
|
||||||
hrt_abstime last_send_update = 0;
|
hrt_abstime last_send_update = 0;
|
||||||
|
|
||||||
while (!_mavlink->_task_should_exit) {
|
while (!_mavlink->should_exit()) {
|
||||||
|
|
||||||
// check for parameter updates
|
// check for parameter updates
|
||||||
if (_parameter_update_sub.updated()) {
|
if (_parameter_update_sub.updated()) {
|
||||||
|
|
|
@ -131,6 +131,8 @@ public:
|
||||||
void enable_message_statistics() { _message_statistics_enabled = true; }
|
void enable_message_statistics() { _message_statistics_enabled = true; }
|
||||||
void print_detailed_rx_stats() const;
|
void print_detailed_rx_stats() const;
|
||||||
|
|
||||||
|
void request_stop() { _should_exit.store(true); }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static void *start_trampoline(void *context);
|
static void *start_trampoline(void *context);
|
||||||
void run();
|
void run();
|
||||||
|
|
|
@ -50,11 +50,8 @@
|
||||||
#include <systemlib/err.h>
|
#include <systemlib/err.h>
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
|
||||||
#include <uORB/topics/actuator_armed.h>
|
|
||||||
|
|
||||||
__EXPORT int nshterm_main(int argc, char *argv[]);
|
__EXPORT int nshterm_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
|
||||||
static void print_usage(void)
|
static void print_usage(void)
|
||||||
{
|
{
|
||||||
PRINT_MODULE_DESCRIPTION("Start an NSH shell on a given port.\n"
|
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);
|
PRINT_MODULE_USAGE_ARG("<file:dev>", "Device on which to start the shell (eg. /dev/ttyACM0)", false);
|
||||||
}
|
}
|
||||||
|
|
||||||
int
|
int nshterm_main(int argc, char *argv[])
|
||||||
nshterm_main(int argc, char *argv[])
|
|
||||||
{
|
{
|
||||||
if (argc < 2) {
|
if (argc < 2) {
|
||||||
print_usage();
|
print_usage();
|
||||||
return 1;
|
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 */
|
/* set up the serial port with output processing */
|
||||||
|
int fd = open(argv[1], O_RDWR);
|
||||||
|
|
||||||
/* Try to set baud rate */
|
/* Try to set baud rate */
|
||||||
struct termios uart_config;
|
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 */
|
/* Back up the original uart configuration to restore it after exit */
|
||||||
if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
|
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);
|
close(fd);
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
@ -140,7 +89,7 @@ nshterm_main(int argc, char *argv[])
|
||||||
uart_config.c_oflag |= (ONLCR | OPOST);
|
uart_config.c_oflag |= (ONLCR | OPOST);
|
||||||
|
|
||||||
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
|
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);
|
close(fd);
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
@ -157,5 +106,7 @@ nshterm_main(int argc, char *argv[])
|
||||||
|
|
||||||
close(fd);
|
close(fd);
|
||||||
|
|
||||||
|
PX4_INFO("exiting");
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue