From cb8868191445c640bfef78db85112a7ade0625da Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 10 Sep 2012 11:42:30 +1000
Subject: [PATCH] APM: added support for MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN

this allows for remote reboot of APM
---
 ArduPlane/ArduPlane.pde   |  1 +
 ArduPlane/GCS_Mavlink.pde | 10 ++++++++--
 ArduPlane/system.pde      | 14 ++++++++++++++
 3 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index d8a129c356..26a03be474 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -21,6 +21,7 @@
 #include <avr/io.h>
 #include <avr/eeprom.h>
 #include <avr/pgmspace.h>
+#include <avr/wdt.h>
 #include <math.h>
 
 // Libraries
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 8e04284409..ccd824003d 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1042,7 +1042,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         mavlink_msg_command_long_decode(msg, &packet);
         if (mavlink_check_target(packet.target_system, packet.target_component)) break;
 
-        uint8_t result;
+        uint8_t result = MAV_RESULT_UNSUPPORTED;
 
         // do command
         send_text(SEVERITY_LOW,PSTR("command received: "));
@@ -1110,8 +1110,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             result = MAV_RESULT_ACCEPTED;
             break;
 
+        case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+            if (packet.param1 == 1) {
+                reboot_apm();
+                result = MAV_RESULT_ACCEPTED;
+            }
+            break;
+
         default:
-            result = MAV_RESULT_UNSUPPORTED;
             break;
         }
 
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 12f3805674..5c58249cb3 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -570,3 +570,17 @@ uint16_t board_voltage(void)
     return vcc.read_vcc();
 }
 
+
+/*
+  force a software reset of the APM
+ */
+static void reboot_apm(void)
+{
+    Serial.println_P(PSTR("REBOOTING"));
+    delay(100);
+    // see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1250663814/
+    // for the method
+    cli();
+    wdt_enable(WDTO_15MS);
+    while (1);
+}