diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp
index 5355127e53..2fbfb85428 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN_Servers.cpp
@@ -83,6 +83,29 @@ protected:
     }
 };
 
+
+class AP_UAVCAN_RestartRequestHandler : public uavcan::IRestartRequestHandler {
+public:
+    bool handleRestartRequest(uavcan::NodeID request_source) override {
+        // swiped from reboot handling in GCS_Common.cpp
+        AP_Notify *notify = AP_Notify::instance();
+        if (notify) {
+            AP_Notify::flags.firmware_update = 1;
+            notify->update();
+        }
+        // force safety on
+        hal.rcout->force_safety_on();
+        hal.rcout->force_safety_no_wait();
+
+        // flush pending parameter writes
+        AP_Param::flush();
+
+        hal.scheduler->delay(200);
+        hal.scheduler->reboot(false);
+        return true;
+    }
+};
+
 class AP_UAVCAN_FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend
 {
     /**
@@ -271,6 +294,14 @@ bool AP_UAVCAN_Servers::init(uavcan::Node<0> &node)
         }
     }
 
+    if (_restart_request_handler == nullptr) {
+        _restart_request_handler = new AP_UAVCAN_RestartRequestHandler();
+        if (_restart_request_handler == nullptr) {
+            goto failed;
+        }
+    }
+    node.setRestartRequestHandler(_restart_request_handler);
+
     //Start Dynamic Node Server
     ret = _server_instance->init(node.getHardwareVersion().unique_id);
     if (ret < 0) {
@@ -281,6 +312,7 @@ bool AP_UAVCAN_Servers::init(uavcan::Node<0> &node)
     return true;
 
 failed:
+    delete _restart_request_handler;
     delete _storage_backend;
     delete _tracer;
     delete _server_instance;
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h b/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h
index df0716a917..45bd5ad6f0 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN_Servers.h
@@ -11,6 +11,7 @@
 class AP_UAVCAN_FileEventTracer;
 class AP_UAVCAN_FileStorageBackend;
 class AP_UAVCAN_CentralizedServer;
+class AP_UAVCAN_RestartRequestHandler;
 
 class AP_UAVCAN_Servers
 {
@@ -23,6 +24,8 @@ private:
     AP_UAVCAN_CentralizedServer *_server_instance;
     AP_UAVCAN_FileEventTracer *_tracer;
     AP_UAVCAN_FileStorageBackend *_storage_backend;
+    AP_UAVCAN_RestartRequestHandler *_restart_request_handler; // one for all nodes....
+
 };
 
 #endif