diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..b90ae05 --- /dev/null +++ b/.clang-format @@ -0,0 +1,12 @@ +BasedOnStyle: Google +IndentWidth: 4 +AlignConsecutiveMacros: Consecutive +AlignConsecutiveDeclarations: Consecutive +AlignConsecutiveAssignments: Consecutive +BreakBeforeBraces: Custom +IndentGotoLabels: false +BraceWrapping: + AfterEnum: false + AfterStruct: false + SplitEmptyFunction: false +ColumnLimit: 120 diff --git a/CMakeLists.txt b/CMakeLists.txt index c9a5bed..94d83d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.10) -project(fire_rt1021_evk_template) +project(fire_rt1021_evk_fspi) enable_language(CXX) enable_language(ASM) @@ -91,13 +91,18 @@ set(TARGET_SOURCES "board/dcd.c" "board/peripherals.c" "board/pin_mux.c" + "src/app_fspi.c" + "src/app_mpu_impl.c" "src/main.c" + "src/mpu6500.c" "xip/fire_rt1021_evk_flexspi_nor_config.c" ) set(TARGET_C_DEFINES "CPU_MIMXRT1021DAG5A" "MCUXPRESSO_SDK" + "PRINTF_ADVANCED_ENABLE=1" + "PRINTF_FLOAT_ENABLE=1" "SERIAL_PORT_TYPE_UART" "__STARTUP_CLEAR_BSS" "__STARTUP_INITIALIZE_NONCACHEDATA" diff --git a/MIMXRT1021xxxxx.mex b/MIMXRT1021xxxxx.mex index 0d227ab..d2fe603 100644 --- a/MIMXRT1021xxxxx.mex +++ b/MIMXRT1021xxxxx.mex @@ -43,6 +43,37 @@ + + Configures pin routing and optionally pin electrical features. + + true + core0 + true + + + + + true + + + + + true + + + + + true + + + + + + + + + + @@ -58,22 +89,22 @@ - + true - + INPUT - + true - + OUTPUT @@ -125,6 +156,79 @@ + false + + + + + + + + true + + + + + INPUT + + + + + true + + + + + OUTPUT + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true diff --git a/SDK b/SDK new file mode 160000 index 0000000..d983b26 --- /dev/null +++ b/SDK @@ -0,0 +1 @@ +Subproject commit d983b267f0e67a8f06c2dad2d7a5e93519a699b0 diff --git a/board/clock_config.c b/board/clock_config.c index 5d07854..682a105 100644 --- a/board/clock_config.c +++ b/board/clock_config.c @@ -38,7 +38,7 @@ processor_version: 12.0.1 ******************************************************************************/ void BOARD_InitBootClocks(void) { - BOARD_BootClockRUN(); + BOARD_BootClockPLL480M(); } /******************************************************************************* @@ -47,7 +47,6 @@ void BOARD_InitBootClocks(void) /* TEXT BELOW IS USED AS SETTING FOR TOOLS ************************************* !!Configuration name: BOARD_BootClockRUN -called_from_default_init: true outputs: - {id: AHB_CLK_ROOT.outFreq, value: 24 MHz} - {id: CAN_CLK_ROOT.outFreq, value: 2 MHz} @@ -362,3 +361,334 @@ void BOARD_BootClockRUN(void) SystemCoreClock = BOARD_BOOTCLOCKRUN_CORE_CLOCK; } +/******************************************************************************* + ******************** Configuration BOARD_BootClockPLL480M ********************* + ******************************************************************************/ +/* TEXT BELOW IS USED AS SETTING FOR TOOLS ************************************* +!!Configuration +name: BOARD_BootClockPLL480M +called_from_default_init: true +outputs: +- {id: AHB_CLK_ROOT.outFreq, value: 24 MHz} +- {id: CAN_CLK_ROOT.outFreq, value: 2 MHz} +- {id: CLK_1M.outFreq, value: 1 MHz} +- {id: CLK_24M.outFreq, value: 24 MHz} +- {id: ENET_125M_CLK.outFreq, value: 2.4 MHz} +- {id: ENET_25M_REF_CLK.outFreq, value: 1.2 MHz} +- {id: ENET_500M_REF_CLK.outFreq, value: 24 MHz} +- {id: FLEXIO1_CLK_ROOT.outFreq, value: 1.5 MHz} +- {id: FLEXSPI_CLK_ROOT.outFreq, value: 1 MHz} +- {id: GPT1_ipg_clk_highfreq.outFreq, value: 2000/11 kHz} +- {id: GPT2_ipg_clk_highfreq.outFreq, value: 2000/11 kHz} +- {id: IPG_CLK_ROOT.outFreq, value: 6 MHz} +- {id: LPI2C_CLK_ROOT.outFreq, value: 3 MHz} +- {id: LPSPI_CLK_ROOT.outFreq, value: 4.8 MHz} +- {id: MQS_MCLK.outFreq, value: 3 MHz} +- {id: PERCLK_CLK_ROOT.outFreq, value: 2000/11 kHz} +- {id: SAI1_CLK_ROOT.outFreq, value: 3 MHz} +- {id: SAI1_MCLK1.outFreq, value: 3 MHz} +- {id: SAI1_MCLK2.outFreq, value: 3 MHz} +- {id: SAI1_MCLK3.outFreq, value: 1.5 MHz} +- {id: SAI2_CLK_ROOT.outFreq, value: 3 MHz} +- {id: SAI2_MCLK1.outFreq, value: 3 MHz} +- {id: SAI2_MCLK3.outFreq, value: 1.5 MHz} +- {id: SAI3_CLK_ROOT.outFreq, value: 3 MHz} +- {id: SAI3_MCLK1.outFreq, value: 3 MHz} +- {id: SAI3_MCLK3.outFreq, value: 1.5 MHz} +- {id: SEMC_CLK_ROOT.outFreq, value: 8 MHz} +- {id: SPDIF0_CLK_ROOT.outFreq, value: 1.5 MHz} +- {id: TRACE_CLK_ROOT.outFreq, value: 6 MHz} +- {id: UART_CLK_ROOT.outFreq, value: 4 MHz} +- {id: USDHC1_CLK_ROOT.outFreq, value: 12 MHz} +- {id: USDHC2_CLK_ROOT.outFreq, value: 12 MHz} +settings: +- {id: CCM.FLEXSPI_PODF.scale, value: '8', locked: true} +- {id: CCM.IPG_PODF.scale, value: '4'} +- {id: CCM.LPSPI_PODF.scale, value: '5'} +- {id: CCM.PERCLK_PODF.scale, value: '33'} +- {id: CCM.SEMC_CLK_SEL.sel, value: CCM.SEMC_ALT_CLK_SEL} +- {id: CCM.SEMC_PODF.scale, value: '3', locked: true} +- {id: CCM_ANALOG.PLL2.num, value: '0'} +- {id: CCM_ANALOG.PLL2_PFD2_DIV.scale, value: '32'} +- {id: CCM_ANALOG.PLL2_PFD3_DIV.scale, value: '20'} + * BE CAREFUL MODIFYING THIS COMMENT - IT IS YAML SETTINGS FOR TOOLS **********/ + +/******************************************************************************* + * Variables for BOARD_BootClockPLL480M configuration + ******************************************************************************/ +const clock_sys_pll_config_t sysPllConfig_BOARD_BootClockPLL480M = + { + .loopDivider = 1, /* PLL loop divider, Fout = Fin * ( 20 + loopDivider*2 + numerator / denominator ) */ + .numerator = 0, /* 30 bit numerator of fractional loop divider */ + .denominator = 60000, /* 30 bit denominator of fractional loop divider */ + .src = 0, /* Bypass clock source, 0 - OSC 24M, 1 - CLK1_P and CLK1_N */ + }; +const clock_enet_pll_config_t enetPllConfig_BOARD_BootClockPLL480M = + { + .enableClkOutput = true, /* Enable the PLL providing the ENET 125MHz reference clock */ + .enableClkOutput500M = true, /* Enable the PLL providing the ENET 500MHz reference clock */ + .enableClkOutput25M = true, /* Enable the PLL providing the ENET 25MHz reference clock */ + .loopDivider = 1, /* Set frequency of ethernet reference clock to 2.4 MHz */ + .src = 0, /* Bypass clock source, 0 - OSC 24M, 1 - CLK1_P and CLK1_N */ + }; +/******************************************************************************* + * Code for BOARD_BootClockPLL480M configuration + ******************************************************************************/ +void BOARD_BootClockPLL480M(void) +{ + /* Enable 1MHz clock output. */ + XTALOSC24M->OSC_CONFIG2 |= XTALOSC24M_OSC_CONFIG2_ENABLE_1M_MASK; + /* Use free 1MHz clock output. */ + XTALOSC24M->OSC_CONFIG2 &= ~XTALOSC24M_OSC_CONFIG2_MUX_1M_MASK; + /* Set XTAL 24MHz clock frequency. */ + CLOCK_SetXtalFreq(24000000U); + /* Enable XTAL 24MHz clock source. */ + CLOCK_InitExternalClk(0); + /* Enable internal RC. */ + CLOCK_InitRcOsc24M(); + /* Switch clock source to external OSC. */ + CLOCK_SwitchOsc(kCLOCK_XtalOsc); + /* Set Oscillator ready counter value. */ + CCM->CCR = (CCM->CCR & (~CCM_CCR_OSCNT_MASK)) | CCM_CCR_OSCNT(127); + /* Setting PeriphClk2Mux and PeriphMux to provide stable clock before PLLs are initialed */ + CLOCK_SetMux(kCLOCK_PeriphClk2Mux, 1); /* Set PERIPH_CLK2 MUX to OSC */ + CLOCK_SetMux(kCLOCK_PeriphMux, 1); /* Set PERIPH_CLK MUX to PERIPH_CLK2 */ + /* Set AHB_PODF. */ + CLOCK_SetDiv(kCLOCK_AhbDiv, 0); + /* Disable IPG clock gate. */ + CLOCK_DisableClock(kCLOCK_Adc1); + CLOCK_DisableClock(kCLOCK_Adc2); + CLOCK_DisableClock(kCLOCK_Xbar1); + CLOCK_DisableClock(kCLOCK_Xbar2); + /* Set IPG_PODF. */ + CLOCK_SetDiv(kCLOCK_IpgDiv, 3); + /* Set ARM_PODF. */ + CLOCK_SetDiv(kCLOCK_ArmDiv, 0); + /* Set PERIPH_CLK2_PODF. */ + CLOCK_SetDiv(kCLOCK_PeriphClk2Div, 0); + /* Disable PERCLK clock gate. */ + CLOCK_DisableClock(kCLOCK_Gpt1); + CLOCK_DisableClock(kCLOCK_Gpt1S); + CLOCK_DisableClock(kCLOCK_Gpt2); + CLOCK_DisableClock(kCLOCK_Gpt2S); + CLOCK_DisableClock(kCLOCK_Pit); + /* Set PERCLK_PODF. */ + CLOCK_SetDiv(kCLOCK_PerclkDiv, 32); + /* Disable USDHC1 clock gate. */ + CLOCK_DisableClock(kCLOCK_Usdhc1); + /* Set USDHC1_PODF. */ + CLOCK_SetDiv(kCLOCK_Usdhc1Div, 1); + /* Set Usdhc1 clock source. */ + CLOCK_SetMux(kCLOCK_Usdhc1Mux, 0); + /* Disable USDHC2 clock gate. */ + CLOCK_DisableClock(kCLOCK_Usdhc2); + /* Set USDHC2_PODF. */ + CLOCK_SetDiv(kCLOCK_Usdhc2Div, 1); + /* Set Usdhc2 clock source. */ + CLOCK_SetMux(kCLOCK_Usdhc2Mux, 0); + /* In SDK projects, SDRAM (configured by SEMC) will be initialized in either debug script or dcd. + * With this macro SKIP_SYSCLK_INIT, system pll (selected to be SEMC source clock in SDK projects) will be left unchanged. + * Note: If another clock source is selected for SEMC, user may want to avoid changing that clock as well.*/ +#ifndef SKIP_SYSCLK_INIT + /* Disable Semc clock gate. */ + CLOCK_DisableClock(kCLOCK_Semc); + /* Set SEMC_PODF. */ + CLOCK_SetDiv(kCLOCK_SemcDiv, 2); + /* Set Semc alt clock source. */ + CLOCK_SetMux(kCLOCK_SemcAltMux, 0); + /* Set Semc clock source. */ + CLOCK_SetMux(kCLOCK_SemcMux, 1); +#endif + /* In SDK projects, external flash (configured by FLEXSPI) will be initialized by dcd. + * With this macro XIP_EXTERNAL_FLASH, usb1 pll (selected to be FLEXSPI clock source in SDK projects) will be left unchanged. + * Note: If another clock source is selected for FLEXSPI, user may want to avoid changing that clock as well.*/ +#if !(defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1)) + /* Disable Flexspi clock gate. */ + CLOCK_DisableClock(kCLOCK_FlexSpi); + /* Set FLEXSPI_PODF. */ + CLOCK_SetDiv(kCLOCK_FlexspiDiv, 7); + /* Set Flexspi clock source. */ + CLOCK_SetMux(kCLOCK_FlexspiMux, 0); +#endif + /* Disable LPSPI clock gate. */ + CLOCK_DisableClock(kCLOCK_Lpspi1); + CLOCK_DisableClock(kCLOCK_Lpspi2); + CLOCK_DisableClock(kCLOCK_Lpspi3); + CLOCK_DisableClock(kCLOCK_Lpspi4); + /* Set LPSPI_PODF. */ + CLOCK_SetDiv(kCLOCK_LpspiDiv, 4); + /* Set Lpspi clock source. */ + CLOCK_SetMux(kCLOCK_LpspiMux, 2); + /* Disable TRACE clock gate. */ + CLOCK_DisableClock(kCLOCK_Trace); + /* Set TRACE_PODF. */ + CLOCK_SetDiv(kCLOCK_TraceDiv, 3); + /* Set Trace clock source. */ + CLOCK_SetMux(kCLOCK_TraceMux, 2); + /* Disable SAI1 clock gate. */ + CLOCK_DisableClock(kCLOCK_Sai1); + /* Set SAI1_CLK_PRED. */ + CLOCK_SetDiv(kCLOCK_Sai1PreDiv, 3); + /* Set SAI1_CLK_PODF. */ + CLOCK_SetDiv(kCLOCK_Sai1Div, 1); + /* Set Sai1 clock source. */ + CLOCK_SetMux(kCLOCK_Sai1Mux, 0); + /* Disable SAI2 clock gate. */ + CLOCK_DisableClock(kCLOCK_Sai2); + /* Set SAI2_CLK_PRED. */ + CLOCK_SetDiv(kCLOCK_Sai2PreDiv, 3); + /* Set SAI2_CLK_PODF. */ + CLOCK_SetDiv(kCLOCK_Sai2Div, 1); + /* Set Sai2 clock source. */ + CLOCK_SetMux(kCLOCK_Sai2Mux, 0); + /* Disable SAI3 clock gate. */ + CLOCK_DisableClock(kCLOCK_Sai3); + /* Set SAI3_CLK_PRED. */ + CLOCK_SetDiv(kCLOCK_Sai3PreDiv, 3); + /* Set SAI3_CLK_PODF. */ + CLOCK_SetDiv(kCLOCK_Sai3Div, 1); + /* Set Sai3 clock source. */ + CLOCK_SetMux(kCLOCK_Sai3Mux, 0); + /* Disable Lpi2c clock gate. */ + CLOCK_DisableClock(kCLOCK_Lpi2c1); + CLOCK_DisableClock(kCLOCK_Lpi2c2); + CLOCK_DisableClock(kCLOCK_Lpi2c3); + /* Set LPI2C_CLK_PODF. */ + CLOCK_SetDiv(kCLOCK_Lpi2cDiv, 0); + /* Set Lpi2c clock source. */ + CLOCK_SetMux(kCLOCK_Lpi2cMux, 0); + /* Disable CAN clock gate. */ + CLOCK_DisableClock(kCLOCK_Can1); + CLOCK_DisableClock(kCLOCK_Can2); + CLOCK_DisableClock(kCLOCK_Can1S); + CLOCK_DisableClock(kCLOCK_Can2S); + /* Set CAN_CLK_PODF. */ + CLOCK_SetDiv(kCLOCK_CanDiv, 1); + /* Set Can clock source. */ + CLOCK_SetMux(kCLOCK_CanMux, 2); + /* Disable UART clock gate. */ + CLOCK_DisableClock(kCLOCK_Lpuart1); + CLOCK_DisableClock(kCLOCK_Lpuart2); + CLOCK_DisableClock(kCLOCK_Lpuart3); + CLOCK_DisableClock(kCLOCK_Lpuart4); + CLOCK_DisableClock(kCLOCK_Lpuart5); + CLOCK_DisableClock(kCLOCK_Lpuart6); + CLOCK_DisableClock(kCLOCK_Lpuart7); + CLOCK_DisableClock(kCLOCK_Lpuart8); + /* Set UART_CLK_PODF. */ + CLOCK_SetDiv(kCLOCK_UartDiv, 0); + /* Set Uart clock source. */ + CLOCK_SetMux(kCLOCK_UartMux, 0); + /* Disable SPDIF clock gate. */ + CLOCK_DisableClock(kCLOCK_Spdif); + /* Set SPDIF0_CLK_PRED. */ + CLOCK_SetDiv(kCLOCK_Spdif0PreDiv, 1); + /* Set SPDIF0_CLK_PODF. */ + CLOCK_SetDiv(kCLOCK_Spdif0Div, 7); + /* Set Spdif clock source. */ + CLOCK_SetMux(kCLOCK_SpdifMux, 3); + /* Disable Flexio1 clock gate. */ + CLOCK_DisableClock(kCLOCK_Flexio1); + /* Set FLEXIO1_CLK_PRED. */ + CLOCK_SetDiv(kCLOCK_Flexio1PreDiv, 1); + /* Set FLEXIO1_CLK_PODF. */ + CLOCK_SetDiv(kCLOCK_Flexio1Div, 7); + /* Set Flexio1 clock source. */ + CLOCK_SetMux(kCLOCK_Flexio1Mux, 3); + /* Set Pll3 sw clock source. */ + CLOCK_SetMux(kCLOCK_Pll3SwMux, 0); + /* In SDK projects, SDRAM (configured by SEMC) will be initialized in either debug script or dcd. + * With this macro SKIP_SYSCLK_INIT, system pll (selected to be SEMC source clock in SDK projects) will be left unchanged. + * Note: If another clock source is selected for SEMC, user may want to avoid changing that clock as well.*/ +#ifndef SKIP_SYSCLK_INIT +#if defined(XIP_BOOT_HEADER_DCD_ENABLE) && (XIP_BOOT_HEADER_DCD_ENABLE == 1) + #warning "SKIP_SYSCLK_INIT should be defined to keep system pll (selected to be SEMC source clock in SDK projects) unchanged." +#endif + /* Init System PLL. */ + CLOCK_InitSysPll(&sysPllConfig_BOARD_BootClockPLL480M); + /* Init System pfd0. */ + CLOCK_InitSysPfd(kCLOCK_Pfd0, 27); + /* Init System pfd1. */ + CLOCK_InitSysPfd(kCLOCK_Pfd1, 16); + /* Init System pfd2. */ + CLOCK_InitSysPfd(kCLOCK_Pfd2, 32); + /* Init System pfd3. */ + CLOCK_InitSysPfd(kCLOCK_Pfd3, 20); + /* Bypass System PLL. */ + CLOCK_SetPllBypass(CCM_ANALOG, kCLOCK_PllSys, 1); +#endif + /* In SDK projects, external flash (configured by FLEXSPI) will be initialized by dcd. + * With this macro XIP_EXTERNAL_FLASH, usb1 pll (selected to be FLEXSPI clock source in SDK projects) will be left unchanged. + * Note: If another clock source is selected for FLEXSPI, user may want to avoid changing that clock as well.*/ +#if !(defined(XIP_EXTERNAL_FLASH) && (XIP_EXTERNAL_FLASH == 1)) + /* DeInit Usb1 PLL. */ + CLOCK_DeinitUsb1Pll(); + /* Bypass Usb1 PLL. */ + CLOCK_SetPllBypass(CCM_ANALOG, kCLOCK_PllUsb1, 1); + /* Enable Usb1 PLL output. */ + CCM_ANALOG->PLL_USB1 |= CCM_ANALOG_PLL_USB1_ENABLE_MASK; +#endif + /* DeInit Audio PLL. */ + CLOCK_DeinitAudioPll(); + /* Bypass Audio PLL. */ + CLOCK_SetPllBypass(CCM_ANALOG, kCLOCK_PllAudio, 1); + /* Set divider for Audio PLL. */ + CCM_ANALOG->MISC2 &= ~CCM_ANALOG_MISC2_AUDIO_DIV_LSB_MASK; + CCM_ANALOG->MISC2 &= ~CCM_ANALOG_MISC2_AUDIO_DIV_MSB_MASK; + /* Enable Audio PLL output. */ + CCM_ANALOG->PLL_AUDIO |= CCM_ANALOG_PLL_AUDIO_ENABLE_MASK; + /* Init Enet PLL. */ + CLOCK_InitEnetPll(&enetPllConfig_BOARD_BootClockPLL480M); + /* Bypass Enet PLL. */ + CLOCK_SetPllBypass(CCM_ANALOG, kCLOCK_PllEnet, 1); + /* Set preperiph clock source. */ + CLOCK_SetMux(kCLOCK_PrePeriphMux, 2); + /* Set periph clock source. */ + CLOCK_SetMux(kCLOCK_PeriphMux, 0); + /* Set periph clock2 clock source. */ + CLOCK_SetMux(kCLOCK_PeriphClk2Mux, 0); + /* Set per clock source. */ + CLOCK_SetMux(kCLOCK_PerclkMux, 0); + /* Set clock out1 divider. */ + CCM->CCOSR = (CCM->CCOSR & (~CCM_CCOSR_CLKO1_DIV_MASK)) | CCM_CCOSR_CLKO1_DIV(0); + /* Set clock out1 source. */ + CCM->CCOSR = (CCM->CCOSR & (~CCM_CCOSR_CLKO1_SEL_MASK)) | CCM_CCOSR_CLKO1_SEL(1); + /* Set clock out2 divider. */ + CCM->CCOSR = (CCM->CCOSR & (~CCM_CCOSR_CLKO2_DIV_MASK)) | CCM_CCOSR_CLKO2_DIV(0); + /* Set clock out2 source. */ + CCM->CCOSR = (CCM->CCOSR & (~CCM_CCOSR_CLKO2_SEL_MASK)) | CCM_CCOSR_CLKO2_SEL(3); + /* Set clock out1 drives clock out1. */ + CCM->CCOSR &= ~CCM_CCOSR_CLK_OUT_SEL_MASK; + /* Disable clock out1. */ + CCM->CCOSR &= ~CCM_CCOSR_CLKO1_EN_MASK; + /* Disable clock out2. */ + CCM->CCOSR &= ~CCM_CCOSR_CLKO2_EN_MASK; + /* Set SAI1 MCLK1 clock source. */ + IOMUXC_SetSaiMClkClockSource(IOMUXC_GPR, kIOMUXC_GPR_SAI1MClk1Sel, 0); + /* Set SAI1 MCLK2 clock source. */ + IOMUXC_SetSaiMClkClockSource(IOMUXC_GPR, kIOMUXC_GPR_SAI1MClk2Sel, 0); + /* Set SAI1 MCLK3 clock source. */ + IOMUXC_SetSaiMClkClockSource(IOMUXC_GPR, kIOMUXC_GPR_SAI1MClk3Sel, 0); + /* Set SAI2 MCLK3 clock source. */ + IOMUXC_SetSaiMClkClockSource(IOMUXC_GPR, kIOMUXC_GPR_SAI2MClk3Sel, 0); + /* Set SAI3 MCLK3 clock source. */ + IOMUXC_SetSaiMClkClockSource(IOMUXC_GPR, kIOMUXC_GPR_SAI3MClk3Sel, 0); + /* Set MQS configuration. */ + IOMUXC_MQSConfig(IOMUXC_GPR,kIOMUXC_MqsPwmOverSampleRate32, 0); + /* Set ENET Ref clock source. */ +#if defined(IOMUXC_GPR_GPR1_ENET_REF_CLK_DIR_MASK) + IOMUXC_GPR->GPR1 &= ~IOMUXC_GPR_GPR1_ENET_REF_CLK_DIR_MASK; +#elif defined(IOMUXC_GPR_GPR1_ENET1_TX_CLK_DIR_MASK) + /* Backward compatibility for original bitfield name */ + IOMUXC_GPR->GPR1 &= ~IOMUXC_GPR_GPR1_ENET1_TX_CLK_DIR_MASK; +#else +#error "Neither IOMUXC_GPR_GPR1_ENET_REF_CLK_DIR_MASK nor IOMUXC_GPR_GPR1_ENET1_TX_CLK_DIR_MASK is defined." +#endif /* defined(IOMUXC_GPR_GPR1_ENET_REF_CLK_DIR_MASK) */ + /* Set GPT1 High frequency reference clock source. */ + IOMUXC_GPR->GPR5 &= ~IOMUXC_GPR_GPR5_VREF_1M_CLK_GPT1_MASK; + /* Set GPT2 High frequency reference clock source. */ + IOMUXC_GPR->GPR5 &= ~IOMUXC_GPR_GPR5_VREF_1M_CLK_GPT2_MASK; + /* Set SystemCoreClock variable. */ + SystemCoreClock = BOARD_BOOTCLOCKPLL480M_CORE_CLOCK; +} + diff --git a/board/clock_config.h b/board/clock_config.h index 4d34732..257d191 100644 --- a/board/clock_config.h +++ b/board/clock_config.h @@ -102,5 +102,80 @@ void BOARD_BootClockRUN(void); } #endif /* __cplusplus*/ +/******************************************************************************* + ******************** Configuration BOARD_BootClockPLL480M ********************* + ******************************************************************************/ +/******************************************************************************* + * Definitions for BOARD_BootClockPLL480M configuration + ******************************************************************************/ +#define BOARD_BOOTCLOCKPLL480M_CORE_CLOCK 24000000U /*!< Core clock frequency: 24000000Hz */ + +/* Clock outputs (values are in Hz): */ +#define BOARD_BOOTCLOCKPLL480M_AHB_CLK_ROOT 24000000UL +#define BOARD_BOOTCLOCKPLL480M_CAN_CLK_ROOT 2000000UL +#define BOARD_BOOTCLOCKPLL480M_CKIL_SYNC_CLK_ROOT 0UL +#define BOARD_BOOTCLOCKPLL480M_CLKO1_CLK 0UL +#define BOARD_BOOTCLOCKPLL480M_CLKO2_CLK 0UL +#define BOARD_BOOTCLOCKPLL480M_CLK_1M 1000000UL +#define BOARD_BOOTCLOCKPLL480M_CLK_24M 24000000UL +#define BOARD_BOOTCLOCKPLL480M_ENET_125M_CLK 2400000UL +#define BOARD_BOOTCLOCKPLL480M_ENET_25M_REF_CLK 1200000UL +#define BOARD_BOOTCLOCKPLL480M_ENET_500M_REF_CLK 24000000UL +#define BOARD_BOOTCLOCKPLL480M_ENET_REF_CLK 0UL +#define BOARD_BOOTCLOCKPLL480M_ENET_TX_CLK 0UL +#define BOARD_BOOTCLOCKPLL480M_FLEXIO1_CLK_ROOT 1500000UL +#define BOARD_BOOTCLOCKPLL480M_FLEXSPI_CLK_ROOT 1000000UL +#define BOARD_BOOTCLOCKPLL480M_GPT1_IPG_CLK_HIGHFREQ 181818UL +#define BOARD_BOOTCLOCKPLL480M_GPT2_IPG_CLK_HIGHFREQ 181818UL +#define BOARD_BOOTCLOCKPLL480M_IPG_CLK_ROOT 6000000UL +#define BOARD_BOOTCLOCKPLL480M_LPI2C_CLK_ROOT 3000000UL +#define BOARD_BOOTCLOCKPLL480M_LPSPI_CLK_ROOT 4800000UL +#define BOARD_BOOTCLOCKPLL480M_MQS_MCLK 3000000UL +#define BOARD_BOOTCLOCKPLL480M_PERCLK_CLK_ROOT 181818UL +#define BOARD_BOOTCLOCKPLL480M_SAI1_CLK_ROOT 3000000UL +#define BOARD_BOOTCLOCKPLL480M_SAI1_MCLK1 3000000UL +#define BOARD_BOOTCLOCKPLL480M_SAI1_MCLK2 3000000UL +#define BOARD_BOOTCLOCKPLL480M_SAI1_MCLK3 1500000UL +#define BOARD_BOOTCLOCKPLL480M_SAI2_CLK_ROOT 3000000UL +#define BOARD_BOOTCLOCKPLL480M_SAI2_MCLK1 3000000UL +#define BOARD_BOOTCLOCKPLL480M_SAI2_MCLK2 0UL +#define BOARD_BOOTCLOCKPLL480M_SAI2_MCLK3 1500000UL +#define BOARD_BOOTCLOCKPLL480M_SAI3_CLK_ROOT 3000000UL +#define BOARD_BOOTCLOCKPLL480M_SAI3_MCLK1 3000000UL +#define BOARD_BOOTCLOCKPLL480M_SAI3_MCLK2 0UL +#define BOARD_BOOTCLOCKPLL480M_SAI3_MCLK3 1500000UL +#define BOARD_BOOTCLOCKPLL480M_SEMC_CLK_ROOT 8000000UL +#define BOARD_BOOTCLOCKPLL480M_SPDIF0_CLK_ROOT 1500000UL +#define BOARD_BOOTCLOCKPLL480M_SPDIF0_EXTCLK_OUT 0UL +#define BOARD_BOOTCLOCKPLL480M_TRACE_CLK_ROOT 6000000UL +#define BOARD_BOOTCLOCKPLL480M_UART_CLK_ROOT 4000000UL +#define BOARD_BOOTCLOCKPLL480M_USBPHY1_CLK 0UL +#define BOARD_BOOTCLOCKPLL480M_USDHC1_CLK_ROOT 12000000UL +#define BOARD_BOOTCLOCKPLL480M_USDHC2_CLK_ROOT 12000000UL + +/*! @brief Sys PLL for BOARD_BootClockPLL480M configuration. + */ +extern const clock_sys_pll_config_t sysPllConfig_BOARD_BootClockPLL480M; +/*! @brief Enet PLL set for BOARD_BootClockPLL480M configuration. + */ +extern const clock_enet_pll_config_t enetPllConfig_BOARD_BootClockPLL480M; + +/******************************************************************************* + * API for BOARD_BootClockPLL480M configuration + ******************************************************************************/ +#if defined(__cplusplus) +extern "C" { +#endif /* __cplusplus*/ + +/*! + * @brief This function executes configuration of clocks. + * + */ +void BOARD_BootClockPLL480M(void); + +#if defined(__cplusplus) +} +#endif /* __cplusplus*/ + #endif /* _CLOCK_CONFIG_H_ */ diff --git a/board/pin_mux.c b/board/pin_mux.c index 5ec4df0..637185d 100644 --- a/board/pin_mux.c +++ b/board/pin_mux.c @@ -15,6 +15,7 @@ processor_version: 12.0.1 */ #include "fsl_common.h" +#include "fsl_iomuxc.h" #include "pin_mux.h" /* FUNCTION ************************************************************************************************************ @@ -25,6 +26,7 @@ processor_version: 12.0.1 * END ****************************************************************************************************************/ void BOARD_InitBootPins(void) { BOARD_InitPins(); + BOARD_InitFlexSPIPins(); } /* @@ -44,6 +46,34 @@ BOARD_InitPins: void BOARD_InitPins(void) { } + +/* + * TEXT BELOW IS USED AS SETTING FOR TOOLS ************************************* +BOARD_InitFlexSPIPins: +- options: {callFromInitBoot: 'true', coreID: core0, enableClock: 'true'} +- pin_list: + - {pin_num: '30', peripheral: FLEXSPI, signal: FLEXSPI_B_DATA0, pin_signal: GPIO_SD_B1_02} + - {pin_num: '27', peripheral: FLEXSPI, signal: FLEXSPI_B_DATA1, pin_signal: GPIO_SD_B1_04} + - {pin_num: '32', peripheral: FLEXSPI, signal: FLEXSPI_B_SCLK, pin_signal: GPIO_SD_B1_01} + - {pin_num: '26', peripheral: FLEXSPI, signal: FLEXSPI_B_SS0_B, pin_signal: GPIO_SD_B1_05} + * BE CAREFUL MODIFYING THIS COMMENT - IT IS YAML SETTINGS FOR TOOLS *********** + */ + +/* FUNCTION ************************************************************************************************************ + * + * Function Name : BOARD_InitFlexSPIPins + * Description : Configures pin routing and optionally pin electrical features. + * + * END ****************************************************************************************************************/ +void BOARD_InitFlexSPIPins(void) { + CLOCK_EnableClock(kCLOCK_Iomuxc); + + IOMUXC_SetPinMux(IOMUXC_GPIO_SD_B1_01_FLEXSPI_B_SCLK, 0U); + IOMUXC_SetPinMux(IOMUXC_GPIO_SD_B1_02_FLEXSPI_B_DATA00, 0U); + IOMUXC_SetPinMux(IOMUXC_GPIO_SD_B1_04_FLEXSPI_B_DATA01, 0U); + IOMUXC_SetPinMux(IOMUXC_GPIO_SD_B1_05_FLEXSPI_B_SS0_B, 0U); +} + /*********************************************************************************************************************** * EOF **********************************************************************************************************************/ diff --git a/board/pin_mux.h b/board/pin_mux.h index 736e3be..dc0621d 100644 --- a/board/pin_mux.h +++ b/board/pin_mux.h @@ -44,6 +44,13 @@ void BOARD_InitBootPins(void); */ void BOARD_InitPins(void); + +/*! + * @brief Configures pin routing and optionally pin electrical features. + * + */ +void BOARD_InitFlexSPIPins(void); + #if defined(__cplusplus) } #endif diff --git a/include/app_fspi.h b/include/app_fspi.h new file mode 100644 index 0000000..1180368 --- /dev/null +++ b/include/app_fspi.h @@ -0,0 +1,12 @@ +#ifndef APP_FSPI_H +#define APP_FSPI_H + +#include "fsl_common.h" + +status_t fspi_init(void); +status_t fspi_write(uint8_t *data, uint32_t len); +status_t fspi_read(uint8_t *data, uint32_t len); +status_t fspi_read_registers(uint8_t addr, uint8_t *data, uint32_t len); +status_t fspi_write_registers(uint8_t addr, uint8_t *data, uint32_t len); + +#endif \ No newline at end of file diff --git a/include/app_mpu_impl.h b/include/app_mpu_impl.h new file mode 100644 index 0000000..f45b0fe --- /dev/null +++ b/include/app_mpu_impl.h @@ -0,0 +1,10 @@ +#ifndef APP_MPU_IMPL_H +#define APP_MPU_IMPL_H + +#include "mpu6500.h" + +mpu6500_ret_t app_mpu_impl_read_reg(void *handle, uint8_t reg, uint8_t *data, uint32_t len); +mpu6500_ret_t app_mpu_impl_write_reg(void *handle, uint8_t reg, uint8_t *data, uint32_t len); +mpu6500_ret_t app_mpu_impl_delay(void *handle, uint32_t msec); + +#endif \ No newline at end of file diff --git a/include/mpu6500.h b/include/mpu6500.h new file mode 100644 index 0000000..7148e9a --- /dev/null +++ b/include/mpu6500.h @@ -0,0 +1,52 @@ +#ifndef MPU6500_H +#define MPU6500_H + +#include + +typedef enum { + MPU6500_RET_SUCCESS, + MPU6500_RET_FAIL, +} mpu6500_ret_t; + +typedef enum { + MPU6500_CH_TEMP = 0x41U, + MPU6500_CH_ACCEL_X = 0x3BU, + MPU6500_CH_ACCEL_Y = 0x3DU, + MPU6500_CH_ACCEL_Z = 0x3FU, + MPU6500_CH_GYRO_X = 0x43U, + MPU6500_CH_GYRO_Y = 0x45U, + MPU6500_CH_GYRO_Z = 0x47U, +} mpu6500_channel_t; + +typedef struct { + int16_t temp_raw; + int16_t accel_x_raw; + int16_t accel_y_raw; + int16_t accel_z_raw; + int16_t gyro_x_raw; + int16_t gyro_y_raw; + int16_t gyro_z_raw; +} mpu6500_result_t; + +typedef mpu6500_ret_t (*mpu6500_ops_read_register_t)(void *handle, uint8_t addr, uint8_t *data, uint32_t len); +typedef mpu6500_ret_t (*mpu6500_ops_write_register_t)(void *handle, uint8_t addr, uint8_t *data, uint32_t len); +typedef mpu6500_ret_t (*mpu6500_ops_delay)(void *handle, uint32_t msec); + +typedef struct { + mpu6500_ops_read_register_t read_register; + mpu6500_ops_write_register_t write_register; + mpu6500_ops_delay delay; +} mpu6500_ops_t; + +typedef struct { + mpu6500_ops_t ops; + void *user_data; +} mpu6500_t; + +mpu6500_ret_t mpu6500_init(mpu6500_t *mpu); +mpu6500_ret_t mpu6500_read_channel(mpu6500_t *mpu, mpu6500_channel_t ch, int16_t *result); +mpu6500_ret_t mpu6500_read_result(mpu6500_t *mpu, mpu6500_result_t *result); +mpu6500_ret_t mpu6500_get_accel_fullscale(mpu6500_t *mpu, uint16_t *f_scale); +mpu6500_ret_t mpu6500_get_gyro_fullscale(mpu6500_t *mpu, uint16_t *f_scale); + +#endif \ No newline at end of file diff --git a/src/app_fspi.c b/src/app_fspi.c new file mode 100644 index 0000000..e554b67 --- /dev/null +++ b/src/app_fspi.c @@ -0,0 +1,123 @@ +#include "app_fspi.h" + +#include "fsl_clock.h" +#include "fsl_flexspi.h" + +#define FSPI_BASE FLEXSPI +#define FSPI_PORT kFLEXSPI_PortB1 + +#define FSPI_SEQ_WR_IDX 0 +#define FSPI_SEQ_W1_IDX 1 +#define FSPI_SEQ_RD_IDX 2 +#define FSPI_SEQ_R_REG_IDX 3 +#define FSPI_SEQ_INVALID 4 + +/* clang-format off */ +static uint32_t s_fspi_lut[4 * (FSPI_SEQ_INVALID + 1)] = { + /* Multi-byte write sequence, 1st is RADDR to avoid internal confusion */ + [4 * FSPI_SEQ_WR_IDX + 0] = FLEXSPI_LUT_SEQ(kFLEXSPI_Command_RADDR_SDR, kFLEXSPI_1PAD, 8U, kFLEXSPI_Command_WRITE_SDR, kFLEXSPI_1PAD, 0U), + /* Single-byte write sequence, use RADDR to avoid internal confusion */ + [4 * FSPI_SEQ_W1_IDX + 0] = FLEXSPI_LUT_SEQ(kFLEXSPI_Command_RADDR_SDR, kFLEXSPI_1PAD, 8U, kFLEXSPI_Command_STOP, kFLEXSPI_1PAD, 0U), + /* Multi-byte read sequence */ + [4 * FSPI_SEQ_RD_IDX + 0] = FLEXSPI_LUT_SEQ(kFLEXSPI_Command_READ_SDR, kFLEXSPI_1PAD, 0U, kFLEXSPI_Command_STOP, kFLEXSPI_1PAD, 0U), + /* Register read sequence, the first byte is register address, the length can be modified by changing op0. */ + [4 * FSPI_SEQ_R_REG_IDX + 0] = FLEXSPI_LUT_SEQ(kFLEXSPI_Command_RADDR_SDR, kFLEXSPI_1PAD, 8U, kFLEXSPI_Command_READ_SDR, kFLEXSPI_1PAD, 0U), +}; +/* clang-format on */ + +status_t fspi_init(void) { + status_t ret = kStatus_Success; + flexspi_config_t fspi_cfg; + + FLEXSPI_GetDefaultConfig(&fspi_cfg); + + FLEXSPI_Init(FSPI_BASE, &fspi_cfg); + + flexspi_device_config_t fspi_dev_cfg = { + .flexspiRootClk = CLOCK_GetClockRootFreq(kCLOCK_FlexspiClkRoot), + .flashSize = 1 * 1024 * 1024, /* Don't care */ + .CSIntervalUnit = kFLEXSPI_CsIntervalUnit1SckCycle, + .CSInterval = 2, + .CSHoldTime = 3, + .CSSetupTime = 3, + .dataValidTime = 2, + .columnspace = 0, + .enableWordAddress = false, + .AWRSeqIndex = FSPI_SEQ_INVALID, + .AWRSeqNumber = 1, + .ARDSeqIndex = FSPI_SEQ_INVALID, + .ARDSeqNumber = 1, + .AHBWriteWaitUnit = kFLEXSPI_AhbWriteWaitUnit2AhbCycle, + .AHBWriteWaitInterval = 0, + .enableWriteMask = false, + }; + + FLEXSPI_SetFlashConfig(FSPI_BASE, &fspi_dev_cfg, FSPI_PORT); + FLEXSPI_UpdateLUT(FSPI_BASE, 0, s_fspi_lut, 4 * (FSPI_SEQ_INVALID + 1)); + + FLEXSPI_SoftwareReset(FSPI_BASE); + + return ret; +} + +status_t fspi_write(uint8_t *data, uint32_t len) { + flexspi_transfer_t ip_xfer = { + .port = FSPI_PORT, + .deviceAddress = data[0], + .cmdType = kFLEXSPI_Write, + .seqIndex = FSPI_SEQ_W1_IDX, + .SeqNumber = 1U, + .data = (uint32_t *)data, + .dataSize = 1, + }; + + if (len > 1) { + ip_xfer.seqIndex = FSPI_SEQ_WR_IDX; + ip_xfer.dataSize = len - 1; + ip_xfer.data = (uint32_t *)(&data[1]); + } + + return FLEXSPI_TransferBlocking(FSPI_BASE, &ip_xfer); +} + +status_t fspi_read(uint8_t *data, uint32_t len) { + flexspi_transfer_t ip_xfer = { + .port = FSPI_PORT, + .deviceAddress = 0UL, + .cmdType = kFLEXSPI_Read, + .seqIndex = FSPI_SEQ_RD_IDX, + .SeqNumber = 1U, + .data = (uint32_t *)data, + .dataSize = len, + }; + + return FLEXSPI_TransferBlocking(FSPI_BASE, &ip_xfer); +} + +status_t fspi_read_registers(uint8_t addr, uint8_t *data, uint32_t len) { + flexspi_transfer_t ip_xfer = { + .port = FSPI_PORT, + .deviceAddress = addr, + .cmdType = kFLEXSPI_Read, + .seqIndex = FSPI_SEQ_R_REG_IDX, + .SeqNumber = 1U, + .data = (uint32_t *)data, + .dataSize = len, + }; + + return FLEXSPI_TransferBlocking(FSPI_BASE, &ip_xfer); +} + +status_t fspi_write_registers(uint8_t addr, uint8_t *data, uint32_t len) { + flexspi_transfer_t ip_xfer = { + .port = FSPI_PORT, + .deviceAddress = addr, + .cmdType = kFLEXSPI_Write, + .seqIndex = FSPI_SEQ_WR_IDX, + .SeqNumber = 1U, + .data = (uint32_t *)data, + .dataSize = len, + }; + + return FLEXSPI_TransferBlocking(FSPI_BASE, &ip_xfer); +} \ No newline at end of file diff --git a/src/app_mpu_impl.c b/src/app_mpu_impl.c new file mode 100644 index 0000000..6a6eb24 --- /dev/null +++ b/src/app_mpu_impl.c @@ -0,0 +1,29 @@ +#include "app_fspi.h" + +#include "app_mpu_impl.h" + +mpu6500_ret_t app_mpu_impl_read_reg(void *handle, uint8_t reg, uint8_t *data, uint32_t len) { + uint8_t read_addr = reg | 0x80; + + if(fspi_read_registers(read_addr, data, len) != kStatus_Success) { + return MPU6500_RET_FAIL; + } + + return MPU6500_RET_SUCCESS; +} + +mpu6500_ret_t app_mpu_impl_write_reg(void *handle, uint8_t reg, uint8_t *data, uint32_t len) { + uint8_t write_addr = reg & ~(0x80); + + if(fspi_write_registers(write_addr, data, len) != kStatus_Success) { + return MPU6500_RET_FAIL; + } + + return MPU6500_RET_SUCCESS; +} + +mpu6500_ret_t app_mpu_impl_delay(void *handle, uint32_t msec) { + SDK_DelayAtLeastUs(msec * 1000, CLOCK_GetCoreSysClkFreq()); + + return MPU6500_RET_SUCCESS; +} \ No newline at end of file diff --git a/src/main.c b/src/main.c index e42a6d2..6382587 100644 --- a/src/main.c +++ b/src/main.c @@ -1,47 +1,84 @@ -/* - * Copyright (c) 2013 - 2015, Freescale Semiconductor, Inc. - * Copyright 2016-2017 NXP - * All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include "fsl_device_registers.h" -#include "fsl_debug_console.h" -#include "pin_mux.h" -#include "clock_config.h" +/* Board */ #include "board.h" +#include "clock_config.h" +#include "pin_mux.h" -/******************************************************************************* - * Definitions - ******************************************************************************/ +/* Debug Console */ +#include "fsl_debug_console.h" +/* Private FSPI */ +#include "app_fspi.h" +#include "app_mpu_impl.h" -/******************************************************************************* - * Prototypes - ******************************************************************************/ +#define RAW16_TO_ACTUAL(raw, fs) ((float)raw * (float)fs / 32768.0) -/******************************************************************************* - * Code - ******************************************************************************/ -/*! - * @brief Main function - */ -int main(void) -{ - char ch; +mpu6500_t s_mpu = { + .ops = + { + .read_register = app_mpu_impl_read_reg, + .write_register = app_mpu_impl_write_reg, + .delay = app_mpu_impl_delay, + }, + .user_data = NULL, +}; +int main(void) { /* Init board hardware. */ BOARD_ConfigMPU(); BOARD_InitBootPins(); BOARD_InitBootClocks(); BOARD_InitDebugConsole(); - PRINTF("hello world.\r\n"); + PRINTF("FSPI example start:\r\n"); - while (1) - { - ch = GETCHAR(); - PUTCHAR(ch); + status_t ret = fspi_init(); + if (ret != kStatus_Success) { + PRINTF("FSPI init failed.\r\n"); + goto dead_loop; + } + + if (mpu6500_init(&s_mpu) != MPU6500_RET_SUCCESS) { + PRINTF("MPU6500 initialization failed.\r\n"); + goto dead_loop; + } + + mpu6500_result_t sensor_result; + + uint16_t accel_fs; + uint16_t gyro_fs; + + if (mpu6500_get_accel_fullscale(&s_mpu, &accel_fs) != MPU6500_RET_SUCCESS) { + PRINTF("Failed to get accel range.\r\n"); + goto dead_loop; + } + + if (mpu6500_get_gyro_fullscale(&s_mpu, &gyro_fs) != MPU6500_RET_SUCCESS) { + PRINTF("Failed to get gyro range.\r\n"); + goto dead_loop; + } + + for (;;) { + SDK_DelayAtLeastUs(10 * 1000, CLOCK_GetCoreSysClkFreq()); + if (mpu6500_read_result(&s_mpu, &sensor_result) != MPU6500_RET_SUCCESS) { + PRINTF("Sensor read failed.\r\n"); + continue; + } + + float accel_x = RAW16_TO_ACTUAL(sensor_result.accel_x_raw, accel_fs) * 9.80; + float accel_y = RAW16_TO_ACTUAL(sensor_result.accel_y_raw, accel_fs) * 9.80; + float accel_z = RAW16_TO_ACTUAL(sensor_result.accel_z_raw, accel_fs) * 9.80; + + float gyro_x = RAW16_TO_ACTUAL(sensor_result.gyro_x_raw, gyro_fs); + float gyro_y = RAW16_TO_ACTUAL(sensor_result.gyro_y_raw, gyro_fs); + float gyro_z = RAW16_TO_ACTUAL(sensor_result.gyro_z_raw, gyro_fs); + + PRINTF( + "/* %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6d */\r\n", + accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, sensor_result.temp_raw); + } + +dead_loop: + for (;;) { + /* -- */ } } diff --git a/src/mpu6500.c b/src/mpu6500.c new file mode 100644 index 0000000..717a327 --- /dev/null +++ b/src/mpu6500.c @@ -0,0 +1,131 @@ +#include "mpu6500.h" + +#define MPU6500_REG_GYRO_CONFIG 0x1BU +#define MPU6500_REG_ACCEL_CONFIG 0x1CU +#define MPU6500_REG_ACCEL_XOUT_H 0x3BU +#define MPU6500_REG_WHO_AM_I 0x75U +#define MPU6500_REG_SIGNAL_PATH_RESET 0x68U +#define MPU6500_REG_PWR_MGMT_1 0x6BU +#define mPU6500_REG_PWR_MGMT_2 0x6CU + +#define MPU6500_CONST_WHOAMI 0x70U + +#define MPU6500_BTW_CPU(h, l) (((h & 0xFFU) << 8U) | (l & 0xFFU)) + +/** + * @brief Reset MPU6500. + * Refer to description field of the PWR_MGMT1 register in RegMap manual + * @param mpu Pointer to mpu6500_t struct + * @return mpu6500_ret_t MPU6500_RET_SUCCESS for success, others for failure. + */ +static mpu6500_ret_t mpu6500_reset(mpu6500_t *mpu) { + mpu6500_ret_t ret = MPU6500_RET_SUCCESS; + + uint8_t buf[1]; + + ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_PWR_MGMT_1, buf, 0x01); + if (ret != MPU6500_RET_SUCCESS) { + return ret; + } + + buf[0] |= 0x80; /* H_RESET */ + ret = mpu->ops.write_register(mpu->user_data, MPU6500_REG_PWR_MGMT_1, buf, 0x01); + if (ret != MPU6500_RET_SUCCESS) { + return ret; + } + + ret = mpu->ops.delay(mpu->user_data, 100); + if (ret != MPU6500_RET_SUCCESS) { + return ret; + } + + buf[0] = 0x07; /* GYRO_RST | ACCEL_RST | TEMP_RST */ + ret = mpu->ops.write_register(mpu->user_data, MPU6500_REG_SIGNAL_PATH_RESET, buf, 0x01); + if (ret != MPU6500_RET_SUCCESS) { + return ret; + } + + ret = mpu->ops.delay(mpu->user_data, 100); + return ret; +} + +mpu6500_ret_t mpu6500_init(mpu6500_t *mpu) { + mpu6500_ret_t ret = MPU6500_RET_SUCCESS; + + uint8_t rx_buf[1]; + + ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_WHO_AM_I, rx_buf, 0x01); + if (ret != MPU6500_RET_SUCCESS) { + return ret; + } + + ret = mpu6500_reset(mpu); + return ret; +} + +mpu6500_ret_t mpu6500_read_channel(mpu6500_t *mpu, mpu6500_channel_t ch, int16_t *result) { + mpu6500_ret_t ret = MPU6500_RET_SUCCESS; + + uint8_t buf[2]; + + ret = mpu->ops.read_register(mpu->user_data, ch, buf, 0x02); + if (ret != MPU6500_RET_SUCCESS) { + return ret; + } + + *result = MPU6500_BTW_CPU(buf[0], buf[1]); + + return ret; +} + +mpu6500_ret_t mpu6500_read_result(mpu6500_t *mpu, mpu6500_result_t *result) { + mpu6500_ret_t ret = MPU6500_RET_SUCCESS; + + uint8_t buf[14]; + ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_ACCEL_XOUT_H, buf, 14); + if (ret != MPU6500_RET_SUCCESS) { + return ret; + } + + result->accel_x_raw = MPU6500_BTW_CPU(buf[0], buf[1]); + result->accel_y_raw = MPU6500_BTW_CPU(buf[2], buf[3]); + result->accel_z_raw = MPU6500_BTW_CPU(buf[4], buf[5]); + result->temp_raw = MPU6500_BTW_CPU(buf[6], buf[7]); + result->gyro_x_raw = MPU6500_BTW_CPU(buf[8], buf[9]); + result->gyro_y_raw = MPU6500_BTW_CPU(buf[10], buf[11]); + result->gyro_z_raw = MPU6500_BTW_CPU(buf[12], buf[13]); + + return ret; +} + +mpu6500_ret_t mpu6500_get_accel_fullscale(mpu6500_t *mpu, uint16_t *f_scale) { + mpu6500_ret_t ret = MPU6500_RET_SUCCESS; + + uint8_t buf[1]; + ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_ACCEL_CONFIG, buf, 0x01); + if (ret != MPU6500_RET_SUCCESS) { + return ret; + } + + uint8_t fs = (buf[0] >> 3U) & 0x03U; + + *f_scale = 1 << (fs + 1); /* 00: 2g, 01: 4g, 10: 8g, 11: 16g */ + + return ret; +} + +mpu6500_ret_t mpu6500_get_gyro_fullscale(mpu6500_t *mpu, uint16_t *f_scale) { + mpu6500_ret_t ret = MPU6500_RET_SUCCESS; + + uint8_t buf[1]; + ret = mpu->ops.read_register(mpu->user_data, MPU6500_REG_GYRO_CONFIG, buf, 0x01); + if(ret != MPU6500_RET_SUCCESS) { + return ret; + } + + uint8_t fs = (buf[0] >> 3U) & 0x03U; + + *f_scale = 250 * (1 << fs); + + return ret; +}