diff --git a/platforms/nuttx/src/px4/espressif/esp32/io_pins/pwm_servo.c b/platforms/nuttx/src/px4/espressif/esp32/io_pins/pwm_servo.c
index da2b7e932a..f171621aee 100644
--- a/platforms/nuttx/src/px4/espressif/esp32/io_pins/pwm_servo.c
+++ b/platforms/nuttx/src/px4/espressif/esp32/io_pins/pwm_servo.c
@@ -78,7 +78,6 @@
 #define LEDC_LSCH0_DUTY_REG          (DR_REG_LEDC_BASE + 0x00A8)
 #define LEDC_CONF_REG          (DR_REG_LEDC_BASE + 0x0190)
 
-
 #define LEDC_SIG_OUT_EN_LSCH0  1 << 2
 #define LEDC_PARA_UP_LSCH0  1 << 4
 #define DPORT_LEDC_CLK_EN   1 << 11
@@ -129,7 +128,8 @@ void get_optimal_timer_setup(uint32_t desired_freq)
 
 	uint32_t shifted = 1;
 	timer_rate = desired_freq;
-	reload = (20480000000 / desired_freq + LEDC_CLKDIV_MAX) / LEDC_CLKDIV_MAX;
+	uint64_t pwm_clk = 80000000;
+	reload = (pwm_clk * 256 / desired_freq + LEDC_CLKDIV_MAX) / LEDC_CLKDIV_MAX;
 
 	if (reload == 0) {
 		reload = 1;
@@ -148,7 +148,7 @@ void get_optimal_timer_setup(uint32_t desired_freq)
 	}
 
 	shift = shifted;
-	prescaler = (20480000000 / reload) / desired_freq;
+	prescaler = (pwm_clk * 256 / reload) / desired_freq;
 }
 
 int up_pwm_servo_set(unsigned channel, uint16_t value)
@@ -157,10 +157,23 @@ int up_pwm_servo_set(unsigned channel, uint16_t value)
 	uint32_t regval = b16toi(duty * reload + b16HALF);
 
 	irqstate_t flags;
-
 	flags = px4_enter_critical_section();
+	SET_CHAN_REG(channel, LEDC_LSCH0_CONF0_REG, 0);
+	SET_CHAN_REG(channel, LEDC_LSCH0_CONF1_REG, 0);
+
+	/* Set pulse phase 0 */
+	SET_CHAN_REG(channel, LEDC_LSCH0_HPOINT_REG, 0);
 	SET_CHAN_REG(channel, LEDC_LSCH0_DUTY_REG, regval << 4);
+
+	SET_CHAN_BITS(channel, LEDC_LSCH0_CONF0_REG, LEDC_SIG_OUT_EN_LSCH0);
+
+	/* Start Duty counter  */
+	SET_CHAN_BITS(channel, LEDC_LSCH0_CONF1_REG, LEDC_DUTY_START_LSCH0);
+
+	/* Update duty and phase to hardware */
 	SET_CHAN_BITS(channel, LEDC_LSCH0_CONF0_REG, LEDC_PARA_UP_LSCH0);
+
+
 	px4_leave_critical_section(flags);
 
 	return OK;