From 229cbce50b4cf09122ed55003a42d4b8ec014505 Mon Sep 17 00:00:00 2001
From: Peter van der Perk <57130844+PetervdPerk-NXP@users.noreply.github.com>
Date: Mon, 8 Jul 2024 18:57:15 +0200
Subject: [PATCH] imxrt dshot timing fix (#23365)

* imxrt: Change PLL settings for more accurate dshot timing
* Update NuttX submodule
---
 boards/px4/fmu-v6xrt/nuttx-config/include/board.h |  1 +
 boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c      | 12 ++++++------
 platforms/nuttx/NuttX/nuttx                       |  2 +-
 platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c   |  5 ++---
 4 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/boards/px4/fmu-v6xrt/nuttx-config/include/board.h b/boards/px4/fmu-v6xrt/nuttx-config/include/board.h
index 83832f228da1..3c0753df7dd4 100644
--- a/boards/px4/fmu-v6xrt/nuttx-config/include/board.h
+++ b/boards/px4/fmu-v6xrt/nuttx-config/include/board.h
@@ -52,6 +52,7 @@
 #define IMXRT_IPG_PODF_DIVIDER 5
 #define BOARD_GPT_FREQUENCY  24000000
 #define BOARD_XTAL_FREQUENCY 24000000
+#define BOARD_FLEXIO_PREQ    108000000
 
 /* SDIO *********************************************************************/
 
diff --git a/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c b/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c
index db591baf81c9..3223d585aa76 100644
--- a/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c
+++ b/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c
@@ -114,11 +114,11 @@ const struct clock_configuration_s g_initial_clkconfig = {
 			.div           = 1,
 			.mux           = ACMP_CLK_ROOT_OSC_RC_48M_DIV2,
 		},
-		.flexio1_clk_root = /* 240 / 2 = 120Mhz */
+		.flexio1_clk_root = /* 432 / 4 = 108Mhz */
 		{
 			.enable        = 1,
-			.div           = 2,
-			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL3_DIV2,
+			.div           = 4,
+			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL2_PFD3,
 		},
 		.flexio2_clk_root =
 		{
@@ -492,9 +492,9 @@ const struct clock_configuration_s g_initial_clkconfig = {
 		.mfd             = 268435455,
 		.ss_enable       = 0,
 		.pfd0            = 27, /* (528 * 18) / 27 = 352 MHz */
-		.pfd1            = 16, /* (528 * 16) / 16 = 594 MHz */
-		.pfd2            = 24, /* (528 * 24) / 27 = 396 MHz */
-		.pfd3            = 32, /* (528 * 32) / 27 = 297 MHz */
+		.pfd1            = 16, /* (528 * 18) / 16 = 594 MHz */
+		.pfd2            = 24, /* (528 * 18) / 24 = 396 MHz */
+		.pfd3            = 22, /* (528 * 18) / 22 = 216 MHz */
 	},
 	.sys_pll3 =
 	{
diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx
index d72688cba139..d3b85f3e545d 160000
--- a/platforms/nuttx/NuttX/nuttx
+++ b/platforms/nuttx/NuttX/nuttx
@@ -1 +1 @@
-Subproject commit d72688cba1396c882ab74fc921483c8decb0b094
+Subproject commit d3b85f3e545de3692c100143f4a9ae67762ce679
diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
index cb92dd1209a3..22a27427f9dc 100644
--- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
+++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c
@@ -46,7 +46,6 @@
 #include "arm_internal.h"
 
 #define FLEXIO_BASE			IMXRT_FLEXIO1_BASE
-#define FLEXIO_PREQ			120000000
 #define DSHOT_TIMERS			FLEXIO_SHIFTBUFNIS_COUNT
 #define DSHOT_THROTTLE_POSITION		5u
 #define DSHOT_TELEMETRY_POSITION	4u
@@ -305,8 +304,8 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
 int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
 {
 	/* Calculate dshot timings based on dshot_pwm_freq */
-	dshot_tcmp = 0x2F00 | (((FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2)) & 0xFF);
-	bdshot_tcmp = 0x2900 | (((FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 1) & 0xFF);
+	dshot_tcmp = 0x2F00 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF);
+	bdshot_tcmp = 0x2900 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 3) & 0xFF);
 
 	/* Clock FlexIO peripheral */
 	imxrt_clockall_flexio1();