diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index d4365d2a91..f2b505de25 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -125,11 +125,18 @@ void AP_Periph_FW::init()
     battery.lib.init();
 #endif
 
-#ifdef HAL_PERIPH_NEOPIXEL_COUNT
+#if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_RC_OUT)
     hal.rcout->init();
+#endif
+
+#ifdef HAL_PERIPH_NEOPIXEL_COUNT
     hal.rcout->set_serial_led_num_LEDs(HAL_PERIPH_NEOPIXEL_CHAN, AP_HAL::RCOutput::MODE_NEOPIXEL);
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+    rcout_init();
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_ADSB
     adsb_init();
 #endif
@@ -272,6 +279,10 @@ void AP_Periph_FW::update()
 #ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT
         check_for_serial_reboot_cmd(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT);
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+        SRV_Channels::enable_aux_servos();
+#endif
     }
 
     static uint32_t last_error_ms;
diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index 1489dc0261..fc8064e234 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -5,6 +5,7 @@
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Compass/AP_Compass.h>
 #include <AP_Baro/AP_Baro.h>
+#include "SRV_Channel/SRV_Channel.h"
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_Airspeed/AP_Airspeed.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
@@ -127,7 +128,17 @@ public:
     HWESC_Telem hwesc_telem;
     void hwesc_telem_update();
 #endif
-    
+
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+    SRV_Channels servo_channels;
+
+    void rcout_init();
+    void rcout_esc(int16_t *rc, uint8_t num_channels);
+    void rcout_srv(const uint8_t actuator_id, const float command_value);
+    void rcout_update();
+    void rcout_handle_safety_state(uint8_t safety_state);
+#endif
+
     // setup the var_info table
     AP_Param param_loader{var_info};
 
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index f11ebf5010..3c6cb70b0d 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -113,6 +113,13 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(esc_number, "ESC_NUMBER", 0),
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+    // Servo driver
+    // @Group: OUT
+    // @Path: ../libraries/SRV_Channel/SRV_Channels.cpp
+    GOBJECT(servo_channels, "OUT",     SRV_Channels),
+#endif
+
     AP_VAREND
 };
 
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 1e3704d963..0fb5501cd1 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -32,6 +32,7 @@ public:
         k_param_debug,
         k_param_serial_number,
         k_param_adsb_port,
+        k_param_servo_channels,
     };
 
     AP_Int16 format_version;
diff --git a/Tools/AP_Periph/README.md b/Tools/AP_Periph/README.md
index fbf7cf6ef9..cd723266bb 100644
--- a/Tools/AP_Periph/README.md
+++ b/Tools/AP_Periph/README.md
@@ -34,6 +34,7 @@ UAVCAN sensor types. Support is included for:
  - LEDs (GPIO, I2C or WS2812 serial)
  - Safety LED and Safety Switch
  - Buzzer (tonealarm or simple GPIO)
+ - RC Output (All standard RCOutput protocols)
 
 An AP_Periph UAVCAN firmware supports these UAVCAN features:
 
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index 2edb861939..7cf40083f0 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -44,6 +44,9 @@
 #include <uavcan/equipment/gnss/RTCMStream.h>
 #include <uavcan/equipment/power/BatteryInfo.h>
 #include <uavcan/protocol/debug/LogMessage.h>
+#include <uavcan/equipment/esc/RawCommand.h>
+#include <uavcan/equipment/actuator/ArrayCommand.h>
+#include <uavcan/equipment/actuator/Command.h>
 #include <stdio.h>
 #include <drivers/stm32/canard_stm32.h>
 #include <AP_HAL/I2CDevice.h>
@@ -495,7 +498,7 @@ static void can_buzzer_update(void)
 }
 #endif // HAL_PERIPH_ENABLE_BUZZER
 
-#ifdef HAL_GPIO_PIN_SAFE_LED
+#if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT)
 static uint8_t safety_state;
 
 /*
@@ -508,6 +511,9 @@ static void handle_safety_state(CanardInstance* ins, CanardRxTransfer* transfer)
         return;
     }
     safety_state = req.status;
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+    periph.rcout_handle_safety_state(safety_state);
+#endif
 }
 #endif // HAL_GPIO_PIN_SAFE_LED
 
@@ -604,6 +610,59 @@ static void handle_lightscommand(CanardInstance* ins, CanardRxTransfer* transfer
 }
 #endif // AP_PERIPH_HAVE_LED
 
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+static void handle_esc_rawcommand(CanardInstance* ins, CanardRxTransfer* transfer)
+{
+    uavcan_equipment_esc_RawCommand cmd;
+    uint8_t arraybuf[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE];
+    uint8_t *arraybuf_ptr = arraybuf;
+    if (uavcan_equipment_esc_RawCommand_decode(transfer, transfer->payload_len, &cmd, &arraybuf_ptr) < 0) {
+        return;
+    }
+    periph.rcout_esc(cmd.cmd.data, cmd.cmd.len);
+}
+
+static void handle_act_command(CanardInstance* ins, CanardRxTransfer* transfer)
+{
+    // manual decoding due to TAO bug in libcanard generated code
+    if (transfer->payload_len < 1 || transfer->payload_len > UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE+1) {
+        return;
+    }
+
+    const uint8_t data_count = (transfer->payload_len / UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_MAX_SIZE);
+    uavcan_equipment_actuator_Command data[data_count] {};
+
+    uint32_t offset = 0;
+    for (uint8_t i=0; i<data_count; i++) {
+        canardDecodeScalar(transfer, offset, 8, false, (void*)&data[i].actuator_id);
+        offset += 8;
+        canardDecodeScalar(transfer, offset, 8, false, (void*)&data[i].command_type);
+        offset += 8;
+
+#ifndef CANARD_USE_FLOAT16_CAST
+    uint16_t tmp_float = 0;
+#else
+    CANARD_USE_FLOAT16_CAST tmp_float = 0;
+#endif
+        canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+        offset += 16;
+#ifndef CANARD_USE_FLOAT16_CAST
+        data[i].command_value = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+        data[i].command_value = (float)tmp_float;
+#endif
+    }
+
+    for (uint8_t i=0; i < data_count; i++) {
+        if (data[i].command_type != UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_COMMAND_TYPE_UNITLESS) {
+            // this is the only type we support
+            continue;
+        }
+        periph.rcout_srv(data[i].actuator_id, data[i].command_value);
+    }
+}
+#endif // HAL_PERIPH_ENABLE_RC_OUT
+
 #ifdef HAL_GPIO_PIN_SAFE_LED
 /*
   update safety LED
@@ -731,7 +790,7 @@ static void onTransferReceived(CanardInstance* ins,
         break;
 #endif
 
-#ifdef HAL_GPIO_PIN_SAFE_LED
+#if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT)
     case ARDUPILOT_INDICATION_SAFETYSTATE_ID:
         handle_safety_state(ins, transfer);
         break;
@@ -748,6 +807,16 @@ static void onTransferReceived(CanardInstance* ins,
         handle_lightscommand(ins, transfer);
         break;
 #endif
+
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+    case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID:
+        handle_esc_rawcommand(ins, transfer);
+        break;
+
+    case UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID:
+        handle_act_command(ins, transfer);
+        break;
+#endif
     }
 }
 
@@ -802,7 +871,7 @@ static bool shouldAcceptTransfer(const CanardInstance* ins,
         *out_data_type_signature = UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_SIGNATURE;
         return true;
 #endif
-#ifdef HAL_GPIO_PIN_SAFE_LED
+#if defined(HAL_GPIO_PIN_SAFE_LED) || defined(HAL_PERIPH_ENABLE_RC_OUT)
     case ARDUPILOT_INDICATION_SAFETYSTATE_ID:
         *out_data_type_signature = ARDUPILOT_INDICATION_SAFETYSTATE_SIGNATURE;
         return true;
@@ -816,6 +885,15 @@ static bool shouldAcceptTransfer(const CanardInstance* ins,
     case UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID:
         *out_data_type_signature = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE;
         return true;
+#endif
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+    case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID:
+        *out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE;
+        return true;
+    
+    case UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID:
+        *out_data_type_signature = UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE;
+        return true;
 #endif
     default:
         break;
@@ -1229,7 +1307,9 @@ void AP_Periph_FW::can_update()
 #ifdef HAL_PERIPH_ENABLE_MSP
     msp_sensor_update();
 #endif
-
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+    rcout_update();
+#endif
     processTx();
     processRx();
 }
diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp
new file mode 100644
index 0000000000..3738b2f52c
--- /dev/null
+++ b/Tools/AP_Periph/rc_out.cpp
@@ -0,0 +1,100 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#include <AP_HAL/AP_HAL.h>
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
+#include "AP_Periph.h"
+
+// magic value from UAVCAN driver packet
+// dsdl/uavcan/equipment/esc/1030.RawCommand.uavcan
+// Raw ESC command normalized into [-8192, 8191]
+#define UAVCAN_ESC_MAX_VALUE    8191
+
+#define SERVO_OUT_RCIN_MAX      16  // SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16
+#define SERVO_OUT_MOTOR_MAX     12  // SRV_Channel::k_motor1 ... SRV_Channel::k_motor8, SRV_Channel::k_motor9 ... SRV_Channel::k_motor12
+
+#if HAL_PWM_COUNT == 0
+    #error "You must define a PWM output in your hwdef.dat"
+#endif
+
+static bool has_new_data_to_update;
+
+extern const AP_HAL::HAL &hal;
+
+void AP_Periph_FW::rcout_init()
+{
+    for (uint8_t i=0; i<HAL_PWM_COUNT; i++) {
+        servo_channels.set_default_function(i, SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + i));
+    }
+
+    for (uint8_t i=0; i<SERVO_OUT_RCIN_MAX; i++) {
+        SRV_Channels::set_angle(SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + i), 1000);
+    }
+    for (uint8_t i=0; i<SERVO_OUT_MOTOR_MAX; i++) {
+        SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), UAVCAN_ESC_MAX_VALUE);
+    }
+}
+
+void AP_Periph_FW::rcout_esc(int16_t *rc, uint8_t num_channels)
+{
+    if (rc == nullptr) {
+        return;
+    }
+
+    const uint8_t channel_count = MIN(num_channels, SERVO_OUT_MOTOR_MAX);
+    for (uint8_t i=0; i<channel_count; i++) {
+        SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), rc[i]);
+    }
+
+    has_new_data_to_update = true;
+}
+
+void AP_Periph_FW::rcout_srv(uint8_t actuator_id, const float command_value)
+{
+    if ((actuator_id == 0) || (actuator_id > HAL_PWM_COUNT)) {
+        // not supported or out of range
+        return;
+    }
+
+    const SRV_Channel::Aux_servo_function_t function = SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + actuator_id - 1);
+    SRV_Channels::set_output_norm(function, command_value);
+
+    has_new_data_to_update = true;
+}
+
+void AP_Periph_FW::rcout_handle_safety_state(uint8_t safety_state)
+{
+    if (safety_state == 255) {
+        hal.rcout->force_safety_off();
+    } else {
+        hal.rcout->force_safety_on();
+    }
+    has_new_data_to_update = true;
+}
+
+void AP_Periph_FW::rcout_update()
+{
+    if (!has_new_data_to_update) {
+        return;
+    }
+    has_new_data_to_update = false;
+
+    SRV_Channels::calc_pwm();
+    SRV_Channels::cork();
+    SRV_Channels::output_ch_all();
+    SRV_Channels::push();
+}
+
+#endif // HAL_PERIPH_ENABLE_RC_OUT
+
diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript
index 9d2729ddce..905fe40acb 100644
--- a/Tools/AP_Periph/wscript
+++ b/Tools/AP_Periph/wscript
@@ -37,6 +37,7 @@ def build(bld):
         'AP_RangeFinder',
         'AP_ROMFS',
         'AP_MSP',
+        'SRV_Channel',
         ],
         exclude_src=[
             'libraries/AP_HAL_ChibiOS/Storage.cpp'