2 files changed, 38 insertions, 7 deletions
diff --git a/Arduino/ems-light/Monitor.cpp b/Arduino/ems-light/Monitor.cpp
index 90ba398..fd77089 100755
--- a/Arduino/ems-light/Monitor.cpp
+++ b/Arduino/ems-light/Monitor.cpp
@@ -271,6 +271,39 @@ int getFreeMemory()
     
 }
 
+#ifdef ARDUINO_SAMD_ZERO
+#include "Adafruit_SleepyDog.h"
+#include "Reset.h"
+
+void jump2bootloader(){
+    initiateReset(10);
+}
+void reboot(){
+    Watchdog.enable(150);
+    for(;;);
+}
+#elif defined(ARDUINO_AVR_MICRO) || defined(ARDUINO_AVR_LEONARDO)
+#include <avr/wdt.h>
+#ifndef MAGIC_KEY
+#define MAGIC_KEY 0x7777
+#endif
+#ifndef MAGIC_KEY_POS
+#define MAGIC_KEY_POS 0x0800
+#endif
+void jump2bootloader(){
+    int16_t magic_key_pos = MAGIC_KEY_POS;
+    *(uint16_t *)magic_key_pos = MAGIC_KEY;
+    wdt_enable(WDTO_120MS);
+    for(;;);
+}
+void reboot(){
+    int16_t magic_key_pos = MAGIC_KEY_POS;
+    *(uint16_t *)magic_key_pos = 0;
+    wdt_enable(WDTO_120MS);
+    for(;;);
+    
+}
+#endif
 
 
 
diff --git a/Arduino/ems-light/Monitor.h b/Arduino/ems-light/Monitor.h
index 6bc72c9..2c01c65 100755
--- a/Arduino/ems-light/Monitor.h
+++ b/Arduino/ems-light/Monitor.h
@@ -53,10 +53,8 @@ class Monitor;
 extern Monitor monitor;
 #include "Machine.h"
 
-#ifdef ARDUINO_SAMD_ZERO
-#include "Adafruit_SleepyDog.h"
-#include "Reset.h"
-#endif
+void jump2bootloader();
+void reboot();
 
 
 #include "TaskScheduler.h"
@@ -84,7 +82,6 @@ enum severityIndex {
 #define MAX_MONITOR_LINE_LENGTH (MAX_RETURN_VALUE-6)
 
 typedef void(*actionptr)(uint8_t, uint8_t, char *);
-//typedef void(actionfunc)(uint8_t, uint8_t, char *);
 
 extern char actionBuffer[MAX_RETURN_VALUE];
 extern actionptr actions[];
@@ -100,6 +97,7 @@ extern Machine machine;
 extern int getFreeMemory();
 extern char commandBuffer[];
 
+
 class Monitor
 {
   private:
@@ -167,7 +165,7 @@ class Monitor
         if (verb=='!') {
             monitor.warn("Rebooting!");
             MONITOR_UART.end();
-            Watchdog.enable(150);
+            reboot();
         } else {
             monitor.update("NAK","RST Requires verb!");
         }
@@ -176,7 +174,7 @@ class Monitor
     static void BLD(uint8_t kwIndex, uint8_t verb, char *args) {
         if (verb=='!') {
             monitor.warn("Jumping to Bootloader!");
-            initiateReset(10);
+            jump2bootloader();
         } else {
             monitor.update("NAK","BLD Requires verb!");
         }