fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Factor cdc_acm_init away from px4_init

Browse Source 
In protected build, this needs to go to user-space initialization as it
calls apps (sercon) and launches mavlink.

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen 2021-10-08 09:07:49 +03:00 committed by Daniel Agar
parent 39c0c68167
commit 356de6ccf1
3 changed files with 252 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
platforms/nuttx/src/px4/common/CMakeLists.txt
View File

@ -38,6 +38,7 @@ if(NOT PX4_BOARD MATCHES "io-v2")
board_crashdump.c
board_dma_alloc.c
board_fat_dma_alloc.c
cdc_acm_check.cpp
console_buffer.cpp
cpuload.cpp
gpio.c

247
platforms/nuttx/src/px4/common/cdc_acm_check.cpp Normal file
View File

@ -0,0 +1,247 @@
/****************************************************************************
*
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#if defined(CONFIG_SYSTEM_CDCACM)
__BEGIN_DECLS
#include <syslog.h>
#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));
}
}
void cdcacm_init(void)
{
work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, 0);
}
#endif // CONFIG_SYSTEM_CDCACM

214
platforms/nuttx/src/px4/common/px4_init.cpp
View File

@ -56,213 +56,7 @@
#include <px4_platform_common/crypto.h>
#endif
#if defined(CONFIG_SYSTEM_CDCACM)
__BEGIN_DECLS
#include <nuttx/wqueue.h>
#include <builtin/builtin.h>
#include <termios.h>
#include <sys/ioctl.h>
extern int sercon_main(int c, char **argv);
extern int serdis_main(int c, char **argv);
__END_DECLS
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
#define USB_DEVICE_PATH "/dev/ttyACM0"
static struct work_s usb_serial_work;
static bool vbus_present_prev = false;
static int ttyacm_fd = -1;
enum class UsbAutoStartState {
disconnected,
connecting,
connected,
disconnecting,
} usb_auto_start_state{UsbAutoStartState::disconnected};
static void mavlink_usb_check(void *arg)
{
int rescheduled = -1;
uORB::SubscriptionData<actuator_armed_s> actuator_armed_sub{ORB_ID(actuator_armed)};
const bool armed = actuator_armed_sub.get().armed;
const bool vbus_present = (board_read_VBUS_state() == PX4_OK);
if (!armed) {
switch (usb_auto_start_state) {
case UsbAutoStartState::disconnected:
if (vbus_present && vbus_present_prev) {
if (sercon_main(0, nullptr) == EXIT_SUCCESS) {
usb_auto_start_state = UsbAutoStartState::connecting;
rescheduled = work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, USEC2TICK(100000));
}
} else if (vbus_present && !vbus_present_prev) {
// check again sooner if USB just connected
rescheduled = work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, USEC2TICK(100000));
}
break;
case UsbAutoStartState::connecting:
if (vbus_present && vbus_present_prev) {
if (ttyacm_fd < 0) {
ttyacm_fd = ::open(USB_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
}
if (ttyacm_fd >= 0) {
int bytes_available = 0;
int retval = ::ioctl(ttyacm_fd, FIONREAD, &bytes_available);
if ((retval == OK) && (bytes_available >= 3)) {
char buffer[80];
// non-blocking read
int nread = ::read(ttyacm_fd, buffer, sizeof(buffer));
#if defined(DEBUG_BUILD)
if (nread > 0) {
fprintf(stderr, "%d bytes\n", nread);
for (int i = 0; i < nread; i++) {
fprintf(stderr, "|%X", buffer[i]);
}
fprintf(stderr, "\n");
}
#endif // DEBUG_BUILD
if (nread > 0) {
bool launch_mavlink = false;
bool launch_nshterm = false;
static constexpr int MAVLINK_HEARTBEAT_MIN_LENGTH = 9;
if (nread >= MAVLINK_HEARTBEAT_MIN_LENGTH) {
// scan buffer for mavlink HEARTBEAT (v1 & v2)
for (int i = 0; i < nread - MAVLINK_HEARTBEAT_MIN_LENGTH; i++) {
if ((buffer[i] == 0xFE) && (buffer[i + 1] == 9) && (buffer[i + 5] == 0)) {
// mavlink v1 HEARTBEAT
// buffer[0]: start byte (0xFE for mavlink v1)
// buffer[1]: length (9 for HEARTBEAT)
// buffer[3]: SYSID
// buffer[4]: COMPID
// buffer[5]: mavlink message id (0 for HEARTBEAT)
syslog(LOG_INFO, "%s: launching mavlink (HEARTBEAT v1 from SYSID:%d COMPID:%d)\n",
USB_DEVICE_PATH, buffer[i + 3], buffer[i + 4]);
launch_mavlink = true;
break;
} else if ((buffer[i] == 0xFD) && (buffer[i + 1] == 9)
&& (buffer[i + 7] == 0) && (buffer[i + 8] == 0) && (buffer[i + 9] == 0)) {
// mavlink v2 HEARTBEAT
// buffer[0]: start byte (0xFD for mavlink v2)
// buffer[1]: length (9 for HEARTBEAT)
// buffer[5]: SYSID
// buffer[6]: COMPID
// buffer[7:9]: mavlink message id (0 for HEARTBEAT)
syslog(LOG_INFO, "%s: launching mavlink (HEARTBEAT v2 from SYSID:%d COMPID:%d)\n",
USB_DEVICE_PATH, buffer[i + 5], buffer[i + 6]);
launch_mavlink = true;
break;
}
}
}
if (!launch_mavlink && (nread >= 3)) {
// nshterm (3 carriage returns)
// scan buffer looking for 3 consecutive carriage returns (0xD)
for (int i = 1; i < nread - 1; i++) {
if (buffer[i - 1] == 0xD && buffer[i] == 0xD && buffer[i + 1] == 0xD) {
syslog(LOG_INFO, "%s: launching nshterm\n", USB_DEVICE_PATH);
launch_nshterm = true;
break;
}
}
}
if (launch_mavlink || launch_nshterm) {
// cleanup serial port
close(ttyacm_fd);
ttyacm_fd = -1;
static const char *mavlink_argv[] {"mavlink", "start", "-d", USB_DEVICE_PATH, nullptr};
static const char *nshterm_argv[] {"nshterm", USB_DEVICE_PATH, nullptr};
char **exec_argv = nullptr;
if (launch_nshterm) {
exec_argv = (char **)nshterm_argv;
} else if (launch_mavlink) {
exec_argv = (char **)mavlink_argv;
}
sched_lock();
if (exec_builtin(exec_argv[0], exec_argv, nullptr, 0) > 0) {
usb_auto_start_state = UsbAutoStartState::connected;
} else {
usb_auto_start_state = UsbAutoStartState::disconnecting;
}
sched_unlock();
}
}
}
}
} else {
// cleanup
if (ttyacm_fd >= 0) {
close(ttyacm_fd);
ttyacm_fd = -1;
}
usb_auto_start_state = UsbAutoStartState::disconnecting;
}
break;
case UsbAutoStartState::connected:
if (!vbus_present && !vbus_present_prev) {
sched_lock();
static const char app[] {"mavlink"};
static const char *stop_argv[] {"mavlink", "stop", "-d", USB_DEVICE_PATH, NULL};
exec_builtin(app, (char **)stop_argv, NULL, 0);
sched_unlock();
usb_auto_start_state = UsbAutoStartState::disconnecting;
}
break;
case UsbAutoStartState::disconnecting:
// serial disconnect if unused
serdis_main(0, NULL);
usb_auto_start_state = UsbAutoStartState::disconnected;
break;
}
}
vbus_present_prev = vbus_present;
if (rescheduled != PX4_OK) {
work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, NULL, USEC2TICK(1000000));
}
}
#endif // CONFIG_SYSTEM_CDCACM
extern void cdcacm_init(void);
int px4_platform_init()
{
@ -348,9 +142,9 @@ 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
#if defined(CONFIG_SYSTEM_CDCACM) && defined(CONFIG_BUILD_FLAT)
cdcacm_init();
#endif
return PX4_OK;
}