diff --git a/main.cpp b/main.cpp index fd18e75..4f0c560 100644 --- a/main.cpp +++ b/main.cpp @@ -178,6 +178,8 @@ protocol::serial::Feedback feedback; #ifdef FEATURE_CAN std::atomic timeoutCntLeft = 0; std::atomic timeoutCntRight = 0; + +uint32_t *reboot_request_address = 0; #endif uint32_t main_loop_counter; @@ -252,6 +254,7 @@ void parseCanCommand(); void applyIncomingCanMessage(); void sendCanFeedback(); void sendFlasherFeedback(); +void handleFlasher(); #endif #ifdef FEATURE_BUTTON @@ -410,7 +413,7 @@ int main() parseCanCommand(); sendCanFeedback(); - sendFlasherFeedback(); + handleFlasher(); #endif #ifdef FEATURE_BUTTON @@ -1670,11 +1673,21 @@ void applyIncomingCanMessage() break; case MotorController::Command::Poweroff: case MotorController::Command::Poweroff: + { + if (header.DLC >= 2) + { + // Reboot selected image + reboot_request_address = (uint32_t *)((*(uint8_t *)buf == 0) ? APP_A_START : APP_B_START); + } + else + { #ifdef FEATURE_BUTTON - if (*((bool*)buf)) - poweroff(); + if (*((bool*)buf)) + poweroff(); #endif - break; + } + break; + } default: #ifndef CAN_LOG_UNKNOWN_ADDR if constexpr (false) @@ -1798,6 +1811,16 @@ void sendFlasherFeedback() { //while (true); } } + +void handleFlasher() +{ + sendFlasherFeedback(); + + if (reboot_request_address) + { + reboot_new_image(reboot_request_address); + } +} #endif #ifdef FEATURE_BUTTON