From 6d78054f5001548ea2f1d48bb59966e2352bc9fe Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 3 Oct 2021 15:32:54 -0400 Subject: [PATCH] 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 --- .ci/Jenkinsfile-hardware | 4 +- ROMFS/cannode/init.d/rcS | 5 - ROMFS/px4fmu_common/init.d/rcS | 5 - ROMFS/px4fmu_test/init.d/rcS | 8 - Tools/mavlink_shell.py | 4 +- .../airmind/mindpx-v2/init/rc.board_mavlink | 7 - boards/atl/mantis-edu/init/rc.board_mavlink | 3 - .../bitcraze/crazyflie/init/rc.board_mavlink | 7 - .../crazyflie21/init/rc.board_mavlink | 7 - boards/cuav/nora/init/rc.board_mavlink | 7 - boards/cuav/x7pro/init/rc.board_mavlink | 7 - .../cubeorange/init/rc.board_mavlink | 3 - .../cubeyellow/init/rc.board_mavlink | 7 - .../holybro/durandal-v1/init/rc.board_mavlink | 7 - boards/holybro/kakutef7/init/rc.board_mavlink | 7 - boards/holybro/pix32v5/init/rc.board_mavlink | 7 - boards/modalai/fc-v1/init/rc.board_mavlink | 7 - boards/modalai/fc-v2/init/rc.board_mavlink | 7 - .../ctrl-zero-f7-oem/init/rc.board_mavlink | 7 - boards/mro/ctrl-zero-f7/init/rc.board_mavlink | 7 - .../ctrl-zero-h7-oem/init/rc.board_mavlink | 7 - boards/mro/ctrl-zero-h7/init/rc.board_mavlink | 7 - boards/mro/pixracerpro/init/rc.board_mavlink | 7 - boards/mro/x21-777/init/rc.board_mavlink | 7 - boards/mro/x21/init/rc.board_mavlink | 7 - boards/nxp/fmuk66-e/init/rc.board_mavlink | 7 - boards/nxp/fmuk66-v3/init/rc.board_mavlink | 7 - boards/nxp/fmurt1062-v1/init/rc.board_mavlink | 7 - boards/omnibus/f4sd/init/rc.board_mavlink | 7 - boards/px4/fmu-v2/init/rc.board_mavlink | 7 - boards/px4/fmu-v3/init/rc.board_mavlink | 7 - boards/px4/fmu-v4/init/rc.board_mavlink | 8 - boards/px4/fmu-v4pro/init/rc.board_mavlink | 7 - boards/px4/fmu-v5/init/rc.board_mavlink | 7 - boards/px4/fmu-v5x/init/rc.board_mavlink | 13 - boards/px4/fmu-v6u/init/rc.board_mavlink | 7 - boards/px4/fmu-v6x/init/rc.board_mavlink | 7 - .../spracing/h7extreme/init/rc.board_mavlink | 7 - boards/uvify/core/init/rc.board_mavlink | 7 - platforms/nuttx/src/px4/common/px4_init.cpp | 212 ++++++++++++ src/modules/commander/Commander.cpp | 25 -- src/modules/mavlink/mavlink_main.cpp | 311 +++++++++++------- src/modules/mavlink/mavlink_main.h | 24 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mavlink/mavlink_receiver.h | 2 + src/systemcmds/nshterm/nshterm.c | 61 +--- 46 files changed, 447 insertions(+), 453 deletions(-) delete mode 100644 boards/airmind/mindpx-v2/init/rc.board_mavlink delete mode 100644 boards/bitcraze/crazyflie/init/rc.board_mavlink delete mode 100644 boards/bitcraze/crazyflie21/init/rc.board_mavlink delete mode 100644 boards/cuav/nora/init/rc.board_mavlink delete mode 100644 boards/cuav/x7pro/init/rc.board_mavlink delete mode 100644 boards/cubepilot/cubeyellow/init/rc.board_mavlink delete mode 100644 boards/holybro/durandal-v1/init/rc.board_mavlink delete mode 100644 boards/holybro/kakutef7/init/rc.board_mavlink delete mode 100644 boards/holybro/pix32v5/init/rc.board_mavlink delete mode 100644 boards/modalai/fc-v1/init/rc.board_mavlink delete mode 100644 boards/modalai/fc-v2/init/rc.board_mavlink delete mode 100644 boards/mro/ctrl-zero-f7-oem/init/rc.board_mavlink delete mode 100644 boards/mro/ctrl-zero-f7/init/rc.board_mavlink delete mode 100644 boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink delete mode 100644 boards/mro/ctrl-zero-h7/init/rc.board_mavlink delete mode 100644 boards/mro/pixracerpro/init/rc.board_mavlink delete mode 100644 boards/mro/x21-777/init/rc.board_mavlink delete mode 100644 boards/mro/x21/init/rc.board_mavlink delete mode 100644 boards/nxp/fmuk66-e/init/rc.board_mavlink delete mode 100644 boards/nxp/fmuk66-v3/init/rc.board_mavlink delete mode 100644 boards/nxp/fmurt1062-v1/init/rc.board_mavlink delete mode 100644 boards/omnibus/f4sd/init/rc.board_mavlink delete mode 100644 boards/px4/fmu-v2/init/rc.board_mavlink delete mode 100644 boards/px4/fmu-v3/init/rc.board_mavlink delete mode 100644 boards/px4/fmu-v4/init/rc.board_mavlink delete mode 100644 boards/px4/fmu-v4pro/init/rc.board_mavlink delete mode 100644 boards/px4/fmu-v5/init/rc.board_mavlink delete mode 100644 boards/px4/fmu-v5x/init/rc.board_mavlink delete mode 100644 boards/px4/fmu-v6u/init/rc.board_mavlink delete mode 100644 boards/px4/fmu-v6x/init/rc.board_mavlink delete mode 100644 boards/spracing/h7extreme/init/rc.board_mavlink delete mode 100644 boards/uvify/core/init/rc.board_mavlink diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index c640452cd7db..aa3a3cd91ec5 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -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' diff --git a/ROMFS/cannode/init.d/rcS b/ROMFS/cannode/init.d/rcS index 50ad4ff15b9c..cfb5a93927ed 100644 --- a/ROMFS/cannode/init.d/rcS +++ b/ROMFS/cannode/init.d/rcS @@ -16,11 +16,6 @@ set +e #------------------------------------------------------------------------------ set R / -# -# Start CDC/ACM serial driver. -# -sercon - # # Print full system version. # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index d32a8576d67c..34aa64f5d4e8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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. # diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index de7022c56a05..2a146a3e8365 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -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. # diff --git a/Tools/mavlink_shell.py b/Tools/mavlink_shell.py index 999f445931fc..35c8d130ed5a 100755 --- a/Tools/mavlink_shell.py +++ b/Tools/mavlink_shell.py @@ -45,6 +45,7 @@ def __init__(self, portname, baudrate, devnum=0, debug=0): 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 erase_last_n_chars(N): # 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: diff --git a/boards/airmind/mindpx-v2/init/rc.board_mavlink b/boards/airmind/mindpx-v2/init/rc.board_mavlink deleted file mode 100644 index 76e6351fb08c..000000000000 --- a/boards/airmind/mindpx-v2/init/rc.board_mavlink +++ /dev/null @@ -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 diff --git a/boards/atl/mantis-edu/init/rc.board_mavlink b/boards/atl/mantis-edu/init/rc.board_mavlink index befe2646bdd2..c8bc7b224acb 100644 --- a/boards/atl/mantis-edu/init/rc.board_mavlink +++ b/boards/atl/mantis-edu/init/rc.board_mavlink @@ -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 diff --git a/boards/bitcraze/crazyflie/init/rc.board_mavlink b/boards/bitcraze/crazyflie/init/rc.board_mavlink deleted file mode 100644 index ae5089a30f4c..000000000000 --- a/boards/bitcraze/crazyflie/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Bitcraze Crazyflie specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/bitcraze/crazyflie21/init/rc.board_mavlink b/boards/bitcraze/crazyflie21/init/rc.board_mavlink deleted file mode 100644 index ae5089a30f4c..000000000000 --- a/boards/bitcraze/crazyflie21/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Bitcraze Crazyflie specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/cuav/nora/init/rc.board_mavlink b/boards/cuav/nora/init/rc.board_mavlink deleted file mode 100644 index 75c525665179..000000000000 --- a/boards/cuav/nora/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/cuav/x7pro/init/rc.board_mavlink b/boards/cuav/x7pro/init/rc.board_mavlink deleted file mode 100644 index 75c525665179..000000000000 --- a/boards/cuav/x7pro/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/cubepilot/cubeorange/init/rc.board_mavlink b/boards/cubepilot/cubeorange/init/rc.board_mavlink index 6b0f1badded9..1dd06663bdba 100644 --- a/boards/cubepilot/cubeorange/init/rc.board_mavlink +++ b/boards/cubepilot/cubeorange/init/rc.board_mavlink @@ -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 diff --git a/boards/cubepilot/cubeyellow/init/rc.board_mavlink b/boards/cubepilot/cubeyellow/init/rc.board_mavlink deleted file mode 100644 index 08fed10e71ec..000000000000 --- a/boards/cubepilot/cubeyellow/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/holybro/durandal-v1/init/rc.board_mavlink b/boards/holybro/durandal-v1/init/rc.board_mavlink deleted file mode 100644 index 92ce9ddf39b6..000000000000 --- a/boards/holybro/durandal-v1/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Durandal specific specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/holybro/kakutef7/init/rc.board_mavlink b/boards/holybro/kakutef7/init/rc.board_mavlink deleted file mode 100644 index be01959ebc2a..000000000000 --- a/boards/holybro/kakutef7/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# KakuteF7 specific specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/holybro/pix32v5/init/rc.board_mavlink b/boards/holybro/pix32v5/init/rc.board_mavlink deleted file mode 100644 index 2ec618d05e31..000000000000 --- a/boards/holybro/pix32v5/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv5 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/modalai/fc-v1/init/rc.board_mavlink b/boards/modalai/fc-v1/init/rc.board_mavlink deleted file mode 100644 index b3c20f583525..000000000000 --- a/boards/modalai/fc-v1/init/rc.board_mavlink +++ /dev/null @@ -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 diff --git a/boards/modalai/fc-v2/init/rc.board_mavlink b/boards/modalai/fc-v2/init/rc.board_mavlink deleted file mode 100644 index 4aed5925fe99..000000000000 --- a/boards/modalai/fc-v2/init/rc.board_mavlink +++ /dev/null @@ -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 diff --git a/boards/mro/ctrl-zero-f7-oem/init/rc.board_mavlink b/boards/mro/ctrl-zero-f7-oem/init/rc.board_mavlink deleted file mode 100644 index 9a4a80a6d3e5..000000000000 --- a/boards/mro/ctrl-zero-f7-oem/init/rc.board_mavlink +++ /dev/null @@ -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 diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_mavlink b/boards/mro/ctrl-zero-f7/init/rc.board_mavlink deleted file mode 100644 index 9a4a80a6d3e5..000000000000 --- a/boards/mro/ctrl-zero-f7/init/rc.board_mavlink +++ /dev/null @@ -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 diff --git a/boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink b/boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink deleted file mode 100644 index 75c525665179..000000000000 --- a/boards/mro/ctrl-zero-h7-oem/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/ctrl-zero-h7/init/rc.board_mavlink b/boards/mro/ctrl-zero-h7/init/rc.board_mavlink deleted file mode 100644 index 75c525665179..000000000000 --- a/boards/mro/ctrl-zero-h7/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/pixracerpro/init/rc.board_mavlink b/boards/mro/pixracerpro/init/rc.board_mavlink deleted file mode 100644 index 75c525665179..000000000000 --- a/boards/mro/pixracerpro/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/x21-777/init/rc.board_mavlink b/boards/mro/x21-777/init/rc.board_mavlink deleted file mode 100644 index 08fed10e71ec..000000000000 --- a/boards/mro/x21-777/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# Board specific MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/mro/x21/init/rc.board_mavlink b/boards/mro/x21/init/rc.board_mavlink deleted file mode 100644 index a005e687686c..000000000000 --- a/boards/mro/x21/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# mRo x21 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/nxp/fmuk66-e/init/rc.board_mavlink b/boards/nxp/fmuk66-e/init/rc.board_mavlink deleted file mode 100644 index fbe9af9a8bf7..000000000000 --- a/boards/nxp/fmuk66-e/init/rc.board_mavlink +++ /dev/null @@ -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 diff --git a/boards/nxp/fmuk66-v3/init/rc.board_mavlink b/boards/nxp/fmuk66-v3/init/rc.board_mavlink deleted file mode 100644 index 8be295660bcd..000000000000 --- a/boards/nxp/fmuk66-v3/init/rc.board_mavlink +++ /dev/null @@ -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 diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_mavlink b/boards/nxp/fmurt1062-v1/init/rc.board_mavlink deleted file mode 100644 index 2ec618d05e31..000000000000 --- a/boards/nxp/fmurt1062-v1/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv5 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/omnibus/f4sd/init/rc.board_mavlink b/boards/omnibus/f4sd/init/rc.board_mavlink deleted file mode 100644 index 93ae6863c926..000000000000 --- a/boards/omnibus/f4sd/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# OmnibusF4SD specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v2/init/rc.board_mavlink b/boards/px4/fmu-v2/init/rc.board_mavlink deleted file mode 100644 index cc337d34ae00..000000000000 --- a/boards/px4/fmu-v2/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv2 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v3/init/rc.board_mavlink b/boards/px4/fmu-v3/init/rc.board_mavlink deleted file mode 100644 index c915fefe8c72..000000000000 --- a/boards/px4/fmu-v3/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv3 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v4/init/rc.board_mavlink b/boards/px4/fmu-v4/init/rc.board_mavlink deleted file mode 100644 index 90328850a83b..000000000000 --- a/boards/px4/fmu-v4/init/rc.board_mavlink +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv4 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 - diff --git a/boards/px4/fmu-v4pro/init/rc.board_mavlink b/boards/px4/fmu-v4pro/init/rc.board_mavlink deleted file mode 100644 index 9302d547fa35..000000000000 --- a/boards/px4/fmu-v4pro/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv4pro specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v5/init/rc.board_mavlink b/boards/px4/fmu-v5/init/rc.board_mavlink deleted file mode 100644 index 2ec618d05e31..000000000000 --- a/boards/px4/fmu-v5/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv5 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink deleted file mode 100644 index 07ed2047b404..000000000000 --- a/boards/px4/fmu-v5x/init/rc.board_mavlink +++ /dev/null @@ -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 diff --git a/boards/px4/fmu-v6u/init/rc.board_mavlink b/boards/px4/fmu-v6u/init/rc.board_mavlink deleted file mode 100644 index 106d8d207c6e..000000000000 --- a/boards/px4/fmu-v6u/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv6U specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/px4/fmu-v6x/init/rc.board_mavlink b/boards/px4/fmu-v6x/init/rc.board_mavlink deleted file mode 100644 index 5146dc069c96..000000000000 --- a/boards/px4/fmu-v6x/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# PX4 FMUv6X specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/boards/spracing/h7extreme/init/rc.board_mavlink b/boards/spracing/h7extreme/init/rc.board_mavlink deleted file mode 100644 index 43c856670763..000000000000 --- a/boards/spracing/h7extreme/init/rc.board_mavlink +++ /dev/null @@ -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 diff --git a/boards/uvify/core/init/rc.board_mavlink b/boards/uvify/core/init/rc.board_mavlink deleted file mode 100644 index 5b92aba400a8..000000000000 --- a/boards/uvify/core/init/rc.board_mavlink +++ /dev/null @@ -1,7 +0,0 @@ -#!/bin/sh -# -# UVify UVF4 specific board MAVLink startup script. -#------------------------------------------------------------------------------ - -# Start MAVLink on the USB port -mavlink start -d /dev/ttyACM0 diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index 9105109519ad..21324c518099 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -56,6 +56,214 @@ #include #endif +#if defined(CONFIG_SYSTEM_CDCACM) +__BEGIN_DECLS +#include +#include + +#include +#include + +extern int sercon_main(int c, char **argv); +extern int serdis_main(int c, char **argv); +__END_DECLS + +#include +#include + +#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_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; } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 944a84637bd3..763838473938 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -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; } } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6f0b80757f0a..99575b24c8da 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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 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 @@ Start mavlink on UDP port 14556 and enable the HIGHRES_IMU message with 50Hz: 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, "", "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); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 3a3c340888d2..d17f44db8acc 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -125,6 +125,14 @@ class Mavlink final : public ModuleParams */ 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 @@ class Mavlink final : public ModuleParams */ 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 @@ class Mavlink final : public ModuleParams 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 @@ class Mavlink final : public ModuleParams 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 @@ class Mavlink final : public ModuleParams 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 @@ class Mavlink final : public ModuleParams uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::PublicationMulti _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 @@ class Mavlink final : public ModuleParams 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 _streams; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f539b122d4bd..bfac5533794d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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()) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8b30e1cd7238..ea2712841612 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -131,6 +131,8 @@ class MavlinkReceiver : public ModuleParams 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(); diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index c7ed1696a348..7c44ac688227 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -50,11 +50,8 @@ #include #include -#include - __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("", "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; }