diff --git a/libraries/AP_HAL_AVR/Scheduler.cpp b/libraries/AP_HAL_AVR/Scheduler.cpp index ac8db44413..6fc27fd22b 100644 --- a/libraries/AP_HAL_AVR/Scheduler.cpp +++ b/libraries/AP_HAL_AVR/Scheduler.cpp @@ -1,14 +1,16 @@ +#include +#include + #include "HAL_AVR.h" #include "Scheduler.h" using namespace AP_HAL_AVR; -#include -#include - #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +extern const AP_HAL::HAL& hal; + static volatile uint32_t timer0_overflow_count = 0; static volatile uint32_t timer0_millis = 0; static uint8_t timer0_fract = 0; @@ -335,3 +337,19 @@ void AVRScheduler::end_atomic() { sei(); } } + +void AVRScheduler::reboot() { + hal.uartA->println_P(PSTR("GOING DOWN FOR A REBOOT\r\n")); + hal.scheduler->delay(100); + + cli(); + /* Making a null pointer call will cause all AVRs to reboot + * but they may not come back alive properly - we need to setup + * the IO the way the bootloader would. + * pch will go back and fix this. + */ + void (*fn)(void) = NULL; + fn(); + + for(;;); +} diff --git a/libraries/AP_HAL_AVR/Scheduler.h b/libraries/AP_HAL_AVR/Scheduler.h index 1f779841c1..0a9afef001 100644 --- a/libraries/AP_HAL_AVR/Scheduler.h +++ b/libraries/AP_HAL_AVR/Scheduler.h @@ -27,6 +27,7 @@ public: void resume_timer_procs(); void begin_atomic(); void end_atomic(); + void reboot(); private: /* Implementation specific methods: */