From 6f87a4546d75166fa0e0f5ed2f416483dcb66f3a Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 20 Apr 2022 19:42:58 -0400
Subject: [PATCH] platforms/nuttx: cdc_acm_check implement mavlink reboot
 directly

---
 .../nuttx/src/px4/common/cdc_acm_check.cpp    | 48 ++++++++++++++++++-
 1 file changed, 47 insertions(+), 1 deletion(-)

diff --git a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp
index e1f582e57a..5d9702152f 100644
--- a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp
+++ b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp
@@ -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;