diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
index 873ffeb537..d75b0b08f8 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp
@@ -787,6 +787,28 @@ void AP_UAVCAN::SRV_write(uint16_t pulse_len, uint8_t ch)
     _SRV_conf[ch].active = true;
 }
 
+void AP_UAVCAN::SRV_push_servos()
+{
+    if (!SRV_sem_take()) {
+        return;
+    }
+
+    for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
+        // Check if this channels has any function assigned
+        if (SRV_Channels::channel_function(i)) {
+            SRV_write(SRV_Channels::srv_channel(i)->get_output_pwm(), i);
+        }
+    }
+
+    if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
+        SRV_arm_actuators(true);
+    } else {
+        SRV_arm_actuators(false);
+    }
+
+    SRV_sem_give();
+}
+
 uint8_t AP_UAVCAN::find_gps_without_listener(void)
 {
     for (uint8_t i = 0; i < AP_UAVCAN_MAX_LISTENERS; i++) {
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h
index 2a24ed4eff..595a24880b 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN.h
@@ -269,6 +269,7 @@ public:
     void SRV_force_safety_off(void);
     void SRV_arm_actuators(bool arm);
     void SRV_write(uint16_t pulse_len, uint8_t ch);
+    void SRV_push_servos(void);
     bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue);
 
     void set_parent_can_mgr(AP_HAL::CANManager* parent_can_mgr)