diff --git a/libraries/AP_HAL_ESP32/RCOutput.cpp b/libraries/AP_HAL_ESP32/RCOutput.cpp
index c456b3d8edad48..2c6a7ac5307432 100644
--- a/libraries/AP_HAL_ESP32/RCOutput.cpp
+++ b/libraries/AP_HAL_ESP32/RCOutput.cpp
@@ -12,7 +12,7 @@
  * You should have received a copy of the GNU General Public License along
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  *
- * Code by Charles "Silvanosky" Villard and David "Buzz" Bussenschutt
+ * Code by Charles "Silvanosky" Villard, David "Buzz" Bussenschutt and Andrey "ARg" Romanov
  *
  */
 
@@ -27,6 +27,13 @@
 
 #include <stdio.h>
 
+#include "esp_log.h"
+
+#define SERVO_TIMEBASE_RESOLUTION_HZ 1000000  // 1MHz, 1us per tick
+#define SERVO_TIMEBASE_PERIOD        20000    // 20000 ticks, 20ms
+
+#define TAG "RCOut"
+
 extern const AP_HAL::HAL& hal;
 
 using namespace ESP32;
@@ -50,63 +57,90 @@ void RCOutput::init()
 {
     _max_channels = MAX_CHANNELS;
 
-
-    //32 and 33 are special as they dont default to gpio, but can be if u disable their rtc setup:
+#ifdef CONFIG_IDF_TARGET_ESP32
+    // only on plain esp32
+    // 32 and 33 are special as they dont default to gpio, but can be if u disable their rtc setup:
     rtc_gpio_deinit(GPIO_NUM_32);
     rtc_gpio_deinit(GPIO_NUM_33);
+#endif
 
     printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n");
     printf("RCOutput::init() - channels available: %d \n",(int)MAX_CHANNELS);
     printf("oooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo\n");
 
-    static const mcpwm_io_signals_t signals[] = {
-        MCPWM0A,
-        MCPWM0B,
-        MCPWM1A,
-        MCPWM1B,
-        MCPWM2A,
-        MCPWM2B
-    };
-    static const mcpwm_timer_t timers[] = {
-        MCPWM_TIMER_0,
-        MCPWM_TIMER_1,
-        MCPWM_TIMER_2
-
-    };
-    static const mcpwm_unit_t units[] = {
-        MCPWM_UNIT_0,
-        MCPWM_UNIT_1
-    };
-    static const mcpwm_operator_t operators[] = {
-        MCPWM_OPR_A,
-        MCPWM_OPR_B
-    };
-
-    for (uint8_t i = 0; i < MAX_CHANNELS; ++i) {
-        auto unit = units[i/6];
-        auto signal = signals[i % 6];
-        auto timer = timers[i/2];
+    const int MCPWM_CNT = SOC_MCPWM_OPERATORS_PER_GROUP*SOC_MCPWM_GENERATORS_PER_OPERATOR;
+
+    for (int i = 0; i < MAX_CHANNELS; ++i) {
+
+        mcpwm_timer_handle_t h_timer;
+        mcpwm_oper_handle_t  h_oper;
+
+        ESP_LOGI(TAG, "Initialize CH%02d", i+1);
 
         //Save struct infos
         pwm_out &out = pwm_group_list[i];
+        out.group_id = i/MCPWM_CNT;
         out.gpio_num = outputs_pins[i];
-        out.unit_num = unit;
-        out.timer_num = timer;
-        out.io_signal = signal;
-        out.op = operators[i%2];
         out.chan = i;
 
-        //Setup gpio
-        mcpwm_gpio_init(unit, signal, outputs_pins[i]);
-        //Setup MCPWM module
-        mcpwm_config_t pwm_config;
-        pwm_config.frequency = 50;    //frequency = 50Hz, i.e. for every servo motor time period should be 20ms
-        pwm_config.cmpr_a = 0;    //duty cycle of PWMxA = 0
-        pwm_config.cmpr_b = 0;    //duty cycle of PWMxb = 0
-        pwm_config.counter_mode = MCPWM_UP_COUNTER;
-        pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
-        mcpwm_init(unit, timer, &pwm_config);
-        mcpwm_start(unit, timer);
+        if (0 == i % MCPWM_CNT) {
+            
+            mcpwm_timer_config_t timer_config = {
+                .group_id = out.group_id,
+                .clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT,
+                .resolution_hz = SERVO_TIMEBASE_RESOLUTION_HZ,
+                .count_mode = MCPWM_TIMER_COUNT_MODE_UP,
+                .period_ticks = SERVO_TIMEBASE_PERIOD,
+            };
+    
+            ESP_LOGI(TAG, "Initialize timer");
+            ESP_ERROR_CHECK(mcpwm_new_timer(&timer_config, &h_timer));
+
+            out.freq = timer_config.resolution_hz/timer_config.period_ticks;
+
+            ESP_LOGI(TAG, "Enable and start timer");
+            ESP_ERROR_CHECK(mcpwm_timer_enable(h_timer));
+            ESP_ERROR_CHECK(mcpwm_timer_start_stop(h_timer, MCPWM_TIMER_START_NO_STOP));
+        }
+        out.h_timer = h_timer;
+         
+
+        if (0 == i % SOC_MCPWM_GENERATORS_PER_OPERATOR) {
+
+            ESP_LOGI(TAG, "Initialize operator");
+
+            mcpwm_operator_config_t operator_config = {
+                .group_id = out.group_id, // operator must be in the same group to the timer
+            };
+            ESP_ERROR_CHECK(mcpwm_new_operator(&operator_config, &h_oper));
+        }
+        out.h_oper = h_oper;
+
+        ESP_LOGI(TAG, "Connect timer and operator");
+        ESP_ERROR_CHECK(mcpwm_operator_connect_timer(out.h_oper, out.h_timer));
+
+        ESP_LOGI(TAG, "Create comparator and generator from the operator");
+        mcpwm_comparator_config_t comparator_config = {};
+        comparator_config.flags.update_cmp_on_tez = true;
+
+        ESP_ERROR_CHECK(mcpwm_new_comparator(out.h_oper, &comparator_config, &out.h_cmpr));
+
+        mcpwm_generator_config_t generator_config = {
+            .gen_gpio_num = out.gpio_num,
+        };
+        ESP_ERROR_CHECK(mcpwm_new_generator(out.h_oper, &generator_config, &out.h_gen));
+
+        ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(out.h_cmpr, 1500));
+        out.value = 1500;
+        
+        ESP_LOGI(TAG, "Set generator action on timer and compare event");
+        // go high on counter empty
+        ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(out.h_gen,
+            MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));
+        // go low on compare threshold
+        ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(out.h_gen,
+            MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, out.h_cmpr, MCPWM_GEN_ACTION_LOW)));
+
     }
 
     _initialized = true;
@@ -123,7 +157,8 @@ void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
     for (uint8_t i = 0; i < MAX_CHANNELS; i++) {
         if (chmask & 1 << i) {
             pwm_out &out = pwm_group_list[i];
-            mcpwm_set_frequency(out.unit_num, out.timer_num, freq_hz);
+            ESP_ERROR_CHECK(mcpwm_timer_set_period( out.h_timer, SERVO_TIMEBASE_RESOLUTION_HZ/freq_hz));
+            out.freq = freq_hz;
         }
     }
 }
@@ -143,7 +178,7 @@ uint16_t RCOutput::get_freq(uint8_t chan)
     }
 
     pwm_out &out = pwm_group_list[chan];
-    return mcpwm_get_frequency(out.unit_num, out.timer_num);
+    return out.freq;
 }
 
 void RCOutput::enable_ch(uint8_t chan)
@@ -153,7 +188,7 @@ void RCOutput::enable_ch(uint8_t chan)
     }
 
     pwm_out &out = pwm_group_list[chan];
-    mcpwm_start(out.unit_num, out.timer_num);
+    ESP_ERROR_CHECK(mcpwm_timer_start_stop(out.h_timer, MCPWM_TIMER_START_NO_STOP));
 }
 
 void RCOutput::disable_ch(uint8_t chan)
@@ -164,7 +199,7 @@ void RCOutput::disable_ch(uint8_t chan)
 
     write(chan, 0);
     pwm_out &out = pwm_group_list[chan];
-    mcpwm_stop(out.unit_num, out.timer_num);
+    ESP_ERROR_CHECK(mcpwm_timer_start_stop(out.h_timer, MCPWM_TIMER_STOP_EMPTY));
 }
 
 void RCOutput::write(uint8_t chan, uint16_t period_us)
@@ -189,9 +224,7 @@ uint16_t RCOutput::read(uint8_t chan)
     }
 
     pwm_out &out = pwm_group_list[chan];
-    double freq = mcpwm_get_frequency(out.unit_num, out.timer_num);
-    double dprc = mcpwm_get_duty(out.unit_num, out.timer_num, out.op);
-    return (1000000.0 * (dprc / 100.)) / freq;
+    return out.value;
 }
 
 void RCOutput::read(uint16_t *period_us, uint8_t len)
@@ -248,7 +281,8 @@ void RCOutput::write_int(uint8_t chan, uint16_t period_us)
     }
 
     pwm_out &out = pwm_group_list[chan];
-    mcpwm_set_duty_in_us(out.unit_num, out.timer_num, out.op, period_us);
+    ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(out.h_cmpr, period_us));
+    out.value = period_us;
 }
 
 /*
diff --git a/libraries/AP_HAL_ESP32/RCOutput.h b/libraries/AP_HAL_ESP32/RCOutput.h
index cf70a8fb89d4e4..bdaf4f348322a9 100644
--- a/libraries/AP_HAL_ESP32/RCOutput.h
+++ b/libraries/AP_HAL_ESP32/RCOutput.h
@@ -12,7 +12,7 @@
  * You should have received a copy of the GNU General Public License along
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  *
- * Code by Charles "Silvanosky" Villard and David "Buzz" Bussenschutt
+ * Code by Charles "Silvanosky" Villard, David "Buzz" Bussenschutt and Andrey "ARg" Romanov
  *
  */
 
@@ -21,10 +21,12 @@
 
 #include <AP_HAL/RCOutput.h>
 #include "HAL_ESP32_Namespace.h"
-#include "driver/mcpwm.h"
 #define HAL_PARAM_DEFAULTS_PATH nullptr
 #include <AP_HAL/Util.h>
 
+#include "driver/gpio.h"
+#include "driver/mcpwm_prelude.h"
+
 namespace ESP32
 {
 
@@ -96,13 +98,19 @@ class RCOutput : public AP_HAL::RCOutput
 
 
 private:
+
     struct pwm_out {
-        int gpio_num;
-        mcpwm_unit_t unit_num;
-        mcpwm_timer_t timer_num;
-        mcpwm_io_signals_t io_signal;
-        mcpwm_operator_t op;
-        uint8_t chan;
+
+         uint8_t chan;
+         uint8_t gpio_num;
+         uint8_t group_id;
+         int     freq;
+         int     value;
+
+         mcpwm_timer_handle_t h_timer;
+         mcpwm_oper_handle_t  h_oper;
+         mcpwm_cmpr_handle_t  h_cmpr;
+         mcpwm_gen_handle_t   h_gen;        
     };