dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

platforms/nuttx: cdc_acm_check implement mavlink reboot directly

This commit is contained in:
Daniel Agar 2022-04-20 19:42:58 -04:00
parent 9073f3ccdf
commit 6f87a4546d
1 changed files with 47 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
platforms/nuttx/src/px4/common/cdc_acm_check.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -45,6 +45,8 @@ extern int sercon_main(int c, char **argv);
extern int serdis_main(int c, char **argv);
__END_DECLS
#include <px4_platform_common/shutdown.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_armed.h>
@ -154,6 +156,50 @@ static void mavlink_usb_check(void *arg)
if (nread > 0) {
// Mavlink reboot/shutdown command
// COMMAND_LONG (#76) with command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246)
static constexpr int MAVLINK_COMMAND_LONG_MIN_LENGTH = 41;
if (nread >= MAVLINK_COMMAND_LONG_MIN_LENGTH) {
// scan buffer for mavlink COMMAND_LONG
for (int i = 0; i < nread - MAVLINK_COMMAND_LONG_MIN_LENGTH; i++) {
if ((buffer[i] == 0xFE) // Mavlink v1 start byte
&& (buffer[i + 5] == 76) // 76=0x4C COMMAND_LONG
&& (buffer[i + 34] == 246) // 246=0xF6 MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN
) {
// mavlink v1 COMMAND_LONG
// buffer[0]: start byte (0xFE for mavlink v1)
// buffer[3]: SYSID
// buffer[4]: COMPID
// buffer[5]: message id (COMMAND_LONG 76=0x4C)
// buffer[6-10]: COMMAND_LONG param 1 (little endian float)
// buffer[34]: COMMAND_LONG command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246/0xF6)
float param1_raw = 0;
memcpy(&param1_raw, &buffer[i + 6], 4);
int param1 = roundf(param1_raw);
syslog(LOG_INFO, "%s: Mavlink MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN param 1: %d (SYSID:%d COMPID:%d)\n",
USB_DEVICE_PATH, param1, buffer[i + 3], buffer[i + 4]);
if (param1 == 1) {
// 1: Reboot autopilot
px4_reboot_request(false, 0);
} else if (param1 == 2) {
// 2: Shutdown autopilot
#if defined(BOARD_HAS_POWER_CONTROL)
px4_shutdown_request(0);
#endif // BOARD_HAS_POWER_CONTROL
} else if (param1 == 3) {
// 3: Reboot autopilot and keep it in the bootloader until upgraded.
px4_reboot_request(true, 0);
}
}
}
}
bool launch_mavlink = false;
bool launch_nshterm = false;
bool launch_passthru = false;