更新GeekIMU-42688P适配固件，更新基于Python的可视化工具

1.Hardware/GeekIMU-Wireless/.gitattributes

@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto

1.Hardware/GeekIMU-Wireless/README.md

@@ -0,0 +1,132 @@
+# Marzipan
+
+![sample](./assets/sample.jpg)
+
+<p align="center">
+  <img src="https://licensebuttons.net/l/by-nc/4.0/88x31.png"></img>
+</p>
+
+<p align="center">
+  English | <a href="./README_CN.md">简体中文</a>
+</p>
+
+
+A compact and easy soldering modular Smol Slimes / nRFSlime solution with dimensions of 40x28x17mm!
+
+If you'd like to learn more about Smol Slimes / nRFSlime, feel free to read the [Smol Slime - SlimeVR Docs](https://docs.slimevr.dev/diy/smol-slime.html).
+
+>[!CAUTION]
+>The current version (1.1) has the following issues:
+>
+>- When using the ICM-45686 module from LWKJ, the sensor cannot be detected upon powering up the switch; you’ll need to press the button again to start it properly.
+>- The button's functionality does not behave as expected.
+>
+>Pull Requests are welcome to help fix these issues!
+
+## Description
+
+### PCB
+
+The PCB is designed using the **EasyEDA** Pro version, and the repository provides the source files for further modification.
+
+For different pre-made sensor modules, I’ve created the following PCBs for different types. However, due to time constraints, only the `VGGALXI` version has been verified. The others should theoretically work.
+
+- **`VGGALXI`**: Pin order from bottom to top is `VCC GND GND SDA SCL X INT` (**verified**)
+- **`VGLAXXI`**: Pin order from bottom to top is `VCC GND SCL SDA X X INT` (**not verified**)
+- **`KOUNO'S_ICM45`**: Designed for KOUNO's module (**not verified**)
+
+### Case
+
+For the case, two versions are provided:
+
+- **`Case_v5.stl`**, which requires M2 heat insert nuts.
+- **`Case_v5_No_Heat_Insert_Nut.stl`**, where screws can be directly fastened into the case.
+
+The version with heat insert nuts is more durable.
+
+For Bambu Lab printers, pre-configured printing settings are provided here.
+
+**https://makerworld.com/zh/models/966982**
+
+The case files in the repository are designed to fit **25mm-wide straps** and are also compatible with ReboCap’s quick-release system.
+
+If you prefer wider straps, feel free to modify the `.step` source files.
+
+### Firmware
+
+The pin positions of this PCB match the default layout in the original project (`SDA-006`, `SCL-008`, `INT-017`), so you can directly flash the firmware provided on the [Smol Slime Docs](https://docs.slimevr.dev/diy/smol-slime.html) without additional changes.
+
+## BOM
+
+### PCB and Assembly
+
+The table below shows the components needed for **one tracker**. Note that you’ll also need to purchase one **nRF52840 SuperMini/Dongle** as a receiver. For more details, refer to the Smol Slime page.
+
+According to the Discord community, one receiver can pair with approximately **20 trackers** (this hasn’t been fully tested, but it should work for at least 6).
+
+|         Component         |  Qty  |               Notes                |
+| :-----------------------: |  :--: | :--------------------------------: |
+| nRF52840 SuperMini Board |    1   |                                    |
+| MSK-12D19 Slide Switch |   1   |     A legacy switch; other switches may also work     |
+|  3*6*4.3 Tactile Switch (Side)  |   1   |     Other side tactile switches can also be used     |
+| 401230 / 501230 Battery |   1   | 501230 is recommended; smaller batteries are also fine |
+| Double-sided tape for battery |   1   |                |
+|   Sensor Module |    1   |                                    |
+|        M2*14 Screws        |   2   |            Used to secure the case            |
+|    M2*4.5*3 Heat Insert Nut    |   2   | Not needed for the screw-only case version |
+| 25mm-wide strap | 1 | You can modify the case files for other straps |
+
+### 3D Printing
+
+For 3D printing, PETG is recommended, but PLA is also fine.
+
+|    Component    | Qty  |                            Notes                             |
+| :-------------: | :--: | :----------------------------------------------------------: |
+|     Lid_v5      |  1   |                           The lid                            |
+|     Case_v5     |  1   | Case body that requires heat insert nuts; use `Case_v5_No_Heat_Insert_Nut` if not needed |
+|  Button_Cap_v5  |  1   |               Button cap for extending buttons               |
+|  Switch_Cap_v5  |  1   |              Switch cap for extending switches               |
+| Screw_Holder_v5 |  2   |   Screw support pillars; optional but increases durability   |
+
+## Assembly Instructions
+
+### Soldering and Assembly
+
+- (Optional) Use a probe or other tools to check if the SuperMini, sensor module, etc., are functioning properly.
+
+- **Solder the sensor module first**:
+  - Make sure the sensor’s pin order matches the PCB.
+  - Align the pins and secure them by melting solder from the back of the PCB. After soldering, use a multimeter to ensure the circuit is properly connected.
+
+- Solder the switch and button.
+
+- Make sure the switch is in the OFF position, then solder the battery.
+
+- Finally, solder the SuperMini module.
+
+- After confirming normal functionality, stick the battery to the back of the PCB using double-sided tape in the designated rectangular area.
+- Embed the heat insert nut into the Case.
+
+- Install the button and switch caps into the case, align the holes, and insert the main body into the case.
+
+- Insert the screw support pillars, attach the lid, and fasten the screws.
+
+### Flashing Firmware
+
+- Refer to the [Smol Slime Docs](https://docs.slimevr.dev/diy/smol-slime.html) for instructions on flashing firmware.
+  - When using **LWKJ's ICM-45686 module + `VGGALXI` layout**, the firmware configuration `Tracker SuperMini Disabled Disabled` works properly.
+
+
+## Links
+
+- **SlimeVR nRF Receiver Firmware:** https://github.com/SlimeVR/SlimeVR-Tracker-nRF-Receiver
+
+- **SlimeVR nRF Tracker Firmware:** https://github.com/SlimeVR/SlimeVR-Tracker-nRF
+
+- **Scawanf's PCB R3 on Github:** https://github.com/SlimeVR/SlimeVR-Tracker-nRF-PCB
+
+- **Scawanf's PCB R3 on OSHWLab:** https://oshwlab.com/sctanf/slimenrf3
+
+- **SlimeVR Discord:** https://discord.gg/SlimeVR
+
+- **Smol Slime Docs:** https://docs.slimevr.dev/diy/smol-slime.html

1.Hardware/GeekIMU-Wireless/README_CN.md

@@ -0,0 +1,116 @@
+# Marzipan
+
+![sample](./assets/sample.jpg)
+
+<p align="center">
+  <img src="https://licensebuttons.net/l/by-nc/4.0/88x31.png"></img>
+</p>
+
+<p align="center">
+  <a href="./README.md">English</a> | 简体中文
+</p>
+
+一款简单焊接的，小巧的模块化 Smol Slimes / nRFSlime 方案！尺寸仅为 40\*28\*17mm。
+
+如果想要了解更多关于 Smol Slimes / nRFSlime 的事情，欢迎阅读 [Smol Slime - SlimeVR Docs](https://docs.slimevr.dev/diy/smol-slime.html)。
+
+>[!CAUTION]
+>
+>目前的版本存在以下问题：
+>
+>- 使用 LWKJ 的 ICM-45686 模块情况下，打开开关启动时无法识别传感器，需要再点击一下按钮才能正常启动。
+>- 按钮工作情况与预期不符。
+>  
+>欢迎提出 PR 进行修复！
+
+
+## 文件说明
+
+### PCB
+
+PCB 使用立创 EDA Pro 版本绘制，在仓库中提供了源文件，欢迎进行修改。
+
+针对不同的成品传感器模块，我创建了以下适配不同类型的 PCB，但是由于时间问题仅验证了 `VGGALXI` 版本，其他版本理论上是没问题的。
+
+- `VGGALXI`: 孔位顺序从下到上为 `VCC GND GND SDA SCL X INT`（已验证）
+- `VGLAXXI`: 孔位顺序从下到上为 `VCC GND SCL SDA X X INT`（未验证）
+- `KOUNO'S_ICM45`: 为 KOUNO 的模块而设计。（未验证）
+
+### 外壳
+
+外壳方面，提供了需要 M2 热熔螺母的版本 `Case_v5.stl` 以及可以直接将螺丝拧到外壳里的版本 `Case_v5_No_Heat_Insert_Nut.stl`。热熔螺母的版本会更加耐用一些。
+
+**对于拓竹打印机，则可以在这里获取预设好的打印配置：**
+
+**https://makerworld.com/zh/models/966982**
+
+仓库中提供的外壳文件适配 25mm 宽度的绑带，~~同时也适配 ReboCap 的快拆（逃~~。
+
+如果想要更宽的绑带可以自行修改 `.step` 格式的源文件。
+
+### 固件
+
+本 PCB 的孔位与原项目中默认孔位相符（`SDA-006`, `SCL-008`, `INT-017`），因此可以直接使用 [Smol Slime](https://docs.slimevr.dev/diy/smol-slime.html) 网站中所提供的固件进行刷写，无需额外更改。
+
+## BOM
+
+### PCB 和组装
+
+这里的是完成一个追踪器所需要的数量。注意，除了所需要的追踪器之外还需要购买一个 nRF52840 SuperMini / Dongle 作为接收器，具体也请参考 Smol Slime 页面中的内容。根据 Discord，一个接收器可以匹配 20 个左右追踪器（尚未验证，但是至少带 6 个是没问题的）。
+
+|         元件名称          | 价格 | 数量 |                 备注                 |
+| :-----------------------: | :--: | :--: | :----------------------------------: |
+| nRF52840 SuperMini 开发板 |  15  |  1   |                                      |
+|    MSK-12D19 滑动开关     | 0.1  |  1   |      一种祖传开关，其他开关也行      |
+|  3\*6\*4.3 轻触开关 侧插  | 0.1  |  1   |      也可以换成其他侧向轻触开关      |
+|   401230 / 501230 电池    |  4   |  1   |  推荐使用 501230 电池，更小的也可以  |
+|    固定电池用的双面胶     |  2   |  1   |         甚至超市买的都行（？         |
+|        传感器模块         |  ?   |  1   |                                      |
+|        M2\*14 螺丝        | 0.1  |  2   |             用于固定外壳             |
+|    M2\*4.5\*3 热熔螺母    | 0.1  |  2   |  如果使用直接拧的外壳版本，则不需要  |
+|        25mm 宽绑带        |  2   |  1   | 当然也可以修改外壳文件来适配不同绑带 |
+
+### 3D 打印
+
+推荐使用 PETG 进行打印，PLA 也没问题。
+
+|    元件名称     | 数量 |            备注            |
+| :-------------: | :--: | :------------------------: |
+|     Lid_v5         |  1   | 盖子 |
+|     Case_v5        | 1 | 需要热熔螺母的盒子本体，如果不需要则使用 Case_v5_No_Heat_Insert_Nut |
+|  Button_Cap_v5      | 1 | 按钮帽，用于延长按钮 |
+|  Switch_Cap_v5    | 1 | 开关帽，用于延长开关 |
+| Screw_Holder_v5   |  2   |    螺丝支撑柱，加上会让结构更牢固，也可以不加    |
+## 安装说明
+
+### 焊接并组装
+
+- 使用探针等工具检查 SuperMini，传感器模块等是否工作正常。
+
+- 必须首先焊接传感器模块，注意检查传感器的孔位顺序是否与 PCB 相符。将对应的孔位对齐后，从 PCB 背面融化焊锡，完成固定。完成焊接后可以使用万用表检查电路是否正常接通。
+- 焊接开关与按钮。
+- 确保开关处于关闭状态，然后焊接电池。
+- 最后焊接 SuperMini 模块。
+- 检查可以正常工作后，将电池用双面胶黏贴在 PCB 背面对应方框的位置。
+- 将热熔螺母嵌入壳体中。
+- 将按钮帽和开关帽装入壳体，对准孔位将本体塞进去。
+- 放入螺丝支撑柱，装上盖子，拧螺丝即可。
+
+### 刷入固件
+
+- 具体请参考 [Smol Slime - SlimeVR Docs](https://docs.slimevr.dev/diy/smol-slime.html) 中刷入固件的部分。
+  - 使用 LWKJ 的 45686 模块 + VGGALXI 布局的情况下，使用 `Tracker SuperMini Disabled Disabled ` 的固件可以正常工作。
+
+## 其他链接
+
+- **SlimeVR nRF Receiver Firmware:** https://github.com/SlimeVR/SlimeVR-Tracker-nRF-Receiver
+
+- **SlimeVR nRF Tracker Firmware:** https://github.com/SlimeVR/SlimeVR-Tracker-nRF
+
+- **Scawanf's PCB R3 on Github:** https://github.com/SlimeVR/SlimeVR-Tracker-nRF-PCB
+
+- **Scawanf's PCB R3 on OSHWLab:** https://oshwlab.com/sctanf/slimenrf3
+
+- **SlimeVR Discord:** https://discord.gg/SlimeVR
+
+- **Smol Slime Docs:** https://docs.slimevr.dev/diy/smol-slime.html

1.Hardware/GeekIMUv2/GeekIMU.PrjPCB

@@ -140,10 +140,6 @@ GenerateClassCluster=0
 DocumentUniqueId=CNKHXQGS
 
 [GeneratedDocument1]
-DocumentPath=Project Outputs for GeekIMU\Design Rule Check - GeekIMU.html
-DItemRevisionGUID=
-
-[GeneratedDocument2]
 DocumentPath=GeekIMU.PcbDoc.htm
 DItemRevisionGUID=
 

2.Firmware/ICM42688P_HAL/.mxproject

@@ -0,0 +1,31 @@
+[PreviousLibFiles]
+LibFiles=Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_spi.h;Drivers\STM32F1xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_def.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_rcc.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_rcc_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_bus.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_rcc.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_system.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_utils.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_gpio.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_gpio_ex.h;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio_ex.c;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_gpio.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_dma_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_dma.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_dma.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_cortex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_cortex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_pwr.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_pwr.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_flash.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_flash_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_exti.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_exti.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_tim.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_tim_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_uart.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_usart.h;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_spi.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_dma.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_cortex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_pwr.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_exti.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim_ex.c;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_uart.c;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_spi.h;Drivers\STM32F1xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_def.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_rcc.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_rcc_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_bus.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_rcc.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_system.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_utils.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_gpio.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_gpio_ex.h;Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio_ex.c;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_gpio.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_dma_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_dma.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_dma.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_cortex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_cortex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_pwr.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_pwr.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_flash.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_flash_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_exti.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_exti.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_tim.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_tim_ex.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_hal_uart.h;Drivers\STM32F1xx_HAL_Driver\Inc\stm32f1xx_ll_usart.h;Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f103xe.h;Drivers\CMSIS\Device\ST\STM32F1xx\Include\stm32f1xx.h;Drivers\CMSIS\Device\ST\STM32F1xx\Include\system_stm32f1xx.h;Drivers\CMSIS\Device\ST\STM32F1xx\Source\Templates\system_stm32f1xx.c;Drivers\CMSIS\Include\cmsis_armcc.h;Drivers\CMSIS\Include\cmsis_armclang.h;Drivers\CMSIS\Include\cmsis_compiler.h;Drivers\CMSIS\Include\cmsis_gcc.h;Drivers\CMSIS\Include\cmsis_iccarm.h;Drivers\CMSIS\Include\cmsis_version.h;Drivers\CMSIS\Include\core_armv8mbl.h;Drivers\CMSIS\Include\core_armv8mml.h;Drivers\CMSIS\Include\core_cm0.h;Drivers\CMSIS\Include\core_cm0plus.h;Drivers\CMSIS\Include\core_cm1.h;Drivers\CMSIS\Include\core_cm23.h;Drivers\CMSIS\Include\core_cm3.h;Drivers\CMSIS\Include\core_cm33.h;Drivers\CMSIS\Include\core_cm4.h;Drivers\CMSIS\Include\core_cm7.h;Drivers\CMSIS\Include\core_sc000.h;Drivers\CMSIS\Include\core_sc300.h;Drivers\CMSIS\Include\mpu_armv7.h;Drivers\CMSIS\Include\mpu_armv8.h;Drivers\CMSIS\Include\tz_context.h;
+
+[PreviousUsedKeilFiles]
+SourceFiles=..\Core\Src\main.c;..\Core\Src\gpio.c;..\Core\Src\spi.c;..\Core\Src\usart.c;..\Core\Src\stm32f1xx_it.c;..\Core\Src\stm32f1xx_hal_msp.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio_ex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_spi.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc_ex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_dma.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_cortex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_pwr.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash_ex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_exti.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim_ex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_uart.c;..\Drivers\CMSIS\Device\ST\STM32F1xx\Source\Templates\system_stm32f1xx.c;..\Core\Src\system_stm32f1xx.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio_ex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_spi.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_rcc_ex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_gpio.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_dma.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_cortex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_pwr.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_flash_ex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_exti.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_tim_ex.c;..\Drivers\STM32F1xx_HAL_Driver\Src\stm32f1xx_hal_uart.c;..\Drivers\CMSIS\Device\ST\STM32F1xx\Source\Templates\system_stm32f1xx.c;..\Core\Src\system_stm32f1xx.c;;;
+HeaderPath=..\Drivers\STM32F1xx_HAL_Driver\Inc;..\Drivers\STM32F1xx_HAL_Driver\Inc\Legacy;..\Drivers\CMSIS\Device\ST\STM32F1xx\Include;..\Drivers\CMSIS\Include;..\Core\Inc;
+CDefines=USE_HAL_DRIVER;STM32F103xE;USE_HAL_DRIVER;USE_HAL_DRIVER;
+
+[PreviousGenFiles]
+AdvancedFolderStructure=true
+HeaderFileListSize=6
+HeaderFiles#0=..\Core\Inc\gpio.h
+HeaderFiles#1=..\Core\Inc\spi.h
+HeaderFiles#2=..\Core\Inc\usart.h
+HeaderFiles#3=..\Core\Inc\stm32f1xx_it.h
+HeaderFiles#4=..\Core\Inc\stm32f1xx_hal_conf.h
+HeaderFiles#5=..\Core\Inc\main.h
+HeaderFolderListSize=1
+HeaderPath#0=..\Core\Inc
+HeaderFiles=;
+SourceFileListSize=6
+SourceFiles#0=..\Core\Src\gpio.c
+SourceFiles#1=..\Core\Src\spi.c
+SourceFiles#2=..\Core\Src\usart.c
+SourceFiles#3=..\Core\Src\stm32f1xx_it.c
+SourceFiles#4=..\Core\Src\stm32f1xx_hal_msp.c
+SourceFiles#5=..\Core\Src\main.c
+SourceFolderListSize=1
+SourcePath#0=..\Core\Src
+SourceFiles=;
+

2.Firmware/ICM42688P_HAL/Core/Inc/CONFIG_PARAM.h

@@ -0,0 +1,54 @@
+#ifndef __CONFIG_PARAM_H
+#define __CONFIG_PARAM_H
+
+#include "public_data.h"
+
+typedef struct 
+{
+	float kp;
+	float ki;
+	float kd;
+} pidInit_t;
+
+
+typedef struct
+{
+	int16_t accZero[3];
+	int16_t accGain[3];
+} accBias_t;
+
+typedef struct
+{
+	int16_t magZero[3];
+} magBias_t;
+
+typedef struct 
+{
+    int16_t rollDeciDegrees;
+    int16_t pitchDeciDegrees;
+    int16_t yawDeciDegrees;
+} boardAlignment_t;
+
+
+
+typedef struct	
+{
+	uint8_t version;						/*软件版本号*/
+	pidInit_t pid[10];			/*PID参数*/
+	accBias_t accBias;				/*加速度校准值*/
+	magBias_t magBias;				/*磁力计校准值*/
+	boardAlignment_t boardAlign;	/*板子微调*/
+	uint16_t thrustBase;					/*油门基础值*/
+	uint8_t cksum;						/*校验*/
+} configParam_t;
+
+extern configParam_t configParam;
+
+
+void saveConfigAndNotify(void);
+
+
+#endif 
+
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/STABILIZER_TYPES.h

@@ -0,0 +1,215 @@
+#ifndef __STABILIZER_TYPES_H__
+#define __STABILIZER_TYPES_H__
+
+
+#include "main.h"
+
+#if defined(__CC_ARM) 
+	#pragma anon_unions
+#endif
+
+
+
+#define RATE_5_HZ		5
+#define RATE_10_HZ		10
+#define RATE_20_HZ		20
+#define RATE_25_HZ		25
+#define RATE_50_HZ		50
+#define RATE_100_HZ		100
+#define RATE_200_HZ 	200
+#define RATE_250_HZ 	250
+#define RATE_500_HZ 	500
+#define RATE_1000_HZ 	1000
+
+
+//#define RATE_DO_EXECUTE(RATE_HZ, TICK) ((TICK % (RATE_1000_HZ / RATE_HZ)) == 0)
+
+
+
+typedef union 
+{
+	struct
+	{
+		int16_t x;
+		int16_t y;
+		int16_t z;
+	};
+	int16_t axis[3];
+} Axis3i16;
+
+typedef union 
+{
+	struct 
+	{
+		int32_t x;
+		int32_t y;
+		int32_t z;
+	};
+	int32_t axis[3];
+} Axis3i32;
+
+typedef union 
+{
+	struct 
+	{
+		int64_t x;
+		int64_t y;
+		int64_t z;
+	};
+	int64_t axis[3];
+} Axis3i64;
+
+typedef union 
+{
+	struct 
+	{
+		float x;
+		float y;
+		float z;
+	};
+	float axis[3];
+} Axis3f;
+
+
+//姿态数据结构
+typedef struct  
+{
+//	uint32_t timestamp;	/*时间戳*/
+	float roll;
+	float pitch;
+	float yaw;
+} attitude_t;
+
+//姿态数据结构
+typedef struct  
+{
+	uint8_t FlashSign;	
+	float roll;
+	float pitch;
+	float yaw;
+} attitudeZero_t;
+
+
+struct  vec3_s 
+{
+	uint32_t timestamp;
+	float x;
+	float y;
+	float z;
+};
+
+typedef struct vec3_s point_t;
+typedef struct vec3_s velocity_t;
+typedef struct vec3_s acc_t;
+
+//Z轴距离传感器数据结构
+typedef struct zDistance_s 
+{
+	uint32_t timestamp;
+	float distance;
+} zDistance_t;
+
+//光流数据结构
+typedef struct flowMeasurement_s 
+{
+	uint32_t timestamp;
+	union 
+	{
+		struct 
+		{
+			float dpixelx;  // Accumulated pixel count x
+			float dpixely;  // Accumulated pixel count y
+		};
+		float dpixel[2];  // Accumulated pixel count
+	};
+	float stdDevX;      // Measurement standard deviation
+	float stdDevY;      // Measurement standard deviation
+	float dt;           // Time during which pixels were accumulated
+} flowMeasurement_t;
+
+//TOF数据结构
+typedef struct tofMeasurement_s 
+{
+	uint32_t timestamp;
+	float distance;
+	float stdDev;
+} tofMeasurement_t;
+
+//气压数据结构
+typedef struct
+{
+	uint32_t timestamp;
+	float pressure;
+	float pressureZero;
+	float temperature;
+	float asl;
+} baro_t;
+
+//所有传感器数据集合
+typedef struct
+{
+	Axis3f acc;				//加速度（G）
+	Axis3f gyro;			//陀螺仪（deg/s）
+	Axis3f mag;				//磁力计（gauss）
+	baro_t baro;	
+  //ultrasonic_t ultrasonic; 	
+} sensorData_t;
+
+//四轴姿态数据结构
+typedef struct
+{
+	//ultrasonic_t ultrasonic;
+	Axis3f gyro;
+	attitude_t 	attitude;	//姿态角度（deg）
+	attitude_t 	attitude_last;	//姿态角度（deg）
+	int AttitudeCalibration;
+	attitudeZero_t 	attitudeZero;	//姿态角度零偏（deg）
+	point_t 	position;	//估算的位置（cm）
+	velocity_t 	velocity;	//估算的速度（cm/s）
+	acc_t acc;				//估算的加速度（cm/ss）
+	int id;
+} state_t;
+
+
+typedef enum
+{
+	modeDisable = 0,
+	modeAbs,
+	modeVelocity
+} mode_e;
+
+typedef struct
+{
+	mode_e x;
+	mode_e y;
+	mode_e z;
+	mode_e roll;
+	mode_e pitch;
+	mode_e yaw;
+}mode_t;
+
+//目标姿态数据结构
+typedef struct
+{
+	attitude_t attitude;		//目标姿态角度（deg）
+	attitude_t attitudeRate;	//目标角速度（deg/s）
+	point_t position;         	//目标位置（cm）
+	velocity_t velocity;      	//目标速度（cm/s）
+	mode_t mode;				
+	float thrust;				
+} setpoint_t;
+
+//控制数据结构
+typedef struct
+{
+	float roll;
+	float pitch;
+	float yaw;
+	uint16_t thrust;
+	char abnormal;
+} control_t;
+
+
+#endif
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/accelerometer.h

@@ -0,0 +1,25 @@
+#ifndef __ACCELEROMETER_H
+#define __ACCELEROMETER_H
+
+#include "public_data.h"
+#include "filter.h"
+#include "icm42688p.h"
+#include "gyro.h"
+
+#define GRAVITY_CMSS    980.665f
+#define GRAVITY_MSS     9.80665f
+
+#define CALIBRATING_ACC_CYCLES          400
+
+extern Axis3i16 accADC;
+bool ACC_Calibration(void);
+void accUpdate(Axis3f *acc);
+bool accInit(float accUpdateRate);
+
+
+#endif
+
+
+
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/communicate.h

@@ -0,0 +1,28 @@
+#ifndef __COMMUNICATE_H__
+#define __COMMUNICATE_H__
+
+#include "usart.h"
+#include "main.h"
+#include "stdio.h"
+#include "string.h"
+#include "stdlib.h"
+#include "sensors.h"
+//#include "bsp_hal_ps2.h"
+//#include "motor.h"
+//#include "Bluetooth.h"
+
+
+typedef struct  
+{
+
+	float dt;
+	uint32_t ms,ms1;
+	
+} dt_ms_t;
+
+
+
+
+#endif
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/filter.h

@@ -0,0 +1,72 @@
+#ifndef __FILTER_H
+#define __FILTER_H
+
+#include "public_data.h"
+
+typedef struct 
+{
+    float Q_angle;
+    float Q_bias;
+    float R_measure;
+    float angle;
+    float bias;
+    float P[2][2];
+
+}Kalman_t; 
+
+//1. 结构体类型定义
+typedef struct 
+{
+    float LastP;//上次估算协方差 初始化值为0.02
+    float NOWP ;//当前估算协方差 初始化值为0
+    float out;//卡尔曼滤波器输出 初始化值为0
+    float Kg;//卡尔曼增益 初始化值为0
+    float Q;//过程噪声协方差 初始化值为0.05
+    float R;//观测噪声3协方差 初始化值为0.99
+}KFP;//卡尔曼滤波参数
+
+typedef struct pt1Filter_s {
+    float state;
+    float k;
+} pt1Filter_t;
+
+typedef enum {
+    FILTER_LPF,    // 2nd order Butterworth section
+    FILTER_NOTCH,
+    FILTER_BPF,
+} biquadFilterType_e;
+
+/* this holds the data required to update samples thru a filter */
+typedef struct biquadFilter_s {
+    float b0, b1, b2, a1, a2;
+    float x1, x2, y1, y2;
+} biquadFilter_t;
+
+
+float kalmanFilter(KFP *kfp,float input);
+float filterGetNotchQ(float centerFreq, float cutoffFreq);
+//二阶滤波器初始化
+void biquadFilterUpdate(biquadFilter_t *filter, uint16_t filterFreq, uint16_t refreshRate, float Q, biquadFilterType_e filterType);
+//二阶陷波器初始化
+void biquadFilterInitNotch(biquadFilter_t *filter, uint16_t samplingFreq, uint16_t filterFreq, uint16_t cutoffHz);
+//二阶低通滤波器初始化
+void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq ,uint16_t samplingFreq);
+// Computes a biquad_t filter on a sample
+float biquadFilterApply(biquadFilter_t *filter, float input);
+void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
+//pt1获取滤波增益(截止频率 采样时间)
+float pt1FilterGain(float f_cut, float dT);
+//pt1初始化低通滤波器
+void pt1FilterInit(pt1Filter_t *filter, float k);
+//更新滤波增益
+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
+//pt1低通滤波器应用
+float pt1FilterApply(pt1Filter_t *filter, float input);
+
+void KalmanInit(Kalman_t *Kalman) ;
+float Kalman_getAngle(Kalman_t *Kalman,float newAngle, float newRate, float dt) ;
+
+
+
+#endif //__FILTER_H
+

2.Firmware/ICM42688P_HAL/Core/Inc/gpio.h

@@ -0,0 +1,49 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    gpio.h
+  * @brief   This file contains all the function prototypes for
+  *          the gpio.c file
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __GPIO_H__
+#define __GPIO_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+
+/* USER CODE BEGIN Includes */
+
+/* USER CODE END Includes */
+
+/* USER CODE BEGIN Private defines */
+
+/* USER CODE END Private defines */
+
+void GPIO_Init(void);
+
+/* USER CODE BEGIN Prototypes */
+
+/* USER CODE END Prototypes */
+
+#ifdef __cplusplus
+}
+#endif
+#endif /*__ GPIO_H__ */
+

2.Firmware/ICM42688P_HAL/Core/Inc/gyro.h

@@ -0,0 +1,41 @@
+#ifndef __GYRO_H
+#define __GYRO_H
+
+
+#include "public_data.h"
+#include "filter.h"
+#include "icm42688p.h"
+//#include "flash_data.h"
+
+#define XYZ_AXIS_COUNT 3
+
+#define CALIBRATING_GYRO_CYCLES         500
+
+
+
+typedef struct gyroCalibration_s 
+{
+	Axis3i32 gyroSum;
+	Axis3i16 gyroZero;
+	stdev_t var[XYZ_AXIS_COUNT];
+	uint16_t cycleCount;
+	int ok;
+	int8_t gyroZeroFlashSign;
+} gyroCalibration_t;
+
+
+
+extern Axis3i16 gyroADC;
+extern gyroCalibration_t gyroCalibration;
+
+bool GYRO_Calibration(void);
+void gyroUpdate(Axis3f *gyro);
+bool gyroInit(float gyroUpdateRate);
+void GYRO_Calibration_write(uint8_t data);
+
+
+
+#endif
+
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/icm42688p.h

@@ -0,0 +1,563 @@
+#ifndef __ICM42688P_H__
+#define __ICM42688P_H__
+
+#include "spi.h"
+#include "gpio.h"
+#include "public_data.h"
+#include "communicate.h"
+
+#define SPI_CS_Enable() 	  HAL_GPIO_WritePin(SPI2_CS_GPIO_Port, SPI2_CS_Pin, GPIO_PIN_RESET)
+#define SPI_CS_Disable() 		HAL_GPIO_WritePin(SPI2_CS_GPIO_Port, SPI2_CS_Pin, GPIO_PIN_SET)
+
+
+// expected value in UB0_REG_WHO_AM_I reg
+#define WHO_AM_I 0x47 
+
+// Accesible from all user banks
+#define REG_BANK_SEL 0x76
+
+
+// User Bank 0
+#define UB0_REG_DEVICE_CONFIG 0x11 
+// break
+#define UB0_REG_DRIVE_CONFIG 0x13 
+#define UB0_REG_INT_CONFIG 0x14 
+// break
+#define UB0_REG_FIFO_CONFIG 0x16 
+// break
+#define UB0_REG_TEMP_DATA1 0x1D 
+#define UB0_REG_TEMP_DATA0 0x1E 
+#define UB0_REG_ACCEL_DATA_X1 0x1F 
+#define UB0_REG_ACCEL_DATA_X0 0x20 
+#define UB0_REG_ACCEL_DATA_Y1 0x21 
+#define UB0_REG_ACCEL_DATA_Y0 0x22 
+#define UB0_REG_ACCEL_DATA_Z1 0x23 
+#define UB0_REG_ACCEL_DATA_Z0 0x24 
+#define UB0_REG_GYRO_DATA_X1 0x25 
+#define UB0_REG_GYRO_DATA_X0 0x26 
+#define UB0_REG_GYRO_DATA_Y1 0x27 
+#define UB0_REG_GYRO_DATA_Y0 0x28 
+#define UB0_REG_GYRO_DATA_Z1 0x29 
+#define UB0_REG_GYRO_DATA_Z0 0x2A 
+#define UB0_REG_TMST_FSYNCH 0x2B 
+#define UB0_REG_TMST_FSYNCL 0x2C 
+#define UB0_REG_INT_STATUS 0x2D 
+#define UB0_REG_FIFO_COUNTH 0x2E 
+#define UB0_REG_FIFO_COUNTL 0x2F 
+#define UB0_REG_FIFO_DATA 0x30 
+#define UB0_REG_APEX_DATA0 0x31 
+#define UB0_REG_APEX_DATA1 0x32 
+#define UB0_REG_APEX_DATA2 0x33 
+#define UB0_REG_APEX_DATA3 0x34 
+#define UB0_REG_APEX_DATA4 0x35 
+#define UB0_REG_APEX_DATA5 0x36 
+#define UB0_REG_INT_STATUS2 0x37 
+#define UB0_REG_INT_STATUS3 0x38 
+// break
+#define UB0_REG_SIGNAL_PATH_RESET 0x4B 
+#define UB0_REG_INTF_CONFIG0 0x4C 
+#define UB0_REG_INTF_CONFIG1 0x4D 
+#define UB0_REG_PWR_MGMT0 0x4E 
+#define UB0_REG_GYRO_CONFIG0 0x4F 
+#define UB0_REG_ACCEL_CONFIG0 0x50 
+#define UB0_REG_GYRO_CONFIG1 0x51 
+#define UB0_REG_GYRO_ACCEL_CONFIG0 0x52 
+#define UB0_REG_ACCEL_CONFIG1 0x53 
+#define UB0_REG_TMST_CONFIG 0x54 
+// break
+#define UB0_REG_APEX_CONFIG0 0x56 
+#define UB0_REG_SMD_CONFIG 0x57 
+// break
+#define UB0_REG_FIFO_CONFIG1 0x5F 
+#define UB0_REG_FIFO_CONFIG2 0x60 
+#define UB0_REG_FIFO_CONFIG3 0x61 
+#define UB0_REG_FSYNC_CONFIG 0x62 
+#define UB0_REG_INT_CONFIG0 0x63 
+#define UB0_REG_INT_CONFIG1 0x64 
+#define UB0_REG_INT_SOURCE0 0x65 
+#define UB0_REG_INT_SOURCE1 0x66 
+// break
+#define UB0_REG_INT_SOURCE3 0x68 
+#define UB0_REG_INT_SOURCE4 0x69 
+// break
+#define UB0_REG_FIFO_LOST_PKT0 0x6C 
+#define UB0_REG_FIFO_LOST_PKT1 0x6D 
+// break
+#define UB0_REG_SELF_TEST_CONFIG 0x70 
+// break
+#define UB0_REG_WHO_AM_I 0x75 
+
+
+// User Bank 1
+#define UB1_REG_SENSOR_CONFIG0 0x03 
+// break
+#define UB1_REG_GYRO_CONFIG_STATIC2 0x0B 
+#define UB1_REG_GYRO_CONFIG_STATIC3 0x0C 
+#define UB1_REG_GYRO_CONFIG_STATIC4 0x0D 
+#define UB1_REG_GYRO_CONFIG_STATIC5 0x0E 
+#define UB1_REG_GYRO_CONFIG_STATIC6 0x0F 
+#define UB1_REG_GYRO_CONFIG_STATIC7 0x10 
+#define UB1_REG_GYRO_CONFIG_STATIC8 0x11 
+#define UB1_REG_GYRO_CONFIG_STATIC9 0x12 
+#define UB1_REG_GYRO_CONFIG_STATIC10 0x13 
+// break
+#define UB1_REG_XG_ST_DATA 0x5F 
+#define UB1_REG_YG_ST_DATA 0x60 
+#define UB1_REG_ZG_ST_DATA 0x61 
+#define UB1_REG_TMSTVAL0 0x62 
+#define UB1_REG_TMSTVAL1 0x63 
+#define UB1_REG_TMSTVAL2 0x64 
+// break
+#define UB1_REG_INTF_CONFIG4 0x7A 
+#define UB1_REG_INTF_CONFIG5 0x7B 
+#define UB1_REG_INTF_CONFIG6 0x7C 
+
+
+// User Bank 2
+#define UB2_REG_ACCEL_CONFIG_STATIC2 0x03 
+#define UB2_REG_ACCEL_CONFIG_STATIC3 0x04 
+#define UB2_REG_ACCEL_CONFIG_STATIC4 0x05 
+// break
+#define UB2_REG_XA_ST_DATA 0x3B 
+#define UB2_REG_YA_ST_DATA 0x3C 
+#define UB2_REG_ZA_ST_DATA 0x3D 
+
+
+// User Bank 4
+#define UB4_REG_APEX_CONFIG1 0x40 
+#define UB4_REG_APEX_CONFIG2 0x41 
+#define UB4_REG_APEX_CONFIG3 0x42 
+#define UB4_REG_APEX_CONFIG4 0x43 
+#define UB4_REG_APEX_CONFIG5 0x44 
+#define UB4_REG_APEX_CONFIG6 0x45 
+#define UB4_REG_APEX_CONFIG7 0x46 
+#define UB4_REG_APEX_CONFIG8 0x47 
+#define UB4_REG_APEX_CONFIG9 0x48 
+// break
+#define UB4_REG_ACCEL_WOM_X_THR 0x4A 
+#define UB4_REG_ACCEL_WOM_Y_THR 0x4B 
+#define UB4_REG_ACCEL_WOM_Z_THR 0x4C 
+#define UB4_REG_INT_SOURCE6 0x4D 
+#define UB4_REG_INT_SOURCE7 0x4E 
+#define UB4_REG_INT_SOURCE8 0x4F 
+#define UB4_REG_INT_SOURCE9 0x50 
+#define UB4_REG_INT_SOURCE10 0x51 
+// break
+#define UB4_REG_OFFSET_USER0 0x77 
+#define UB4_REG_OFFSET_USER1 0x78 
+#define UB4_REG_OFFSET_USER2 0x79 
+#define UB4_REG_OFFSET_USER3 0x7A 
+#define UB4_REG_OFFSET_USER4 0x7B 
+#define UB4_REG_OFFSET_USER5 0x7C 
+#define UB4_REG_OFFSET_USER6 0x7D 
+#define UB4_REG_OFFSET_USER7 0x7E 
+#define UB4_REG_OFFSET_USER8 0x7F 
+
+
+// BANK 1
+// #define GYRO_CONFIG_STATIC2 0x0B
+#define GYRO_NF_ENABLE 0x00 
+#define GYRO_NF_DISABLE 0x01 
+#define GYRO_AAF_ENABLE 0x00 
+#define GYRO_AAF_DISABLE 0x02 
+
+// BANK 2
+// #define ACCEL_CONFIG_STATIC2 0x03 
+#define ACCEL_AAF_ENABLE 0x00 
+#define ACCEL_AAF_DISABLE 0x01 
+		
+		
+#define gyrX() _gyr[0] 
+#define gyrY() _gyr[1] 
+#define gyrZ() _gyr[2] 		
+
+
+#define gyrX1() _gyr1[0] 
+#define gyrY1() _gyr1[1] 
+#define gyrZ1() _gyr1[2] 	
+
+
+#define accX() _acc[0]
+#define accY() _acc[1]
+#define accZ() _acc[2]
+
+#define accX1() _acc1[0]
+#define accY1() _acc1[1]
+#define accZ1() _acc1[2]
+
+
+#define X_AXIS   0
+#define Y_AXIS   2
+#define Z_AXIS   4
+
+#define GYRO     0
+#define ACCEL    1
+#define ALL      5
+
+
+#define FSR_0             0
+#define FSR_1             1
+#define FSR_2             2
+#define FSR_3             3
+#define FSR_4             4
+#define FSR_5             5
+#define FSR_6             6
+#define FSR_7             7
+
+
+#define ODR_32KHZ         1
+#define ODR_16KHZ         2
+#define ODR_8KHZ          3
+#define ODR_4KHZ          4
+#define ODR_2KHZ          5
+#define ODR_1KHZ          6
+#define ODR_200HZ         7
+#define ODR_100HZ         8
+#define ODR_50HZ          9
+#define ODR_25KHZ         10
+#define ODR_12_5KHZ       11
+#define ODR_6_25KHZ       12
+#define ODR_3_125HZ       13
+#define ODR_1_5625HZ      14
+#define ODR_500HZ         15
+
+
+
+
+
+
+/**
+ * @struct sAccelConfig0_t
+ * @brief  Register:Accel_Config0
+ * @n        -------------------------------------------------------------------------------------------------------------------------------------- -------------------
+ * @n        |            b7          |        b6       |         b5         |         b4         |          b3        |       b2       |       b1       |      b0    |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        |                           ACCEL_FS_SEL                        |      Reserved      |                                ACCEL_ODR                          |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        ACCEL_FS_SEL :Full scale select for accelerometer UI interface output
+ * @n                      000: ¡À16g (default)
+ * @n                      001: ¡À8g
+ * @n                      010: ¡À4g
+ * @n                      011: ¡À2g
+ * @n                      100: Reserved
+ * @n                      101: Reserved
+ * @n                      110: Reserved
+ * @n                      111: Reserved
+ * @n        ACCEL_ODR :Accelerometer ODR selection for UI interface output
+ * @n                   0000: Reserved
+ * @n                   0001: 32kHz (LN mode)
+ * @n                   0010: 16kHz (LN mode)
+ * @n                   0011: 8kHz (LN mode)
+ * @n                   0100: 4kHz (LN mode)
+ * @n                   0101: 2kHz (LN mode)
+ * @n                   0110: 1kHz (LN mode) (default)
+ * @n                   0111: 200Hz (LP or LN mode) 
+ * @n                   1000: 100Hz (LP or LN mode)
+ * @n                   1001: 50Hz (LP or LN mode)
+ * @n                   1010: 25Hz (LP or LN mode)
+ * @n                   1011: 12.5Hz (LP or LN mode)
+ * @n                   1100: 6.25Hz (LP mode)
+ * @n                   1101: 3.125Hz (LP mode)
+ * @n                   1110: 1.5625Hz (LP mode)
+ * @n                   1111: 500Hz (LP or LN mode)
+ */
+typedef struct {
+	uint8_t   accelODR: 4; 
+	uint8_t   reserved: 1; 
+	uint8_t   accelFsSel: 3; 
+} __attribute__ ((packed)) sAccelConfig0_t;
+
+
+/**
+ * @struct sGyroConfig0_t
+ * @brief  Register:Gyro_Config0
+ * @n        -------------------------------------------------------------------------------------------------------------------------------------- -------------------
+ * @n        |            b7          |        b6       |         b5         |         b4         |          b3        |       b2       |       b1       |      b0    |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        |                            GYRO_FS_SEL                        |      Reserved      |                                 GYRO_ODR                          |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        GYRO_FS_SEL : Full scale select for gyroscope UI interface output
+ * @n                      000: ¡À2000dps (default)
+ * @n                      001: ¡À1000dps
+ * @n                      010: ¡À500dps
+ * @n                      011: ¡À250dps
+ * @n                      100: ¡À125dps
+ * @n                      101: ¡À62.5dps
+ * @n                      110: ¡À31.25dps
+ * @n                      111: ¡À15.625dps
+ * @n        GYRO_ODR :Gyroscope ODR selection for UI interface output
+ * @n                  0000: Reserved
+ * @n                  0001: 32kHz
+ * @n                  0010: 16kHz
+ * @n                  0011: 8kHz
+ * @n                  0100: 4kHz
+ * @n                  0101: 2kHz
+ * @n                  0110: 1kHz (default)
+ * @n                  0111: 200Hz 
+ * @n                  1000: 100Hz
+ * @n                  1001: 50Hz
+ * @n                  1010: 25Hz
+ * @n                  1011: 12.5Hz
+ * @n                  1100: Reserved
+ * @n                  1101: Reserved
+ * @n                  1110: Reserved
+ * @n                  1111: 500Hz
+ */
+typedef struct {
+	uint8_t   gyroODR: 4; 
+	uint8_t   reserved: 1; 
+	uint8_t   gyroFsSel: 3; 
+} __attribute__ ((packed)) sGyroConfig0_t;
+
+
+
+
+/**
+ * @struct sAccelConfig1_t
+ * @brief  Register:Accel_Config1
+ * @n        -------------------------------------------------------------------------------------------------------------------------------------- -------------------
+ * @n        |            b7          |        b6       |         b5         |         b4         |          b3        |       b2       |       b1       |      b0    |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        |                            Reserved                           |          ACCEL_UI_FILT_ORD              |         ACCEL_DEC2_M2_ORD       |  Reserved  |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        ACCEL_UI_FILT_ORD : Selects order of ACCEL UI filter
+ * @n                            00: 1st Order
+ * @n                            01: 2nd Order
+ * @n                            10: 3rd Order
+ * @n                            11: Reserved
+ * @n        ACCEL_DEC2_M2_ORD : Order of Accelerometer DEC2_M2 filter
+ * @n                            00: Reserved
+ * @n                            01: Reserved
+ * @n                            10: 3rd order
+ * @n                            11: Reserved
+ */
+typedef struct {
+	uint8_t   reserved: 1; 
+	uint8_t   accelDec2M2ORD: 2; 
+	uint8_t   accelUIFiltORD: 2; 
+	uint8_t   reserved2: 3;
+} __attribute__ ((packed)) sAccelConfig1_t;
+
+
+
+/**
+ * @struct sGyroAccelConfig0_t
+ * @brief  Register:Gyro_Accel_Config0
+ * @n        -------------------------------------------------------------------------------------------------------------------------------------- -------------------
+ * @n        |            b7          |        b6       |         b5         |         b4         |          b3        |       b2       |       b1       |      b0    |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        |                                       ACCEL_UI_FILT_BW                             |                             GYRO_UI_FILT_BW                       |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        ACCEL_UI_FILT_BW : Bandwidth for Accel LPF
+ * @n                           LN Mode:
+ * @n                              0 BW=ODR/2
+ * @n                              1 BW=max(400Hz, ODR)/4 (default)
+ * @n                              2 BW=max(400Hz, ODR)/5
+ * @n                              3 BW=max(400Hz, ODR)/8
+ * @n                              4 BW=max(400Hz, ODR)/10
+ * @n                              5 BW=max(400Hz, ODR)/16
+ * @n                              6 BW=max(400Hz, ODR)/20
+ * @n                              7 BW=max(400Hz, ODR)/40
+ * @n                              8 to 13: Reserved
+ * @n                              14 Low Latency option: Trivial decimation @ ODR of Dec2 filter output. Dec2 
+ * @n                              runs at max(400Hz, ODR) 
+ * @n                              15 Low Latency option: Trivial decimation @ ODR of Dec2 filter output. Dec2 
+ * @n                              runs at max(200Hz, 8*ODR)
+ * @n                            LP Mode:
+ * @n                              0 Reserved
+ * @n                              1 1x AVG filter (default)
+ * @n                              2 to 5 Reserved
+ * @n                              6 16x AVG filter
+ * @n                              7 to 15 Reserved
+ * @n        GYRO_UI_FILT_BW :Bandwidth for Gyro LPF
+ * @n                           LN Mode:
+ * @n                            0 BW=ODR/2
+ * @n                            1 BW=max(400Hz, ODR)/4 (default)
+ * @n                            2 BW=max(400Hz, ODR)/5
+ * @n                            3 BW=max(400Hz, ODR)/8
+ * @n                            4 BW=max(400Hz, ODR)/10
+ * @n                            5 BW=max(400Hz, ODR)/16
+ * @n                            6 BW=max(400Hz, ODR)/20
+ * @n                            7 BW=max(400Hz, ODR)/40
+ * @n                            8 to 13: Reserved
+ * @n                            14 Low Latency option: Trivial decimation @ ODR of Dec2 filter output. Dec2 runs at max(400Hz, ODR) 
+ * @n                            15 Low Latency option: Trivial decimation @ ODR of Dec2 filter output. Dec2 runs at max(200Hz, 8*ODR)
+ */
+typedef struct {
+	uint8_t   gyroUIFiltBW: 4; 
+	uint8_t   accelUIFiltBW: 4; 
+} __attribute__ ((packed)) sGyroAccelConfig0_t;
+
+
+/**
+ * @struct sGyroConfig1_t
+ * @brief  Register:Gyro_Config1
+ * @n        -------------------------------------------------------------------------------------------------------------------------------------- -------------------
+ * @n        |            b7          |        b6       |         b5         |         b4         |          b3        |       b2       |       b1       |      b0    |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        |                          TEMP_FILT_BW                         |     reserved       |            GYRO_UI_FILT_ORD         |      GYRO_DEC2_M2_ORD       |
+ * @n        ----------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        TEMP_FILT_BW : Set the bandwidth of the temperature signal DLPF
+ * @n                      000: DLPF BW = 4000Hz; DLPF Latency = 0.125ms (default)
+ * @n                      001: DLPF BW = 170Hz; DLPF Latency = 1ms
+ * @n                      010: DLPF BW = 82Hz; DLPF Latency = 2ms
+ * @n                      011: DLPF BW = 40Hz; DLPF Latency = 4ms
+ * @n                      100: DLPF BW = 20Hz; DLPF Latency = 8ms
+ * @n                      101: DLPF BW = 10Hz; DLPF Latency = 16ms
+ * @n                      110: DLPF BW = 5Hz; DLPF Latency = 32ms
+ * @n                      111: DLPF BW = 5Hz; DLPF Latency = 32ms
+ * @n        GYRO_UI_FILT_ORD :Selects order of GYRO UI filter
+ * @n                           00: 1st Order
+ * @n                           01: 2nd Order
+ * @n                           10: 3rd Order
+ * @n                           11: Reserved
+ * @n        GYRO_DEC2_M2_ORD :Selects order of GYRO DEC2_M2 Filter
+ * @n                          00: Reserved
+ * @n                          01: Reserved
+ * @n                          10: 3rd Order
+ * @n                          11: Reserved
+ */
+typedef struct {
+	uint8_t   gyroDec2M2ODR: 2; 
+	uint8_t   gyroUIFiltODR: 2; 
+	uint8_t   reserved: 1; 
+	uint8_t   agyroFiltBW: 3; 
+} __attribute__ ((packed)) sGyroConfig1_t;
+
+
+/**
+ * @struct sAccelConfigStatic4_t
+ * @brief  Register:Accel_Config_Static4
+ * @n        -------------------------------------------------------------------------------------------------------------------------------------- --------------------------------------------------
+ * @n        |            b7        |           b6         |           b5           |           b4           |           b3           |          b2        |          b1        |        b0          |
+ * @n        -----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        |                                  ACCEL_AAF_BITSHIFT                                           |                               ACCEL_AAF_DELTSQR[11:8]                                 |
+ * @n        -----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        ACCEL_AAF_BITSHIFT  Controls bandwidth of the accelerometer anti-alias filter
+ * @n        ACCEL_AAF_DELTSQR[11:8]  Controls bandwidth of the accelerometer anti-alias filter
+ */
+typedef struct {
+	uint8_t   accelAAFDeltsqr: 4; 
+	uint8_t   accelAAFBitshift: 4; 
+} __attribute__ ((packed)) sAccelConfigStatic4_t;
+
+
+/**
+ * @struct sAccelConfigStatic2_t
+ * @brief  Register:Accel_Config_Static2
+ * @n        -------------------------------------------------------------------------------------------------------------------------------------- --------------------------------------------------
+ * @n        |            b7        |           b6         |           b5           |           b4           |           b3           |          b2        |          b1        |        b0          |
+ * @n        -----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        |       Reserved       |                                                          ACCEL_AAF_DELT                                                                   |    ACCEL_AAF_DIS   |
+ * @n        -----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        ACCEL_AAF_DELT  Controls bandwidth of the accelerometer anti-alias filter
+ * @n        ACCEL_AAF_DIS   0: Enable accelerometer anti-aliasing filter (default)
+ * @n                        1: Disable accelerometer anti-aliasing filter
+ */
+typedef struct {
+	uint8_t   accelAAFDis: 1; 
+	uint8_t   accelAAFDelt: 6;
+	uint8_t   reserved: 1; 
+} __attribute__ ((packed)) sAccelConfigStatic2_t;
+
+
+/**
+ * @struct sGyroConfigStatic2_t
+ * @brief  Register:Gyro_Config_Static2
+ * @n        -------------------------------------------------------------------------------------------------------------------------------------- ---------------------------------------
+ * @n        |            b7        |           b6         |           b5          |         b4         |          b3       |         b2       |          b1         |        b0          |
+ * @n        ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        |                                                                     Reserved                                                    |     GYRO_AAF_DIS    |     GYRO_NF_DIS    |
+ * @n        ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        GYRO_AAF_DIS :0: Enable gyroscope anti-aliasing filter (default)
+ * @n                      1: Disable gyroscope anti-aliasing filter
+ * @n        GYRO_NF_DIS   0: Enable Notch Filter (default)
+ * @n                      1: Disable Notch Filter
+ */
+typedef struct {
+	uint8_t   gyroNFDis: 1; 
+	uint8_t   gyroAAFDis: 1; 
+	uint8_t   reserved: 6;
+} __attribute__ ((packed)) sGyroConfigStatic2_t;
+
+
+/**
+ * @struct sGyroConfigStatic5_t
+ * @brief  Register:Gyro_Config_Static5
+ * @n         -------------------------------------------------------------------------------------------------------------------------------------- ---------------------------------------
+ * @n         |            b7        |           b6         |           b5          |         b4         |          b3       |         b2       |          b1         |        b0          |
+ * @n         ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n         |                                     GYRO_AAF_BITSHIFT                                    |                            GYRO_AAF_DELTSQR[11:8]                               |
+ * @n         ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n         GYRO_AAF_BITSHIFT :Controls bandwidth of the gyroscope anti-alias filter
+ * @n         GYRO_AAF_DELTSQR[11:8]   Controls bandwidth of the gyroscope anti-alias filter
+*/
+typedef struct {
+	uint8_t   gyroAAFDeltsqr: 4; 
+	uint8_t   gyroAAFBitshift: 4; 
+} __attribute__ ((packed)) sGyroConfigStatic5_t;
+
+
+
+/**
+ * @struct sGyroConfigStatic9_t
+ * @brief  Register:Gyro_Config_Static9
+ * @n        -------------------------------------------------------------------------------------------------------------------------------------- --------------------------------------------------
+ * @n        |            b7        |           b6         |           b5           |           b4           |           b3           |          b2        |          b1        |        b0          |
+ * @n        -----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        |                  Reserved                   | GYRO_Z_NF_COSWZ_SEL[0] | GYRO_Y_NF_COSWZ_SEL[0] | GYRO_X_NF_COSWZ_SEL[0] | GYRO_Z_NF_COSWZ[8] | GYRO_Y_NF_COSWZ[8] | GYRO_X_NF_COSWZ[8] |
+ * @n        -----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+ * @n        GYRO_Z_NF_COSWZ_SEL[0] :Used for gyroscope Z-axis notch filter frequency selection
+ * @n        GYRO_Y_NF_COSWZ_SEL[0] :Used for gyroscope Y-axis notch filter frequency selection
+ * @n        GYRO_X_NF_COSWZ_SEL[0] :Used for gyroscope X-axis notch filter frequency selection
+ * @n        GYRO_Z_NF_COSWZ[8] :Used for gyroscope Z-axis notch filter frequency selection
+ * @n        GYRO_Y_NF_COSWZ[8] :Used for gyroscope Y-axis notch filter frequency selection
+ * @n        GYRO_X_NF_COSWZ[8] :Used for gyroscope X-axis notch filter frequency selection
+ */
+typedef struct {
+	uint8_t   gyroNFCoswzX8: 1; 
+	uint8_t   gyroNFCoswzY8: 1; 
+	uint8_t   gyroNFCoswzZ8: 1;
+	uint8_t   gyroNFCoswzSelX: 1;
+	uint8_t   gyroNFCoswzSelY: 1;
+	uint8_t   gyroNFCoswzSelZ: 1;
+	uint8_t   reserved:2;
+} __attribute__ ((packed)) sGyroConfigStatic9_t;
+
+
+enum GyroFS{
+	dps2000 = 0x00,
+	dps1000 = 0x01,
+	dps500 = 0x02,
+	dps250 = 0x03,
+	dps125 = 0x04,
+	dps62_5 = 0x05,
+	dps31_25 = 0x06,
+	dps15_625 = 0x07
+};
+
+enum AccelFS{
+	gpm16 = 0x00,
+	gpm8 = 0x01,
+	gpm4 = 0x02,
+	gpm2 = 0x03
+};
+
+
+extern float _acc[3];
+extern float _gyr[3];
+extern float _acc1[3];
+extern float _gyr1[3];
+extern float _accelScale;
+extern float _gyroScale;
+
+
+int ICM42688Begin(void);
+int ICM42688_readRegisters(uint8_t subAddress, uint8_t* dest,uint8_t len); 
+void ICM42688_Init(void);
+int getAGT(void);
+
+#endif 
+
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/imu.h

@@ -0,0 +1,26 @@
+#ifndef __IMU_H
+#define __IMU_H
+
+//#include "communicate.h"
+#include "public_data.h"
+//#include "SENSORS_TYPES.h"
+#include "STABILIZER_TYPES.h"
+
+//旋转机体坐标系加速度到NEU坐标系
+void imuTransformVectorBodyToEarth(Axis3f *v);
+//NEU坐标系加速度到NEU坐标系
+void imuTransformVectorEarthToBody(Axis3f *v);
+
+void imuMahonyAHRSupdate(float gx, float gy, float gz,float ax, float ay, float az,float mx, float my, float mz,bool useMag,float dt);
+//四元数和欧拉角计算
+void imuUpdate(Axis3f acc, Axis3f gyro, state_t *state , float dt);	/*数据融合 互补滤波*/
+void imuUpdateAttitude(const sensorData_t  *sensorData, state_t *state, float dt);
+void MadgwickQuaternionUpdate(Axis3f acc, Axis3f gyro, Axis3f mag, attitude_t *attitude , float dt);
+void imuInit(void);
+
+
+#endif
+
+
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/main.h

@@ -0,0 +1,71 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file           : main.h
+  * @brief          : Header for main.c file.
+  *                   This file contains the common defines of the application.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __MAIN_H
+#define __MAIN_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+#include "stm32f1xx_hal.h"
+
+/* Private includes ----------------------------------------------------------*/
+/* USER CODE BEGIN Includes */
+
+/* USER CODE END Includes */
+
+/* Exported types ------------------------------------------------------------*/
+/* USER CODE BEGIN ET */
+
+/* USER CODE END ET */
+
+/* Exported constants --------------------------------------------------------*/
+/* USER CODE BEGIN EC */
+
+/* USER CODE END EC */
+
+/* Exported macro ------------------------------------------------------------*/
+/* USER CODE BEGIN EM */
+
+/* USER CODE END EM */
+
+/* Exported functions prototypes ---------------------------------------------*/
+void Error_Handler(void);
+
+/* USER CODE BEGIN EFP */
+
+/* USER CODE END EFP */
+
+/* Private defines -----------------------------------------------------------*/
+#define SPI2_CS_Pin GPIO_PIN_4
+#define SPI2_CS_GPIO_Port GPIOA
+
+/* USER CODE BEGIN Private defines */
+
+/* USER CODE END Private defines */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __MAIN_H */

2.Firmware/ICM42688P_HAL/Core/Inc/maths.h

@@ -0,0 +1,111 @@
+#ifndef __MATHS_H
+#define __MATHS_H
+
+#include "math.h"
+#include "main.h"
+
+#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
+
+
+#ifndef sq
+#define sq(x) ((x)*(x))
+#endif
+
+#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+
+// Use floating point M_PI instead explicitly.
+#define M_PIf       3.14159265358979323846f
+#define M_LN2f      0.69314718055994530942f
+#define M_Ef        2.71828182845904523536f
+
+#define DEG2RAD		0.017453293f	/* ¶Èת»¡¶È ¦Ð/180 */
+#define RAD2DEG		57.29578f		/* »¡¶Èת¶È 180/¦Ð */
+
+#define RAD    (M_PIf / 180.0f)
+
+#define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100)
+#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100)
+
+#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10)
+#define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10)
+
+#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
+#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)
+
+#define DEGREES_PER_DEKADEGREE 10
+#define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE)
+#define DEKADEGREES_TO_DEGREES(angle) ((angle) * DEGREES_PER_DEKADEGREE)
+
+#define DEGREES_TO_RADIANS(angle) ((angle) * RAD)
+#define RADIANS_TO_DEGREES(angle) ((angle) / RAD)
+#define DECIDEGREES_TO_RADIANS(angle) (((angle) / 10.0f) * RAD)
+#define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD)
+
+#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
+#define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)
+
+#define MIN(a, b) 	(((a) < (b)) ? (a) : (b))
+#define MAX(a, b) 	(((a) > (b)) ? (a) : (b))
+
+#define ABS(x) 		(((x) < 0) ? (-x) : (x))
+
+
+typedef enum {
+    X = 0,
+    Y,
+    Z
+} axis_e;
+
+typedef struct stdev_s
+{
+    float m_oldM, m_newM, m_oldS, m_newS;
+    int m_n;
+} stdev_t;
+
+
+// Floating point Euler angles.
+// Be carefull, could be either of degrees or radians.
+typedef struct fp_angles {
+    float roll;
+    float pitch;
+    float yaw;
+} fp_angles_def;
+
+
+typedef union {
+    float raw[3];
+    fp_angles_def angles;
+} fp_angles_t;
+
+
+
+typedef struct {
+    float XtY[4];
+    float XtX[4][4];
+} sensorCalibrationState_t;
+
+
+float constrainFloat(float data,float min,float max);
+int16_t constrainInt16_t(int16_t data,int16_t min,int16_t max);
+uint16_t constrainUint16_t(uint16_t data,uint16_t min,uint16_t max);
+
+
+void sensorCalibrationResetState(sensorCalibrationState_t * state);
+void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]);	
+void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
+float sin_approx(float x);
+float cos_approx(float x);
+float atan2_approx(float y, float x);
+float acos_approx(float x);
+void devClear(stdev_t *dev);
+void devPush(stdev_t *dev, float x);
+float devStandardDeviation(stdev_t *dev);
+void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]);
+float map(long x, long in_min, long in_max, float out_min, float out_max);
+int32_t applyDeadband(int32_t value, int32_t deadband);
+float applyDeadbandF(float value, float deadband);
+float mapFloat(float x, float in_min, float in_max, float out_min, float out_max);
+
+#endif
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/public_data.h

@@ -0,0 +1,20 @@
+#ifndef __public_data_H
+#define __public_data_H
+
+#include <stdint.h>
+#include "maths.h"
+#include <math.h>
+#include <stdbool.h>
+#include "stdio.h"
+#include "string.h"
+#include "stdlib.h"
+#include "stabilizer_types.h"
+#include "runtime_config.h"
+//#include "SENSORS_TYPES.h"
+#include "sensorsalignment.h"
+#include "CONFIG_PARAM.h"
+//#include "tim.h"
+
+#endif 
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/runtime_config.h

@@ -0,0 +1,82 @@
+#ifndef __RUNTIME_CONFIG_H
+#define __RUNTIME_CONFIG_H
+
+#include "communicate.h"
+
+
+
+//解锁标志位
+typedef enum 
+{
+    ARMED                                           = (1 << 1),
+    WAS_EVER_ARMED                                  = (1 << 2),
+    ARMING_DISABLED_NOT_LEVEL                       = (1 << 3),
+    ARMING_DISABLED_SENSORS_CALIBRATING             = (1 << 4),
+    ARMING_DISABLED_COMPASS_NOT_CALIBRATED          = (1 << 5),
+    ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED    = (1 << 6),
+    ARMING_DISABLED_HARDWARE_FAILURE                = (1 << 7),
+	ARMING_DISABLED_FLASH_WRITING                   = (1 << 8),
+	ARMING_DISABLED_PID_BYPASS			            = (1 << 9),
+	
+    ARMING_DISABLED_ALL_FLAGS                       = (ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_COMPASS_NOT_CALIBRATED | 
+                                                       ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED | ARMING_DISABLED_HARDWARE_FAILURE | ARMING_DISABLED_FLASH_WRITING |
+													   ARMING_DISABLED_PID_BYPASS
+													   )
+} armingFlag_e;
+
+extern uint32_t armingFlags;
+
+
+#define isArmingDisabled()          (armingFlags & (ARMING_DISABLED_ALL_FLAGS))
+#define DISABLE_ARMING_FLAG(mask)   (armingFlags &= ~(mask))
+#define ENABLE_ARMING_FLAG(mask)    (armingFlags |= (mask))
+#define ARMING_FLAG(mask)           (armingFlags & (mask))
+
+
+
+//飞行模式标志位
+typedef enum 
+{
+	ANGLE_MODE        = (1 << 0),
+	ACRO_MODE         = (1 << 1),
+	HEADFREE_MODE     = (1 << 2),
+	NAV_ALTHOLD_MODE  = (1 << 3),
+	NAV_RTH_MODE      = (1 << 4),
+	NAV_POSHOLD_MODE  = (1 << 5),
+	NAV_LAUNCH_MODE   = (1 << 7),
+	FAILSAFE_MODE     = (1 << 8),
+	BEEPER_ON_MODE    = (1 << 9),//蜂鸣器常开，寻机模式。
+} flightModeFlags_e;
+extern uint32_t flightModeFlags;
+
+
+#define DISABLE_STATE(mask) (stateFlags &= ~(mask))
+#define ENABLE_STATE(mask) (stateFlags |= (mask))
+#define STATE(mask) (stateFlags & (mask))
+
+
+
+//运行状态标志位
+typedef enum 
+{
+	ACCELEROMETER_CALIBRATED  = (1 << 1),
+	COMPASS_CALIBRATED        = (1 << 2),
+	CALIBRATE_MAG             = (1 << 3),
+	SMALL_ANGLE               = (1 << 4),
+	FLASH_WRITING             = (1 << 5),
+} stateFlags_t;
+extern uint32_t stateFlags;
+
+#define DISABLE_STATE(mask) (stateFlags &= ~(mask))
+#define ENABLE_STATE(mask) (stateFlags |= (mask))
+#define STATE(mask) (stateFlags & (mask))
+
+
+
+
+
+
+
+#endif
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/sensors.h

@@ -0,0 +1,34 @@
+#ifndef __SENSORS_H
+#define __SENSORS_H
+
+//#include "stabilizer_types.h"
+#include "gyro.h"
+#include "accelerometer.h"
+//#include "icm42688p.h"
+#include "imu.h"
+
+
+/*传感器读取更新频率*/
+#define GYRO_UPDATE_RATE		RATE_500_HZ
+#define ACC_UPDATE_RATE			RATE_500_HZ
+#define MAG_UPDATE_RATE			RATE_10_HZ
+#define BARO_UPDATE_RATE		RATE_50_HZ
+
+
+extern sensorData_t  sensorData;	/*传感器数据*/
+extern state_t  state;		/*四轴姿态*/
+extern float sensors_dt;
+
+extern uint16_t VoltageADC;//电压ADC数据
+extern float Voltage;//电压
+
+void sensorsInit(void);
+/*传感器任务*/
+void sensorsTask(float dt);
+float IMU_Task_time(void);
+void ReadVoltage(void);
+	
+#endif //__SENSORS_H
+
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/sensorsalignment.h

@@ -0,0 +1,30 @@
+#ifndef __SENSORS_ALIGNMENT_H
+#define __SENSORS_ALIGNMENT_H
+
+#include "public_data.h"
+
+typedef enum 
+{
+    ALIGN_DEFAULT = 0, 
+    CW0_DEG = 1,
+    CW90_DEG = 2,
+    CW180_DEG = 3,
+    CW270_DEG = 4,
+    CW0_DEG_FLIP = 5,
+    CW90_DEG_FLIP = 6,
+    CW180_DEG_FLIP = 7,
+    CW270_DEG_FLIP = 8
+} sensor_align_e;
+
+
+
+void applySensorAlignment(int16_t * dest, int16_t * src, uint8_t rotation);
+void applyBoardAlignment(int16_t *vec);
+void initBoardAlignment(void);
+
+#endif
+
+
+
+
+

2.Firmware/ICM42688P_HAL/Core/Inc/spi.h

@@ -0,0 +1,52 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    spi.h
+  * @brief   This file contains all the function prototypes for
+  *          the spi.c file
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __SPI_H__
+#define __SPI_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+
+/* USER CODE BEGIN Includes */
+
+/* USER CODE END Includes */
+
+extern SPI_HandleTypeDef hspi2;
+
+/* USER CODE BEGIN Private defines */
+
+/* USER CODE END Private defines */
+
+void MX_SPI2_Init(void);
+
+/* USER CODE BEGIN Prototypes */
+
+/* USER CODE END Prototypes */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __SPI_H__ */
+

2.Firmware/ICM42688P_HAL/Core/Inc/stm32f1xx_hal_conf.h

@@ -0,0 +1,391 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    stm32f1xx_hal_conf.h
+  * @brief   HAL configuration file.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2017 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __STM32F1xx_HAL_CONF_H
+#define __STM32F1xx_HAL_CONF_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Exported types ------------------------------------------------------------*/
+/* Exported constants --------------------------------------------------------*/
+
+/* ########################## Module Selection ############################## */
+/**
+  * @brief This is the list of modules to be used in the HAL driver
+  */
+
+#define HAL_MODULE_ENABLED
+  /*#define HAL_ADC_MODULE_ENABLED   */
+/*#define HAL_CRYP_MODULE_ENABLED   */
+/*#define HAL_CAN_MODULE_ENABLED   */
+/*#define HAL_CAN_LEGACY_MODULE_ENABLED   */
+/*#define HAL_CEC_MODULE_ENABLED   */
+/*#define HAL_CORTEX_MODULE_ENABLED   */
+/*#define HAL_CRC_MODULE_ENABLED   */
+/*#define HAL_DAC_MODULE_ENABLED   */
+/*#define HAL_DMA_MODULE_ENABLED   */
+/*#define HAL_ETH_MODULE_ENABLED   */
+/*#define HAL_FLASH_MODULE_ENABLED   */
+#define HAL_GPIO_MODULE_ENABLED
+/*#define HAL_I2C_MODULE_ENABLED   */
+/*#define HAL_I2S_MODULE_ENABLED   */
+/*#define HAL_IRDA_MODULE_ENABLED   */
+/*#define HAL_IWDG_MODULE_ENABLED   */
+/*#define HAL_NOR_MODULE_ENABLED   */
+/*#define HAL_NAND_MODULE_ENABLED   */
+/*#define HAL_PCCARD_MODULE_ENABLED   */
+/*#define HAL_PCD_MODULE_ENABLED   */
+/*#define HAL_HCD_MODULE_ENABLED   */
+/*#define HAL_PWR_MODULE_ENABLED   */
+/*#define HAL_RCC_MODULE_ENABLED   */
+/*#define HAL_RTC_MODULE_ENABLED   */
+/*#define HAL_SD_MODULE_ENABLED   */
+/*#define HAL_MMC_MODULE_ENABLED   */
+/*#define HAL_SDRAM_MODULE_ENABLED   */
+/*#define HAL_SMARTCARD_MODULE_ENABLED   */
+#define HAL_SPI_MODULE_ENABLED
+/*#define HAL_SRAM_MODULE_ENABLED   */
+/*#define HAL_TIM_MODULE_ENABLED   */
+#define HAL_UART_MODULE_ENABLED
+/*#define HAL_USART_MODULE_ENABLED   */
+/*#define HAL_WWDG_MODULE_ENABLED   */
+
+#define HAL_CORTEX_MODULE_ENABLED
+#define HAL_DMA_MODULE_ENABLED
+#define HAL_FLASH_MODULE_ENABLED
+#define HAL_EXTI_MODULE_ENABLED
+#define HAL_GPIO_MODULE_ENABLED
+#define HAL_PWR_MODULE_ENABLED
+#define HAL_RCC_MODULE_ENABLED
+
+/* ########################## Oscillator Values adaptation ####################*/
+/**
+  * @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
+  *        This value is used by the RCC HAL module to compute the system frequency
+  *        (when HSE is used as system clock source, directly or through the PLL).
+  */
+#if !defined  (HSE_VALUE)
+  #define HSE_VALUE    8000000U /*!< Value of the External oscillator in Hz */
+#endif /* HSE_VALUE */
+
+#if !defined  (HSE_STARTUP_TIMEOUT)
+  #define HSE_STARTUP_TIMEOUT    100U   /*!< Time out for HSE start up, in ms */
+#endif /* HSE_STARTUP_TIMEOUT */
+
+/**
+  * @brief Internal High Speed oscillator (HSI) value.
+  *        This value is used by the RCC HAL module to compute the system frequency
+  *        (when HSI is used as system clock source, directly or through the PLL).
+  */
+#if !defined  (HSI_VALUE)
+  #define HSI_VALUE    8000000U /*!< Value of the Internal oscillator in Hz*/
+#endif /* HSI_VALUE */
+
+/**
+  * @brief Internal Low Speed oscillator (LSI) value.
+  */
+#if !defined  (LSI_VALUE)
+ #define LSI_VALUE               40000U    /*!< LSI Typical Value in Hz */
+#endif /* LSI_VALUE */                     /*!< Value of the Internal Low Speed oscillator in Hz
+                                                The real value may vary depending on the variations
+                                                in voltage and temperature. */
+
+/**
+  * @brief External Low Speed oscillator (LSE) value.
+  *        This value is used by the UART, RTC HAL module to compute the system frequency
+  */
+#if !defined  (LSE_VALUE)
+  #define LSE_VALUE    32768U /*!< Value of the External oscillator in Hz*/
+#endif /* LSE_VALUE */
+
+#if !defined  (LSE_STARTUP_TIMEOUT)
+  #define LSE_STARTUP_TIMEOUT    5000U   /*!< Time out for LSE start up, in ms */
+#endif /* LSE_STARTUP_TIMEOUT */
+
+/* Tip: To avoid modifying this file each time you need to use different HSE,
+   ===  you can define the HSE value in your toolchain compiler preprocessor. */
+
+/* ########################### System Configuration ######################### */
+/**
+  * @brief This is the HAL system configuration section
+  */
+#define  VDD_VALUE                    3300U /*!< Value of VDD in mv */
+#define  TICK_INT_PRIORITY            15U    /*!< tick interrupt priority (lowest by default)  */
+#define  USE_RTOS                     0U
+#define  PREFETCH_ENABLE              1U
+
+#define  USE_HAL_ADC_REGISTER_CALLBACKS         0U /* ADC register callback disabled       */
+#define  USE_HAL_CAN_REGISTER_CALLBACKS         0U /* CAN register callback disabled       */
+#define  USE_HAL_CEC_REGISTER_CALLBACKS         0U /* CEC register callback disabled       */
+#define  USE_HAL_DAC_REGISTER_CALLBACKS         0U /* DAC register callback disabled       */
+#define  USE_HAL_ETH_REGISTER_CALLBACKS         0U /* ETH register callback disabled       */
+#define  USE_HAL_HCD_REGISTER_CALLBACKS         0U /* HCD register callback disabled       */
+#define  USE_HAL_I2C_REGISTER_CALLBACKS         0U /* I2C register callback disabled       */
+#define  USE_HAL_I2S_REGISTER_CALLBACKS         0U /* I2S register callback disabled       */
+#define  USE_HAL_MMC_REGISTER_CALLBACKS         0U /* MMC register callback disabled       */
+#define  USE_HAL_NAND_REGISTER_CALLBACKS        0U /* NAND register callback disabled      */
+#define  USE_HAL_NOR_REGISTER_CALLBACKS         0U /* NOR register callback disabled       */
+#define  USE_HAL_PCCARD_REGISTER_CALLBACKS      0U /* PCCARD register callback disabled    */
+#define  USE_HAL_PCD_REGISTER_CALLBACKS         0U /* PCD register callback disabled       */
+#define  USE_HAL_RTC_REGISTER_CALLBACKS         0U /* RTC register callback disabled       */
+#define  USE_HAL_SD_REGISTER_CALLBACKS          0U /* SD register callback disabled        */
+#define  USE_HAL_SMARTCARD_REGISTER_CALLBACKS   0U /* SMARTCARD register callback disabled */
+#define  USE_HAL_IRDA_REGISTER_CALLBACKS        0U /* IRDA register callback disabled      */
+#define  USE_HAL_SRAM_REGISTER_CALLBACKS        0U /* SRAM register callback disabled      */
+#define  USE_HAL_SPI_REGISTER_CALLBACKS         0U /* SPI register callback disabled       */
+#define  USE_HAL_TIM_REGISTER_CALLBACKS         0U /* TIM register callback disabled       */
+#define  USE_HAL_UART_REGISTER_CALLBACKS        0U /* UART register callback disabled      */
+#define  USE_HAL_USART_REGISTER_CALLBACKS       0U /* USART register callback disabled     */
+#define  USE_HAL_WWDG_REGISTER_CALLBACKS        0U /* WWDG register callback disabled      */
+
+/* ########################## Assert Selection ############################## */
+/**
+  * @brief Uncomment the line below to expanse the "assert_param" macro in the
+  *        HAL drivers code
+  */
+/* #define USE_FULL_ASSERT    1U */
+
+/* ################## Ethernet peripheral configuration ##################### */
+
+/* Section 1 : Ethernet peripheral configuration */
+
+/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */
+#define MAC_ADDR0   2U
+#define MAC_ADDR1   0U
+#define MAC_ADDR2   0U
+#define MAC_ADDR3   0U
+#define MAC_ADDR4   0U
+#define MAC_ADDR5   0U
+
+/* Definition of the Ethernet driver buffers size and count */
+#define ETH_RX_BUF_SIZE                ETH_MAX_PACKET_SIZE /* buffer size for receive               */
+#define ETH_TX_BUF_SIZE                ETH_MAX_PACKET_SIZE /* buffer size for transmit              */
+#define ETH_RXBUFNB                    8U       /* 4 Rx buffers of size ETH_RX_BUF_SIZE  */
+#define ETH_TXBUFNB                    4U       /* 4 Tx buffers of size ETH_TX_BUF_SIZE  */
+
+/* Section 2: PHY configuration section */
+
+/* DP83848_PHY_ADDRESS Address*/
+#define DP83848_PHY_ADDRESS           0x01U
+/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
+#define PHY_RESET_DELAY                 0x000000FFU
+/* PHY Configuration delay */
+#define PHY_CONFIG_DELAY                0x00000FFFU
+
+#define PHY_READ_TO                     0x0000FFFFU
+#define PHY_WRITE_TO                    0x0000FFFFU
+
+/* Section 3: Common PHY Registers */
+
+#define PHY_BCR                         ((uint16_t)0x00)    /*!< Transceiver Basic Control Register   */
+#define PHY_BSR                         ((uint16_t)0x01)    /*!< Transceiver Basic Status Register    */
+
+#define PHY_RESET                       ((uint16_t)0x8000)  /*!< PHY Reset */
+#define PHY_LOOPBACK                    ((uint16_t)0x4000)  /*!< Select loop-back mode */
+#define PHY_FULLDUPLEX_100M             ((uint16_t)0x2100)  /*!< Set the full-duplex mode at 100 Mb/s */
+#define PHY_HALFDUPLEX_100M             ((uint16_t)0x2000)  /*!< Set the half-duplex mode at 100 Mb/s */
+#define PHY_FULLDUPLEX_10M              ((uint16_t)0x0100)  /*!< Set the full-duplex mode at 10 Mb/s  */
+#define PHY_HALFDUPLEX_10M              ((uint16_t)0x0000)  /*!< Set the half-duplex mode at 10 Mb/s  */
+#define PHY_AUTONEGOTIATION             ((uint16_t)0x1000)  /*!< Enable auto-negotiation function     */
+#define PHY_RESTART_AUTONEGOTIATION     ((uint16_t)0x0200)  /*!< Restart auto-negotiation function    */
+#define PHY_POWERDOWN                   ((uint16_t)0x0800)  /*!< Select the power down mode           */
+#define PHY_ISOLATE                     ((uint16_t)0x0400)  /*!< Isolate PHY from MII                 */
+
+#define PHY_AUTONEGO_COMPLETE           ((uint16_t)0x0020)  /*!< Auto-Negotiation process completed   */
+#define PHY_LINKED_STATUS               ((uint16_t)0x0004)  /*!< Valid link established               */
+#define PHY_JABBER_DETECTION            ((uint16_t)0x0002)  /*!< Jabber condition detected            */
+
+/* Section 4: Extended PHY Registers */
+#define PHY_SR                          ((uint16_t)0x10U)    /*!< PHY status register Offset                      */
+
+#define PHY_SPEED_STATUS                ((uint16_t)0x0002U)  /*!< PHY Speed mask                                  */
+#define PHY_DUPLEX_STATUS               ((uint16_t)0x0004U)  /*!< PHY Duplex mask                                 */
+
+/* ################## SPI peripheral configuration ########################## */
+
+/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver
+* Activated: CRC code is present inside driver
+* Deactivated: CRC code cleaned from driver
+*/
+
+#define USE_SPI_CRC                     0U
+
+/* Includes ------------------------------------------------------------------*/
+/**
+  * @brief Include module's header file
+  */
+
+#ifdef HAL_RCC_MODULE_ENABLED
+#include "stm32f1xx_hal_rcc.h"
+#endif /* HAL_RCC_MODULE_ENABLED */
+
+#ifdef HAL_GPIO_MODULE_ENABLED
+#include "stm32f1xx_hal_gpio.h"
+#endif /* HAL_GPIO_MODULE_ENABLED */
+
+#ifdef HAL_EXTI_MODULE_ENABLED
+#include "stm32f1xx_hal_exti.h"
+#endif /* HAL_EXTI_MODULE_ENABLED */
+
+#ifdef HAL_DMA_MODULE_ENABLED
+#include "stm32f1xx_hal_dma.h"
+#endif /* HAL_DMA_MODULE_ENABLED */
+
+#ifdef HAL_ETH_MODULE_ENABLED
+#include "stm32f1xx_hal_eth.h"
+#endif /* HAL_ETH_MODULE_ENABLED */
+
+#ifdef HAL_CAN_MODULE_ENABLED
+#include "stm32f1xx_hal_can.h"
+#endif /* HAL_CAN_MODULE_ENABLED */
+
+#ifdef HAL_CAN_LEGACY_MODULE_ENABLED
+  #include "Legacy/stm32f1xx_hal_can_legacy.h"
+#endif /* HAL_CAN_LEGACY_MODULE_ENABLED */
+
+#ifdef HAL_CEC_MODULE_ENABLED
+#include "stm32f1xx_hal_cec.h"
+#endif /* HAL_CEC_MODULE_ENABLED */
+
+#ifdef HAL_CORTEX_MODULE_ENABLED
+#include "stm32f1xx_hal_cortex.h"
+#endif /* HAL_CORTEX_MODULE_ENABLED */
+
+#ifdef HAL_ADC_MODULE_ENABLED
+#include "stm32f1xx_hal_adc.h"
+#endif /* HAL_ADC_MODULE_ENABLED */
+
+#ifdef HAL_CRC_MODULE_ENABLED
+#include "stm32f1xx_hal_crc.h"
+#endif /* HAL_CRC_MODULE_ENABLED */
+
+#ifdef HAL_DAC_MODULE_ENABLED
+#include "stm32f1xx_hal_dac.h"
+#endif /* HAL_DAC_MODULE_ENABLED */
+
+#ifdef HAL_FLASH_MODULE_ENABLED
+#include "stm32f1xx_hal_flash.h"
+#endif /* HAL_FLASH_MODULE_ENABLED */
+
+#ifdef HAL_SRAM_MODULE_ENABLED
+#include "stm32f1xx_hal_sram.h"
+#endif /* HAL_SRAM_MODULE_ENABLED */
+
+#ifdef HAL_NOR_MODULE_ENABLED
+#include "stm32f1xx_hal_nor.h"
+#endif /* HAL_NOR_MODULE_ENABLED */
+
+#ifdef HAL_I2C_MODULE_ENABLED
+#include "stm32f1xx_hal_i2c.h"
+#endif /* HAL_I2C_MODULE_ENABLED */
+
+#ifdef HAL_I2S_MODULE_ENABLED
+#include "stm32f1xx_hal_i2s.h"
+#endif /* HAL_I2S_MODULE_ENABLED */
+
+#ifdef HAL_IWDG_MODULE_ENABLED
+#include "stm32f1xx_hal_iwdg.h"
+#endif /* HAL_IWDG_MODULE_ENABLED */
+
+#ifdef HAL_PWR_MODULE_ENABLED
+#include "stm32f1xx_hal_pwr.h"
+#endif /* HAL_PWR_MODULE_ENABLED */
+
+#ifdef HAL_RTC_MODULE_ENABLED
+#include "stm32f1xx_hal_rtc.h"
+#endif /* HAL_RTC_MODULE_ENABLED */
+
+#ifdef HAL_PCCARD_MODULE_ENABLED
+#include "stm32f1xx_hal_pccard.h"
+#endif /* HAL_PCCARD_MODULE_ENABLED */
+
+#ifdef HAL_SD_MODULE_ENABLED
+#include "stm32f1xx_hal_sd.h"
+#endif /* HAL_SD_MODULE_ENABLED */
+
+#ifdef HAL_NAND_MODULE_ENABLED
+#include "stm32f1xx_hal_nand.h"
+#endif /* HAL_NAND_MODULE_ENABLED */
+
+#ifdef HAL_SPI_MODULE_ENABLED
+#include "stm32f1xx_hal_spi.h"
+#endif /* HAL_SPI_MODULE_ENABLED */
+
+#ifdef HAL_TIM_MODULE_ENABLED
+#include "stm32f1xx_hal_tim.h"
+#endif /* HAL_TIM_MODULE_ENABLED */
+
+#ifdef HAL_UART_MODULE_ENABLED
+#include "stm32f1xx_hal_uart.h"
+#endif /* HAL_UART_MODULE_ENABLED */
+
+#ifdef HAL_USART_MODULE_ENABLED
+#include "stm32f1xx_hal_usart.h"
+#endif /* HAL_USART_MODULE_ENABLED */
+
+#ifdef HAL_IRDA_MODULE_ENABLED
+#include "stm32f1xx_hal_irda.h"
+#endif /* HAL_IRDA_MODULE_ENABLED */
+
+#ifdef HAL_SMARTCARD_MODULE_ENABLED
+#include "stm32f1xx_hal_smartcard.h"
+#endif /* HAL_SMARTCARD_MODULE_ENABLED */
+
+#ifdef HAL_WWDG_MODULE_ENABLED
+#include "stm32f1xx_hal_wwdg.h"
+#endif /* HAL_WWDG_MODULE_ENABLED */
+
+#ifdef HAL_PCD_MODULE_ENABLED
+#include "stm32f1xx_hal_pcd.h"
+#endif /* HAL_PCD_MODULE_ENABLED */
+
+#ifdef HAL_HCD_MODULE_ENABLED
+#include "stm32f1xx_hal_hcd.h"
+#endif /* HAL_HCD_MODULE_ENABLED */
+
+#ifdef HAL_MMC_MODULE_ENABLED
+#include "stm32f1xx_hal_mmc.h"
+#endif /* HAL_MMC_MODULE_ENABLED */
+
+/* Exported macro ------------------------------------------------------------*/
+#ifdef  USE_FULL_ASSERT
+/**
+  * @brief  The assert_param macro is used for function's parameters check.
+  * @param  expr If expr is false, it calls assert_failed function
+  *         which reports the name of the source file and the source
+  *         line number of the call that failed.
+  *         If expr is true, it returns no value.
+  * @retval None
+  */
+#define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__))
+/* Exported functions ------------------------------------------------------- */
+void assert_failed(uint8_t* file, uint32_t line);
+#else
+#define assert_param(expr) ((void)0U)
+#endif /* USE_FULL_ASSERT */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __STM32F1xx_HAL_CONF_H */
+

2.Firmware/ICM42688P_HAL/Core/Inc/stm32f1xx_it.h

@@ -0,0 +1,67 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    stm32f1xx_it.h
+  * @brief   This file contains the headers of the interrupt handlers.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+ ******************************************************************************
+  */
+/* USER CODE END Header */
+
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __STM32F1xx_IT_H
+#define __STM32F1xx_IT_H
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* Private includes ----------------------------------------------------------*/
+/* USER CODE BEGIN Includes */
+
+/* USER CODE END Includes */
+
+/* Exported types ------------------------------------------------------------*/
+/* USER CODE BEGIN ET */
+
+/* USER CODE END ET */
+
+/* Exported constants --------------------------------------------------------*/
+/* USER CODE BEGIN EC */
+
+/* USER CODE END EC */
+
+/* Exported macro ------------------------------------------------------------*/
+/* USER CODE BEGIN EM */
+
+/* USER CODE END EM */
+
+/* Exported functions prototypes ---------------------------------------------*/
+void NMI_Handler(void);
+void HardFault_Handler(void);
+void MemManage_Handler(void);
+void BusFault_Handler(void);
+void UsageFault_Handler(void);
+void SVC_Handler(void);
+void DebugMon_Handler(void);
+void PendSV_Handler(void);
+void SysTick_Handler(void);
+void USART1_IRQHandler(void);
+/* USER CODE BEGIN EFP */
+
+/* USER CODE END EFP */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __STM32F1xx_IT_H */

2.Firmware/ICM42688P_HAL/Core/Inc/usart.h

@@ -0,0 +1,41 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    usart.h
+  * @brief   This file contains all the function prototypes for
+  *          the usart.c file
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __USART_H__
+#define __USART_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+	
+extern UART_HandleTypeDef huart1;
+
+void MX_USART1_UART_Init(void);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __USART_H__ */
+

2.Firmware/ICM42688P_HAL/Core/Src/accelerometer.c

@@ -0,0 +1,143 @@
+#include "accelerometer.h"
+
+
+typedef struct accCalibration_s
+{
+  Axis3i32 accSum;
+  Axis3i16 accZero;
+  stdev_t var[3];
+  uint16_t cycleCount;
+  int ok;
+  uint16_t accZeroFlashSign;
+} accCalibration_t;
+
+
+//软件二阶低通滤波参数（单位Hz）
+#define ACCEL_LPF_CUTOFF_FREQ 	200.0f
+
+//传感器对齐定义的机体坐标系的安装方向
+#define ACCEL_ALIGN		CW0_DEG
+
+//mpu6000初始化加速度量程为FSR_8G，ACC_1G_ADC = 65536 / (2 * 16) = 4096  
+#define ACC_1G_ADC	4096.0f 
+
+Axis3i16 accADCRaw;		//加速度原始AD数据
+Axis3i16 accADC;		//校准后的AD数据			
+Axis3f accf;			//转换单位为G的数据
+
+static accCalibration_t accCalibration;	//加速度校准结构体参数
+
+biquadFilter_t accFilterLPF[XYZ_AXIS_COUNT];//二阶低通滤波器
+
+
+
+void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
+{
+    accCalibration.cycleCount = calibrationCyclesRequired;
+}
+
+
+//执行校准
+static void performAcclerationCalibration(Axis3i16 accADCSample)
+{
+    for (int axis = 0; axis < 3; axis++) 
+    {
+        //复位数据
+        if (accCalibration.cycleCount == CALIBRATING_ACC_CYCLES) 
+        {
+            accCalibration.accSum.axis[axis] = 0;
+            devClear(&accCalibration.var[axis]);
+            
+        }
+
+        //陀螺数据累加
+        accCalibration.accSum.axis[axis] += accADCSample.axis[axis];
+        
+        //复位零偏 
+        accCalibration.accZero.axis[axis] = 0;
+
+        if (accCalibration.cycleCount == 1) 
+        {    
+            //校准完成
+            accCalibration.accZero.axis[axis] = (accCalibration.accSum.axis[axis] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES;
+            if(axis==2)
+            accCalibration.accZero.axis[axis] -= (int)ACC_1G_ADC;
+            accCalibration.ok = 0;   
+            //EEPROM_write(3);
+        }
+    }
+    accCalibration.cycleCount--;
+    
+}
+
+
+bool ACC_Calibration(void)
+{
+		if(accCalibration.ok == 0)
+			return 1;
+		return 0;
+}
+
+
+
+bool accInit(float accUpdateRate)
+{
+
+  //初始化软件二阶低通滤波
+  for (int axis = 0; axis < 3; axis++)
+  {
+    biquadFilterInitLPF(&accFilterLPF[axis], ACCEL_LPF_CUTOFF_FREQ , accUpdateRate);
+  }
+  accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
+  accCalibration.ok = 0;
+  
+  return true;
+}
+
+
+
+
+void accUpdate(Axis3f *acc)
+{
+		
+		//读取原始数据
+		accADCRaw.x= accX1();
+		accADCRaw.y= accY1();
+		accADCRaw.z= accZ1();  
+		
+		//传感器数据方向对齐
+		applySensorAlignment(accADCRaw.axis, accADCRaw.axis, CW90_DEG);
+		
+		//加速度计校准
+		if (accCalibration.ok==1) 
+		{
+			performAcclerationCalibration(accADCRaw);
+			return;
+		}
+		
+		//计算accADC值，减去零偏
+		accADC.x = accADCRaw.x - accCalibration.accZero.x;
+		accADC.y = accADCRaw.y - accCalibration.accZero.y;
+		accADC.z = accADCRaw.z - accCalibration.accZero.z;
+		
+		////板对齐
+		//applyBoardAlignment(accADC.axis);
+		
+		//转换为单位 g (9.8m/s^2)
+		accf.x = (float)accADC.x / ACC_1G_ADC;
+		accf.y = (float)accADC.y / ACC_1G_ADC;
+		accf.z = (float)accADC.z / ACC_1G_ADC;
+		
+//		//软件低通滤波
+//		for (int axis = 0; axis < 3; axis++) 
+//		{
+//			accf.axis[axis] = biquadFilterApply(&accFilterLPF[axis], accf.axis[axis]);
+//		}
+		
+		*acc = accf; 
+		 
+}
+
+
+
+

2.Firmware/ICM42688P_HAL/Core/Src/communicate.c

@@ -0,0 +1,11 @@
+#include "communicate.h"
+
+
+int fputc(int ch, FILE *f)
+{
+ uint8_t temp[1] = {ch};
+ HAL_UART_Transmit(&huart1, temp, 1, 2);
+ return ch;
+}
+
+

2.Firmware/ICM42688P_HAL/Core/Src/config_param.c

@@ -0,0 +1,19 @@
+#include "config_param.h"
+
+
+configParam_t configParam;
+
+void saveConfigAndNotify(void)
+{
+//	if (!ARMING_FLAG(ARMED)) //锁定状态才可以操作Flash
+//	{
+//		beeperConfirmationBeeps(1);	
+//		xSemaphoreGive(xSemaphore);
+//		semaphoreGiveTime = getSysTickCnt();
+//		ENABLE_STATE(FLASH_WRITING);
+//	}
+}
+
+
+
+

2.Firmware/ICM42688P_HAL/Core/Src/filter.c

@@ -0,0 +1,225 @@
+#include "filter.h"
+
+#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
+
+//2. 以高度为例 定义卡尔曼结构体并初始化参数
+//KFP KFP_X={0.02,0,0,0,0.001,0.005};
+
+/**
+ *卡尔曼滤波器
+ *@param KFP *kfp 卡尔曼结构体参数
+ *   float input 需要滤波的参数的测量值（即传感器的采集值）
+ *@return 滤波后的参数（最优值）
+ */
+ float kalmanFilter(KFP *kfp,float input)
+ {
+     //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
+     kfp->NOWP  = kfp->LastP + kfp->Q;
+     //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
+     kfp->Kg = kfp->NOWP  / (kfp->NOWP  + kfp->R);
+     //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
+     kfp->out = kfp->out + kfp->Kg * (input -kfp->out);//因为这一次的预测值就是上一次的输出值
+     //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
+     kfp->LastP = (1-kfp->Kg) * kfp->NOWP ;
+     return kfp->out;
+ }
+
+
+void KalmanInit(Kalman_t *Kalman) 
+{
+	  
+	  //角度方差
+    Kalman->Q_angle = 0.0f;
+	  //角速度方差
+    Kalman->Q_bias = 0.003f;
+	  //测量噪声
+    Kalman->R_measure = 0.03f;
+ 
+ 
+    Kalman->angle = 0.0f;
+    Kalman->bias = 0.0f;
+ 
+    //协方差矩阵
+    Kalman->P[0][0] = 0.0f;
+    Kalman->P[0][1] = 0.0f;
+    Kalman->P[1][0] = 0.0f;
+    Kalman->P[1][1] = 0.0f;
+};
+ 
+ 
+ 
+ 
+float Kalman_getAngle(Kalman_t *Kalman,float newAngle, float newRate, float dt) //https://zhuanlan.zhihu.com/p/343714747
+	
+{
+	                
+    float rate = newRate - Kalman->bias;//newRate：角速度  Kalman->bias：角速度偏差
+    Kalman->angle += dt * rate;//状态预测角度
+	  
+	  //协方差矩阵预测
+    Kalman->P[0][0] += dt * (dt*Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle);//角度与角速度的协方差
+    Kalman->P[0][1] -= dt * Kalman->P[1][1]; //角度与角度的协方差
+    Kalman->P[1][0] -= dt * Kalman->P[1][1]; //角速度与角速度的协方差
+    Kalman->P[1][1] += Kalman->Q_bias * dt;  //角速度与角度的协方差
+ 
+    //计算卡尔曼增益
+    float S = Kalman->P[0][0] + Kalman->R_measure;
+    float K[2];
+    K[0] = Kalman->P[0][0] / S;//角度增益
+    K[1] = Kalman->P[1][0] / S;//角速度偏差增益
+ 
+    //状态更新
+    float y = newAngle - Kalman->angle;//测量角度-预测角度
+    Kalman->angle += K[0] * y;//角度 
+    Kalman->bias += K[1] * y;//角速度偏差
+ 
+    
+    float P00_temp = Kalman->P[0][0];
+    float P01_temp = Kalman->P[0][1];
+ 
+    //协方差矩阵更新
+    Kalman->P[0][0] -= K[0] * P00_temp; //P[0][0] = P[0][0] - （角度增益 * P[0][0]）
+    Kalman->P[0][1] -= K[0] * P01_temp;
+    Kalman->P[1][0] -= K[1] * P00_temp;
+    Kalman->P[1][1] -= K[1] * P01_temp;//P[1][1] = P[1][1] - K[1] * P[0][1]
+ 
+ 
+    return Kalman->angle;
+};
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+// PT1低通滤波器
+//pt1获取滤波增益(截止频率 采样时间)
+float pt1FilterGain(float f_cut, float dT)
+{
+    float RC = 1 / (2 * M_PIf * f_cut);
+    return dT / (RC + dT);
+}
+
+//pt1初始化低通滤波器
+void pt1FilterInit(pt1Filter_t *filter, float k)
+{
+    filter->state = 0.0f;
+    filter->k = k;
+}
+
+//更新滤波增益
+void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k)
+{
+    filter->k = k;
+}
+
+//pt1低通滤波器应用
+float pt1FilterApply(pt1Filter_t *filter, float input)
+{
+    filter->state = filter->state + filter->k * (input - filter->state);
+    return filter->state;
+}
+
+
+//获取陷波滤波器Q给定的中心频率(f0)和较低的截止频率(f1)
+//Q = f0 / (f2 - f1);F2 = f02/ f1
+//（中心频率 截止频率）
+float filterGetNotchQ(float centerFreq, float cutoffFreq)
+{
+    return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq);
+}
+
+//二阶低通滤波器初始化
+void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq ,uint16_t samplingFreq)
+{
+    biquadFilterInit(filter, filterFreq, samplingFreq,  BIQUAD_Q, FILTER_LPF);
+}
+
+
+void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
+{
+    biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType);
+
+    // zero initial samples
+    filter->x1 = filter->x2 = 0;
+    filter->y1 = filter->y2 = 0;
+}
+
+
+
+
+//二阶滤波器初始化
+void biquadFilterUpdate(biquadFilter_t *filter, uint16_t filterFreq, uint16_t refreshRate, float Q, biquadFilterType_e filterType)
+{
+    // setup variables
+    const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f;
+    const float sn = sin_approx(omega);
+    const float cs = cos_approx(omega);
+    const float alpha = sn / (2.0f * Q);
+
+    switch (filterType) {
+    case FILTER_LPF:
+        // 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
+        // described in http://www.ti.com/lit/an/slaa447/slaa447.pdf
+        filter->b1 = 1 - cs;
+        filter->b0 = filter->b1 * 0.5f;
+        filter->b2 = filter->b0;
+        filter->a1 = -2 * cs;
+        filter->a2 = 1 - alpha;
+        break;
+    case FILTER_NOTCH:
+        filter->b0 = 1;
+        filter->b1 = -2 * cs;
+        filter->b2 = 1;
+        filter->a1 = filter->b1;
+        filter->a2 = 1 - alpha;
+        break;
+    case FILTER_BPF:
+        filter->b0 = alpha;
+        filter->b1 = 0;
+        filter->b2 = -alpha;
+        filter->a1 = -2 * cs;
+        filter->a2 = 1 - alpha;
+        break;
+    }
+
+    const float a0 = 1 + alpha;
+
+    // precompute the coefficients
+    filter->b0 /= a0;
+    filter->b1 /= a0;
+    filter->b2 /= a0;
+    filter->a1 /= a0;
+    filter->a2 /= a0;
+
+    // update weight
+    ///filter->weight = weight;
+}
+
+
+
+
+//二阶陷波器初始化(结构体变量 采样频率 中心频率 截止频率)
+void biquadFilterInitNotch(biquadFilter_t *filter, uint16_t samplingFreq, uint16_t filterFreq, uint16_t cutoffHz)
+{
+    float Q = filterGetNotchQ(filterFreq, cutoffHz);
+    biquadFilterInit(filter, samplingFreq, filterFreq, Q, FILTER_NOTCH);
+}
+
+
+
+
+
+/* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
+float biquadFilterApply(biquadFilter_t *filter, float input)
+{
+    const float result = filter->b0 * input + filter->x1;
+
+    filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
+    filter->x2 = filter->b2 * input - filter->a2 * result;
+
+    return result;
+}
+
+

2.Firmware/ICM42688P_HAL/Core/Src/gpio.c

@@ -0,0 +1,31 @@
+/******************** (C) COPYRIGHT 2020 GEEKIMU *******************************
+* File Name          : gpio.c
+* Current Version    : V1.0  & HAL
+* Author             : zhanli 719901725@qq.com
+* Date of Issued     : 2025.02.20 zhanli : Create
+* Comments           : MPU6500   
+********************************************************************************/
+#include "gpio.h"
+
+/** Configure pins
+*/
+void GPIO_Init(void)
+{
+
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+
+  /* GPIO Ports Clock Enable */
+	__HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOC_CLK_ENABLE();
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(SPI2_CS_GPIO_Port, SPI2_CS_Pin, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin : PtPin */
+  GPIO_InitStruct.Pin = SPI2_CS_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(SPI2_CS_GPIO_Port, &GPIO_InitStruct);
+}

2.Firmware/ICM42688P_HAL/Core/Src/gyro.c

@@ -0,0 +1,172 @@
+#include "gyro.h"
+
+
+
+#define GYRO_CALIBRATION_THRESHOLD	4.0f          /* 陀螺仪校准阈值 */
+
+#define GYRO_LPF_CUTOFF_FREQ  	    200.0f        /* 低通滤波参数 */
+
+/*传感器对齐定义的机体坐标系的安装方向*/
+#define GYRO_ALIGN		CW0_DEG
+#define GYRO_SCALE	  16.384f
+
+
+//陀螺仪校准结构体参数
+gyroCalibration_t gyroCalibration = 
+{
+	.cycleCount = CALIBRATING_GYRO_CYCLES,
+};	
+
+Axis3i16 gyroADCRaw;	//陀螺仪原始AD数据
+Axis3i16 gyroADC;		//校准的AD数据
+Axis3f gyrof;			//转换单位为°/s的数据
+
+
+biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];//二阶低通滤波器
+
+
+void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
+{
+    gyroCalibration.cycleCount = calibrationCyclesRequired;
+}
+
+
+void performGyroCalibration(Axis3i16 gyroADCSample)
+{
+    for (int axis = 0; axis < 3; axis++) 
+    {	
+        //复位数据
+        if (gyroCalibration.cycleCount == CALIBRATING_GYRO_CYCLES) 
+        {
+            gyroCalibration.gyroSum.axis[axis] = 0;
+            devClear(&gyroCalibration.var[axis]);
+            
+        }
+
+        //陀螺数据累加
+        gyroCalibration.gyroSum.axis[axis] += gyroADCSample.axis[axis];
+        devPush(&gyroCalibration.var[axis], gyroADCSample.axis[axis]);
+
+        //复位零偏 
+        gyroCalibration.gyroZero.axis[axis] = 0;
+        printf("   cycleCount:%d     \r\n ",gyroCalibration.cycleCount);	
+        if (gyroCalibration.cycleCount == 1) 
+        {
+            const float stddev = devStandardDeviation(&gyroCalibration.var[axis]);
+            //检测方差值是否大于陀螺仪受到移动的阈值
+            //如果大于设定阈值则返回重新校准
+				printf("   axis:%d    ",axis);	
+				printf("   stddev:%f    ",stddev);	
+				printf(" \r\n ");		
+				
+            if ((stddev > GYRO_CALIBRATION_THRESHOLD) || (stddev == 0)) 
+            {
+                gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+                gyroCalibration.ok = 1;
+                return;
+            }
+      
+            //校准完成
+            gyroCalibration.gyroZero.axis[axis] = gyroCalibration.gyroSum.axis[axis] / CALIBRATING_GYRO_CYCLES;
+            gyroCalibration.ok = 0;
+				gyroCalibration.gyroZeroFlashSign = 111;
+				printf("   gyroCalibration.gyroSum.axis[axis]:%d    ",gyroCalibration.gyroSum.axis[axis]);	
+				printf("   gyroZerox:%d    ",gyroCalibration.gyroZero.axis[X]);	
+				printf("   gyroZeroy:%d    ",gyroCalibration.gyroZero.axis[Y]);	
+				printf("   gyroZeroz:%d    ",gyroCalibration.gyroZero.axis[Z]);	
+				printf(" \r\n ");	
+				
+				
+						
+
+        }
+    }
+  
+    gyroCalibration.cycleCount--;
+		
+		if(gyroCalibration.cycleCount==0)
+		{
+				gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);		
+		}
+}
+
+bool GYRO_Calibration(void)
+{
+		if(gyroCalibration.ok == 0)
+			return 1;
+		return 0;
+}
+
+void GYRO_Calibration_write(uint8_t data)
+{
+		gyroCalibration.ok = data;
+}
+
+bool gyroInit(float gyroUpdateRate)//
+{
+  //初始化陀螺仪零偏校准
+  gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
+  gyroCalibration.ok = 0;// 1 校准陀螺仪
+  //初始化二阶低通滤波
+  for (int axis = 0; axis < 3; axis++)
+  {
+    biquadFilterInitLPF(&gyroFilterLPF[axis],GYRO_LPF_CUTOFF_FREQ, gyroUpdateRate);
+    
+  }
+  return true;
+}
+
+
+void gyroUpdate(Axis3f *gyro)
+{
+		if(getAGT()>=0)//读取原始数据
+		{
+				gyroADCRaw.x=  gyrX1();		
+				gyroADCRaw.y=  gyrY1();
+				gyroADCRaw.z=  gyrZ1();
+			
+		}
+
+		//传感器数据方向对齐
+		applySensorAlignment(gyroADCRaw.axis, gyroADCRaw.axis, CW90_DEG);
+		//陀螺仪校准
+		if (gyroCalibration.ok==1) 
+		{
+			performGyroCalibration(gyroADCRaw);
+			
+			gyrof.x = 0.0f;
+			gyrof.y = 0.0f;
+			gyrof.z = 0.0f;
+			*gyro = gyrof;
+			return;
+		}
+
+		//计算gyroADC值，减去零偏
+		gyroADC.x = gyroADCRaw.x - gyroCalibration.gyroZero.x;
+		gyroADC.y = gyroADCRaw.y - gyroCalibration.gyroZero.y;
+		gyroADC.z = gyroADCRaw.z - gyroCalibration.gyroZero.z; 
+		
+		//板对齐
+		// applyBoardAlignment(gyroADC.axis);
+		
+		//转换为单位 °/s 
+		gyrof.x = (float)gyroADC.x / GYRO_SCALE;
+		gyrof.y = (float)gyroADC.y / GYRO_SCALE;
+		gyrof.z = (float)gyroADC.z / GYRO_SCALE;
+		
+				//printf("   Xs:%f    ",gyrof.x);		
+				//printf("   Ys:%f    ",gyrof.y);	
+		//软件二阶低通滤波
+//		for (int axis = 0; axis < 3; axis++) 
+//		{
+//			gyrof.axis[axis] = biquadFilterApply(&gyroFilterLPF[axis], gyrof.axis[axis]);
+//		}   
+		
+		*gyro = gyrof;      
+                     
+}
+
+
+
+
+

2.Firmware/ICM42688P_HAL/Core/Src/icm42688p.c

@@ -0,0 +1,758 @@
+#include "icm42688p.h"
+
+
+#define ICM42xxx_HSPI hspi2
+#define ICM42xxx_BUFFER_LEN 16
+
+sGyroConfigStatic9_t gyroConfigStatic9;
+sGyroConfigStatic5_t gyroConfigStatic5;
+sGyroConfigStatic2_t gyroConfigStatic2;
+
+sAccelConfigStatic2_t accelConfigStatic2;
+sAccelConfigStatic4_t accelConfigStatic4;
+
+sGyroConfig1_t  gyroConfig1;
+sGyroAccelConfig0_t gyroAccelConfig0;
+sGyroConfig0_t gyroConfig0;
+sAccelConfig1_t accelConfig1;
+sAccelConfig0_t accelConfig0;
+
+
+//float _gyroRange;
+//float _accelRange;
+
+///\brief Conversion formula to get temperature in Celsius (Sec 4.13)
+static  float TEMP_DATA_REG_SCALE = 132.48f;
+static  float TEMP_OFFSET = 25.0f;
+
+///\brief Accel calibration简短加速校准
+float _accBD[3] = {0};
+float _accB[3] = {0};
+float _accS[3] = {1.0f, 1.0f, 1.0f};
+float _accMax[3] = {0};
+float _accMin[3] = {0};
+
+// data buffer
+float _t = 0.0f;
+float _acc[3] = {0};
+float _gyr[3] = {0};
+float _acc1[3] = {0};
+float _gyr1[3] = {0};
+
+///\brief Gyro calibration陀螺简易校准
+float _gyroBD[3] = {0};
+float _gyrB[3] = {0};
+	
+static int NUM_CALIB_SAMPLES = 1000; ///< for gyro/accel bias calib用于陀螺/加速度偏置校准
+
+///\brief Full scale resolution factors简短的全比例尺分辨率因子
+float _accelScale = 0.0f;
+float _gyroScale = 0.0f;
+
+///\brief Full scale selections简短的全尺寸选择
+enum AccelFS _accelFS;
+enum GyroFS _gyroFS;
+
+
+uint8_t icm42xxx_spi_tx [ICM42xxx_BUFFER_LEN] ={0xff};
+uint8_t icm42xxx_spi_rx [ICM42xxx_BUFFER_LEN] ={0xff};
+// buffer for reading from sensor
+uint8_t _buffer[15] = {0};
+uint8_t _bank = 0; ///< current user bank
+	
+
+
+
+/* writes a byte to ICM42688 register given a register address and data */
+int writeRegister(uint8_t subAddress, uint8_t data) 
+{
+	SPI_CS_Enable();   //将片选线置低，告诉从机开始通信
+	
+	icm42xxx_spi_tx[0] = subAddress & 0x7F;	// 写入时，寄存器地址字节的第一位为 0；0x7f <-> 01111111
+	icm42xxx_spi_tx[1] = data;	// MOSI上第二个字节为写入的数据
+	
+	HAL_SPI_TransmitReceive(&ICM42xxx_HSPI,&icm42xxx_spi_tx[0],&icm42xxx_spi_rx[0], 2,0XFFFF);
+  
+	SPI_CS_Disable();     //将片选线重新置高，告诉从机结束通信
+  
+	ICM42688_readRegisters(subAddress,_buffer,1);
+  /* check the read back register against the written register */
+  if(_buffer[0] == data) 
+	{
+    return 1;
+  }
+  else
+	{
+    return -1;
+  }
+}
+
+int setBank(uint8_t bank) 
+{
+  // if we are already on this bank, bail
+  if (_bank == bank) return 1;
+
+  _bank = bank;
+
+  return writeRegister(REG_BANK_SEL, bank);
+}
+
+
+void ICM42688Reset() //复位
+{ 
+  setBank(0);
+  writeRegister(UB0_REG_DEVICE_CONFIG, 0x01);
+  // wait for ICM42688 to come back up
+  HAL_Delay(1);
+}
+
+/* reads registers from ICM42688 given a starting register address, number of bytes, and a pointer to store data */
+int ICM42688_readRegisters(uint8_t subAddress, uint8_t* dest,uint8_t len) 
+{
+	if(len>ICM42xxx_BUFFER_LEN-1)
+			return -1;	// 超出缓存空间！	
+
+  /* write data to device */
+	SPI_CS_Enable();   //将片选线置低，告诉从机开始通信
+	
+	icm42xxx_spi_tx[0] = subAddress | 0x80;	// 读取时，寄存器地址字节的第一位为 1
+	memset(icm42xxx_spi_tx+1,0x00,len);	// 接收数据时，注意MOSI上不能再发地址，否则会破坏突发读的地址连续性
+	if( HAL_SPI_TransmitReceive(&ICM42xxx_HSPI,&icm42xxx_spi_tx[0],&icm42xxx_spi_rx[0], len+1,0XFF)!= HAL_OK) //因为先发地址后读数据，因此这里长度是len+1
+	{
+		return -2;	// SPI读取操作出错
+	}
+	
+	SPI_CS_Disable();     //将片选线重新置高，告诉从机结束通信
+	memcpy(dest,icm42xxx_spi_rx+1,len);
+
+  return 0;
+  
+}
+
+
+///* sets the accelerometer full scale range to values other than default *///将加速度计满量程设置为默认值*/以外的值
+int setAccelFS(enum AccelFS fssel) 
+{
+
+  setBank(0);
+
+  // read current register value
+  uint8_t reg;
+  if (ICM42688_readRegisters(UB0_REG_ACCEL_CONFIG0,&reg,1) < 0) return -1;
+
+  // only change FS_SEL in reg
+  reg = (fssel << 5) | (reg & 0x1F);
+
+  if (writeRegister(UB0_REG_ACCEL_CONFIG0, reg) < 0) return -2;
+
+  _accelScale = (float)(1 << (4 - fssel)) / 32768.0f;
+  _accelFS = fssel;
+
+  return 1;
+}
+
+
+/* sets the gyro full scale range to values other than default */
+int setGyroFS(enum GyroFS fssel) {
+
+
+  setBank(0);
+
+  // read current register value
+  uint8_t reg;
+  if (ICM42688_readRegisters(UB0_REG_GYRO_CONFIG0,&reg,1) < 0) return -1;
+
+  // only change FS_SEL in reg
+  reg = (fssel << 5) | (reg & 0x1F);
+
+  if (writeRegister(UB0_REG_GYRO_CONFIG0, reg) < 0) return -2;
+
+  _gyroScale = (2000.0f / (float)(1 << fssel)) / 32768.0f;
+  _gyroFS = fssel;
+  return 1;
+}
+
+int setFilters(uint8_t gyroFilters, uint8_t accFilters) 
+{
+  if (setBank(1) < 0) return -1;
+
+  if (gyroFilters == 1) 
+	{
+    if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, GYRO_NF_ENABLE | GYRO_AAF_ENABLE) < 0) 
+		{
+      return -2;
+    }
+  }
+  else 
+	{
+    if (writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, GYRO_NF_DISABLE | GYRO_AAF_DISABLE) < 0) 
+		{
+      return -3;
+    }
+  }
+  
+  if (setBank(2) < 0) return -4;
+
+  if (accFilters == 1) 
+	{
+    if (writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, ACCEL_AAF_ENABLE) < 0) 
+		{
+      return -5;
+    }
+  }
+  else 
+	{
+    if (writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, ACCEL_AAF_DISABLE) < 0) 
+		{
+      return -6;
+    }
+  }
+  if (setBank(0) < 0) return -7;
+  return 1;
+}
+
+
+/* reads the most current data from ICM42688 and stores in buffer */
+int getAGT(void) 
+{
+ 
+  // grab the data from the ICM42688
+  if (ICM42688_readRegisters(UB0_REG_TEMP_DATA1,_buffer,14) < 0) return -1;
+
+  // combine bytes into 16 bit values
+  int16_t rawMeas[7]; // temp, accel xyz, gyro xyz
+  for (size_t i=0; i<7; i++) {
+    rawMeas[i] = ((int16_t)_buffer[i*2] << 8) | _buffer[i*2+1];
+  }
+
+  _t = ((float)(rawMeas[0]) / TEMP_DATA_REG_SCALE) + TEMP_OFFSET;
+
+  _acc[0] = ((rawMeas[1] * _accelScale) - _accB[0]) * _accS[0];
+  _acc[1] = ((rawMeas[2] * _accelScale) - _accB[1]) * _accS[1];
+  _acc[2] = ((rawMeas[3] * _accelScale) - _accB[2]) * _accS[2];
+
+  _acc1[0] = (rawMeas[1] - _accB[0]) * _accS[0];
+  _acc1[1] = (rawMeas[2] - _accB[1]) * _accS[1];
+  _acc1[2] = (rawMeas[3] - _accB[2]) * _accS[2];
+
+  _gyr[0] = (rawMeas[4] * _gyroScale) - _gyrB[0];
+  _gyr[1] = (rawMeas[5] * _gyroScale) - _gyrB[1];
+  _gyr[2] = (rawMeas[6] * _gyroScale) - _gyrB[2];
+
+  _gyr1[0] = rawMeas[4] - _gyrB[0];
+  _gyr1[1] = rawMeas[5] - _gyrB[1];
+  _gyr1[2] = rawMeas[6] - _gyrB[2];
+	
+  return 1;
+}
+
+
+/* estimates the gyro biases *///估计陀螺偏差
+int calibrateGyro(void) {
+  // set at a lower range (more resolution) since IMU not moving设置在较低的范围(更高的分辨率)，因为IMU不移动
+  const enum GyroFS current_fssel = _gyroFS;
+  if (setGyroFS(dps250) < 0) return -1;
+
+  // take samples and find bias抽取样本，找出偏差
+  _gyroBD[0] = 0;
+  _gyroBD[1] = 0;
+  _gyroBD[2] = 0;
+  for (size_t i=0; i < NUM_CALIB_SAMPLES; i++) {
+    getAGT();
+    _gyroBD[0] += (gyrX() + _gyrB[0]) / NUM_CALIB_SAMPLES;
+    _gyroBD[1] += (gyrY() + _gyrB[1]) / NUM_CALIB_SAMPLES;
+    _gyroBD[2] += (gyrZ() + _gyrB[2]) / NUM_CALIB_SAMPLES;
+    HAL_Delay(1);
+  }
+  _gyrB[0] = _gyroBD[0];
+  _gyrB[1] = _gyroBD[1];
+  _gyrB[2] = _gyroBD[2];
+
+  // recover the full scale setting
+  if (setGyroFS(current_fssel) < 0) return -4;
+  return 1;
+}
+
+
+void setGyroNotchFilterFHz(double freq, uint8_t axis) //設置陷波器
+{
+  uint8_t bank = 1;
+	writeRegister(REG_BANK_SEL, bank);
+  double fdesired = freq * 1000;
+  double coswz = cos(2 * 3.14 * fdesired / 32);
+  int16_t nfCoswz;
+  uint8_t nfCoswzSel;
+  if (fabs(coswz) <= 0.875) {
+    nfCoswz = round(coswz * 256);
+    nfCoswzSel = 0;
+  } else {
+    nfCoswzSel = 1;
+    if (coswz > 0.875) {
+      nfCoswz = round(8 * (1 - coswz) * 256);
+    } else if (coswz < -0.875) {
+      nfCoswz = round(-8 * (1 + coswz) * 256);
+    }
+  }
+	uint8_t * _pBuf; 
+  if (axis == X_AXIS) {
+    gyroConfigStatic9.gyroNFCoswzSelX = nfCoswzSel;
+    gyroConfigStatic9.gyroNFCoswzX8 = nfCoswz >> 8;
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC6, nfCoswz);
+		_pBuf = (uint8_t *) &gyroConfigStatic9;	
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC9, _pBuf[0]);
+  } else if (axis == Y_AXIS) {
+    gyroConfigStatic9.gyroNFCoswzSelY = nfCoswzSel;
+    gyroConfigStatic9.gyroNFCoswzY8 = nfCoswz >> 8;
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC7, nfCoswz);
+		_pBuf = (uint8_t *) &gyroConfigStatic9;	
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC9, _pBuf[0]);
+  } else if (axis == Z_AXIS) {
+    gyroConfigStatic9.gyroNFCoswzSelZ = nfCoswzSel;
+    gyroConfigStatic9.gyroNFCoswzZ8 = nfCoswz >> 8;
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC8, nfCoswz);
+		_pBuf = (uint8_t *) &gyroConfigStatic9;	
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC9, _pBuf[0]);
+  } else if (axis == ALL)
+  {
+    gyroConfigStatic9.gyroNFCoswzSelX = nfCoswzSel;
+    gyroConfigStatic9.gyroNFCoswzX8 = nfCoswz >> 8;
+    gyroConfigStatic9.gyroNFCoswzSelY = nfCoswzSel;
+    gyroConfigStatic9.gyroNFCoswzY8 = nfCoswz >> 8;
+    gyroConfigStatic9.gyroNFCoswzSelZ = nfCoswzSel;
+    gyroConfigStatic9.gyroNFCoswzZ8 = nfCoswz >> 8;
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC6, nfCoswz);
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC7, nfCoswz);
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC8, nfCoswz);
+		_pBuf = (uint8_t *) &gyroConfigStatic9;	
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC9, _pBuf[0]);
+  }
+  bank = 0;
+  writeRegister(REG_BANK_SEL, bank);
+}
+
+void setGyroNFbandwidth(uint8_t bw)
+{
+  uint8_t bank = 1;
+  writeRegister(REG_BANK_SEL, bank );
+  uint8_t bandWidth = (bw << 4) | 0x01;
+  writeRegister(UB1_REG_GYRO_CONFIG_STATIC10, bandWidth );
+  bank = 0;
+  writeRegister(REG_BANK_SEL, bank );
+}
+
+
+void setGyroNotchFilter(uint8_t mode)
+{
+  if (mode==1) {
+    gyroConfigStatic2.gyroNFDis = 0;
+  } else {
+    gyroConfigStatic2.gyroNFDis = 1;
+  }
+  uint8_t bank = 1;
+	uint8_t * _pBuf; 
+  writeRegister(REG_BANK_SEL, bank );
+	_pBuf = (uint8_t *) &gyroConfigStatic9;	
+  writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, _pBuf[0] );
+  bank = 0;
+  writeRegister(REG_BANK_SEL, bank );
+}
+
+
+
+void setAAFBandwidth(uint8_t who, uint8_t BWIndex) //設置二階濾波器帶寬
+{
+  uint8_t bank = 0;
+	uint8_t * _pBuf; 
+  uint16_t AAFDeltsqr = BWIndex * BWIndex;
+  if (who == GYRO) {
+    bank = 1;
+    writeRegister(REG_BANK_SEL, bank);
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC3, BWIndex);
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC4, AAFDeltsqr);
+    gyroConfigStatic5.gyroAAFDeltsqr = AAFDeltsqr >> 8;
+    if (BWIndex == 1) {
+      gyroConfigStatic5.gyroAAFBitshift = 15;
+    } else if (BWIndex == 2) {
+      gyroConfigStatic5.gyroAAFBitshift = 13;
+    } else if (BWIndex == 3) {
+      gyroConfigStatic5.gyroAAFBitshift = 12;
+    } else if (BWIndex == 4) {
+      gyroConfigStatic5.gyroAAFBitshift = 11;
+    } else if (BWIndex == 5 || BWIndex == 6) {
+      gyroConfigStatic5.gyroAAFBitshift = 10;
+    } else if (BWIndex > 6 && BWIndex < 10) {
+      gyroConfigStatic5.gyroAAFBitshift = 9;
+    } else if (BWIndex > 9 && BWIndex < 14) {
+      gyroConfigStatic5.gyroAAFBitshift = 8;
+    } else if (BWIndex > 13 && BWIndex < 19) {
+      gyroConfigStatic5.gyroAAFBitshift = 7;
+    } else if (BWIndex > 18 && BWIndex < 27) {
+      gyroConfigStatic5.gyroAAFBitshift = 6;
+    } else if (BWIndex > 26 && BWIndex < 37) {
+      gyroConfigStatic5.gyroAAFBitshift = 5;
+    } else if (BWIndex > 36 && BWIndex < 53) {
+      gyroConfigStatic5.gyroAAFBitshift = 4;
+    } else if (BWIndex > 53 && BWIndex <= 63) {
+      gyroConfigStatic5.gyroAAFBitshift = 3;
+    }
+		_pBuf = (uint8_t *) &gyroConfigStatic5;	
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC5, _pBuf[0]);
+    bank = 0;
+    writeRegister(REG_BANK_SEL, bank);
+  } else if (who == ACCEL) {
+    bank = 2;
+    writeRegister(REG_BANK_SEL, bank);
+    accelConfigStatic2.accelAAFDelt = BWIndex;
+		_pBuf = (uint8_t *) &accelConfigStatic2;	
+    writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, _pBuf[0]);
+    writeRegister(UB2_REG_ACCEL_CONFIG_STATIC3, AAFDeltsqr);
+    accelConfigStatic4.accelAAFDeltsqr = AAFDeltsqr >> 8;
+    if (BWIndex == 1) {
+      accelConfigStatic4.accelAAFBitshift = 15;
+    } else if (BWIndex == 2) {
+      accelConfigStatic4.accelAAFBitshift = 13;
+    } else if (BWIndex == 3) {
+      accelConfigStatic4.accelAAFBitshift = 12;
+    } else if (BWIndex == 4) {
+      accelConfigStatic4.accelAAFBitshift = 11;
+    } else if (BWIndex == 5 || BWIndex == 6) {
+      accelConfigStatic4.accelAAFBitshift = 10;
+    } else if (BWIndex > 6 && BWIndex < 10) {
+      accelConfigStatic4.accelAAFBitshift = 9;
+    } else if (BWIndex > 9 && BWIndex < 14) {
+      accelConfigStatic4.accelAAFBitshift = 8;
+    } else if (BWIndex > 13 && BWIndex < 19) {
+      accelConfigStatic4.accelAAFBitshift = 7;
+    } else if (BWIndex > 18 && BWIndex < 27) {
+      accelConfigStatic4.accelAAFBitshift = 6;
+    } else if (BWIndex > 26 && BWIndex < 37) {
+      accelConfigStatic4.accelAAFBitshift = 5;
+    } else if (BWIndex > 36 && BWIndex < 53) {
+      accelConfigStatic4.accelAAFBitshift = 4;
+    } else if (BWIndex > 53 && BWIndex <= 63) {
+      accelConfigStatic4.accelAAFBitshift = 3;
+    }
+		_pBuf = (uint8_t *) &accelConfigStatic4;
+    writeRegister(UB2_REG_ACCEL_CONFIG_STATIC4, _pBuf[0]);
+
+    bank = 0;
+    writeRegister(REG_BANK_SEL, bank);
+  } else if (who == ALL) {
+    bank = 1;
+    writeRegister(REG_BANK_SEL, bank );
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC3, BWIndex );
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC4, AAFDeltsqr );
+    gyroConfigStatic5.gyroAAFDeltsqr = AAFDeltsqr >> 8;
+    if (BWIndex == 1) {
+      gyroConfigStatic5.gyroAAFBitshift = 15;
+    } else if (BWIndex == 2) {
+      gyroConfigStatic5.gyroAAFBitshift = 13;
+    } else if (BWIndex == 3) {
+      gyroConfigStatic5.gyroAAFBitshift = 12;
+    } else if (BWIndex == 4) {
+      gyroConfigStatic5.gyroAAFBitshift = 11;
+    } else if (BWIndex == 5 || BWIndex == 6) {
+      gyroConfigStatic5.gyroAAFBitshift = 10;
+    } else if (BWIndex > 6 && BWIndex < 10) {
+      gyroConfigStatic5.gyroAAFBitshift = 9;
+    } else if (BWIndex > 9 && BWIndex < 14) {
+      gyroConfigStatic5.gyroAAFBitshift = 8;
+    } else if (BWIndex > 13 && BWIndex < 19) {
+      gyroConfigStatic5.gyroAAFBitshift = 7;
+    } else if (BWIndex > 18 && BWIndex < 27) {
+      gyroConfigStatic5.gyroAAFBitshift = 6;
+    } else if (BWIndex > 26 && BWIndex < 37) {
+      gyroConfigStatic5.gyroAAFBitshift = 5;
+    } else if (BWIndex > 36 && BWIndex < 53) {
+      gyroConfigStatic5.gyroAAFBitshift = 4;
+    } else if (BWIndex > 53 && BWIndex <= 63) {
+      gyroConfigStatic5.gyroAAFBitshift = 3;
+    }
+		_pBuf = (uint8_t *) &gyroConfigStatic5;
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC5, _pBuf[0]);
+    bank = 2;
+    writeRegister(REG_BANK_SEL, bank );
+    accelConfigStatic2.accelAAFDelt = BWIndex;
+		_pBuf = (uint8_t *) &accelConfigStatic2;
+    writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, _pBuf[0] );
+    writeRegister(UB2_REG_ACCEL_CONFIG_STATIC3, AAFDeltsqr );
+    accelConfigStatic4.accelAAFDeltsqr = AAFDeltsqr >> 8;
+    if (BWIndex == 1) {
+      accelConfigStatic4.accelAAFBitshift = 15;
+    } else if (BWIndex == 2) {
+      accelConfigStatic4.accelAAFBitshift = 13;
+    } else if (BWIndex == 3) {
+      accelConfigStatic4.accelAAFBitshift = 12;
+    } else if (BWIndex == 4) {
+      accelConfigStatic4.accelAAFBitshift = 11;
+    } else if (BWIndex == 5 || BWIndex == 6) {
+      accelConfigStatic4.accelAAFBitshift = 10;
+    } else if (BWIndex > 6 && BWIndex < 10) {
+      accelConfigStatic4.accelAAFBitshift = 9;
+    } else if (BWIndex > 9 && BWIndex < 14) {
+      accelConfigStatic4.accelAAFBitshift = 8;
+    } else if (BWIndex > 13 && BWIndex < 19) {
+      accelConfigStatic4.accelAAFBitshift = 7;
+    } else if (BWIndex > 18 && BWIndex < 27) {
+      accelConfigStatic4.accelAAFBitshift = 6;
+    } else if (BWIndex > 26 && BWIndex < 37) {
+      accelConfigStatic4.accelAAFBitshift = 5;
+    } else if (BWIndex > 36 && BWIndex < 53) {
+      accelConfigStatic4.accelAAFBitshift = 4;
+    } else if (BWIndex > 53 && BWIndex <= 63) {
+      accelConfigStatic4.accelAAFBitshift = 3;
+    }
+		_pBuf = (uint8_t *) &accelConfigStatic4;
+    writeRegister(UB2_REG_ACCEL_CONFIG_STATIC4, _pBuf[0] );
+    bank = 0;
+    writeRegister(REG_BANK_SEL, bank );
+  }
+}
+
+
+
+void setAAF(uint8_t who, uint8_t mode)
+{
+  uint8_t bank = 0;
+	uint8_t * _pBuf; 
+  if (who == GYRO) {
+    if (mode==1) {
+      gyroConfigStatic2.gyroAAFDis = 0;
+    } else {
+      gyroConfigStatic2.gyroAAFDis = 1;
+    }
+    bank = 1;
+    writeRegister(REG_BANK_SEL, bank );
+		_pBuf = (uint8_t *) &gyroConfigStatic2;
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, _pBuf[0] );
+  } else if (who == ACCEL) {
+    if (mode==1) {
+      accelConfigStatic2.accelAAFDis = 0;
+    } else {
+      accelConfigStatic2.accelAAFDis = 1;
+    }
+    bank = 2;
+    writeRegister(REG_BANK_SEL, bank );
+		_pBuf = (uint8_t *) &accelConfigStatic2;
+    writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, _pBuf[0] );
+  } else if (who == ALL) {
+    if (mode==1) {
+      gyroConfigStatic2.gyroAAFDis = 0;
+      accelConfigStatic2.accelAAFDis = 0;
+    } else {
+      gyroConfigStatic2.gyroAAFDis = 1;
+      accelConfigStatic2.accelAAFDis = 1;
+    }
+    bank = 1;
+    writeRegister(REG_BANK_SEL, bank);
+		_pBuf = (uint8_t *) &gyroConfigStatic2;
+    writeRegister(UB1_REG_GYRO_CONFIG_STATIC2, _pBuf[0]);
+    bank = 2;
+    writeRegister(REG_BANK_SEL, bank);
+		_pBuf = (uint8_t *) &accelConfigStatic2;
+    writeRegister(UB2_REG_ACCEL_CONFIG_STATIC2, _pBuf[0]);
+  }
+  bank = 0;
+  writeRegister(REG_BANK_SEL, bank);
+}
+
+
+int setUIFilter(uint8_t who, uint8_t filterOrder , uint8_t UIFilterIndex)
+{
+  int ret = 0;
+	uint8_t * _pBuf; 
+  setBank(0);
+  if (filterOrder > 3 || UIFilterIndex > 15) {
+    ret = -1;
+  } else {
+    if (who == GYRO) {
+      gyroConfig1.gyroUIFiltODR = filterOrder;
+			_pBuf = (uint8_t *) &gyroConfig1;
+      writeRegister(UB0_REG_GYRO_CONFIG1, _pBuf[0]);
+      gyroAccelConfig0.gyroUIFiltBW = UIFilterIndex;
+			_pBuf = (uint8_t *) &gyroAccelConfig0;
+      writeRegister(UB0_REG_GYRO_ACCEL_CONFIG0, _pBuf[0]);
+    } else if (who == ACCEL) {
+      accelConfig1.accelUIFiltORD = filterOrder;
+			_pBuf = (uint8_t *) &accelConfig1;
+      writeRegister(UB0_REG_ACCEL_CONFIG1, _pBuf[0]);
+      gyroAccelConfig0.accelUIFiltBW = UIFilterIndex;
+			_pBuf = (uint8_t *) &gyroAccelConfig0;
+      writeRegister(UB0_REG_GYRO_ACCEL_CONFIG0, _pBuf[0]);
+    } else if (who == ALL) {
+      gyroConfig1.gyroUIFiltODR = filterOrder;
+			_pBuf = (uint8_t *) &gyroConfig1;
+      writeRegister(UB0_REG_GYRO_CONFIG1, _pBuf[0]);
+      accelConfig1.accelUIFiltORD = filterOrder;
+			_pBuf = (uint8_t *) &accelConfig1;
+      writeRegister(UB0_REG_ACCEL_CONFIG1, _pBuf[0]);
+      gyroAccelConfig0.gyroUIFiltBW = UIFilterIndex;
+      gyroAccelConfig0.accelUIFiltBW = UIFilterIndex;
+			_pBuf = (uint8_t *) &gyroAccelConfig0;
+      writeRegister(UB0_REG_GYRO_ACCEL_CONFIG0, _pBuf[0]);
+    }
+  }
+  return ret;
+}
+
+
+int setODRAndFSR(uint8_t who, uint8_t ODR, uint8_t FSR)
+{
+  int ret = 0;
+	uint8_t * _pBuf; 
+  
+  setBank(0);
+  
+  if (who == GYRO) {
+    if (ODR > 15 || FSR > FSR_7) 
+    {
+      ret = -1;
+    } 
+    else 
+    {
+      gyroConfig0.gyroODR = ODR;
+      gyroConfig0.gyroFsSel = FSR;
+			_pBuf = (uint8_t *) &gyroConfig0;
+      writeRegister(UB0_REG_GYRO_CONFIG0, _pBuf[0]);
+   
+      switch (FSR) 
+      {
+        case FSR_0:
+          break;
+        case FSR_1:
+          setGyroFS(dps2000);
+          break;
+        case FSR_2:
+          setGyroFS(dps1000);
+          break;
+        case FSR_3:
+          setGyroFS(dps500);
+          break;
+        case FSR_4:
+          setGyroFS(dps250);
+          break;
+        case FSR_5:
+          setGyroFS(dps125);
+          break;
+        case FSR_6:
+          setGyroFS(dps62_5);
+          break;
+        case FSR_7:
+          setGyroFS(dps31_25);
+          break;
+      }
+
+    }
+  } 
+  else if (who == ACCEL) 
+  {
+    if (ODR > 15 || FSR > FSR_3) 
+    {
+      ret = -2;
+    } else {
+      accelConfig0.accelODR = ODR;
+      accelConfig0.accelFsSel = FSR;
+			_pBuf = (uint8_t *) &accelConfig0;
+      writeRegister(UB0_REG_ACCEL_CONFIG0, _pBuf[0] );
+      
+      switch (FSR) {
+        case FSR_0:
+          setAccelFS(gpm16);
+          break;
+        case FSR_1:
+          setAccelFS(gpm8);
+          break;
+        case FSR_2:
+          setAccelFS(gpm4);
+          break;
+        case FSR_3:
+          setAccelFS(gpm2);
+          break;
+      }
+    }
+  }
+  return ret;
+}
+
+
+
+
+/* starts communication with the ICM42688P */
+int ICM42688Begin(void) 
+{
+
+  // reset the ICM42688
+  ICM42688Reset();
+
+  // check the WHO AM I byte
+	setBank(0);
+	ICM42688_readRegisters(UB0_REG_WHO_AM_I,_buffer,1);
+  if(_buffer[0] != WHO_AM_I) 
+	{
+		printf("ID = %x \n\r", _buffer[0]);
+    return -3;// 表示读取身份错误
+  }
+	
+
+  // turn on accel and gyro in Low Noise (LN) Mode在低噪声(LN)模式下打开加速和陀螺
+  if(writeRegister(UB0_REG_PWR_MGMT0, 0x0F) < 0) 
+	{
+    return -4;
+  }
+
+  // 16G is default -- do this to set up accel resolution scaling
+  int ret = setAccelFS(gpm16);
+  if (ret < 0) return ret;
+
+  // 2000DPS is default -- do this to set up gyro resolution scaling
+  ret = setGyroFS(dps2000);
+  if (ret < 0) return ret;
+
+  // disable inner filters (Notch filter, Anti-alias filter, UI filter block)
+//  if (setFilters(0, 0) < 0) {
+//     return -7;
+//   }
+
+//  // estimate gyro bias
+//  if (calibrateGyro() < 0) {
+//    return -8;
+//  }
+//  // successful init, return 1
+  return 1;
+}
+
+
+void ICM42688_Init(void)
+{
+  // start communication with IMU
+  int status = ICM42688Begin();
+  if (status < 0) {
+    printf("IMU初始化不成功  \r\n ");
+    printf("检查IMU接线或尝试循环供电  \r\n ");
+		printf("错误状态:%d    ",status);		
+		printf(" \r\n ");	
+    while(1) {
+			status = ICM42688Begin();
+		}
+  }
+	printf("\r\n  ICM42688_Init 完成 \r\n");
+
+
+  setGyroNotchFilterFHz(1,ALL);//設置陷波頻率 1KH 
+  setGyroNFbandwidth(1);//帶寬
+  setGyroNotchFilter(1);//使能了NF 
+  setAAFBandwidth(GYRO,6);//設置二階濾波器帶寬6=258hz 2=84
+  setAAF(GYRO,1);//使能了AAF  
+  setUIFilter(GYRO,2 ,1);// 3阶 Accel LPF的带宽=ODR/4   dps2000
+  setODRAndFSR(/* who= */GYRO,/* ODR= */ODR_500HZ, /* FSR = */FSR_1);
+	
+	
+	
+  setAAFBandwidth(ACCEL,6);//設置二階濾波器帶寬258hz
+  setAAF(ACCEL,true);//使能了AAF  
+  setUIFilter(ACCEL,2 ,7);// 3阶 Accel LPF的带宽=OODR/40	 gpm8
+	setODRAndFSR(/* who= */ACCEL,/* ODR= */ODR_500HZ, /* FSR = */FSR_1);
+
+}
+
+

2.Firmware/ICM42688P_HAL/Core/Src/imu.c

@@ -0,0 +1,442 @@
+#include "imu.h"
+
+
+
+#define DCM_KP_ACC      0.400f    //加速度补偿陀螺仪PI参数
+#define DCM_KI_ACC      0.001f
+
+#define DCM_KP_MAG			1.000f		//磁力计补偿陀螺仪PI参数
+#define DCM_KI_MAG			0.000f
+
+
+#define IMU_SMALL_ANGLE    15.0f   //满足水平状态的最小角度（单位deg）
+
+#define SPIN_RATE_LIMIT     20    //旋转速率
+
+
+//float Kp = 0.4f;		/*比例增益*/
+//float Ki = 0.001f;		/*积分增益*/
+//float exInt = 0.0f;
+//float eyInt = 0.0f;
+//float ezInt = 0.0f;		/*积分误差累计*/
+
+
+float imuAttitudeYaw;//范围：-180~180，用于上传到匿名上位机（支持范围-180~180）
+static float q0 = 1.0f, q1 = 1.0f, q2 = 0.0f, q3 = 0.0f, q4 = 0.0f;//四元数
+static float rMat[3][3];//四元数的旋转矩阵
+static float smallAngleCosZ;//水平最小角余弦值
+
+
+//旋转机体坐标系加速度到NEU坐标系
+void imuTransformVectorBodyToEarth(Axis3f *v)
+{
+    const float x = rMat[0][0] * v->x + rMat[0][1] * v->y + rMat[0][2] * v->z;
+    const float y = rMat[1][0] * v->x + rMat[1][1] * v->y + rMat[1][2] * v->z;
+    const float z = rMat[2][0] * v->x + rMat[2][1] * v->y + rMat[2][2] * v->z;
+
+    v->x = x;
+    v->y = -y;//
+    v->z = z;
+}
+
+//NEU坐标系加速度到NEU坐标系
+void imuTransformVectorEarthToBody(Axis3f *v)
+{
+    v->y = -v->y;
+
+    /* From earth frame to body frame */
+    const float x = rMat[0][0] * v->x + rMat[1][0] * v->y + rMat[2][0] * v->z;
+    const float y = rMat[0][1] * v->x + rMat[1][1] * v->y + rMat[2][1] * v->z;
+    const float z = rMat[0][2] * v->x + rMat[1][2] * v->y + rMat[2][2] * v->z;
+
+    v->x = x;
+    v->y = y;
+    v->z = z;
+}
+
+
+float degreesToRadians(int16_t degrees1)
+{
+    return degrees1 * RAD;
+}
+
+
+//计算四元数的旋转矩阵
+static void imuComputeRotationMatrix(void)
+{
+    float q1q1 = q1 * q1;
+    float q2q2 = q2 * q2;
+    float q3q3 = q3 * q3;
+
+    float q0q1 = q0 * q1;
+    float q0q2 = q0 * q2;
+    float q0q3 = q0 * q3;
+    float q1q2 = q1 * q2;
+    float q1q3 = q1 * q3;
+    float q2q3 = q2 * q3;
+
+    rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
+    rMat[0][1] = 2.0f * (q1q2 + -q0q3);
+    rMat[0][2] = 2.0f * (q1q3 - -q0q2);
+
+    rMat[1][0] = 2.0f * (q1q2 - -q0q3);
+    rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
+    rMat[1][2] = 2.0f * (q2q3 + -q0q1);
+
+    rMat[2][0] = 2.0f * (q1q3 + -q0q2);
+    rMat[2][1] = 2.0f * (q2q3 - -q0q1);
+    rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
+}
+
+
+void imuInit(void)
+{
+
+    smallAngleCosZ = cos_approx(degreesToRadians(IMU_SMALL_ANGLE));//最小倾角余弦值
+    imuComputeRotationMatrix();//计算四元数的旋转矩阵
+}
+
+static float invSqrt(float x)
+{
+    return 1.0f / sqrtf(x);
+}
+
+
+//磁力计快速增益
+static float imuMagFastPGainSaleFactor(void)
+{
+	//四轴上电后磁力计融合需要一段时间
+	//为了快速融合，前100次使用快速增益
+	static int32_t FastPGainCount = 100;
+	if(FastPGainCount>0)//
+	{
+		FastPGainCount--;
+		return 10.0f;	
+	}
+	else
+		return 1.0f;
+}
+
+
+//快速增益
+static float imuACCFastPGainSaleFactor(void)
+{
+	//四轴上电后ACC融合需要一段时间
+	//为了快速融合，前100次使用快速增益
+	static int32_t FastPGainCount = 100;
+	if(FastPGainCount>0)//
+	{
+		FastPGainCount--;
+		return 100.0f;	
+	}
+	else
+		return 1.0f;
+}
+
+
+
+//计算四元数和旋转矩阵
+void imuMahonyAHRSupdate(float gx, float gy, float gz,float ax, float ay, float az,float mx, float my, float mz,bool useMag,float dt)
+{
+  static float integralAccX = 0.0f,  integralAccY = 0.0f, integralAccZ = 0.0f;    //加速度积分误差
+	static float integralMagX = 0.0f,  integralMagY = 0.0f, integralMagZ = 0.0f;    //磁力计积分误差
+  float ex, ey, ez;
+
+    //计算旋转速率(rad/s)
+    const float spin_rate_sq = sq(gx) + sq(gy) + sq(gz);
+
+    //Step 1: Yaw correction
+    if (useMag) 
+	  {
+				const float magMagnitudeSq = mx * mx + my * my + mz * mz;
+				float kpMag = DCM_KP_MAG * imuMagFastPGainSaleFactor();
+				
+				if (magMagnitudeSq > 0.01f) 
+				{
+					//单位化磁力计测量值
+					const float magRecipNorm = invSqrt(magMagnitudeSq);
+					mx *= magRecipNorm;
+					my *= magRecipNorm;
+					mz *= magRecipNorm;
+				
+//					printf("   mx:%f    ",mx);
+//					printf("   my:%f    ",my);
+//					printf("   mz:%f    ",mz);
+//					printf("   magRecipNorm:%f    ",magRecipNorm);
+
+//					printf(" \r\n ");						
+					
+					
+					
+					//计算X\Y方向的磁通，磁北方向磁通
+					const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
+					const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
+					const float bx = sqrtf(hx * hx + hy * hy);
+
+					//磁力计误差是估计磁北和测量磁北之间的交叉乘积
+					const float ez_ef = -(hy * bx);
+
+					//旋转误差到机体坐标系
+					ex = rMat[2][0] * ez_ef;
+					ey = rMat[2][1] * ez_ef;
+					ez = rMat[2][2] * ez_ef;
+				}
+				else 
+				{
+					ex = 0;
+					ey = 0;
+					ez = 0;
+				}
+
+				//累计误差补偿
+				if (DCM_KI_MAG > 0.0f) 
+				{
+					//如果旋转速率大于限制值则停止积分
+					if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT))) 
+					{
+						integralMagX += DCM_KI_MAG * ex * dt;
+						integralMagY += DCM_KI_MAG * ey * dt;
+						integralMagZ += DCM_KI_MAG * ez * dt;
+
+						gx += integralMagX;
+						gy += integralMagY;
+						gz += integralMagZ;
+					}
+				}	
+				
+				//误差补偿
+				gx += kpMag * ex;
+				gy += kpMag * ey;
+				gz += kpMag * ez;
+	}	
+	
+	
+	
+    //Step 2: Roll and pitch correction
+  if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))//加速度计输出有效时,利用加速度计补偿陀螺仪
+  {
+    //单位化加速计测量值
+    const float accRecipNorm = invSqrt(ax * ax + ay * ay + az * az);
+    ax *= accRecipNorm;
+    ay *= accRecipNorm;
+    az *= accRecipNorm;
+
+    //加速计读取的方向与重力加速计方向的差值，用向量叉乘计算
+    ex = (ay * rMat[2][2] - az * rMat[2][1]);
+    ey = (az * rMat[2][0] - ax * rMat[2][2]);
+    ez = (ax * rMat[2][1] - ay * rMat[2][0]);
+
+    //累计误差补偿
+    if (DCM_KI_ACC > 0.0f) 
+    {
+      //如果旋转速率大于限制值则停止积分
+      if (spin_rate_sq < sq(DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)))
+      {
+        /*误差累计，与积分常数相乘*/
+        integralAccX += DCM_KI_ACC * ex * dt;
+        integralAccY += DCM_KI_ACC * ey * dt;
+        integralAccZ += DCM_KI_ACC * ez * dt;
+
+        gx += integralAccX;
+        gy += integralAccY;
+        gz += integralAccZ;
+      }
+    }
+
+    //误差补偿 用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量
+		float ak = imuACCFastPGainSaleFactor();
+		//printf(" ak:%f ",ak);
+    gx += DCM_KP_ACC * ex * ak;
+    gy += DCM_KP_ACC * ey * ak;
+    gz += DCM_KP_ACC * ez * ak;
+		
+  }
+  
+  //一阶近似算法，四元数运动学方程的离散化形式和积分
+    gx *= (0.5f * dt);
+    gy *= (0.5f * dt);
+    gz *= (0.5f * dt);
+
+    const float qa = q0;
+    const float qb = q1;
+    const float qc = q2;
+    q0 += (-qb * gx - qc * gy - q3 * gz);
+    q1 += (qa * gx + qc * gz - q3 * gy);
+    q2 += (qa * gy - qb * gz + q3 * gx);
+    q3 += (qa * gz + qb * gy - qc * gx);
+
+  //单位化四元数
+    const float quatRecipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= quatRecipNorm;
+    q1 *= quatRecipNorm;
+    q2 *= quatRecipNorm;
+    q3 *= quatRecipNorm;
+
+    //计算四元数的旋转矩阵
+    imuComputeRotationMatrix();
+}
+
+
+
+//更新欧拉角
+static void imuUpdateEulerAngles(attitude_t *attitude)
+{
+  attitude->roll = RADIANS_TO_DEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
+  attitude->pitch = RADIANS_TO_DEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));//arcsin = 0.5PI - arccos
+  attitude->yaw = RADIANS_TO_DEGREES(atan2_approx(rMat[1][0], rMat[0][0]));
+
+  imuAttitudeYaw = attitude->yaw;
+
+  if (attitude->yaw < 0.0f)//转换位0~360
+    attitude->yaw += 360.0f;
+
+  //更新最小倾角状态
+  if (rMat[2][2] > smallAngleCosZ) 
+    ENABLE_STATE(SMALL_ANGLE);
+  else 
+    DISABLE_STATE(SMALL_ANGLE);
+    
+}
+
+
+//四元数和欧拉角计算
+void imuUpdateAttitude(const sensorData_t *sensorData, state_t *state, float dt)
+{
+  bool useMag = 0;//compassIsHealthy();	
+	
+	Axis3f gyro = sensorData->gyro;
+	Axis3f acc  = sensorData->acc;
+	Axis3f mag  = sensorData->mag;
+   
+
+
+	//角速度单位由度转为弧度
+	gyro.x = gyro.x * DEG2RAD;
+	gyro.y = gyro.y * DEG2RAD;
+	gyro.z = gyro.z * DEG2RAD;
+
+	
+	//计算四元数和旋转矩阵
+    imuMahonyAHRSupdate(gyro.x, gyro.y, gyro.z,
+                        acc.x, acc.y, acc.z,
+                        mag.x, mag.y, mag.z,
+						useMag,dt);
+	
+    //计算欧拉角               
+    imuUpdateEulerAngles(&state->attitude);
+    
+}
+
+
+ 
+float beta = 0.2;//0.04 加速计和磁力计补偿因子，数值越大纠正越快，越不稳定
+ 
+void MadgwickQuaternionUpdate(Axis3f acc, Axis3f gyro, Axis3f mag, attitude_t *attitude , float dt)
+{
+	float norm;
+	float hx, hy, _2bx, _2bz;
+	float s1, s2, s3, s4;
+	float qDot1, qDot2, qDot3, qDot4;
+ 
+	
+	//提前计算好下面要用到的数值防止重复计算，减少运算量
+	float _2q1mx;
+	float _2q1my;
+	float _2q1mz;
+	float _2q2mx;
+	float _4bx;
+	float _4bz;
+	float _2q1 = 2.0f * q1;
+	float _2q2 = 2.0f * q2;
+	float _2q3 = 2.0f * q3;
+	float _2q4 = 2.0f * q4;
+	float _2q1q3 = 2.0f * q1 * q3;
+	float _2q3q4 = 2.0f * q3 * q4;
+	float q1q1 = q1 * q1;
+	float q1q2 = q1 * q2;
+	float q1q3 = q1 * q3;
+	float q1q4 = q1 * q4;
+	float q2q2 = q2 * q2;
+	float q2q3 = q2 * q3;
+	float q2q4 = q2 * q4;
+	float q3q3 = q3 * q3;
+	float q3q4 = q3 * q4;
+	float q4q4 = q4 * q4;
+ 
+			
+    //度转弧度
+	gyro.x = gyro.x * DEG2RAD;	/* 度转弧度 */
+	gyro.y = gyro.y * DEG2RAD;
+	gyro.z = gyro.z * DEG2RAD;
+ 
+	//单位化加速度向量
+	norm = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
+	if (norm == 0.0f) return; // handle NaN
+	norm = 1.0f/norm;
+	acc.x *= norm;
+	acc.y *= norm;
+	acc.z *= norm;
+ 
+	//单位化磁力计向量
+	norm = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
+	if (norm == 0.0f) return; // handle NaN
+	norm = 1.0f/norm;
+	mag.x *= norm;
+	mag.y *= norm;
+	mag.z *= norm;
+ 
+	//提前计算好下面要用到的值
+	_2q1mx = 2.0f * q1 * mag.x;
+	_2q1my = 2.0f * q1 * mag.y;
+	_2q1mz = 2.0f * q1 * mag.z;
+	_2q2mx = 2.0f * q2 * mag.x;
+	//磁力计从机体到地球
+	hx = mag.x * q1q1 - _2q1my * q4 + _2q1mz * q3 + mag.x * q2q2 + _2q2 * mag.y * q3 + _2q2 * mag.z * q4 - mag.x * q3q3 - mag.x * q4q4;
+	hy = _2q1mx * q4 + mag.y * q1q1 - _2q1mz * q2 + _2q2mx * q3 - mag.y * q2q2 + mag.y * q3q3 + _2q3 * mag.z * q4 - mag.y * q4q4;
+	/*让导航坐标系中X轴指向正北方*/
+	_2bx = sqrtf(hx * hx + hy * hy);
+	_2bz = -_2q1mx * q3 + _2q1my * q2 + mag.z * q1q1 + _2q2mx * q4 - mag.z * q2q2 + _2q3 * mag.y * q4 - mag.z * q3q3 + mag.z * q4q4;
+	_4bx = 2.0f * _2bx;
+	_4bz = 2.0f * _2bz;
+ 
+	//梯度下降法计算纠正误差
+	s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q2 * (2.0f * q1q2 + _2q3q4 - acc.y) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
+	s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q1 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
+	s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q4 * (2.0f * q1q2 + _2q3q4 - acc.y) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - acc.z) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
+	s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - acc.x) + _2q3 * (2.0f * q1q2 + _2q3q4 - acc.y) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mag.x) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - mag.y) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mag.z);
+    //单位化计算好的误差向量
+	norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    
+	norm = 1.0f/norm;
+	s1 *= norm;
+	s2 *= norm;
+	s3 *= norm;
+	s4 *= norm;
+ 
+	// 将计算好的误差向量补偿到四元数
+	qDot1 = 0.5f * (-q2 * gyro.x - q3 * gyro.y - q4 * gyro.z) - beta * s1;
+	qDot2 = 0.5f * (q1 * gyro.x + q3 * gyro.z - q4 * gyro.y) - beta * s2;
+	qDot3 = 0.5f * (q1 * gyro.y - q2 * gyro.z + q4 * gyro.x) - beta * s3;
+	qDot4 = 0.5f * (q1 * gyro.z + q2 * gyro.y - q3 * gyro.x) - beta * s4;
+ 
+	//更新四元数的值
+	q1 += qDot1 * dt;
+	q2 += qDot2 * dt;
+	q3 += qDot3 * dt;
+	q4 += qDot4 * dt;
+	norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
+	norm = 1.0f/norm;
+	q1 = q1 * norm;
+	q2 = q2 * norm;
+	q3 = q3 * norm;
+	q4 = q4 * norm;
+	
+    //四元数转换为欧拉角	
+	attitude->yaw   = atan2(2.0f * (q2 * q3 + q1 * q4), q1 * q1 + q2 * q2 - q3 * q3 - q4 * q4) * RAD2DEG;   
+	attitude->pitch = -asin(2.0f * (q2 * q4 - q1 * q3)) * RAD2DEG;
+	attitude->roll  = atan2(2.0f * (q1 * q2 + q3 * q4), q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4) * RAD2DEG;
+	
+}
+
+
+
+

2.Firmware/ICM42688P_HAL/Core/Src/main.c

@@ -0,0 +1,103 @@
+/**
+  ******************************************************************************
+  * @file           : main.c
+  * @brief          : Main program body
+  ******************************************************************************
+  */
+
+/* Includes ------------------------------------------------------------------*/
+#include "stdio.h"
+#include "main.h"
+#include "spi.h"
+#include "usart.h"
+#include "gpio.h"
+
+/* Private includes ----------------------------------------------------------*/
+#include "sensors.h"
+
+/* Private typedef -----------------------------------------------------------*/
+
+/* Private define ------------------------------------------------------------*/
+
+/* Private macro -------------------------------------------------------------*/
+
+/* Private variables ---------------------------------------------------------*/
+
+/* Private function prototypes -----------------------------------------------*/
+void SystemClock_Config(void);
+void Error_Handler(void);
+
+/**
+  * @brief  The application entry point.
+  * @retval int
+  */
+int main(void)
+{
+  /* MCU Configuration--------------------------------------------------------*/
+
+  HAL_Init();                   // Initialize the Hardware Abstraction Layer (HAL)
+
+  SystemClock_Config();         // Call function to configure system clock settings
+
+  GPIO_Init();                  // Initialize GPIO pins as per configuration
+  MX_SPI2_Init();               // Initialize SPI2 peripheral
+  MX_USART1_UART_Init();        // Initialize USART1 UART peripheral
+
+  sensorsInit();                // Initialize sensors as per configuration
+
+  while (1)                     
+  {
+    sensorsTask(0.005);         // Execute sensor tasks with a 5 ms period
+  }
+}
+
+/**
+  * @brief System Clock Configuration
+  * @retval None
+  */
+void SystemClock_Config(void)
+{
+  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
+  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
+
+  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
+  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
+  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
+  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2;
+  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
+  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK
+                              | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
+  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
+  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
+
+  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+}
+
+/**
+  * @brief  This function is executed in case of error occurrence.
+  * @retval None
+  */
+void Error_Handler(void)
+{
+  __disable_irq();
+  while (1)
+  {
+  }
+}
+
+#ifdef  USE_FULL_ASSERT
+void assert_failed(uint8_t *file, uint32_t line)
+{
+}
+#endif /* USE_FULL_ASSERT */

2.Firmware/ICM42688P_HAL/Core/Src/maths.c

@@ -0,0 +1,342 @@
+#include "maths.h"
+
+
+//#include "config_param.h"
+
+
+
+#define sinPolyCoef3 -1.666665710e-1f                                          // Double: -1.666665709650470145824129400050267289858e-1
+#define sinPolyCoef5  8.333017292e-3f                                          // Double:  8.333017291562218127986291618761571373087e-3
+#define sinPolyCoef7 -1.980661520e-4f                                          // Double: -1.980661520135080504411629636078917643846e-4
+#define sinPolyCoef9  2.600054768e-6f  
+
+
+
+float map(long x, long in_min, long in_max, float out_min, float out_max) 
+{
+    long divisor = (in_max - in_min);
+    if(divisor == 0){
+        return -1; //AVR returns -1, SAM returns 0
+    }
+    return (x - in_min) * (out_max - out_min) / divisor + out_min;
+}
+
+
+
+float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) 
+{
+    float divisor = (in_max - in_min);
+    if(divisor == 0){
+        return -1; //AVR returns -1, SAM returns 0
+    }
+    return (x - in_min) * (out_max - out_min) / divisor + out_min;
+}
+
+
+float constrainFloat(float data,float min,float max)
+{
+		if(data > max)
+			data = max;
+		else if(data < min)
+			data = min;
+		
+	  return 	data;		
+}
+
+int16_t constrainInt16_t(int16_t data,int16_t min,int16_t max)
+{
+		if(data > max)
+			data = max;
+		else if(data < min)
+			data = min;
+		
+	  return 	data;
+}
+
+uint16_t constrainUint16_t(uint16_t data,uint16_t min,uint16_t max)
+{
+		if(data > max)
+			data = max;
+		else if(data < min)
+			data = min;
+		
+	  return 	data;
+}
+
+
+void devClear(stdev_t *dev)
+{
+    dev->m_n = 0;
+}
+
+
+void devPush(stdev_t *dev, float x)
+{
+    dev->m_n++;
+    if (dev->m_n == 1) {
+        dev->m_oldM = dev->m_newM = x;
+        dev->m_oldS = 0.0f;
+    } else {
+        dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
+        dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
+        dev->m_oldM = dev->m_newM;
+        dev->m_oldS = dev->m_newS;
+    }
+}
+
+
+float devVariance(stdev_t *dev)
+{
+    return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
+}
+
+
+float devStandardDeviation(stdev_t *dev)
+{
+    return sqrtf(devVariance(dev));
+}
+
+
+float sin_approx(float x)
+{
+    int32_t xint = x;
+    if (xint < -32 || xint > 32) return 0.0f;                               // Stop here on error input (5 * 360 Deg)
+    while (x >  M_PIf) x -= (2.0f * M_PIf);                                 // always wrap input angle to -PI..PI
+    while (x < -M_PIf) x += (2.0f * M_PIf);
+    if (x >  (0.5f * M_PIf)) x =  (0.5f * M_PIf) - (x - (0.5f * M_PIf));   // We just pick -90..+90 Degree
+    else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
+    float x2 = x * x;
+    return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
+}
+
+float cos_approx(float x)
+{
+    return sin_approx(x + (0.5f * M_PIf));
+}
+
+
+
+// https://github.com/Crashpilot1000/HarakiriWebstore1/blob/396715f73c6fcf859e0db0f34e12fe44bace6483/src/mw.c#L1292
+// http://http.developer.nvidia.com/Cg/atan2.html (not working correctly!)
+// Poly coefficients by @ledvinap (https://github.com/cleanflight/cleanflight/pull/1107)
+// Max absolute error 0,000027 degree
+float atan2_approx(float y, float x)
+{
+    #define atanPolyCoef1  3.14551665884836e-07f
+    #define atanPolyCoef2  0.99997356613987f
+    #define atanPolyCoef3  0.14744007058297684f
+    #define atanPolyCoef4  0.3099814292351353f
+    #define atanPolyCoef5  0.05030176425872175f
+    #define atanPolyCoef6  0.1471039133652469f
+    #define atanPolyCoef7  0.6444640676891548f
+
+    float res, absX, absY;
+    absX = fabsf(x);
+    absY = fabsf(y);
+    res  = MAX(absX, absY);
+    if (res) res = MIN(absX, absY) / res;
+    else res = 0.0f;
+    res = -((((atanPolyCoef5 * res - atanPolyCoef4) * res - atanPolyCoef3) * res - atanPolyCoef2) * res - atanPolyCoef1) / ((atanPolyCoef7 * res + atanPolyCoef6) * res + 1.0f);
+    if (absY > absX) res = (M_PIf / 2.0f) - res;
+    if (x < 0) res = M_PIf - res;
+    if (y < 0) res = -res;
+    return res;
+}
+
+// http://http.developer.nvidia.com/Cg/acos.html
+// Handbook of Mathematical Functions
+// M. Abramowitz and I.A. Stegun, Ed.
+// Absolute error <= 6.7e-5
+float acos_approx(float x)
+{
+    float xa = fabsf(x);
+    float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
+    if (x < 0.0f)
+        return M_PIf - result;
+    else
+        return result;
+}
+
+
+
+
+
+/**
+ * Sensor offset calculation code based on Freescale's AN4246
+ * Initial implementation by @HaukeRa
+ * Modified to be re-usable by @DigitalEntity
+ */
+void sensorCalibrationResetState(sensorCalibrationState_t * state)
+{
+    for (int i = 0; i < 4; i++){
+        for (int j = 0; j < 4; j++){
+            state->XtX[i][j] = 0;
+        }
+
+        state->XtY[i] = 0;
+    }
+}
+
+void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3])
+{
+    state->XtX[0][0] += (float)sample[0] * sample[0];
+    state->XtX[0][1] += (float)sample[0] * sample[1];
+    state->XtX[0][2] += (float)sample[0] * sample[2];
+    state->XtX[0][3] += (float)sample[0];
+
+    state->XtX[1][0] += (float)sample[1] * sample[0];
+    state->XtX[1][1] += (float)sample[1] * sample[1];
+    state->XtX[1][2] += (float)sample[1] * sample[2];
+    state->XtX[1][3] += (float)sample[1];
+
+    state->XtX[2][0] += (float)sample[2] * sample[0];
+    state->XtX[2][1] += (float)sample[2] * sample[1];
+    state->XtX[2][2] += (float)sample[2] * sample[2];
+    state->XtX[2][3] += (float)sample[2];
+
+    state->XtX[3][0] += (float)sample[0];
+    state->XtX[3][1] += (float)sample[1];
+    state->XtX[3][2] += (float)sample[2];
+    state->XtX[3][3] += 1;
+
+    float squareSum = ((float)sample[0] * sample[0]) + ((float)sample[1] * sample[1]) + ((float)sample[2] * sample[2]);
+    state->XtY[0] += sample[0] * squareSum;
+    state->XtY[1] += sample[1] * squareSum;
+    state->XtY[2] += sample[2] * squareSum;
+    state->XtY[3] += squareSum;
+}
+
+static void sensorCalibration_gaussLR(float mat[4][4]) {
+    uint8_t n = 4;
+    int i, j, k;
+    for (i = 0; i < 4; i++) {
+        // Determine R
+        for (j = i; j < 4; j++) {
+            for (k = 0; k < i; k++) {
+                mat[i][j] -= mat[i][k] * mat[k][j];
+            }
+        }
+        // Determine L
+        for (j = i + 1; j < n; j++) {
+            for (k = 0; k < i; k++) {
+                mat[j][i] -= mat[j][k] * mat[k][i];
+            }
+            mat[j][i] /= mat[i][i];
+        }
+    }
+}
+
+
+void sensorCalibration_ForwardSubstitution(float LR[4][4], float y[4], float b[4]) {
+    int i, k;
+    for (i = 0; i < 4; ++i) {
+        y[i] = b[i];
+        for (k = 0; k < i; ++k) {
+            y[i] -= LR[i][k] * y[k];
+        }
+        //y[i] /= MAT_ELEM_AT(LR,i,i); //Do not use, LR(i,i) is 1 anyways and not stored in this matrix
+    }
+}
+
+void sensorCalibration_BackwardSubstitution(float LR[4][4], float x[4], float y[4]) {
+    int i, k;
+    for (i = 3 ; i >= 0; --i) {
+        x[i] = y[i];
+        for (k = i + 1; k < 4; ++k) {
+            x[i] -= LR[i][k] * x[k];
+        }
+        x[i] /= LR[i][i];
+    }
+}
+
+
+// solve linear equation
+// https://en.wikipedia.org/wiki/Gaussian_elimination
+static void sensorCalibration_SolveLGS(float A[4][4], float x[4], float b[4]) {
+    int i;
+    float y[4];
+
+    sensorCalibration_gaussLR(A);
+
+    for (i = 0; i < 4; ++i) {
+        y[i] = 0;
+    }
+
+    sensorCalibration_ForwardSubstitution(A, y, b);
+    sensorCalibration_BackwardSubstitution(A, x, y);
+}
+
+
+
+
+void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
+{
+    float beta[4];
+    sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
+
+    for (int i = 0; i < 3; i++) {
+        result[i] = beta[i] / 2;
+    }
+}
+
+
+void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
+{
+    float cosx, sinx, cosy, siny, cosz, sinz;
+    float coszcosx, sinzcosx, coszsinx, sinzsinx;
+
+    cosx = cos_approx(delta->angles.roll);
+    sinx = sin_approx(delta->angles.roll);
+    cosy = cos_approx(delta->angles.pitch);
+    siny = sin_approx(delta->angles.pitch);
+    cosz = cos_approx(delta->angles.yaw);
+    sinz = sin_approx(delta->angles.yaw);
+
+    coszcosx = cosz * cosx;
+    sinzcosx = sinz * cosx;
+    coszsinx = sinx * cosz;
+    sinzsinx = sinx * sinz;
+
+    matrix[0][X] = cosz * cosy;
+    matrix[0][Y] = -cosy * sinz;
+    matrix[0][Z] = siny;
+    matrix[1][X] = sinzcosx + (coszsinx * siny);
+    matrix[1][Y] = coszcosx - (sinzsinx * siny);
+    matrix[1][Z] = -sinx * cosy;
+    matrix[2][X] = (sinzsinx) - (coszcosx * siny);
+    matrix[2][Y] = (coszsinx) + (sinzcosx * siny);
+    matrix[2][Z] = cosy * cosx;
+}
+
+
+int32_t applyDeadband(int32_t value, int32_t deadband)
+{
+    if (ABS(value) < deadband) {
+        value = 0;
+    } else if (value > 0) {
+        value -= deadband;
+    } else if (value < 0) {
+        value += deadband;
+    }
+    return value;
+}
+
+
+
+float applyDeadbandF(float value, float deadband)
+{
+    if (ABS(value) < deadband) 
+		{
+        value = 0;
+    } 
+		else if (value > 0) 
+		{
+        value -= deadband;
+    } 
+		else if (value < 0) 
+		{
+        value += deadband;
+    }
+    return value;
+}
+

2.Firmware/ICM42688P_HAL/Core/Src/runtime_config.c

@@ -0,0 +1,12 @@
+#include "runtime_config.h"
+
+
+uint32_t armingFlags = 0;
+uint32_t stateFlags = 0;
+uint32_t flightModeFlags = 0;
+
+
+
+
+
+

2.Firmware/ICM42688P_HAL/Core/Src/sensors.c

@@ -0,0 +1,93 @@
+#include "sensors.h"
+
+// 定义全局变量来存储传感器数据和状态信息
+sensorData_t sensorData;  // 传感器数据结构
+state_t      state;            // 姿态状态结构
+dt_ms_t      SensorsTaskTime;  // 时间记录结构，用于任务调度
+
+bool isMPUPresent = false; // 标志位，指示MPU是否可用
+
+/**
+ * @brief 初始化所有传感器设备
+ *
+ * 此函数负责初始化陀螺仪、加速度计，并进行IMU的姿态解算初始化。
+ */
+void sensorsInit(void)
+{
+  ICM42688_Init(); // 初始化ICM42688陀螺仪
+
+  // 初始化陀螺仪并检查其是否存在
+  isMPUPresent = gyroInit(GYRO_UPDATE_RATE);
+
+  // 初始化加速度计并检查其是否存在
+  isMPUPresent &= accInit(ACC_UPDATE_RATE);
+
+  // 初始化姿态解算器
+  imuInit();
+
+  // 记录当前时间戳，用于后续的时间计算
+  SensorsTaskTime.ms1 = HAL_GetTick();
+}
+
+/**
+ * @brief 定期执行的传感器数据采集和处理任务
+ *
+ * 此函数每隔一段时间（由参数dt指定）读取加速度计和陀螺仪的数据，
+ * 并进行姿态估计和输出。如果陀螺仪和加速度计都已正确初始化，则
+ * 进行姿态更新；否则，跳过更新步骤。
+ *
+ * @param dt 任务执行间隔时间，单位为秒
+ */
+void sensorsTask(float dt)
+{
+  // 获取当前时间戳
+  SensorsTaskTime.ms = HAL_GetTick();
+
+  // 计算自上次执行以来的时间差
+  SensorsTaskTime.dt = (float)(SensorsTaskTime.ms - SensorsTaskTime.ms1) * 0.001f;
+
+  // 如果时间差大于或等于设定的时间间隔，则执行数据采集和处理
+  if (SensorsTaskTime.dt >= dt)
+  {
+    // 更新上一次执行的时间戳
+    SensorsTaskTime.ms1 = SensorsTaskTime.ms;
+
+    // 读取陀螺仪原始数据
+    gyroUpdate(&sensorData.gyro);
+
+    // 读取加速度计原始数据
+    accUpdate(&sensorData.acc);
+
+    // 检查陀螺仪和加速度计是否校准完成
+    if (1) // (GYRO_Calibration() && ACC_Calibration())
+    {
+      // 使用最新读取的传感器数据更新姿态
+      imuUpdateAttitude(&sensorData, &state, SensorsTaskTime.dt);
+
+      // 减去初始角度偏差以获得相对姿态
+      state.attitude.pitch = state.attitude.pitch - state.attitudeZero.pitch;
+      state.attitude.roll = state.attitude.roll - state.attitudeZero.roll;
+    }
+
+    // 打印姿态信息
+    printf("roll:%f ", state.attitude.roll);
+    printf("pitch:%f ", state.attitude.pitch);
+    printf("yaw:%f ", state.attitude.yaw);
+
+    // 打印加速度计数据
+    printf("accX:%f ", sensorData.acc.x);
+    printf("accY:%f ", sensorData.acc.y);
+    printf("accZ:%f ", sensorData.acc.z);
+
+    // 打印陀螺仪数据
+    printf("gyroX:%f ", sensorData.gyro.x);
+    printf("gyroY:%f ", sensorData.gyro.y);
+    printf("gyroZ:%f ", sensorData.gyro.z);
+
+    // 打印换行符
+    printf("\r\n");
+
+    // 重置时间差
+    SensorsTaskTime.dt = 0;
+  }
+}

2.Firmware/ICM42688P_HAL/Core/Src/sensorsalignment.c

@@ -0,0 +1,127 @@
+#include "sensorsalignment.h"
+
+
+static bool standardBoardAlignment = true;     
+static float boardRotation[3][3];//Ðýת¾ØÕó£¨Î¢µ÷Óã©
+
+
+static bool isBoardAlignmentStandard(const boardAlignment_t *boardAlignment)
+{
+    return !boardAlignment->rollDeciDegrees && !boardAlignment->pitchDeciDegrees && !boardAlignment->yawDeciDegrees;
+}
+
+
+
+void initBoardAlignment(void)
+{
+	if (isBoardAlignmentStandard(&configParam.boardAlign)) 
+	{
+		standardBoardAlignment = true;
+	} 
+	else 
+	{
+		fp_angles_t rotationAngles;
+
+		standardBoardAlignment = false;
+
+		rotationAngles.angles.roll  = DECIDEGREES_TO_RADIANS(configParam.boardAlign.rollDeciDegrees );
+		rotationAngles.angles.pitch = DECIDEGREES_TO_RADIANS(configParam.boardAlign.pitchDeciDegrees);
+		rotationAngles.angles.yaw   = DECIDEGREES_TO_RADIANS(configParam.boardAlign.yawDeciDegrees  );
+
+		buildRotationMatrix(&rotationAngles, boardRotation);
+	}
+}
+
+void applyBoardAlignment(int16_t *vec)
+{
+	
+	uint8_t X=0,Y=1,Z=2;
+	
+	if (standardBoardAlignment) 
+	{
+		return;
+	}
+
+	int16_t x = vec[X];
+	int16_t y = vec[Y];
+	int16_t z = vec[Z];
+
+	vec[X] = lrintf(boardRotation[0][X] * x + boardRotation[1][X] * y + boardRotation[2][X] * z);
+	vec[Y] = lrintf(boardRotation[0][Y] * x + boardRotation[1][Y] * y + boardRotation[2][Y] * z);
+	vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
+}
+
+
+void applySensorAlignment(int16_t * dest, int16_t * src, uint8_t rotation)
+{
+	  uint8_t X=0,Y=1,Z=2;
+	  
+    const int16_t x = src[X];
+    const int16_t y = src[Y];
+    const int16_t z = src[Z];
+
+    switch (rotation) 
+		{
+				default:
+				case CW0_DEG:
+						dest[X] = x;
+						dest[Y] = y;
+						dest[Z] = z;
+						break;
+				case CW90_DEG:
+						dest[X] = y;
+						dest[Y] = -x;
+						dest[Z] = z;
+						break;
+				case CW180_DEG:
+						dest[X] = -x;
+						dest[Y] = -y;
+						dest[Z] = z;
+						break;
+				case CW270_DEG:
+						dest[X] = -y;
+						dest[Y] = x;
+						dest[Z] = z;
+						break;
+				case CW0_DEG_FLIP:
+						dest[X] = -x;
+						dest[Y] = y;
+						dest[Z] = -z;
+						break;
+				case CW90_DEG_FLIP:
+						dest[X] = y;
+						dest[Y] = x;
+						dest[Z] = -z;
+						break;
+				case CW180_DEG_FLIP:
+						dest[X] = x;
+						dest[Y] = -y;
+						dest[Z] = -z;
+						break;
+				case CW270_DEG_FLIP:
+						dest[X] = -y;
+						dest[Y] = -x;
+						dest[Z] = -z;
+						break;
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

2.Firmware/ICM42688P_HAL/Core/Src/spi.c

@@ -0,0 +1,101 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    spi.c
+  * @brief   This file provides code for the configuration
+  *          of the SPI instances.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+/* Includes ------------------------------------------------------------------*/
+#include "spi.h"
+
+/* USER CODE BEGIN 0 */
+
+/* USER CODE END 0 */
+
+SPI_HandleTypeDef hspi2;
+
+/* SPI2 init function */
+void MX_SPI2_Init(void)
+{
+  hspi2.Instance = SPI2;
+  hspi2.Init.Mode = SPI_MODE_MASTER;
+  hspi2.Init.Direction = SPI_DIRECTION_2LINES;
+  hspi2.Init.DataSize = SPI_DATASIZE_8BIT;
+  hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH;
+  hspi2.Init.CLKPhase = SPI_PHASE_2EDGE;
+  hspi2.Init.NSS = SPI_NSS_SOFT;
+  hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
+  hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
+  hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
+  hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+  hspi2.Init.CRCPolynomial = 10;
+  if (HAL_SPI_Init(&hspi2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+}
+
+void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
+{
+
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(spiHandle->Instance==SPI2)
+  {
+  /* USER CODE BEGIN SPI2_MspInit 0 */
+
+  /* USER CODE END SPI2_MspInit 0 */
+    /* SPI2 clock enable */
+    __HAL_RCC_SPI2_CLK_ENABLE();
+
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    /**SPI2 GPIO Configuration
+    PB13     ------> SPI2_SCK
+    PB14     ------> SPI2_MISO
+    PB15     ------> SPI2_MOSI
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_13|GPIO_PIN_15;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+    GPIO_InitStruct.Pin = GPIO_PIN_14;
+    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+  }
+}
+
+void HAL_SPI_MspDeInit(SPI_HandleTypeDef* spiHandle)
+{
+
+  if(spiHandle->Instance==SPI2)
+  {
+  /* USER CODE BEGIN SPI2_MspDeInit 0 */
+
+  /* USER CODE END SPI2_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_SPI2_CLK_DISABLE();
+
+    /**SPI2 GPIO Configuration
+    PB13     ------> SPI2_SCK
+    PB14     ------> SPI2_MISO
+    PB15     ------> SPI2_MOSI
+    */
+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_13|GPIO_PIN_14|GPIO_PIN_15);
+
+  }
+}
+

2.Firmware/ICM42688P_HAL/Core/Src/stm32f1xx_hal_msp.c

@@ -0,0 +1,82 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file         stm32f1xx_hal_msp.c
+  * @brief        This file provides code for the MSP Initialization
+  *               and de-Initialization codes.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+
+/* USER CODE BEGIN Includes */
+
+/* USER CODE END Includes */
+
+/* Private typedef -----------------------------------------------------------*/
+/* USER CODE BEGIN TD */
+
+/* USER CODE END TD */
+
+/* Private define ------------------------------------------------------------*/
+/* USER CODE BEGIN Define */
+
+/* USER CODE END Define */
+
+/* Private macro -------------------------------------------------------------*/
+/* USER CODE BEGIN Macro */
+
+/* USER CODE END Macro */
+
+/* Private variables ---------------------------------------------------------*/
+/* USER CODE BEGIN PV */
+
+/* USER CODE END PV */
+
+/* Private function prototypes -----------------------------------------------*/
+/* USER CODE BEGIN PFP */
+
+/* USER CODE END PFP */
+
+/* External functions --------------------------------------------------------*/
+/* USER CODE BEGIN ExternalFunctions */
+
+/* USER CODE END ExternalFunctions */
+
+/* USER CODE BEGIN 0 */
+
+/* USER CODE END 0 */
+/**
+  * Initializes the Global MSP.
+  */
+void HAL_MspInit(void)
+{
+  /* USER CODE BEGIN MspInit 0 */
+
+  /* USER CODE END MspInit 0 */
+
+  __HAL_RCC_AFIO_CLK_ENABLE();
+  __HAL_RCC_PWR_CLK_ENABLE();
+
+  /* System interrupt init*/
+
+  /* USER CODE BEGIN MspInit 1 */
+
+  /* USER CODE END MspInit 1 */
+}
+
+/* USER CODE BEGIN 1 */
+
+/* USER CODE END 1 */

2.Firmware/ICM42688P_HAL/Core/Src/stm32f1xx_it.c

@@ -0,0 +1,218 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    stm32f1xx_it.c
+  * @brief   Interrupt Service Routines.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+#include "stm32f1xx_it.h"
+/* Private includes ----------------------------------------------------------*/
+/* USER CODE BEGIN Includes */
+/* USER CODE END Includes */
+
+/* Private typedef -----------------------------------------------------------*/
+/* USER CODE BEGIN TD */
+
+/* USER CODE END TD */
+
+/* Private define ------------------------------------------------------------*/
+/* USER CODE BEGIN PD */
+
+/* USER CODE END PD */
+
+/* Private macro -------------------------------------------------------------*/
+/* USER CODE BEGIN PM */
+
+/* USER CODE END PM */
+
+/* Private variables ---------------------------------------------------------*/
+/* USER CODE BEGIN PV */
+
+/* USER CODE END PV */
+
+/* Private function prototypes -----------------------------------------------*/
+/* USER CODE BEGIN PFP */
+
+/* USER CODE END PFP */
+
+/* Private user code ---------------------------------------------------------*/
+/* USER CODE BEGIN 0 */
+
+/* USER CODE END 0 */
+
+/* External variables --------------------------------------------------------*/
+extern UART_HandleTypeDef huart1;
+/* USER CODE BEGIN EV */
+
+/* USER CODE END EV */
+
+/******************************************************************************/
+/*           Cortex-M3 Processor Interruption and Exception Handlers          */
+/******************************************************************************/
+/**
+  * @brief This function handles Non maskable interrupt.
+  */
+void NMI_Handler(void)
+{
+  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */
+
+  /* USER CODE END NonMaskableInt_IRQn 0 */
+  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
+  while (1)
+  {
+  }
+  /* USER CODE END NonMaskableInt_IRQn 1 */
+}
+
+/**
+  * @brief This function handles Hard fault interrupt.
+  */
+void HardFault_Handler(void)
+{
+  /* USER CODE BEGIN HardFault_IRQn 0 */
+
+  /* USER CODE END HardFault_IRQn 0 */
+  while (1)
+  {
+    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
+    /* USER CODE END W1_HardFault_IRQn 0 */
+  }
+}
+
+/**
+  * @brief This function handles Memory management fault.
+  */
+void MemManage_Handler(void)
+{
+  /* USER CODE BEGIN MemoryManagement_IRQn 0 */
+
+  /* USER CODE END MemoryManagement_IRQn 0 */
+  while (1)
+  {
+    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
+    /* USER CODE END W1_MemoryManagement_IRQn 0 */
+  }
+}
+
+/**
+  * @brief This function handles Prefetch fault, memory access fault.
+  */
+void BusFault_Handler(void)
+{
+  /* USER CODE BEGIN BusFault_IRQn 0 */
+
+  /* USER CODE END BusFault_IRQn 0 */
+  while (1)
+  {
+    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
+    /* USER CODE END W1_BusFault_IRQn 0 */
+  }
+}
+
+/**
+  * @brief This function handles Undefined instruction or illegal state.
+  */
+void UsageFault_Handler(void)
+{
+  /* USER CODE BEGIN UsageFault_IRQn 0 */
+
+  /* USER CODE END UsageFault_IRQn 0 */
+  while (1)
+  {
+    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
+    /* USER CODE END W1_UsageFault_IRQn 0 */
+  }
+}
+
+/**
+  * @brief This function handles System service call via SWI instruction.
+  */
+void SVC_Handler(void)
+{
+  /* USER CODE BEGIN SVCall_IRQn 0 */
+
+  /* USER CODE END SVCall_IRQn 0 */
+  /* USER CODE BEGIN SVCall_IRQn 1 */
+
+  /* USER CODE END SVCall_IRQn 1 */
+}
+
+/**
+  * @brief This function handles Debug monitor.
+  */
+void DebugMon_Handler(void)
+{
+  /* USER CODE BEGIN DebugMonitor_IRQn 0 */
+
+  /* USER CODE END DebugMonitor_IRQn 0 */
+  /* USER CODE BEGIN DebugMonitor_IRQn 1 */
+
+  /* USER CODE END DebugMonitor_IRQn 1 */
+}
+
+/**
+  * @brief This function handles Pendable request for system service.
+  */
+void PendSV_Handler(void)
+{
+  /* USER CODE BEGIN PendSV_IRQn 0 */
+
+  /* USER CODE END PendSV_IRQn 0 */
+  /* USER CODE BEGIN PendSV_IRQn 1 */
+
+  /* USER CODE END PendSV_IRQn 1 */
+}
+
+/**
+  * @brief This function handles System tick timer.
+  */
+void SysTick_Handler(void)
+{
+  /* USER CODE BEGIN SysTick_IRQn 0 */
+
+  /* USER CODE END SysTick_IRQn 0 */
+  HAL_IncTick();
+  /* USER CODE BEGIN SysTick_IRQn 1 */
+
+  /* USER CODE END SysTick_IRQn 1 */
+}
+
+/******************************************************************************/
+/* STM32F1xx Peripheral Interrupt Handlers                                    */
+/* Add here the Interrupt Handlers for the used peripherals.                  */
+/* For the available peripheral interrupt handler names,                      */
+/* please refer to the startup file (startup_stm32f1xx.s).                    */
+/******************************************************************************/
+
+/**
+  * @brief This function handles USART3 global interrupt.
+  */
+//void USART3_IRQHandler(void)
+//{
+//  /* USER CODE BEGIN USART3_IRQn 0 */
+
+//  /* USER CODE END USART3_IRQn 0 */
+//  HAL_UART_IRQHandler(&huart3);
+//  /* USER CODE BEGIN USART3_IRQn 1 */
+
+//  /* USER CODE END USART3_IRQn 1 */
+//}
+
+void USART1_IRQHandler(void)
+{
+  HAL_UART_IRQHandler(&huart1);
+}

2.Firmware/ICM42688P_HAL/Core/Src/system_stm32f1xx.c

@@ -0,0 +1,406 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32f1xx.c
+  * @author  MCD Application Team
+  * @brief   CMSIS Cortex-M3 Device Peripheral Access Layer System Source File.
+  * 
+  * 1.  This file provides two functions and one global variable to be called from 
+  *     user application:
+  *      - SystemInit(): Setups the system clock (System clock source, PLL Multiplier
+  *                      factors, AHB/APBx prescalers and Flash settings). 
+  *                      This function is called at startup just after reset and 
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32f1xx_xx.s" file.
+  *
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick 
+  *                                  timer or configure other parameters.
+  *                                     
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.
+  *
+  * 2. After each device reset the HSI (8 MHz) is used as system clock source.
+  *    Then SystemInit() function is called, in "startup_stm32f1xx_xx.s" file, to
+  *    configure the system clock before to branch to main program.
+  *
+  * 4. The default value of HSE crystal is set to 8 MHz (or 25 MHz, depending on
+  *    the product used), refer to "HSE_VALUE". 
+  *    When HSE is used as system clock source, directly or through PLL, and you
+  *    are using different crystal you have to adapt the HSE value to your own
+  *    configuration.
+  *        
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2017-2021 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32f1xx_system
+  * @{
+  */  
+  
+/** @addtogroup STM32F1xx_System_Private_Includes
+  * @{
+  */
+
+#include "stm32f1xx.h"
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_TypesDefinitions
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_Defines
+  * @{
+  */
+
+#if !defined  (HSE_VALUE) 
+  #define HSE_VALUE               8000000U /*!< Default value of the External oscillator in Hz.
+                                                This value can be provided and adapted by the user application. */
+#endif /* HSE_VALUE */
+
+#if !defined  (HSI_VALUE)
+  #define HSI_VALUE               8000000U /*!< Default value of the Internal oscillator in Hz.
+                                                This value can be provided and adapted by the user application. */
+#endif /* HSI_VALUE */
+
+/*!< Uncomment the following line if you need to use external SRAM  */ 
+#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG)
+/* #define DATA_IN_ExtSRAM */
+#endif /* STM32F100xE || STM32F101xE || STM32F101xG || STM32F103xE || STM32F103xG */
+
+/* Note: Following vector table addresses must be defined in line with linker
+         configuration. */
+/*!< Uncomment the following line if you need to relocate the vector table
+     anywhere in Flash or Sram, else the vector table is kept at the automatic
+     remap of boot address selected */
+/* #define USER_VECT_TAB_ADDRESS */
+
+#if defined(USER_VECT_TAB_ADDRESS)
+/*!< Uncomment the following line if you need to relocate your vector Table
+     in Sram else user remap will be done in Flash. */
+/* #define VECT_TAB_SRAM */
+#if defined(VECT_TAB_SRAM)
+#define VECT_TAB_BASE_ADDRESS   SRAM_BASE       /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table base offset field.
+                                                     This value must be a multiple of 0x200. */
+#else
+#define VECT_TAB_BASE_ADDRESS   FLASH_BASE      /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table base offset field.
+                                                     This value must be a multiple of 0x200. */
+#endif /* VECT_TAB_SRAM */
+#endif /* USER_VECT_TAB_ADDRESS */
+
+/******************************************************************************/
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_Variables
+  * @{
+  */
+
+  /* This variable is updated in three ways:
+      1) by calling CMSIS function SystemCoreClockUpdate()
+      2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+      3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency 
+         Note: If you use this function to configure the system clock; then there
+               is no need to call the 2 first functions listed above, since SystemCoreClock
+               variable is updated automatically.
+  */
+uint32_t SystemCoreClock = 8000000;
+const uint8_t AHBPrescTable[16U] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
+const uint8_t APBPrescTable[8U] =  {0, 0, 0, 0, 1, 2, 3, 4};
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_FunctionPrototypes
+  * @{
+  */
+
+#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG)
+#ifdef DATA_IN_ExtSRAM
+  static void SystemInit_ExtMemCtl(void); 
+#endif /* DATA_IN_ExtSRAM */
+#endif /* STM32F100xE || STM32F101xE || STM32F101xG || STM32F103xE || STM32F103xG */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32F1xx_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system
+  *         Initialize the Embedded Flash Interface, the PLL and update the 
+  *         SystemCoreClock variable.
+  * @note   This function should be used only after reset.
+  * @param  None
+  * @retval None
+  */
+void SystemInit (void)
+{
+#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG)
+  #ifdef DATA_IN_ExtSRAM
+    SystemInit_ExtMemCtl(); 
+  #endif /* DATA_IN_ExtSRAM */
+#endif 
+
+  /* Configure the Vector Table location -------------------------------------*/
+#if defined(USER_VECT_TAB_ADDRESS)
+  SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM. */
+#endif /* USER_VECT_TAB_ADDRESS */
+}
+
+/**
+  * @brief  Update SystemCoreClock variable according to Clock Register Values.
+  *         The SystemCoreClock variable contains the core clock (HCLK), it can
+  *         be used by the user application to setup the SysTick timer or configure
+  *         other parameters.
+  *           
+  * @note   Each time the core clock (HCLK) changes, this function must be called
+  *         to update SystemCoreClock variable value. Otherwise, any configuration
+  *         based on this variable will be incorrect.         
+  *     
+  * @note   - The system frequency computed by this function is not the real 
+  *           frequency in the chip. It is calculated based on the predefined 
+  *           constant and the selected clock source:
+  *             
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
+  *                                              
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
+  *                          
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) 
+  *             or HSI_VALUE(*) multiplied by the PLL factors.
+  *         
+  *         (*) HSI_VALUE is a constant defined in stm32f1xx.h file (default value
+  *             8 MHz) but the real value may vary depending on the variations
+  *             in voltage and temperature.   
+  *    
+  *         (**) HSE_VALUE is a constant defined in stm32f1xx.h file (default value
+  *              8 MHz or 25 MHz, depending on the product used), user has to ensure
+  *              that HSE_VALUE is same as the real frequency of the crystal used.
+  *              Otherwise, this function may have wrong result.
+  *                
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate (void)
+{
+  uint32_t tmp = 0U, pllmull = 0U, pllsource = 0U;
+
+#if defined(STM32F105xC) || defined(STM32F107xC)
+  uint32_t prediv1source = 0U, prediv1factor = 0U, prediv2factor = 0U, pll2mull = 0U;
+#endif /* STM32F105xC */
+
+#if defined(STM32F100xB) || defined(STM32F100xE)
+  uint32_t prediv1factor = 0U;
+#endif /* STM32F100xB or STM32F100xE */
+    
+  /* Get SYSCLK source -------------------------------------------------------*/
+  tmp = RCC->CFGR & RCC_CFGR_SWS;
+  
+  switch (tmp)
+  {
+    case 0x00U:  /* HSI used as system clock */
+      SystemCoreClock = HSI_VALUE;
+      break;
+    case 0x04U:  /* HSE used as system clock */
+      SystemCoreClock = HSE_VALUE;
+      break;
+    case 0x08U:  /* PLL used as system clock */
+
+      /* Get PLL clock source and multiplication factor ----------------------*/
+      pllmull = RCC->CFGR & RCC_CFGR_PLLMULL;
+      pllsource = RCC->CFGR & RCC_CFGR_PLLSRC;
+      
+#if !defined(STM32F105xC) && !defined(STM32F107xC)      
+      pllmull = ( pllmull >> 18U) + 2U;
+      
+      if (pllsource == 0x00U)
+      {
+        /* HSI oscillator clock divided by 2 selected as PLL clock entry */
+        SystemCoreClock = (HSI_VALUE >> 1U) * pllmull;
+      }
+      else
+      {
+ #if defined(STM32F100xB) || defined(STM32F100xE)
+       prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1U;
+       /* HSE oscillator clock selected as PREDIV1 clock entry */
+       SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull; 
+ #else
+        /* HSE selected as PLL clock entry */
+        if ((RCC->CFGR & RCC_CFGR_PLLXTPRE) != (uint32_t)RESET)
+        {/* HSE oscillator clock divided by 2 */
+          SystemCoreClock = (HSE_VALUE >> 1U) * pllmull;
+        }
+        else
+        {
+          SystemCoreClock = HSE_VALUE * pllmull;
+        }
+ #endif
+      }
+#else
+      pllmull = pllmull >> 18U;
+      
+      if (pllmull != 0x0DU)
+      {
+         pllmull += 2U;
+      }
+      else
+      { /* PLL multiplication factor = PLL input clock * 6.5 */
+        pllmull = 13U / 2U; 
+      }
+            
+      if (pllsource == 0x00U)
+      {
+        /* HSI oscillator clock divided by 2 selected as PLL clock entry */
+        SystemCoreClock = (HSI_VALUE >> 1U) * pllmull;
+      }
+      else
+      {/* PREDIV1 selected as PLL clock entry */
+        
+        /* Get PREDIV1 clock source and division factor */
+        prediv1source = RCC->CFGR2 & RCC_CFGR2_PREDIV1SRC;
+        prediv1factor = (RCC->CFGR2 & RCC_CFGR2_PREDIV1) + 1U;
+        
+        if (prediv1source == 0U)
+        { 
+          /* HSE oscillator clock selected as PREDIV1 clock entry */
+          SystemCoreClock = (HSE_VALUE / prediv1factor) * pllmull;          
+        }
+        else
+        {/* PLL2 clock selected as PREDIV1 clock entry */
+          
+          /* Get PREDIV2 division factor and PLL2 multiplication factor */
+          prediv2factor = ((RCC->CFGR2 & RCC_CFGR2_PREDIV2) >> 4U) + 1U;
+          pll2mull = ((RCC->CFGR2 & RCC_CFGR2_PLL2MUL) >> 8U) + 2U; 
+          SystemCoreClock = (((HSE_VALUE / prediv2factor) * pll2mull) / prediv1factor) * pllmull;                         
+        }
+      }
+#endif /* STM32F105xC */ 
+      break;
+
+    default:
+      SystemCoreClock = HSI_VALUE;
+      break;
+  }
+  
+  /* Compute HCLK clock frequency ----------------*/
+  /* Get HCLK prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4U)];
+  /* HCLK clock frequency */
+  SystemCoreClock >>= tmp;  
+}
+
+#if defined(STM32F100xE) || defined(STM32F101xE) || defined(STM32F101xG) || defined(STM32F103xE) || defined(STM32F103xG)
+/**
+  * @brief  Setup the external memory controller. Called in startup_stm32f1xx.s 
+  *          before jump to __main
+  * @param  None
+  * @retval None
+  */ 
+#ifdef DATA_IN_ExtSRAM
+/**
+  * @brief  Setup the external memory controller. 
+  *         Called in startup_stm32f1xx_xx.s/.c before jump to main.
+  *         This function configures the external SRAM mounted on STM3210E-EVAL
+  *         board (STM32 High density devices). This SRAM will be used as program
+  *         data memory (including heap and stack).
+  * @param  None
+  * @retval None
+  */ 
+void SystemInit_ExtMemCtl(void) 
+{
+  __IO uint32_t tmpreg;
+  /*!< FSMC Bank1 NOR/SRAM3 is used for the STM3210E-EVAL, if another Bank is 
+    required, then adjust the Register Addresses */
+
+  /* Enable FSMC clock */
+  RCC->AHBENR = 0x00000114U;
+
+  /* Delay after an RCC peripheral clock enabling */
+  tmpreg = READ_BIT(RCC->AHBENR, RCC_AHBENR_FSMCEN);
+  
+  /* Enable GPIOD, GPIOE, GPIOF and GPIOG clocks */
+  RCC->APB2ENR = 0x000001E0U;
+  
+  /* Delay after an RCC peripheral clock enabling */
+  tmpreg = READ_BIT(RCC->APB2ENR, RCC_APB2ENR_IOPDEN);
+
+  (void)(tmpreg);
+  
+/* ---------------  SRAM Data lines, NOE and NWE configuration ---------------*/
+/*----------------  SRAM Address lines configuration -------------------------*/
+/*----------------  NOE and NWE configuration --------------------------------*/  
+/*----------------  NE3 configuration ----------------------------------------*/
+/*----------------  NBL0, NBL1 configuration ---------------------------------*/
+  
+  GPIOD->CRL = 0x44BB44BBU;  
+  GPIOD->CRH = 0xBBBBBBBBU;
+
+  GPIOE->CRL = 0xB44444BBU;  
+  GPIOE->CRH = 0xBBBBBBBBU;
+
+  GPIOF->CRL = 0x44BBBBBBU;  
+  GPIOF->CRH = 0xBBBB4444U;
+
+  GPIOG->CRL = 0x44BBBBBBU;  
+  GPIOG->CRH = 0x444B4B44U;
+   
+/*----------------  FSMC Configuration ---------------------------------------*/  
+/*----------------  Enable FSMC Bank1_SRAM Bank ------------------------------*/
+  
+  FSMC_Bank1->BTCR[4U] = 0x00001091U;
+  FSMC_Bank1->BTCR[5U] = 0x00110212U;
+}
+#endif /* DATA_IN_ExtSRAM */
+#endif /* STM32F100xE || STM32F101xE || STM32F101xG || STM32F103xE || STM32F103xG */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+  
+/**
+  * @}
+  */

2.Firmware/ICM42688P_HAL/Core/Src/usart.c

@@ -0,0 +1,134 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    usart.c
+  * @brief   This file provides code for the configuration
+  *          of the USART instances.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+/* Includes ------------------------------------------------------------------*/
+#include "usart.h"
+
+
+UART_HandleTypeDef huart1;
+
+/* USART3 init function */
+
+void MX_USART1_UART_Init(void)
+{
+	huart1.Instance = USART1;
+  huart1.Init.BaudRate = 115200;
+  huart1.Init.WordLength = UART_WORDLENGTH_8B;
+  huart1.Init.StopBits = UART_STOPBITS_1;
+  huart1.Init.Parity = UART_PARITY_NONE;
+  huart1.Init.Mode = UART_MODE_TX_RX;
+  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
+  if (HAL_UART_Init(&huart1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+}
+
+void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
+{
+
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(uartHandle->Instance==USART3)
+  {
+  /* USER CODE BEGIN USART3_MspInit 0 */
+
+  /* USER CODE END USART3_MspInit 0 */
+    /* USART3 clock enable */
+    __HAL_RCC_USART3_CLK_ENABLE();
+
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    /**USART3 GPIO Configuration
+    PC10     ------> USART3_TX
+    PC11     ------> USART3_RX
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_10;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+    GPIO_InitStruct.Pin = GPIO_PIN_11;
+    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+    __HAL_AFIO_REMAP_USART3_PARTIAL();
+
+    /* USART3 interrupt Init */
+    HAL_NVIC_SetPriority(USART3_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(USART3_IRQn);
+		
+  }
+	
+	if(uartHandle->Instance==USART1)
+  {
+    /* Peripheral clock enable */
+    __HAL_RCC_USART1_CLK_ENABLE();
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**USART1 GPIO Configuration
+    PA9     ------> USART1_TX
+    PA10     ------> USART1_RX
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_9;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    GPIO_InitStruct.Pin = GPIO_PIN_10;
+    GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+  }
+}
+
+void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
+{
+
+  if(uartHandle->Instance==USART3)
+  {
+    /* Peripheral clock disable */
+    __HAL_RCC_USART3_CLK_DISABLE();
+
+    /**USART3 GPIO Configuration
+    PC10     ------> USART3_TX
+    PC11     ------> USART3_RX
+    */
+    HAL_GPIO_DeInit(GPIOC, GPIO_PIN_10|GPIO_PIN_11);
+
+    /* USART3 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(USART3_IRQn);
+  }
+	
+	if(uartHandle->Instance==USART1)
+  {
+  /* USER CODE BEGIN USART1_MspDeInit 0 */
+
+  /* USER CODE END USART1_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_USART1_CLK_DISABLE();
+
+    /**USART1 GPIO Configuration
+    PA9     ------> USART1_TX
+    PA10     ------> USART1_RX
+    */
+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10);
+  }
+	
+}

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Include/cmsis_armcc.h

@@ -0,0 +1,865 @@
+/**************************************************************************//**
+ * @file     cmsis_armcc.h
+ * @brief    CMSIS compiler ARMCC (Arm Compiler 5) header file
+ * @version  V5.0.4
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __CMSIS_ARMCC_H
+#define __CMSIS_ARMCC_H
+
+
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 400677)
+  #error "Please use Arm Compiler Toolchain V4.0.677 or later!"
+#endif
+
+/* CMSIS compiler control architecture macros */
+#if ((defined (__TARGET_ARCH_6_M  ) && (__TARGET_ARCH_6_M   == 1)) || \
+     (defined (__TARGET_ARCH_6S_M ) && (__TARGET_ARCH_6S_M  == 1))   )
+  #define __ARM_ARCH_6M__           1
+#endif
+
+#if (defined (__TARGET_ARCH_7_M ) && (__TARGET_ARCH_7_M  == 1))
+  #define __ARM_ARCH_7M__           1
+#endif
+
+#if (defined (__TARGET_ARCH_7E_M) && (__TARGET_ARCH_7E_M == 1))
+  #define __ARM_ARCH_7EM__          1
+#endif
+
+  /* __ARM_ARCH_8M_BASE__  not applicable */
+  /* __ARM_ARCH_8M_MAIN__  not applicable */
+
+
+/* CMSIS compiler specific defines */
+#ifndef   __ASM
+  #define __ASM                                  __asm
+#endif
+#ifndef   __INLINE
+  #define __INLINE                               __inline
+#endif
+#ifndef   __STATIC_INLINE
+  #define __STATIC_INLINE                        static __inline
+#endif
+#ifndef   __STATIC_FORCEINLINE                 
+  #define __STATIC_FORCEINLINE                   static __forceinline
+#endif           
+#ifndef   __NO_RETURN
+  #define __NO_RETURN                            __declspec(noreturn)
+#endif
+#ifndef   __USED
+  #define __USED                                 __attribute__((used))
+#endif
+#ifndef   __WEAK
+  #define __WEAK                                 __attribute__((weak))
+#endif
+#ifndef   __PACKED
+  #define __PACKED                               __attribute__((packed))
+#endif
+#ifndef   __PACKED_STRUCT
+  #define __PACKED_STRUCT                        __packed struct
+#endif
+#ifndef   __PACKED_UNION
+  #define __PACKED_UNION                         __packed union
+#endif
+#ifndef   __UNALIGNED_UINT32        /* deprecated */
+  #define __UNALIGNED_UINT32(x)                  (*((__packed uint32_t *)(x)))
+#endif
+#ifndef   __UNALIGNED_UINT16_WRITE
+  #define __UNALIGNED_UINT16_WRITE(addr, val)    ((*((__packed uint16_t *)(addr))) = (val))
+#endif
+#ifndef   __UNALIGNED_UINT16_READ
+  #define __UNALIGNED_UINT16_READ(addr)          (*((const __packed uint16_t *)(addr)))
+#endif
+#ifndef   __UNALIGNED_UINT32_WRITE
+  #define __UNALIGNED_UINT32_WRITE(addr, val)    ((*((__packed uint32_t *)(addr))) = (val))
+#endif
+#ifndef   __UNALIGNED_UINT32_READ
+  #define __UNALIGNED_UINT32_READ(addr)          (*((const __packed uint32_t *)(addr)))
+#endif
+#ifndef   __ALIGNED
+  #define __ALIGNED(x)                           __attribute__((aligned(x)))
+#endif
+#ifndef   __RESTRICT
+  #define __RESTRICT                             __restrict
+#endif
+
+/* ###########################  Core Function Access  ########################### */
+/** \ingroup  CMSIS_Core_FunctionInterface
+    \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
+  @{
+ */
+
+/**
+  \brief   Enable IRQ Interrupts
+  \details Enables IRQ interrupts by clearing the I-bit in the CPSR.
+           Can only be executed in Privileged modes.
+ */
+/* intrinsic void __enable_irq();     */
+
+
+/**
+  \brief   Disable IRQ Interrupts
+  \details Disables IRQ interrupts by setting the I-bit in the CPSR.
+           Can only be executed in Privileged modes.
+ */
+/* intrinsic void __disable_irq();    */
+
+/**
+  \brief   Get Control Register
+  \details Returns the content of the Control Register.
+  \return               Control Register value
+ */
+__STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+  register uint32_t __regControl         __ASM("control");
+  return(__regControl);
+}
+
+
+/**
+  \brief   Set Control Register
+  \details Writes the given value to the Control Register.
+  \param [in]    control  Control Register value to set
+ */
+__STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+  register uint32_t __regControl         __ASM("control");
+  __regControl = control;
+}
+
+
+/**
+  \brief   Get IPSR Register
+  \details Returns the content of the IPSR Register.
+  \return               IPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_IPSR(void)
+{
+  register uint32_t __regIPSR          __ASM("ipsr");
+  return(__regIPSR);
+}
+
+
+/**
+  \brief   Get APSR Register
+  \details Returns the content of the APSR Register.
+  \return               APSR Register value
+ */
+__STATIC_INLINE uint32_t __get_APSR(void)
+{
+  register uint32_t __regAPSR          __ASM("apsr");
+  return(__regAPSR);
+}
+
+
+/**
+  \brief   Get xPSR Register
+  \details Returns the content of the xPSR Register.
+  \return               xPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_xPSR(void)
+{
+  register uint32_t __regXPSR          __ASM("xpsr");
+  return(__regXPSR);
+}
+
+
+/**
+  \brief   Get Process Stack Pointer
+  \details Returns the current value of the Process Stack Pointer (PSP).
+  \return               PSP Register value
+ */
+__STATIC_INLINE uint32_t __get_PSP(void)
+{
+  register uint32_t __regProcessStackPointer  __ASM("psp");
+  return(__regProcessStackPointer);
+}
+
+
+/**
+  \brief   Set Process Stack Pointer
+  \details Assigns the given value to the Process Stack Pointer (PSP).
+  \param [in]    topOfProcStack  Process Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+  register uint32_t __regProcessStackPointer  __ASM("psp");
+  __regProcessStackPointer = topOfProcStack;
+}
+
+
+/**
+  \brief   Get Main Stack Pointer
+  \details Returns the current value of the Main Stack Pointer (MSP).
+  \return               MSP Register value
+ */
+__STATIC_INLINE uint32_t __get_MSP(void)
+{
+  register uint32_t __regMainStackPointer     __ASM("msp");
+  return(__regMainStackPointer);
+}
+
+
+/**
+  \brief   Set Main Stack Pointer
+  \details Assigns the given value to the Main Stack Pointer (MSP).
+  \param [in]    topOfMainStack  Main Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+  register uint32_t __regMainStackPointer     __ASM("msp");
+  __regMainStackPointer = topOfMainStack;
+}
+
+
+/**
+  \brief   Get Priority Mask
+  \details Returns the current state of the priority mask bit from the Priority Mask Register.
+  \return               Priority Mask value
+ */
+__STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+  register uint32_t __regPriMask         __ASM("primask");
+  return(__regPriMask);
+}
+
+
+/**
+  \brief   Set Priority Mask
+  \details Assigns the given value to the Priority Mask Register.
+  \param [in]    priMask  Priority Mask
+ */
+__STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+  register uint32_t __regPriMask         __ASM("primask");
+  __regPriMask = (priMask);
+}
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__  == 1)) || \
+     (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1))     )
+
+/**
+  \brief   Enable FIQ
+  \details Enables FIQ interrupts by clearing the F-bit in the CPSR.
+           Can only be executed in Privileged modes.
+ */
+#define __enable_fault_irq                __enable_fiq
+
+
+/**
+  \brief   Disable FIQ
+  \details Disables FIQ interrupts by setting the F-bit in the CPSR.
+           Can only be executed in Privileged modes.
+ */
+#define __disable_fault_irq               __disable_fiq
+
+
+/**
+  \brief   Get Base Priority
+  \details Returns the current value of the Base Priority register.
+  \return               Base Priority register value
+ */
+__STATIC_INLINE uint32_t  __get_BASEPRI(void)
+{
+  register uint32_t __regBasePri         __ASM("basepri");
+  return(__regBasePri);
+}
+
+
+/**
+  \brief   Set Base Priority
+  \details Assigns the given value to the Base Priority register.
+  \param [in]    basePri  Base Priority value to set
+ */
+__STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
+{
+  register uint32_t __regBasePri         __ASM("basepri");
+  __regBasePri = (basePri & 0xFFU);
+}
+
+
+/**
+  \brief   Set Base Priority with condition
+  \details Assigns the given value to the Base Priority register only if BASEPRI masking is disabled,
+           or the new value increases the BASEPRI priority level.
+  \param [in]    basePri  Base Priority value to set
+ */
+__STATIC_INLINE void __set_BASEPRI_MAX(uint32_t basePri)
+{
+  register uint32_t __regBasePriMax      __ASM("basepri_max");
+  __regBasePriMax = (basePri & 0xFFU);
+}
+
+
+/**
+  \brief   Get Fault Mask
+  \details Returns the current value of the Fault Mask register.
+  \return               Fault Mask register value
+ */
+__STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+  register uint32_t __regFaultMask       __ASM("faultmask");
+  return(__regFaultMask);
+}
+
+
+/**
+  \brief   Set Fault Mask
+  \details Assigns the given value to the Fault Mask register.
+  \param [in]    faultMask  Fault Mask value to set
+ */
+__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+  register uint32_t __regFaultMask       __ASM("faultmask");
+  __regFaultMask = (faultMask & (uint32_t)1U);
+}
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__  == 1)) || \
+           (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1))     ) */
+
+
+/**
+  \brief   Get FPSCR
+  \details Returns the current value of the Floating Point Status/Control register.
+  \return               Floating Point Status/Control register value
+ */
+__STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+     (defined (__FPU_USED   ) && (__FPU_USED    == 1U))     )
+  register uint32_t __regfpscr         __ASM("fpscr");
+  return(__regfpscr);
+#else
+   return(0U);
+#endif
+}
+
+
+/**
+  \brief   Set FPSCR
+  \details Assigns the given value to the Floating Point Status/Control register.
+  \param [in]    fpscr  Floating Point Status/Control value to set
+ */
+__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+     (defined (__FPU_USED   ) && (__FPU_USED    == 1U))     )
+  register uint32_t __regfpscr         __ASM("fpscr");
+  __regfpscr = (fpscr);
+#else
+  (void)fpscr;
+#endif
+}
+
+
+/*@} end of CMSIS_Core_RegAccFunctions */
+
+
+/* ##########################  Core Instruction Access  ######################### */
+/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
+  Access to dedicated instructions
+  @{
+*/
+
+/**
+  \brief   No Operation
+  \details No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+#define __NOP                             __nop
+
+
+/**
+  \brief   Wait For Interrupt
+  \details Wait For Interrupt is a hint instruction that suspends execution until one of a number of events occurs.
+ */
+#define __WFI                             __wfi
+
+
+/**
+  \brief   Wait For Event
+  \details Wait For Event is a hint instruction that permits the processor to enter
+           a low-power state until one of a number of events occurs.
+ */
+#define __WFE                             __wfe
+
+
+/**
+  \brief   Send Event
+  \details Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+#define __SEV                             __sev
+
+
+/**
+  \brief   Instruction Synchronization Barrier
+  \details Instruction Synchronization Barrier flushes the pipeline in the processor,
+           so that all instructions following the ISB are fetched from cache or memory,
+           after the instruction has been completed.
+ */
+#define __ISB() do {\
+                   __schedule_barrier();\
+                   __isb(0xF);\
+                   __schedule_barrier();\
+                } while (0U)
+
+/**
+  \brief   Data Synchronization Barrier
+  \details Acts as a special kind of Data Memory Barrier.
+           It completes when all explicit memory accesses before this instruction complete.
+ */
+#define __DSB() do {\
+                   __schedule_barrier();\
+                   __dsb(0xF);\
+                   __schedule_barrier();\
+                } while (0U)
+
+/**
+  \brief   Data Memory Barrier
+  \details Ensures the apparent order of the explicit memory operations before
+           and after the instruction, without ensuring their completion.
+ */
+#define __DMB() do {\
+                   __schedule_barrier();\
+                   __dmb(0xF);\
+                   __schedule_barrier();\
+                } while (0U)
+
+                  
+/**
+  \brief   Reverse byte order (32 bit)
+  \details Reverses the byte order in unsigned integer value. For example, 0x12345678 becomes 0x78563412.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#define __REV                             __rev
+
+
+/**
+  \brief   Reverse byte order (16 bit)
+  \details Reverses the byte order within each halfword of a word. For example, 0x12345678 becomes 0x34127856.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)
+{
+  rev16 r0, r0
+  bx lr
+}
+#endif
+
+
+/**
+  \brief   Reverse byte order (16 bit)
+  \details Reverses the byte order in a 16-bit value and returns the signed 16-bit result. For example, 0x0080 becomes 0x8000.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int16_t __REVSH(int16_t value)
+{
+  revsh r0, r0
+  bx lr
+}
+#endif
+
+
+/**
+  \brief   Rotate Right in unsigned value (32 bit)
+  \details Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+  \param [in]    op1  Value to rotate
+  \param [in]    op2  Number of Bits to rotate
+  \return               Rotated value
+ */
+#define __ROR                             __ror
+
+
+/**
+  \brief   Breakpoint
+  \details Causes the processor to enter Debug state.
+           Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+  \param [in]    value  is ignored by the processor.
+                 If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value)                       __breakpoint(value)
+
+
+/**
+  \brief   Reverse bit order of value
+  \details Reverses the bit order of the given value.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__  == 1)) || \
+     (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1))     )
+  #define __RBIT                          __rbit
+#else
+__attribute__((always_inline)) __STATIC_INLINE uint32_t __RBIT(uint32_t value)
+{
+  uint32_t result;
+  uint32_t s = (4U /*sizeof(v)*/ * 8U) - 1U; /* extra shift needed at end */
+
+  result = value;                      /* r will be reversed bits of v; first get LSB of v */
+  for (value >>= 1U; value != 0U; value >>= 1U)
+  {
+    result <<= 1U;
+    result |= value & 1U;
+    s--;
+  }
+  result <<= s;                        /* shift when v's highest bits are zero */
+  return result;
+}
+#endif
+
+
+/**
+  \brief   Count leading zeros
+  \details Counts the number of leading zeros of a data value.
+  \param [in]  value  Value to count the leading zeros
+  \return             number of leading zeros in value
+ */
+#define __CLZ                             __clz
+
+
+#if ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__  == 1)) || \
+     (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1))     )
+
+/**
+  \brief   LDR Exclusive (8 bit)
+  \details Executes a exclusive LDR instruction for 8 bit value.
+  \param [in]    ptr  Pointer to data
+  \return             value of type uint8_t at (*ptr)
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __LDREXB(ptr)                                                        ((uint8_t ) __ldrex(ptr))
+#else
+  #define __LDREXB(ptr)          _Pragma("push") _Pragma("diag_suppress 3731") ((uint8_t ) __ldrex(ptr))  _Pragma("pop")
+#endif
+
+
+/**
+  \brief   LDR Exclusive (16 bit)
+  \details Executes a exclusive LDR instruction for 16 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint16_t at (*ptr)
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __LDREXH(ptr)                                                        ((uint16_t) __ldrex(ptr))
+#else
+  #define __LDREXH(ptr)          _Pragma("push") _Pragma("diag_suppress 3731") ((uint16_t) __ldrex(ptr))  _Pragma("pop")
+#endif
+
+
+/**
+  \brief   LDR Exclusive (32 bit)
+  \details Executes a exclusive LDR instruction for 32 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint32_t at (*ptr)
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __LDREXW(ptr)                                                        ((uint32_t ) __ldrex(ptr))
+#else
+  #define __LDREXW(ptr)          _Pragma("push") _Pragma("diag_suppress 3731") ((uint32_t ) __ldrex(ptr))  _Pragma("pop")
+#endif
+
+
+/**
+  \brief   STR Exclusive (8 bit)
+  \details Executes a exclusive STR instruction for 8 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __STREXB(value, ptr)                                                 __strex(value, ptr)
+#else
+  #define __STREXB(value, ptr)   _Pragma("push") _Pragma("diag_suppress 3731") __strex(value, ptr)        _Pragma("pop")
+#endif
+
+
+/**
+  \brief   STR Exclusive (16 bit)
+  \details Executes a exclusive STR instruction for 16 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __STREXH(value, ptr)                                                 __strex(value, ptr)
+#else
+  #define __STREXH(value, ptr)   _Pragma("push") _Pragma("diag_suppress 3731") __strex(value, ptr)        _Pragma("pop")
+#endif
+
+
+/**
+  \brief   STR Exclusive (32 bit)
+  \details Executes a exclusive STR instruction for 32 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __STREXW(value, ptr)                                                 __strex(value, ptr)
+#else
+  #define __STREXW(value, ptr)   _Pragma("push") _Pragma("diag_suppress 3731") __strex(value, ptr)        _Pragma("pop")
+#endif
+
+
+/**
+  \brief   Remove the exclusive lock
+  \details Removes the exclusive lock which is created by LDREX.
+ */
+#define __CLREX                           __clrex
+
+
+/**
+  \brief   Signed Saturate
+  \details Saturates a signed value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (1..32)
+  \return             Saturated value
+ */
+#define __SSAT                            __ssat
+
+
+/**
+  \brief   Unsigned Saturate
+  \details Saturates an unsigned value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (0..31)
+  \return             Saturated value
+ */
+#define __USAT                            __usat
+
+
+/**
+  \brief   Rotate Right with Extend (32 bit)
+  \details Moves each bit of a bitstring right by one bit.
+           The carry input is shifted in at the left end of the bitstring.
+  \param [in]    value  Value to rotate
+  \return               Rotated value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".rrx_text"))) __STATIC_INLINE __ASM uint32_t __RRX(uint32_t value)
+{
+  rrx r0, r0
+  bx lr
+}
+#endif
+
+
+/**
+  \brief   LDRT Unprivileged (8 bit)
+  \details Executes a Unprivileged LDRT instruction for 8 bit value.
+  \param [in]    ptr  Pointer to data
+  \return             value of type uint8_t at (*ptr)
+ */
+#define __LDRBT(ptr)                      ((uint8_t )  __ldrt(ptr))
+
+
+/**
+  \brief   LDRT Unprivileged (16 bit)
+  \details Executes a Unprivileged LDRT instruction for 16 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint16_t at (*ptr)
+ */
+#define __LDRHT(ptr)                      ((uint16_t)  __ldrt(ptr))
+
+
+/**
+  \brief   LDRT Unprivileged (32 bit)
+  \details Executes a Unprivileged LDRT instruction for 32 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint32_t at (*ptr)
+ */
+#define __LDRT(ptr)                       ((uint32_t ) __ldrt(ptr))
+
+
+/**
+  \brief   STRT Unprivileged (8 bit)
+  \details Executes a Unprivileged STRT instruction for 8 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+ */
+#define __STRBT(value, ptr)               __strt(value, ptr)
+
+
+/**
+  \brief   STRT Unprivileged (16 bit)
+  \details Executes a Unprivileged STRT instruction for 16 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+ */
+#define __STRHT(value, ptr)               __strt(value, ptr)
+
+
+/**
+  \brief   STRT Unprivileged (32 bit)
+  \details Executes a Unprivileged STRT instruction for 32 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+ */
+#define __STRT(value, ptr)                __strt(value, ptr)
+
+#else  /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__  == 1)) || \
+           (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1))     ) */
+
+/**
+  \brief   Signed Saturate
+  \details Saturates a signed value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (1..32)
+  \return             Saturated value
+ */
+__attribute__((always_inline)) __STATIC_INLINE int32_t __SSAT(int32_t val, uint32_t sat)
+{
+  if ((sat >= 1U) && (sat <= 32U))
+  {
+    const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
+    const int32_t min = -1 - max ;
+    if (val > max)
+    {
+      return max;
+    }
+    else if (val < min)
+    {
+      return min;
+    }
+  }
+  return val;
+}
+
+/**
+  \brief   Unsigned Saturate
+  \details Saturates an unsigned value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (0..31)
+  \return             Saturated value
+ */
+__attribute__((always_inline)) __STATIC_INLINE uint32_t __USAT(int32_t val, uint32_t sat)
+{
+  if (sat <= 31U)
+  {
+    const uint32_t max = ((1U << sat) - 1U);
+    if (val > (int32_t)max)
+    {
+      return max;
+    }
+    else if (val < 0)
+    {
+      return 0U;
+    }
+  }
+  return (uint32_t)val;
+}
+
+#endif /* ((defined (__ARM_ARCH_7M__ ) && (__ARM_ARCH_7M__  == 1)) || \
+           (defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1))     ) */
+
+/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
+
+
+/* ###################  Compiler specific Intrinsics  ########################### */
+/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
+  Access to dedicated SIMD instructions
+  @{
+*/
+
+#if ((defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1))     )
+
+#define __SADD8                           __sadd8
+#define __QADD8                           __qadd8
+#define __SHADD8                          __shadd8
+#define __UADD8                           __uadd8
+#define __UQADD8                          __uqadd8
+#define __UHADD8                          __uhadd8
+#define __SSUB8                           __ssub8
+#define __QSUB8                           __qsub8
+#define __SHSUB8                          __shsub8
+#define __USUB8                           __usub8
+#define __UQSUB8                          __uqsub8
+#define __UHSUB8                          __uhsub8
+#define __SADD16                          __sadd16
+#define __QADD16                          __qadd16
+#define __SHADD16                         __shadd16
+#define __UADD16                          __uadd16
+#define __UQADD16                         __uqadd16
+#define __UHADD16                         __uhadd16
+#define __SSUB16                          __ssub16
+#define __QSUB16                          __qsub16
+#define __SHSUB16                         __shsub16
+#define __USUB16                          __usub16
+#define __UQSUB16                         __uqsub16
+#define __UHSUB16                         __uhsub16
+#define __SASX                            __sasx
+#define __QASX                            __qasx
+#define __SHASX                           __shasx
+#define __UASX                            __uasx
+#define __UQASX                           __uqasx
+#define __UHASX                           __uhasx
+#define __SSAX                            __ssax
+#define __QSAX                            __qsax
+#define __SHSAX                           __shsax
+#define __USAX                            __usax
+#define __UQSAX                           __uqsax
+#define __UHSAX                           __uhsax
+#define __USAD8                           __usad8
+#define __USADA8                          __usada8
+#define __SSAT16                          __ssat16
+#define __USAT16                          __usat16
+#define __UXTB16                          __uxtb16
+#define __UXTAB16                         __uxtab16
+#define __SXTB16                          __sxtb16
+#define __SXTAB16                         __sxtab16
+#define __SMUAD                           __smuad
+#define __SMUADX                          __smuadx
+#define __SMLAD                           __smlad
+#define __SMLADX                          __smladx
+#define __SMLALD                          __smlald
+#define __SMLALDX                         __smlaldx
+#define __SMUSD                           __smusd
+#define __SMUSDX                          __smusdx
+#define __SMLSD                           __smlsd
+#define __SMLSDX                          __smlsdx
+#define __SMLSLD                          __smlsld
+#define __SMLSLDX                         __smlsldx
+#define __SEL                             __sel
+#define __QADD                            __qadd
+#define __QSUB                            __qsub
+
+#define __PKHBT(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0x0000FFFFUL) |  \
+                                           ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL)  )
+
+#define __PKHTB(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0xFFFF0000UL) |  \
+                                           ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL)  )
+
+#define __SMMLA(ARG1,ARG2,ARG3)          ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \
+                                                      ((int64_t)(ARG3) << 32U)     ) >> 32U))
+
+#endif /* ((defined (__ARM_ARCH_7EM__) && (__ARM_ARCH_7EM__ == 1))     ) */
+/*@} end of group CMSIS_SIMD_intrinsics */
+
+
+#endif /* __CMSIS_ARMCC_H */

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Include/cmsis_compiler.h

@@ -0,0 +1,266 @@
+/**************************************************************************//**
+ * @file     cmsis_compiler.h
+ * @brief    CMSIS compiler generic header file
+ * @version  V5.0.4
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __CMSIS_COMPILER_H
+#define __CMSIS_COMPILER_H
+
+#include <stdint.h>
+
+/*
+ * Arm Compiler 4/5
+ */
+#if   defined ( __CC_ARM )
+  #include "cmsis_armcc.h"
+
+
+/*
+ * Arm Compiler 6 (armclang)
+ */
+#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
+  #include "cmsis_armclang.h"
+
+
+/*
+ * GNU Compiler
+ */
+#elif defined ( __GNUC__ )
+  #include "cmsis_gcc.h"
+
+
+/*
+ * IAR Compiler
+ */
+#elif defined ( __ICCARM__ )
+  #include <cmsis_iccarm.h>
+
+
+/*
+ * TI Arm Compiler
+ */
+#elif defined ( __TI_ARM__ )
+  #include <cmsis_ccs.h>
+
+  #ifndef   __ASM
+    #define __ASM                                  __asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                               inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE                        static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE                   __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    #define __NO_RETURN                            __attribute__((noreturn))
+  #endif
+  #ifndef   __USED
+    #define __USED                                 __attribute__((used))
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                                 __attribute__((weak))
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                               __attribute__((packed))
+  #endif
+  #ifndef   __PACKED_STRUCT
+    #define __PACKED_STRUCT                        struct __attribute__((packed))
+  #endif
+  #ifndef   __PACKED_UNION
+    #define __PACKED_UNION                         union __attribute__((packed))
+  #endif
+  #ifndef   __UNALIGNED_UINT32        /* deprecated */
+    struct __attribute__((packed)) T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)                  (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT16_WRITE
+    __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+    #define __UNALIGNED_UINT16_WRITE(addr, val)    (void)((((struct T_UINT16_WRITE *)(void*)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT16_READ
+    __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+    #define __UNALIGNED_UINT16_READ(addr)          (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT32_WRITE
+    __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+    #define __UNALIGNED_UINT32_WRITE(addr, val)    (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT32_READ
+    __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+    #define __UNALIGNED_UINT32_READ(addr)          (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #define __ALIGNED(x)                           __attribute__((aligned(x)))
+  #endif
+  #ifndef   __RESTRICT
+    #warning No compiler specific solution for __RESTRICT. __RESTRICT is ignored.
+    #define __RESTRICT
+  #endif
+
+
+/*
+ * TASKING Compiler
+ */
+#elif defined ( __TASKING__ )
+  /*
+   * The CMSIS functions have been implemented as intrinsics in the compiler.
+   * Please use "carm -?i" to get an up to date list of all intrinsics,
+   * Including the CMSIS ones.
+   */
+
+  #ifndef   __ASM
+    #define __ASM                                  __asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                               inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE                        static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE                   __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    #define __NO_RETURN                            __attribute__((noreturn))
+  #endif
+  #ifndef   __USED
+    #define __USED                                 __attribute__((used))
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                                 __attribute__((weak))
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                               __packed__
+  #endif
+  #ifndef   __PACKED_STRUCT
+    #define __PACKED_STRUCT                        struct __packed__
+  #endif
+  #ifndef   __PACKED_UNION
+    #define __PACKED_UNION                         union __packed__
+  #endif
+  #ifndef   __UNALIGNED_UINT32        /* deprecated */
+    struct __packed__ T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)                  (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT16_WRITE
+    __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+    #define __UNALIGNED_UINT16_WRITE(addr, val)    (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT16_READ
+    __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+    #define __UNALIGNED_UINT16_READ(addr)          (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT32_WRITE
+    __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+    #define __UNALIGNED_UINT32_WRITE(addr, val)    (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT32_READ
+    __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+    #define __UNALIGNED_UINT32_READ(addr)          (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #define __ALIGNED(x)              __align(x)
+  #endif
+  #ifndef   __RESTRICT
+    #warning No compiler specific solution for __RESTRICT. __RESTRICT is ignored.
+    #define __RESTRICT
+  #endif
+
+
+/*
+ * COSMIC Compiler
+ */
+#elif defined ( __CSMC__ )
+   #include <cmsis_csm.h>
+
+ #ifndef   __ASM
+    #define __ASM                                  _asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                               inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE                        static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE                   __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    // NO RETURN is automatically detected hence no warning here
+    #define __NO_RETURN
+  #endif
+  #ifndef   __USED
+    #warning No compiler specific solution for __USED. __USED is ignored.
+    #define __USED
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                                 __weak
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                               @packed
+  #endif
+  #ifndef   __PACKED_STRUCT
+    #define __PACKED_STRUCT                        @packed struct
+  #endif
+  #ifndef   __PACKED_UNION
+    #define __PACKED_UNION                         @packed union
+  #endif
+  #ifndef   __UNALIGNED_UINT32        /* deprecated */
+    @packed struct T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)                  (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT16_WRITE
+    __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+    #define __UNALIGNED_UINT16_WRITE(addr, val)    (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT16_READ
+    __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+    #define __UNALIGNED_UINT16_READ(addr)          (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __UNALIGNED_UINT32_WRITE
+    __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+    #define __UNALIGNED_UINT32_WRITE(addr, val)    (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+  #endif
+  #ifndef   __UNALIGNED_UINT32_READ
+    __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+    #define __UNALIGNED_UINT32_READ(addr)          (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #warning No compiler specific solution for __ALIGNED. __ALIGNED is ignored.
+    #define __ALIGNED(x)
+  #endif
+  #ifndef   __RESTRICT
+    #warning No compiler specific solution for __RESTRICT. __RESTRICT is ignored.
+    #define __RESTRICT
+  #endif
+
+
+#else
+  #error Unknown compiler.
+#endif
+
+
+#endif /* __CMSIS_COMPILER_H */
+

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Include/cmsis_iccarm.h

@@ -0,0 +1,935 @@
+/**************************************************************************//**
+ * @file     cmsis_iccarm.h
+ * @brief    CMSIS compiler ICCARM (IAR Compiler for Arm) header file
+ * @version  V5.0.7
+ * @date     19. June 2018
+ ******************************************************************************/
+
+//------------------------------------------------------------------------------
+//
+// Copyright (c) 2017-2018 IAR Systems
+//
+// Licensed under the Apache License, Version 2.0 (the "License")
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+//------------------------------------------------------------------------------
+
+
+#ifndef __CMSIS_ICCARM_H__
+#define __CMSIS_ICCARM_H__
+
+#ifndef __ICCARM__
+  #error This file should only be compiled by ICCARM
+#endif
+
+#pragma system_include
+
+#define __IAR_FT _Pragma("inline=forced") __intrinsic
+
+#if (__VER__ >= 8000000)
+  #define __ICCARM_V8 1
+#else
+  #define __ICCARM_V8 0
+#endif
+
+#ifndef __ALIGNED
+  #if __ICCARM_V8
+    #define __ALIGNED(x) __attribute__((aligned(x)))
+  #elif (__VER__ >= 7080000)
+    /* Needs IAR language extensions */
+    #define __ALIGNED(x) __attribute__((aligned(x)))
+  #else
+    #warning No compiler specific solution for __ALIGNED.__ALIGNED is ignored.
+    #define __ALIGNED(x)
+  #endif
+#endif
+
+
+/* Define compiler macros for CPU architecture, used in CMSIS 5.
+ */
+#if __ARM_ARCH_6M__ || __ARM_ARCH_7M__ || __ARM_ARCH_7EM__ || __ARM_ARCH_8M_BASE__ || __ARM_ARCH_8M_MAIN__
+/* Macros already defined */
+#else
+  #if defined(__ARM8M_MAINLINE__) || defined(__ARM8EM_MAINLINE__)
+    #define __ARM_ARCH_8M_MAIN__ 1
+  #elif defined(__ARM8M_BASELINE__)
+    #define __ARM_ARCH_8M_BASE__ 1
+  #elif defined(__ARM_ARCH_PROFILE) && __ARM_ARCH_PROFILE == 'M'
+    #if __ARM_ARCH == 6
+      #define __ARM_ARCH_6M__ 1
+    #elif __ARM_ARCH == 7
+      #if __ARM_FEATURE_DSP
+        #define __ARM_ARCH_7EM__ 1
+      #else
+        #define __ARM_ARCH_7M__ 1
+      #endif
+    #endif /* __ARM_ARCH */
+  #endif /* __ARM_ARCH_PROFILE == 'M' */
+#endif
+
+/* Alternativ core deduction for older ICCARM's */
+#if !defined(__ARM_ARCH_6M__) && !defined(__ARM_ARCH_7M__) && !defined(__ARM_ARCH_7EM__) && \
+    !defined(__ARM_ARCH_8M_BASE__) && !defined(__ARM_ARCH_8M_MAIN__)
+  #if defined(__ARM6M__) && (__CORE__ == __ARM6M__)
+    #define __ARM_ARCH_6M__ 1
+  #elif defined(__ARM7M__) && (__CORE__ == __ARM7M__)
+    #define __ARM_ARCH_7M__ 1
+  #elif defined(__ARM7EM__) && (__CORE__ == __ARM7EM__)
+    #define __ARM_ARCH_7EM__  1
+  #elif defined(__ARM8M_BASELINE__) && (__CORE == __ARM8M_BASELINE__)
+    #define __ARM_ARCH_8M_BASE__ 1
+  #elif defined(__ARM8M_MAINLINE__) && (__CORE == __ARM8M_MAINLINE__)
+    #define __ARM_ARCH_8M_MAIN__ 1
+  #elif defined(__ARM8EM_MAINLINE__) && (__CORE == __ARM8EM_MAINLINE__)
+    #define __ARM_ARCH_8M_MAIN__ 1
+  #else
+    #error "Unknown target."
+  #endif
+#endif
+
+
+
+#if defined(__ARM_ARCH_6M__) && __ARM_ARCH_6M__==1
+  #define __IAR_M0_FAMILY  1
+#elif defined(__ARM_ARCH_8M_BASE__) && __ARM_ARCH_8M_BASE__==1
+  #define __IAR_M0_FAMILY  1
+#else
+  #define __IAR_M0_FAMILY  0
+#endif
+
+
+#ifndef __ASM
+  #define __ASM __asm
+#endif
+
+#ifndef __INLINE
+  #define __INLINE inline
+#endif
+
+#ifndef   __NO_RETURN
+  #if __ICCARM_V8
+    #define __NO_RETURN __attribute__((__noreturn__))
+  #else
+    #define __NO_RETURN _Pragma("object_attribute=__noreturn")
+  #endif
+#endif
+
+#ifndef   __PACKED
+  #if __ICCARM_V8
+    #define __PACKED __attribute__((packed, aligned(1)))
+  #else
+    /* Needs IAR language extensions */
+    #define __PACKED __packed
+  #endif
+#endif
+
+#ifndef   __PACKED_STRUCT
+  #if __ICCARM_V8
+    #define __PACKED_STRUCT struct __attribute__((packed, aligned(1)))
+  #else
+    /* Needs IAR language extensions */
+    #define __PACKED_STRUCT __packed struct
+  #endif
+#endif
+
+#ifndef   __PACKED_UNION
+  #if __ICCARM_V8
+    #define __PACKED_UNION union __attribute__((packed, aligned(1)))
+  #else
+    /* Needs IAR language extensions */
+    #define __PACKED_UNION __packed union
+  #endif
+#endif
+
+#ifndef   __RESTRICT
+  #define __RESTRICT            __restrict
+#endif
+
+#ifndef   __STATIC_INLINE
+  #define __STATIC_INLINE       static inline
+#endif
+
+#ifndef   __FORCEINLINE
+  #define __FORCEINLINE         _Pragma("inline=forced")
+#endif
+
+#ifndef   __STATIC_FORCEINLINE
+  #define __STATIC_FORCEINLINE  __FORCEINLINE __STATIC_INLINE
+#endif
+
+#ifndef __UNALIGNED_UINT16_READ
+#pragma language=save
+#pragma language=extended
+__IAR_FT uint16_t __iar_uint16_read(void const *ptr)
+{
+  return *(__packed uint16_t*)(ptr);
+}
+#pragma language=restore
+#define __UNALIGNED_UINT16_READ(PTR) __iar_uint16_read(PTR)
+#endif
+
+
+#ifndef __UNALIGNED_UINT16_WRITE
+#pragma language=save
+#pragma language=extended
+__IAR_FT void __iar_uint16_write(void const *ptr, uint16_t val)
+{
+  *(__packed uint16_t*)(ptr) = val;;
+}
+#pragma language=restore
+#define __UNALIGNED_UINT16_WRITE(PTR,VAL) __iar_uint16_write(PTR,VAL)
+#endif
+
+#ifndef __UNALIGNED_UINT32_READ
+#pragma language=save
+#pragma language=extended
+__IAR_FT uint32_t __iar_uint32_read(void const *ptr)
+{
+  return *(__packed uint32_t*)(ptr);
+}
+#pragma language=restore
+#define __UNALIGNED_UINT32_READ(PTR) __iar_uint32_read(PTR)
+#endif
+
+#ifndef __UNALIGNED_UINT32_WRITE
+#pragma language=save
+#pragma language=extended
+__IAR_FT void __iar_uint32_write(void const *ptr, uint32_t val)
+{
+  *(__packed uint32_t*)(ptr) = val;;
+}
+#pragma language=restore
+#define __UNALIGNED_UINT32_WRITE(PTR,VAL) __iar_uint32_write(PTR,VAL)
+#endif
+
+#ifndef __UNALIGNED_UINT32   /* deprecated */
+#pragma language=save
+#pragma language=extended
+__packed struct  __iar_u32 { uint32_t v; };
+#pragma language=restore
+#define __UNALIGNED_UINT32(PTR) (((struct __iar_u32 *)(PTR))->v)
+#endif
+
+#ifndef   __USED
+  #if __ICCARM_V8
+    #define __USED __attribute__((used))
+  #else
+    #define __USED _Pragma("__root")
+  #endif
+#endif
+
+#ifndef   __WEAK
+  #if __ICCARM_V8
+    #define __WEAK __attribute__((weak))
+  #else
+    #define __WEAK _Pragma("__weak")
+  #endif
+#endif
+
+
+#ifndef __ICCARM_INTRINSICS_VERSION__
+  #define __ICCARM_INTRINSICS_VERSION__  0
+#endif
+
+#if __ICCARM_INTRINSICS_VERSION__ == 2
+
+  #if defined(__CLZ)
+    #undef __CLZ
+  #endif
+  #if defined(__REVSH)
+    #undef __REVSH
+  #endif
+  #if defined(__RBIT)
+    #undef __RBIT
+  #endif
+  #if defined(__SSAT)
+    #undef __SSAT
+  #endif
+  #if defined(__USAT)
+    #undef __USAT
+  #endif
+
+  #include "iccarm_builtin.h"
+
+  #define __disable_fault_irq __iar_builtin_disable_fiq
+  #define __disable_irq       __iar_builtin_disable_interrupt
+  #define __enable_fault_irq  __iar_builtin_enable_fiq
+  #define __enable_irq        __iar_builtin_enable_interrupt
+  #define __arm_rsr           __iar_builtin_rsr
+  #define __arm_wsr           __iar_builtin_wsr
+
+
+  #define __get_APSR()                (__arm_rsr("APSR"))
+  #define __get_BASEPRI()             (__arm_rsr("BASEPRI"))
+  #define __get_CONTROL()             (__arm_rsr("CONTROL"))
+  #define __get_FAULTMASK()           (__arm_rsr("FAULTMASK"))
+
+  #if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+       (defined (__FPU_USED   ) && (__FPU_USED    == 1U))     )
+    #define __get_FPSCR()             (__arm_rsr("FPSCR"))
+    #define __set_FPSCR(VALUE)        (__arm_wsr("FPSCR", (VALUE)))
+  #else
+    #define __get_FPSCR()             ( 0 )
+    #define __set_FPSCR(VALUE)        ((void)VALUE)
+  #endif
+
+  #define __get_IPSR()                (__arm_rsr("IPSR"))
+  #define __get_MSP()                 (__arm_rsr("MSP"))
+  #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+       (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+    // without main extensions, the non-secure MSPLIM is RAZ/WI
+    #define __get_MSPLIM()            (0U)
+  #else
+    #define __get_MSPLIM()            (__arm_rsr("MSPLIM"))
+  #endif
+  #define __get_PRIMASK()             (__arm_rsr("PRIMASK"))
+  #define __get_PSP()                 (__arm_rsr("PSP"))
+
+  #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+       (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+    // without main extensions, the non-secure PSPLIM is RAZ/WI
+    #define __get_PSPLIM()            (0U)
+  #else
+    #define __get_PSPLIM()            (__arm_rsr("PSPLIM"))
+  #endif
+
+  #define __get_xPSR()                (__arm_rsr("xPSR"))
+
+  #define __set_BASEPRI(VALUE)        (__arm_wsr("BASEPRI", (VALUE)))
+  #define __set_BASEPRI_MAX(VALUE)    (__arm_wsr("BASEPRI_MAX", (VALUE)))
+  #define __set_CONTROL(VALUE)        (__arm_wsr("CONTROL", (VALUE)))
+  #define __set_FAULTMASK(VALUE)      (__arm_wsr("FAULTMASK", (VALUE)))
+  #define __set_MSP(VALUE)            (__arm_wsr("MSP", (VALUE)))
+
+  #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+       (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+    // without main extensions, the non-secure MSPLIM is RAZ/WI
+    #define __set_MSPLIM(VALUE)       ((void)(VALUE))
+  #else
+    #define __set_MSPLIM(VALUE)       (__arm_wsr("MSPLIM", (VALUE)))
+  #endif
+  #define __set_PRIMASK(VALUE)        (__arm_wsr("PRIMASK", (VALUE)))
+  #define __set_PSP(VALUE)            (__arm_wsr("PSP", (VALUE)))
+  #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+       (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+    // without main extensions, the non-secure PSPLIM is RAZ/WI
+    #define __set_PSPLIM(VALUE)       ((void)(VALUE))
+  #else
+    #define __set_PSPLIM(VALUE)       (__arm_wsr("PSPLIM", (VALUE)))
+  #endif
+
+  #define __TZ_get_CONTROL_NS()       (__arm_rsr("CONTROL_NS"))
+  #define __TZ_set_CONTROL_NS(VALUE)  (__arm_wsr("CONTROL_NS", (VALUE)))
+  #define __TZ_get_PSP_NS()           (__arm_rsr("PSP_NS"))
+  #define __TZ_set_PSP_NS(VALUE)      (__arm_wsr("PSP_NS", (VALUE)))
+  #define __TZ_get_MSP_NS()           (__arm_rsr("MSP_NS"))
+  #define __TZ_set_MSP_NS(VALUE)      (__arm_wsr("MSP_NS", (VALUE)))
+  #define __TZ_get_SP_NS()            (__arm_rsr("SP_NS"))
+  #define __TZ_set_SP_NS(VALUE)       (__arm_wsr("SP_NS", (VALUE)))
+  #define __TZ_get_PRIMASK_NS()       (__arm_rsr("PRIMASK_NS"))
+  #define __TZ_set_PRIMASK_NS(VALUE)  (__arm_wsr("PRIMASK_NS", (VALUE)))
+  #define __TZ_get_BASEPRI_NS()       (__arm_rsr("BASEPRI_NS"))
+  #define __TZ_set_BASEPRI_NS(VALUE)  (__arm_wsr("BASEPRI_NS", (VALUE)))
+  #define __TZ_get_FAULTMASK_NS()     (__arm_rsr("FAULTMASK_NS"))
+  #define __TZ_set_FAULTMASK_NS(VALUE)(__arm_wsr("FAULTMASK_NS", (VALUE)))
+
+  #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+       (!defined (__ARM_FEATURE_CMSE) || (__ARM_FEATURE_CMSE < 3)))
+    // without main extensions, the non-secure PSPLIM is RAZ/WI
+    #define __TZ_get_PSPLIM_NS()      (0U)
+    #define __TZ_set_PSPLIM_NS(VALUE) ((void)(VALUE))
+  #else
+    #define __TZ_get_PSPLIM_NS()      (__arm_rsr("PSPLIM_NS"))
+    #define __TZ_set_PSPLIM_NS(VALUE) (__arm_wsr("PSPLIM_NS", (VALUE)))
+  #endif
+
+  #define __TZ_get_MSPLIM_NS()        (__arm_rsr("MSPLIM_NS"))
+  #define __TZ_set_MSPLIM_NS(VALUE)   (__arm_wsr("MSPLIM_NS", (VALUE)))
+
+  #define __NOP     __iar_builtin_no_operation
+
+  #define __CLZ     __iar_builtin_CLZ
+  #define __CLREX   __iar_builtin_CLREX
+
+  #define __DMB     __iar_builtin_DMB
+  #define __DSB     __iar_builtin_DSB
+  #define __ISB     __iar_builtin_ISB
+
+  #define __LDREXB  __iar_builtin_LDREXB
+  #define __LDREXH  __iar_builtin_LDREXH
+  #define __LDREXW  __iar_builtin_LDREX
+
+  #define __RBIT    __iar_builtin_RBIT
+  #define __REV     __iar_builtin_REV
+  #define __REV16   __iar_builtin_REV16
+
+  __IAR_FT int16_t __REVSH(int16_t val)
+  {
+    return (int16_t) __iar_builtin_REVSH(val);
+  }
+
+  #define __ROR     __iar_builtin_ROR
+  #define __RRX     __iar_builtin_RRX
+
+  #define __SEV     __iar_builtin_SEV
+
+  #if !__IAR_M0_FAMILY
+    #define __SSAT    __iar_builtin_SSAT
+  #endif
+
+  #define __STREXB  __iar_builtin_STREXB
+  #define __STREXH  __iar_builtin_STREXH
+  #define __STREXW  __iar_builtin_STREX
+
+  #if !__IAR_M0_FAMILY
+    #define __USAT    __iar_builtin_USAT
+  #endif
+
+  #define __WFE     __iar_builtin_WFE
+  #define __WFI     __iar_builtin_WFI
+
+  #if __ARM_MEDIA__
+    #define __SADD8   __iar_builtin_SADD8
+    #define __QADD8   __iar_builtin_QADD8
+    #define __SHADD8  __iar_builtin_SHADD8
+    #define __UADD8   __iar_builtin_UADD8
+    #define __UQADD8  __iar_builtin_UQADD8
+    #define __UHADD8  __iar_builtin_UHADD8
+    #define __SSUB8   __iar_builtin_SSUB8
+    #define __QSUB8   __iar_builtin_QSUB8
+    #define __SHSUB8  __iar_builtin_SHSUB8
+    #define __USUB8   __iar_builtin_USUB8
+    #define __UQSUB8  __iar_builtin_UQSUB8
+    #define __UHSUB8  __iar_builtin_UHSUB8
+    #define __SADD16  __iar_builtin_SADD16
+    #define __QADD16  __iar_builtin_QADD16
+    #define __SHADD16 __iar_builtin_SHADD16
+    #define __UADD16  __iar_builtin_UADD16
+    #define __UQADD16 __iar_builtin_UQADD16
+    #define __UHADD16 __iar_builtin_UHADD16
+    #define __SSUB16  __iar_builtin_SSUB16
+    #define __QSUB16  __iar_builtin_QSUB16
+    #define __SHSUB16 __iar_builtin_SHSUB16
+    #define __USUB16  __iar_builtin_USUB16
+    #define __UQSUB16 __iar_builtin_UQSUB16
+    #define __UHSUB16 __iar_builtin_UHSUB16
+    #define __SASX    __iar_builtin_SASX
+    #define __QASX    __iar_builtin_QASX
+    #define __SHASX   __iar_builtin_SHASX
+    #define __UASX    __iar_builtin_UASX
+    #define __UQASX   __iar_builtin_UQASX
+    #define __UHASX   __iar_builtin_UHASX
+    #define __SSAX    __iar_builtin_SSAX
+    #define __QSAX    __iar_builtin_QSAX
+    #define __SHSAX   __iar_builtin_SHSAX
+    #define __USAX    __iar_builtin_USAX
+    #define __UQSAX   __iar_builtin_UQSAX
+    #define __UHSAX   __iar_builtin_UHSAX
+    #define __USAD8   __iar_builtin_USAD8
+    #define __USADA8  __iar_builtin_USADA8
+    #define __SSAT16  __iar_builtin_SSAT16
+    #define __USAT16  __iar_builtin_USAT16
+    #define __UXTB16  __iar_builtin_UXTB16
+    #define __UXTAB16 __iar_builtin_UXTAB16
+    #define __SXTB16  __iar_builtin_SXTB16
+    #define __SXTAB16 __iar_builtin_SXTAB16
+    #define __SMUAD   __iar_builtin_SMUAD
+    #define __SMUADX  __iar_builtin_SMUADX
+    #define __SMMLA   __iar_builtin_SMMLA
+    #define __SMLAD   __iar_builtin_SMLAD
+    #define __SMLADX  __iar_builtin_SMLADX
+    #define __SMLALD  __iar_builtin_SMLALD
+    #define __SMLALDX __iar_builtin_SMLALDX
+    #define __SMUSD   __iar_builtin_SMUSD
+    #define __SMUSDX  __iar_builtin_SMUSDX
+    #define __SMLSD   __iar_builtin_SMLSD
+    #define __SMLSDX  __iar_builtin_SMLSDX
+    #define __SMLSLD  __iar_builtin_SMLSLD
+    #define __SMLSLDX __iar_builtin_SMLSLDX
+    #define __SEL     __iar_builtin_SEL
+    #define __QADD    __iar_builtin_QADD
+    #define __QSUB    __iar_builtin_QSUB
+    #define __PKHBT   __iar_builtin_PKHBT
+    #define __PKHTB   __iar_builtin_PKHTB
+  #endif
+
+#else /* __ICCARM_INTRINSICS_VERSION__ == 2 */
+
+  #if __IAR_M0_FAMILY
+   /* Avoid clash between intrinsics.h and arm_math.h when compiling for Cortex-M0. */
+    #define __CLZ  __cmsis_iar_clz_not_active
+    #define __SSAT __cmsis_iar_ssat_not_active
+    #define __USAT __cmsis_iar_usat_not_active
+    #define __RBIT __cmsis_iar_rbit_not_active
+    #define __get_APSR  __cmsis_iar_get_APSR_not_active
+  #endif
+
+
+  #if (!((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+         (defined (__FPU_USED   ) && (__FPU_USED    == 1U))     ))
+    #define __get_FPSCR __cmsis_iar_get_FPSR_not_active
+    #define __set_FPSCR __cmsis_iar_set_FPSR_not_active
+  #endif
+
+  #ifdef __INTRINSICS_INCLUDED
+  #error intrinsics.h is already included previously!
+  #endif
+
+  #include <intrinsics.h>
+
+  #if __IAR_M0_FAMILY
+   /* Avoid clash between intrinsics.h and arm_math.h when compiling for Cortex-M0. */
+    #undef __CLZ
+    #undef __SSAT
+    #undef __USAT
+    #undef __RBIT
+    #undef __get_APSR
+
+    __STATIC_INLINE uint8_t __CLZ(uint32_t data)
+    {
+      if (data == 0U) { return 32U; }
+
+      uint32_t count = 0U;
+      uint32_t mask = 0x80000000U;
+
+      while ((data & mask) == 0U)
+      {
+        count += 1U;
+        mask = mask >> 1U;
+      }
+      return count;
+    }
+
+    __STATIC_INLINE uint32_t __RBIT(uint32_t v)
+    {
+      uint8_t sc = 31U;
+      uint32_t r = v;
+      for (v >>= 1U; v; v >>= 1U)
+      {
+        r <<= 1U;
+        r |= v & 1U;
+        sc--;
+      }
+      return (r << sc);
+    }
+
+    __STATIC_INLINE  uint32_t __get_APSR(void)
+    {
+      uint32_t res;
+      __asm("MRS      %0,APSR" : "=r" (res));
+      return res;
+    }
+
+  #endif
+
+  #if (!((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+         (defined (__FPU_USED   ) && (__FPU_USED    == 1U))     ))
+    #undef __get_FPSCR
+    #undef __set_FPSCR
+    #define __get_FPSCR()       (0)
+    #define __set_FPSCR(VALUE)  ((void)VALUE)
+  #endif
+
+  #pragma diag_suppress=Pe940
+  #pragma diag_suppress=Pe177
+
+  #define __enable_irq    __enable_interrupt
+  #define __disable_irq   __disable_interrupt
+  #define __NOP           __no_operation
+
+  #define __get_xPSR      __get_PSR
+
+  #if (!defined(__ARM_ARCH_6M__) || __ARM_ARCH_6M__==0)
+
+    __IAR_FT uint32_t __LDREXW(uint32_t volatile *ptr)
+    {
+      return __LDREX((unsigned long *)ptr);
+    }
+
+    __IAR_FT uint32_t __STREXW(uint32_t value, uint32_t volatile *ptr)
+    {
+      return __STREX(value, (unsigned long *)ptr);
+    }
+  #endif
+
+
+  /* __CORTEX_M is defined in core_cm0.h, core_cm3.h and core_cm4.h. */
+  #if (__CORTEX_M >= 0x03)
+
+    __IAR_FT uint32_t __RRX(uint32_t value)
+    {
+      uint32_t result;
+      __ASM("RRX      %0, %1" : "=r"(result) : "r" (value) : "cc");
+      return(result);
+    }
+
+    __IAR_FT void __set_BASEPRI_MAX(uint32_t value)
+    {
+      __asm volatile("MSR      BASEPRI_MAX,%0"::"r" (value));
+    }
+
+
+    #define __enable_fault_irq  __enable_fiq
+    #define __disable_fault_irq __disable_fiq
+
+
+  #endif /* (__CORTEX_M >= 0x03) */
+
+  __IAR_FT uint32_t __ROR(uint32_t op1, uint32_t op2)
+  {
+    return (op1 >> op2) | (op1 << ((sizeof(op1)*8)-op2));
+  }
+
+  #if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+       (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1))    )
+
+   __IAR_FT uint32_t __get_MSPLIM(void)
+    {
+      uint32_t res;
+    #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+         (!defined (__ARM_FEATURE_CMSE  ) || (__ARM_FEATURE_CMSE   < 3)))
+      // without main extensions, the non-secure MSPLIM is RAZ/WI
+      res = 0U;
+    #else
+      __asm volatile("MRS      %0,MSPLIM" : "=r" (res));
+    #endif
+      return res;
+    }
+
+    __IAR_FT void   __set_MSPLIM(uint32_t value)
+    {
+    #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+         (!defined (__ARM_FEATURE_CMSE  ) || (__ARM_FEATURE_CMSE   < 3)))
+      // without main extensions, the non-secure MSPLIM is RAZ/WI
+      (void)value;
+    #else
+      __asm volatile("MSR      MSPLIM,%0" :: "r" (value));
+    #endif
+    }
+
+    __IAR_FT uint32_t __get_PSPLIM(void)
+    {
+      uint32_t res;
+    #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+         (!defined (__ARM_FEATURE_CMSE  ) || (__ARM_FEATURE_CMSE   < 3)))
+      // without main extensions, the non-secure PSPLIM is RAZ/WI
+      res = 0U;
+    #else
+      __asm volatile("MRS      %0,PSPLIM" : "=r" (res));
+    #endif
+      return res;
+    }
+
+    __IAR_FT void   __set_PSPLIM(uint32_t value)
+    {
+    #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+         (!defined (__ARM_FEATURE_CMSE  ) || (__ARM_FEATURE_CMSE   < 3)))
+      // without main extensions, the non-secure PSPLIM is RAZ/WI
+      (void)value;
+    #else
+      __asm volatile("MSR      PSPLIM,%0" :: "r" (value));
+    #endif
+    }
+
+    __IAR_FT uint32_t __TZ_get_CONTROL_NS(void)
+    {
+      uint32_t res;
+      __asm volatile("MRS      %0,CONTROL_NS" : "=r" (res));
+      return res;
+    }
+
+    __IAR_FT void   __TZ_set_CONTROL_NS(uint32_t value)
+    {
+      __asm volatile("MSR      CONTROL_NS,%0" :: "r" (value));
+    }
+
+    __IAR_FT uint32_t   __TZ_get_PSP_NS(void)
+    {
+      uint32_t res;
+      __asm volatile("MRS      %0,PSP_NS" : "=r" (res));
+      return res;
+    }
+
+    __IAR_FT void   __TZ_set_PSP_NS(uint32_t value)
+    {
+      __asm volatile("MSR      PSP_NS,%0" :: "r" (value));
+    }
+
+    __IAR_FT uint32_t   __TZ_get_MSP_NS(void)
+    {
+      uint32_t res;
+      __asm volatile("MRS      %0,MSP_NS" : "=r" (res));
+      return res;
+    }
+
+    __IAR_FT void   __TZ_set_MSP_NS(uint32_t value)
+    {
+      __asm volatile("MSR      MSP_NS,%0" :: "r" (value));
+    }
+
+    __IAR_FT uint32_t   __TZ_get_SP_NS(void)
+    {
+      uint32_t res;
+      __asm volatile("MRS      %0,SP_NS" : "=r" (res));
+      return res;
+    }
+    __IAR_FT void   __TZ_set_SP_NS(uint32_t value)
+    {
+      __asm volatile("MSR      SP_NS,%0" :: "r" (value));
+    }
+
+    __IAR_FT uint32_t   __TZ_get_PRIMASK_NS(void)
+    {
+      uint32_t res;
+      __asm volatile("MRS      %0,PRIMASK_NS" : "=r" (res));
+      return res;
+    }
+
+    __IAR_FT void   __TZ_set_PRIMASK_NS(uint32_t value)
+    {
+      __asm volatile("MSR      PRIMASK_NS,%0" :: "r" (value));
+    }
+
+    __IAR_FT uint32_t   __TZ_get_BASEPRI_NS(void)
+    {
+      uint32_t res;
+      __asm volatile("MRS      %0,BASEPRI_NS" : "=r" (res));
+      return res;
+    }
+
+    __IAR_FT void   __TZ_set_BASEPRI_NS(uint32_t value)
+    {
+      __asm volatile("MSR      BASEPRI_NS,%0" :: "r" (value));
+    }
+
+    __IAR_FT uint32_t   __TZ_get_FAULTMASK_NS(void)
+    {
+      uint32_t res;
+      __asm volatile("MRS      %0,FAULTMASK_NS" : "=r" (res));
+      return res;
+    }
+
+    __IAR_FT void   __TZ_set_FAULTMASK_NS(uint32_t value)
+    {
+      __asm volatile("MSR      FAULTMASK_NS,%0" :: "r" (value));
+    }
+
+    __IAR_FT uint32_t   __TZ_get_PSPLIM_NS(void)
+    {
+      uint32_t res;
+    #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+         (!defined (__ARM_FEATURE_CMSE  ) || (__ARM_FEATURE_CMSE   < 3)))
+      // without main extensions, the non-secure PSPLIM is RAZ/WI
+      res = 0U;
+    #else
+      __asm volatile("MRS      %0,PSPLIM_NS" : "=r" (res));
+    #endif
+      return res;
+    }
+
+    __IAR_FT void   __TZ_set_PSPLIM_NS(uint32_t value)
+    {
+    #if (!(defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) && \
+         (!defined (__ARM_FEATURE_CMSE  ) || (__ARM_FEATURE_CMSE   < 3)))
+      // without main extensions, the non-secure PSPLIM is RAZ/WI
+      (void)value;
+    #else
+      __asm volatile("MSR      PSPLIM_NS,%0" :: "r" (value));
+    #endif
+    }
+
+    __IAR_FT uint32_t   __TZ_get_MSPLIM_NS(void)
+    {
+      uint32_t res;
+      __asm volatile("MRS      %0,MSPLIM_NS" : "=r" (res));
+      return res;
+    }
+
+    __IAR_FT void   __TZ_set_MSPLIM_NS(uint32_t value)
+    {
+      __asm volatile("MSR      MSPLIM_NS,%0" :: "r" (value));
+    }
+
+  #endif /* __ARM_ARCH_8M_MAIN__ or __ARM_ARCH_8M_BASE__ */
+
+#endif   /* __ICCARM_INTRINSICS_VERSION__ == 2 */
+
+#define __BKPT(value)    __asm volatile ("BKPT     %0" : : "i"(value))
+
+#if __IAR_M0_FAMILY
+  __STATIC_INLINE int32_t __SSAT(int32_t val, uint32_t sat)
+  {
+    if ((sat >= 1U) && (sat <= 32U))
+    {
+      const int32_t max = (int32_t)((1U << (sat - 1U)) - 1U);
+      const int32_t min = -1 - max ;
+      if (val > max)
+      {
+        return max;
+      }
+      else if (val < min)
+      {
+        return min;
+      }
+    }
+    return val;
+  }
+
+  __STATIC_INLINE uint32_t __USAT(int32_t val, uint32_t sat)
+  {
+    if (sat <= 31U)
+    {
+      const uint32_t max = ((1U << sat) - 1U);
+      if (val > (int32_t)max)
+      {
+        return max;
+      }
+      else if (val < 0)
+      {
+        return 0U;
+      }
+    }
+    return (uint32_t)val;
+  }
+#endif
+
+#if (__CORTEX_M >= 0x03)   /* __CORTEX_M is defined in core_cm0.h, core_cm3.h and core_cm4.h. */
+
+  __IAR_FT uint8_t __LDRBT(volatile uint8_t *addr)
+  {
+    uint32_t res;
+    __ASM("LDRBT %0, [%1]" : "=r" (res) : "r" (addr) : "memory");
+    return ((uint8_t)res);
+  }
+
+  __IAR_FT uint16_t __LDRHT(volatile uint16_t *addr)
+  {
+    uint32_t res;
+    __ASM("LDRHT %0, [%1]" : "=r" (res) : "r" (addr) : "memory");
+    return ((uint16_t)res);
+  }
+
+  __IAR_FT uint32_t __LDRT(volatile uint32_t *addr)
+  {
+    uint32_t res;
+    __ASM("LDRT %0, [%1]" : "=r" (res) : "r" (addr) : "memory");
+    return res;
+  }
+
+  __IAR_FT void __STRBT(uint8_t value, volatile uint8_t *addr)
+  {
+    __ASM("STRBT %1, [%0]" : : "r" (addr), "r" ((uint32_t)value) : "memory");
+  }
+
+  __IAR_FT void __STRHT(uint16_t value, volatile uint16_t *addr)
+  {
+    __ASM("STRHT %1, [%0]" : : "r" (addr), "r" ((uint32_t)value) : "memory");
+  }
+
+  __IAR_FT void __STRT(uint32_t value, volatile uint32_t *addr)
+  {
+    __ASM("STRT %1, [%0]" : : "r" (addr), "r" (value) : "memory");
+  }
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+#if ((defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1)) || \
+     (defined (__ARM_ARCH_8M_BASE__ ) && (__ARM_ARCH_8M_BASE__ == 1))    )
+
+
+  __IAR_FT uint8_t __LDAB(volatile uint8_t *ptr)
+  {
+    uint32_t res;
+    __ASM volatile ("LDAB %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+    return ((uint8_t)res);
+  }
+
+  __IAR_FT uint16_t __LDAH(volatile uint16_t *ptr)
+  {
+    uint32_t res;
+    __ASM volatile ("LDAH %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+    return ((uint16_t)res);
+  }
+
+  __IAR_FT uint32_t __LDA(volatile uint32_t *ptr)
+  {
+    uint32_t res;
+    __ASM volatile ("LDA %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+    return res;
+  }
+
+  __IAR_FT void __STLB(uint8_t value, volatile uint8_t *ptr)
+  {
+    __ASM volatile ("STLB %1, [%0]" :: "r" (ptr), "r" (value) : "memory");
+  }
+
+  __IAR_FT void __STLH(uint16_t value, volatile uint16_t *ptr)
+  {
+    __ASM volatile ("STLH %1, [%0]" :: "r" (ptr), "r" (value) : "memory");
+  }
+
+  __IAR_FT void __STL(uint32_t value, volatile uint32_t *ptr)
+  {
+    __ASM volatile ("STL %1, [%0]" :: "r" (ptr), "r" (value) : "memory");
+  }
+
+  __IAR_FT uint8_t __LDAEXB(volatile uint8_t *ptr)
+  {
+    uint32_t res;
+    __ASM volatile ("LDAEXB %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+    return ((uint8_t)res);
+  }
+
+  __IAR_FT uint16_t __LDAEXH(volatile uint16_t *ptr)
+  {
+    uint32_t res;
+    __ASM volatile ("LDAEXH %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+    return ((uint16_t)res);
+  }
+
+  __IAR_FT uint32_t __LDAEX(volatile uint32_t *ptr)
+  {
+    uint32_t res;
+    __ASM volatile ("LDAEX %0, [%1]" : "=r" (res) : "r" (ptr) : "memory");
+    return res;
+  }
+
+  __IAR_FT uint32_t __STLEXB(uint8_t value, volatile uint8_t *ptr)
+  {
+    uint32_t res;
+    __ASM volatile ("STLEXB %0, %2, [%1]" : "=r" (res) : "r" (ptr), "r" (value) : "memory");
+    return res;
+  }
+
+  __IAR_FT uint32_t __STLEXH(uint16_t value, volatile uint16_t *ptr)
+  {
+    uint32_t res;
+    __ASM volatile ("STLEXH %0, %2, [%1]" : "=r" (res) : "r" (ptr), "r" (value) : "memory");
+    return res;
+  }
+
+  __IAR_FT uint32_t __STLEX(uint32_t value, volatile uint32_t *ptr)
+  {
+    uint32_t res;
+    __ASM volatile ("STLEX %0, %2, [%1]" : "=r" (res) : "r" (ptr), "r" (value) : "memory");
+    return res;
+  }
+
+#endif /* __ARM_ARCH_8M_MAIN__ or __ARM_ARCH_8M_BASE__ */
+
+#undef __IAR_FT
+#undef __IAR_M0_FAMILY
+#undef __ICCARM_V8
+
+#pragma diag_default=Pe940
+#pragma diag_default=Pe177
+
+#endif /* __CMSIS_ICCARM_H__ */

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Include/cmsis_version.h

@@ -0,0 +1,39 @@
+/**************************************************************************//**
+ * @file     cmsis_version.h
+ * @brief    CMSIS Core(M) Version definitions
+ * @version  V5.0.2
+ * @date     19. April 2017
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2017 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if   defined ( __ICCARM__ )
+  #pragma system_include         /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+  #pragma clang system_header   /* treat file as system include file */
+#endif
+
+#ifndef __CMSIS_VERSION_H
+#define __CMSIS_VERSION_H
+
+/*  CMSIS Version definitions */
+#define __CM_CMSIS_VERSION_MAIN  ( 5U)                                      /*!< [31:16] CMSIS Core(M) main version */
+#define __CM_CMSIS_VERSION_SUB   ( 1U)                                      /*!< [15:0]  CMSIS Core(M) sub version */
+#define __CM_CMSIS_VERSION       ((__CM_CMSIS_VERSION_MAIN << 16U) | \
+                                   __CM_CMSIS_VERSION_SUB           )       /*!< CMSIS Core(M) version number */
+#endif

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Include/core_cm0.h

@@ -0,0 +1,949 @@
+/**************************************************************************//**
+ * @file     core_cm0.h
+ * @brief    CMSIS Cortex-M0 Core Peripheral Access Layer Header File
+ * @version  V5.0.5
+ * @date     28. May 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if   defined ( __ICCARM__ )
+  #pragma system_include         /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+  #pragma clang system_header   /* treat file as system include file */
+#endif
+
+#ifndef __CORE_CM0_H_GENERIC
+#define __CORE_CM0_H_GENERIC
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/**
+  \page CMSIS_MISRA_Exceptions  MISRA-C:2004 Compliance Exceptions
+  CMSIS violates the following MISRA-C:2004 rules:
+
+   \li Required Rule 8.5, object/function definition in header file.<br>
+     Function definitions in header files are used to allow 'inlining'.
+
+   \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
+     Unions are used for effective representation of core registers.
+
+   \li Advisory Rule 19.7, Function-like macro defined.<br>
+     Function-like macros are used to allow more efficient code.
+ */
+
+
+/*******************************************************************************
+ *                 CMSIS definitions
+ ******************************************************************************/
+/**
+  \ingroup Cortex_M0
+  @{
+ */
+
+#include "cmsis_version.h"
+ 
+/*  CMSIS CM0 definitions */
+#define __CM0_CMSIS_VERSION_MAIN  (__CM_CMSIS_VERSION_MAIN)              /*!< \deprecated [31:16] CMSIS HAL main version */
+#define __CM0_CMSIS_VERSION_SUB   (__CM_CMSIS_VERSION_SUB)               /*!< \deprecated [15:0]  CMSIS HAL sub version */
+#define __CM0_CMSIS_VERSION       ((__CM0_CMSIS_VERSION_MAIN << 16U) | \
+                                    __CM0_CMSIS_VERSION_SUB           )  /*!< \deprecated CMSIS HAL version number */
+
+#define __CORTEX_M                (0U)                                   /*!< Cortex-M Core */
+
+/** __FPU_USED indicates whether an FPU is used or not.
+    This core does not support an FPU at all
+*/
+#define __FPU_USED       0U
+
+#if defined ( __CC_ARM )
+  #if defined __TARGET_FPU_VFP
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
+  #if defined __ARM_PCS_VFP
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __GNUC__ )
+  #if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __ICCARM__ )
+  #if defined __ARMVFP__
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __TI_ARM__ )
+  #if defined __TI_VFP_SUPPORT__
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __TASKING__ )
+  #if defined __FPU_VFP__
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __CSMC__ )
+  #if ( __CSMC__ & 0x400U)
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#endif
+
+#include "cmsis_compiler.h"               /* CMSIS compiler specific defines */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CORE_CM0_H_GENERIC */
+
+#ifndef __CMSIS_GENERIC
+
+#ifndef __CORE_CM0_H_DEPENDANT
+#define __CORE_CM0_H_DEPENDANT
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* check device defines and use defaults */
+#if defined __CHECK_DEVICE_DEFINES
+  #ifndef __CM0_REV
+    #define __CM0_REV               0x0000U
+    #warning "__CM0_REV not defined in device header file; using default!"
+  #endif
+
+  #ifndef __NVIC_PRIO_BITS
+    #define __NVIC_PRIO_BITS          2U
+    #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
+  #endif
+
+  #ifndef __Vendor_SysTickConfig
+    #define __Vendor_SysTickConfig    0U
+    #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
+  #endif
+#endif
+
+/* IO definitions (access restrictions to peripheral registers) */
+/**
+    \defgroup CMSIS_glob_defs CMSIS Global Defines
+
+    <strong>IO Type Qualifiers</strong> are used
+    \li to specify the access to peripheral variables.
+    \li for automatic generation of peripheral register debug information.
+*/
+#ifdef __cplusplus
+  #define   __I     volatile             /*!< Defines 'read only' permissions */
+#else
+  #define   __I     volatile const       /*!< Defines 'read only' permissions */
+#endif
+#define     __O     volatile             /*!< Defines 'write only' permissions */
+#define     __IO    volatile             /*!< Defines 'read / write' permissions */
+
+/* following defines should be used for structure members */
+#define     __IM     volatile const      /*! Defines 'read only' structure member permissions */
+#define     __OM     volatile            /*! Defines 'write only' structure member permissions */
+#define     __IOM    volatile            /*! Defines 'read / write' structure member permissions */
+
+/*@} end of group Cortex_M0 */
+
+
+
+/*******************************************************************************
+ *                 Register Abstraction
+  Core Register contain:
+  - Core Register
+  - Core NVIC Register
+  - Core SCB Register
+  - Core SysTick Register
+ ******************************************************************************/
+/**
+  \defgroup CMSIS_core_register Defines and Type Definitions
+  \brief Type definitions and defines for Cortex-M processor based devices.
+*/
+
+/**
+  \ingroup    CMSIS_core_register
+  \defgroup   CMSIS_CORE  Status and Control Registers
+  \brief      Core Register type definitions.
+  @{
+ */
+
+/**
+  \brief  Union type to access the Application Program Status Register (APSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t _reserved0:28;              /*!< bit:  0..27  Reserved */
+    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag */
+    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag */
+    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag */
+    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag */
+  } b;                                   /*!< Structure used for bit  access */
+  uint32_t w;                            /*!< Type      used for word access */
+} APSR_Type;
+
+/* APSR Register Definitions */
+#define APSR_N_Pos                         31U                                            /*!< APSR: N Position */
+#define APSR_N_Msk                         (1UL << APSR_N_Pos)                            /*!< APSR: N Mask */
+
+#define APSR_Z_Pos                         30U                                            /*!< APSR: Z Position */
+#define APSR_Z_Msk                         (1UL << APSR_Z_Pos)                            /*!< APSR: Z Mask */
+
+#define APSR_C_Pos                         29U                                            /*!< APSR: C Position */
+#define APSR_C_Msk                         (1UL << APSR_C_Pos)                            /*!< APSR: C Mask */
+
+#define APSR_V_Pos                         28U                                            /*!< APSR: V Position */
+#define APSR_V_Msk                         (1UL << APSR_V_Pos)                            /*!< APSR: V Mask */
+
+
+/**
+  \brief  Union type to access the Interrupt Program Status Register (IPSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number */
+    uint32_t _reserved0:23;              /*!< bit:  9..31  Reserved */
+  } b;                                   /*!< Structure used for bit  access */
+  uint32_t w;                            /*!< Type      used for word access */
+} IPSR_Type;
+
+/* IPSR Register Definitions */
+#define IPSR_ISR_Pos                        0U                                            /*!< IPSR: ISR Position */
+#define IPSR_ISR_Msk                       (0x1FFUL /*<< IPSR_ISR_Pos*/)                  /*!< IPSR: ISR Mask */
+
+
+/**
+  \brief  Union type to access the Special-Purpose Program Status Registers (xPSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number */
+    uint32_t _reserved0:15;              /*!< bit:  9..23  Reserved */
+    uint32_t T:1;                        /*!< bit:     24  Thumb bit        (read 0) */
+    uint32_t _reserved1:3;               /*!< bit: 25..27  Reserved */
+    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag */
+    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag */
+    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag */
+    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag */
+  } b;                                   /*!< Structure used for bit  access */
+  uint32_t w;                            /*!< Type      used for word access */
+} xPSR_Type;
+
+/* xPSR Register Definitions */
+#define xPSR_N_Pos                         31U                                            /*!< xPSR: N Position */
+#define xPSR_N_Msk                         (1UL << xPSR_N_Pos)                            /*!< xPSR: N Mask */
+
+#define xPSR_Z_Pos                         30U                                            /*!< xPSR: Z Position */
+#define xPSR_Z_Msk                         (1UL << xPSR_Z_Pos)                            /*!< xPSR: Z Mask */
+
+#define xPSR_C_Pos                         29U                                            /*!< xPSR: C Position */
+#define xPSR_C_Msk                         (1UL << xPSR_C_Pos)                            /*!< xPSR: C Mask */
+
+#define xPSR_V_Pos                         28U                                            /*!< xPSR: V Position */
+#define xPSR_V_Msk                         (1UL << xPSR_V_Pos)                            /*!< xPSR: V Mask */
+
+#define xPSR_T_Pos                         24U                                            /*!< xPSR: T Position */
+#define xPSR_T_Msk                         (1UL << xPSR_T_Pos)                            /*!< xPSR: T Mask */
+
+#define xPSR_ISR_Pos                        0U                                            /*!< xPSR: ISR Position */
+#define xPSR_ISR_Msk                       (0x1FFUL /*<< xPSR_ISR_Pos*/)                  /*!< xPSR: ISR Mask */
+
+
+/**
+  \brief  Union type to access the Control Registers (CONTROL).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t _reserved0:1;               /*!< bit:      0  Reserved */
+    uint32_t SPSEL:1;                    /*!< bit:      1  Stack to be used */
+    uint32_t _reserved1:30;              /*!< bit:  2..31  Reserved */
+  } b;                                   /*!< Structure used for bit  access */
+  uint32_t w;                            /*!< Type      used for word access */
+} CONTROL_Type;
+
+/* CONTROL Register Definitions */
+#define CONTROL_SPSEL_Pos                   1U                                            /*!< CONTROL: SPSEL Position */
+#define CONTROL_SPSEL_Msk                  (1UL << CONTROL_SPSEL_Pos)                     /*!< CONTROL: SPSEL Mask */
+
+/*@} end of group CMSIS_CORE */
+
+
+/**
+  \ingroup    CMSIS_core_register
+  \defgroup   CMSIS_NVIC  Nested Vectored Interrupt Controller (NVIC)
+  \brief      Type definitions for the NVIC Registers
+  @{
+ */
+
+/**
+  \brief  Structure type to access the Nested Vectored Interrupt Controller (NVIC).
+ */
+typedef struct
+{
+  __IOM uint32_t ISER[1U];               /*!< Offset: 0x000 (R/W)  Interrupt Set Enable Register */
+        uint32_t RESERVED0[31U];
+  __IOM uint32_t ICER[1U];               /*!< Offset: 0x080 (R/W)  Interrupt Clear Enable Register */
+        uint32_t RSERVED1[31U];
+  __IOM uint32_t ISPR[1U];               /*!< Offset: 0x100 (R/W)  Interrupt Set Pending Register */
+        uint32_t RESERVED2[31U];
+  __IOM uint32_t ICPR[1U];               /*!< Offset: 0x180 (R/W)  Interrupt Clear Pending Register */
+        uint32_t RESERVED3[31U];
+        uint32_t RESERVED4[64U];
+  __IOM uint32_t IP[8U];                 /*!< Offset: 0x300 (R/W)  Interrupt Priority Register */
+}  NVIC_Type;
+
+/*@} end of group CMSIS_NVIC */
+
+
+/**
+  \ingroup  CMSIS_core_register
+  \defgroup CMSIS_SCB     System Control Block (SCB)
+  \brief    Type definitions for the System Control Block Registers
+  @{
+ */
+
+/**
+  \brief  Structure type to access the System Control Block (SCB).
+ */
+typedef struct
+{
+  __IM  uint32_t CPUID;                  /*!< Offset: 0x000 (R/ )  CPUID Base Register */
+  __IOM uint32_t ICSR;                   /*!< Offset: 0x004 (R/W)  Interrupt Control and State Register */
+        uint32_t RESERVED0;
+  __IOM uint32_t AIRCR;                  /*!< Offset: 0x00C (R/W)  Application Interrupt and Reset Control Register */
+  __IOM uint32_t SCR;                    /*!< Offset: 0x010 (R/W)  System Control Register */
+  __IOM uint32_t CCR;                    /*!< Offset: 0x014 (R/W)  Configuration Control Register */
+        uint32_t RESERVED1;
+  __IOM uint32_t SHP[2U];                /*!< Offset: 0x01C (R/W)  System Handlers Priority Registers. [0] is RESERVED */
+  __IOM uint32_t SHCSR;                  /*!< Offset: 0x024 (R/W)  System Handler Control and State Register */
+} SCB_Type;
+
+/* SCB CPUID Register Definitions */
+#define SCB_CPUID_IMPLEMENTER_Pos          24U                                            /*!< SCB CPUID: IMPLEMENTER Position */
+#define SCB_CPUID_IMPLEMENTER_Msk          (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos)          /*!< SCB CPUID: IMPLEMENTER Mask */
+
+#define SCB_CPUID_VARIANT_Pos              20U                                            /*!< SCB CPUID: VARIANT Position */
+#define SCB_CPUID_VARIANT_Msk              (0xFUL << SCB_CPUID_VARIANT_Pos)               /*!< SCB CPUID: VARIANT Mask */
+
+#define SCB_CPUID_ARCHITECTURE_Pos         16U                                            /*!< SCB CPUID: ARCHITECTURE Position */
+#define SCB_CPUID_ARCHITECTURE_Msk         (0xFUL << SCB_CPUID_ARCHITECTURE_Pos)          /*!< SCB CPUID: ARCHITECTURE Mask */
+
+#define SCB_CPUID_PARTNO_Pos                4U                                            /*!< SCB CPUID: PARTNO Position */
+#define SCB_CPUID_PARTNO_Msk               (0xFFFUL << SCB_CPUID_PARTNO_Pos)              /*!< SCB CPUID: PARTNO Mask */
+
+#define SCB_CPUID_REVISION_Pos              0U                                            /*!< SCB CPUID: REVISION Position */
+#define SCB_CPUID_REVISION_Msk             (0xFUL /*<< SCB_CPUID_REVISION_Pos*/)          /*!< SCB CPUID: REVISION Mask */
+
+/* SCB Interrupt Control State Register Definitions */
+#define SCB_ICSR_NMIPENDSET_Pos            31U                                            /*!< SCB ICSR: NMIPENDSET Position */
+#define SCB_ICSR_NMIPENDSET_Msk            (1UL << SCB_ICSR_NMIPENDSET_Pos)               /*!< SCB ICSR: NMIPENDSET Mask */
+
+#define SCB_ICSR_PENDSVSET_Pos             28U                                            /*!< SCB ICSR: PENDSVSET Position */
+#define SCB_ICSR_PENDSVSET_Msk             (1UL << SCB_ICSR_PENDSVSET_Pos)                /*!< SCB ICSR: PENDSVSET Mask */
+
+#define SCB_ICSR_PENDSVCLR_Pos             27U                                            /*!< SCB ICSR: PENDSVCLR Position */
+#define SCB_ICSR_PENDSVCLR_Msk             (1UL << SCB_ICSR_PENDSVCLR_Pos)                /*!< SCB ICSR: PENDSVCLR Mask */
+
+#define SCB_ICSR_PENDSTSET_Pos             26U                                            /*!< SCB ICSR: PENDSTSET Position */
+#define SCB_ICSR_PENDSTSET_Msk             (1UL << SCB_ICSR_PENDSTSET_Pos)                /*!< SCB ICSR: PENDSTSET Mask */
+
+#define SCB_ICSR_PENDSTCLR_Pos             25U                                            /*!< SCB ICSR: PENDSTCLR Position */
+#define SCB_ICSR_PENDSTCLR_Msk             (1UL << SCB_ICSR_PENDSTCLR_Pos)                /*!< SCB ICSR: PENDSTCLR Mask */
+
+#define SCB_ICSR_ISRPREEMPT_Pos            23U                                            /*!< SCB ICSR: ISRPREEMPT Position */
+#define SCB_ICSR_ISRPREEMPT_Msk            (1UL << SCB_ICSR_ISRPREEMPT_Pos)               /*!< SCB ICSR: ISRPREEMPT Mask */
+
+#define SCB_ICSR_ISRPENDING_Pos            22U                                            /*!< SCB ICSR: ISRPENDING Position */
+#define SCB_ICSR_ISRPENDING_Msk            (1UL << SCB_ICSR_ISRPENDING_Pos)               /*!< SCB ICSR: ISRPENDING Mask */
+
+#define SCB_ICSR_VECTPENDING_Pos           12U                                            /*!< SCB ICSR: VECTPENDING Position */
+#define SCB_ICSR_VECTPENDING_Msk           (0x1FFUL << SCB_ICSR_VECTPENDING_Pos)          /*!< SCB ICSR: VECTPENDING Mask */
+
+#define SCB_ICSR_VECTACTIVE_Pos             0U                                            /*!< SCB ICSR: VECTACTIVE Position */
+#define SCB_ICSR_VECTACTIVE_Msk            (0x1FFUL /*<< SCB_ICSR_VECTACTIVE_Pos*/)       /*!< SCB ICSR: VECTACTIVE Mask */
+
+/* SCB Application Interrupt and Reset Control Register Definitions */
+#define SCB_AIRCR_VECTKEY_Pos              16U                                            /*!< SCB AIRCR: VECTKEY Position */
+#define SCB_AIRCR_VECTKEY_Msk              (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos)            /*!< SCB AIRCR: VECTKEY Mask */
+
+#define SCB_AIRCR_VECTKEYSTAT_Pos          16U                                            /*!< SCB AIRCR: VECTKEYSTAT Position */
+#define SCB_AIRCR_VECTKEYSTAT_Msk          (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos)        /*!< SCB AIRCR: VECTKEYSTAT Mask */
+
+#define SCB_AIRCR_ENDIANESS_Pos            15U                                            /*!< SCB AIRCR: ENDIANESS Position */
+#define SCB_AIRCR_ENDIANESS_Msk            (1UL << SCB_AIRCR_ENDIANESS_Pos)               /*!< SCB AIRCR: ENDIANESS Mask */
+
+#define SCB_AIRCR_SYSRESETREQ_Pos           2U                                            /*!< SCB AIRCR: SYSRESETREQ Position */
+#define SCB_AIRCR_SYSRESETREQ_Msk          (1UL << SCB_AIRCR_SYSRESETREQ_Pos)             /*!< SCB AIRCR: SYSRESETREQ Mask */
+
+#define SCB_AIRCR_VECTCLRACTIVE_Pos         1U                                            /*!< SCB AIRCR: VECTCLRACTIVE Position */
+#define SCB_AIRCR_VECTCLRACTIVE_Msk        (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos)           /*!< SCB AIRCR: VECTCLRACTIVE Mask */
+
+/* SCB System Control Register Definitions */
+#define SCB_SCR_SEVONPEND_Pos               4U                                            /*!< SCB SCR: SEVONPEND Position */
+#define SCB_SCR_SEVONPEND_Msk              (1UL << SCB_SCR_SEVONPEND_Pos)                 /*!< SCB SCR: SEVONPEND Mask */
+
+#define SCB_SCR_SLEEPDEEP_Pos               2U                                            /*!< SCB SCR: SLEEPDEEP Position */
+#define SCB_SCR_SLEEPDEEP_Msk              (1UL << SCB_SCR_SLEEPDEEP_Pos)                 /*!< SCB SCR: SLEEPDEEP Mask */
+
+#define SCB_SCR_SLEEPONEXIT_Pos             1U                                            /*!< SCB SCR: SLEEPONEXIT Position */
+#define SCB_SCR_SLEEPONEXIT_Msk            (1UL << SCB_SCR_SLEEPONEXIT_Pos)               /*!< SCB SCR: SLEEPONEXIT Mask */
+
+/* SCB Configuration Control Register Definitions */
+#define SCB_CCR_STKALIGN_Pos                9U                                            /*!< SCB CCR: STKALIGN Position */
+#define SCB_CCR_STKALIGN_Msk               (1UL << SCB_CCR_STKALIGN_Pos)                  /*!< SCB CCR: STKALIGN Mask */
+
+#define SCB_CCR_UNALIGN_TRP_Pos             3U                                            /*!< SCB CCR: UNALIGN_TRP Position */
+#define SCB_CCR_UNALIGN_TRP_Msk            (1UL << SCB_CCR_UNALIGN_TRP_Pos)               /*!< SCB CCR: UNALIGN_TRP Mask */
+
+/* SCB System Handler Control and State Register Definitions */
+#define SCB_SHCSR_SVCALLPENDED_Pos         15U                                            /*!< SCB SHCSR: SVCALLPENDED Position */
+#define SCB_SHCSR_SVCALLPENDED_Msk         (1UL << SCB_SHCSR_SVCALLPENDED_Pos)            /*!< SCB SHCSR: SVCALLPENDED Mask */
+
+/*@} end of group CMSIS_SCB */
+
+
+/**
+  \ingroup  CMSIS_core_register
+  \defgroup CMSIS_SysTick     System Tick Timer (SysTick)
+  \brief    Type definitions for the System Timer Registers.
+  @{
+ */
+
+/**
+  \brief  Structure type to access the System Timer (SysTick).
+ */
+typedef struct
+{
+  __IOM uint32_t CTRL;                   /*!< Offset: 0x000 (R/W)  SysTick Control and Status Register */
+  __IOM uint32_t LOAD;                   /*!< Offset: 0x004 (R/W)  SysTick Reload Value Register */
+  __IOM uint32_t VAL;                    /*!< Offset: 0x008 (R/W)  SysTick Current Value Register */
+  __IM  uint32_t CALIB;                  /*!< Offset: 0x00C (R/ )  SysTick Calibration Register */
+} SysTick_Type;
+
+/* SysTick Control / Status Register Definitions */
+#define SysTick_CTRL_COUNTFLAG_Pos         16U                                            /*!< SysTick CTRL: COUNTFLAG Position */
+#define SysTick_CTRL_COUNTFLAG_Msk         (1UL << SysTick_CTRL_COUNTFLAG_Pos)            /*!< SysTick CTRL: COUNTFLAG Mask */
+
+#define SysTick_CTRL_CLKSOURCE_Pos          2U                                            /*!< SysTick CTRL: CLKSOURCE Position */
+#define SysTick_CTRL_CLKSOURCE_Msk         (1UL << SysTick_CTRL_CLKSOURCE_Pos)            /*!< SysTick CTRL: CLKSOURCE Mask */
+
+#define SysTick_CTRL_TICKINT_Pos            1U                                            /*!< SysTick CTRL: TICKINT Position */
+#define SysTick_CTRL_TICKINT_Msk           (1UL << SysTick_CTRL_TICKINT_Pos)              /*!< SysTick CTRL: TICKINT Mask */
+
+#define SysTick_CTRL_ENABLE_Pos             0U                                            /*!< SysTick CTRL: ENABLE Position */
+#define SysTick_CTRL_ENABLE_Msk            (1UL /*<< SysTick_CTRL_ENABLE_Pos*/)           /*!< SysTick CTRL: ENABLE Mask */
+
+/* SysTick Reload Register Definitions */
+#define SysTick_LOAD_RELOAD_Pos             0U                                            /*!< SysTick LOAD: RELOAD Position */
+#define SysTick_LOAD_RELOAD_Msk            (0xFFFFFFUL /*<< SysTick_LOAD_RELOAD_Pos*/)    /*!< SysTick LOAD: RELOAD Mask */
+
+/* SysTick Current Register Definitions */
+#define SysTick_VAL_CURRENT_Pos             0U                                            /*!< SysTick VAL: CURRENT Position */
+#define SysTick_VAL_CURRENT_Msk            (0xFFFFFFUL /*<< SysTick_VAL_CURRENT_Pos*/)    /*!< SysTick VAL: CURRENT Mask */
+
+/* SysTick Calibration Register Definitions */
+#define SysTick_CALIB_NOREF_Pos            31U                                            /*!< SysTick CALIB: NOREF Position */
+#define SysTick_CALIB_NOREF_Msk            (1UL << SysTick_CALIB_NOREF_Pos)               /*!< SysTick CALIB: NOREF Mask */
+
+#define SysTick_CALIB_SKEW_Pos             30U                                            /*!< SysTick CALIB: SKEW Position */
+#define SysTick_CALIB_SKEW_Msk             (1UL << SysTick_CALIB_SKEW_Pos)                /*!< SysTick CALIB: SKEW Mask */
+
+#define SysTick_CALIB_TENMS_Pos             0U                                            /*!< SysTick CALIB: TENMS Position */
+#define SysTick_CALIB_TENMS_Msk            (0xFFFFFFUL /*<< SysTick_CALIB_TENMS_Pos*/)    /*!< SysTick CALIB: TENMS Mask */
+
+/*@} end of group CMSIS_SysTick */
+
+
+/**
+  \ingroup  CMSIS_core_register
+  \defgroup CMSIS_CoreDebug       Core Debug Registers (CoreDebug)
+  \brief    Cortex-M0 Core Debug Registers (DCB registers, SHCSR, and DFSR) are only accessible over DAP and not via processor.
+            Therefore they are not covered by the Cortex-M0 header file.
+  @{
+ */
+/*@} end of group CMSIS_CoreDebug */
+
+
+/**
+  \ingroup    CMSIS_core_register
+  \defgroup   CMSIS_core_bitfield     Core register bit field macros
+  \brief      Macros for use with bit field definitions (xxx_Pos, xxx_Msk).
+  @{
+ */
+
+/**
+  \brief   Mask and shift a bit field value for use in a register bit range.
+  \param[in] field  Name of the register bit field.
+  \param[in] value  Value of the bit field. This parameter is interpreted as an uint32_t type.
+  \return           Masked and shifted value.
+*/
+#define _VAL2FLD(field, value)    (((uint32_t)(value) << field ## _Pos) & field ## _Msk)
+
+/**
+  \brief     Mask and shift a register value to extract a bit filed value.
+  \param[in] field  Name of the register bit field.
+  \param[in] value  Value of register. This parameter is interpreted as an uint32_t type.
+  \return           Masked and shifted bit field value.
+*/
+#define _FLD2VAL(field, value)    (((uint32_t)(value) & field ## _Msk) >> field ## _Pos)
+
+/*@} end of group CMSIS_core_bitfield */
+
+
+/**
+  \ingroup    CMSIS_core_register
+  \defgroup   CMSIS_core_base     Core Definitions
+  \brief      Definitions for base addresses, unions, and structures.
+  @{
+ */
+
+/* Memory mapping of Core Hardware */
+#define SCS_BASE            (0xE000E000UL)                            /*!< System Control Space Base Address */
+#define SysTick_BASE        (SCS_BASE +  0x0010UL)                    /*!< SysTick Base Address */
+#define NVIC_BASE           (SCS_BASE +  0x0100UL)                    /*!< NVIC Base Address */
+#define SCB_BASE            (SCS_BASE +  0x0D00UL)                    /*!< System Control Block Base Address */
+
+#define SCB                 ((SCB_Type       *)     SCB_BASE      )   /*!< SCB configuration struct */
+#define SysTick             ((SysTick_Type   *)     SysTick_BASE  )   /*!< SysTick configuration struct */
+#define NVIC                ((NVIC_Type      *)     NVIC_BASE     )   /*!< NVIC configuration struct */
+
+
+/*@} */
+
+
+
+/*******************************************************************************
+ *                Hardware Abstraction Layer
+  Core Function Interface contains:
+  - Core NVIC Functions
+  - Core SysTick Functions
+  - Core Register Access Functions
+ ******************************************************************************/
+/**
+  \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
+*/
+
+
+
+/* ##########################   NVIC functions  #################################### */
+/**
+  \ingroup  CMSIS_Core_FunctionInterface
+  \defgroup CMSIS_Core_NVICFunctions NVIC Functions
+  \brief    Functions that manage interrupts and exceptions via the NVIC.
+  @{
+ */
+
+#ifdef CMSIS_NVIC_VIRTUAL
+  #ifndef CMSIS_NVIC_VIRTUAL_HEADER_FILE
+    #define CMSIS_NVIC_VIRTUAL_HEADER_FILE "cmsis_nvic_virtual.h"
+  #endif
+  #include CMSIS_NVIC_VIRTUAL_HEADER_FILE
+#else
+  #define NVIC_SetPriorityGrouping    __NVIC_SetPriorityGrouping
+  #define NVIC_GetPriorityGrouping    __NVIC_GetPriorityGrouping
+  #define NVIC_EnableIRQ              __NVIC_EnableIRQ
+  #define NVIC_GetEnableIRQ           __NVIC_GetEnableIRQ
+  #define NVIC_DisableIRQ             __NVIC_DisableIRQ
+  #define NVIC_GetPendingIRQ          __NVIC_GetPendingIRQ
+  #define NVIC_SetPendingIRQ          __NVIC_SetPendingIRQ
+  #define NVIC_ClearPendingIRQ        __NVIC_ClearPendingIRQ
+/*#define NVIC_GetActive              __NVIC_GetActive             not available for Cortex-M0 */
+  #define NVIC_SetPriority            __NVIC_SetPriority
+  #define NVIC_GetPriority            __NVIC_GetPriority
+  #define NVIC_SystemReset            __NVIC_SystemReset
+#endif /* CMSIS_NVIC_VIRTUAL */
+
+#ifdef CMSIS_VECTAB_VIRTUAL
+  #ifndef CMSIS_VECTAB_VIRTUAL_HEADER_FILE
+    #define CMSIS_VECTAB_VIRTUAL_HEADER_FILE "cmsis_vectab_virtual.h"
+  #endif
+  #include CMSIS_VECTAB_VIRTUAL_HEADER_FILE
+#else
+  #define NVIC_SetVector              __NVIC_SetVector
+  #define NVIC_GetVector              __NVIC_GetVector
+#endif  /* (CMSIS_VECTAB_VIRTUAL) */
+
+#define NVIC_USER_IRQ_OFFSET          16
+
+
+/* The following EXC_RETURN values are saved the LR on exception entry */
+#define EXC_RETURN_HANDLER         (0xFFFFFFF1UL)     /* return to Handler mode, uses MSP after return                               */
+#define EXC_RETURN_THREAD_MSP      (0xFFFFFFF9UL)     /* return to Thread mode, uses MSP after return                                */
+#define EXC_RETURN_THREAD_PSP      (0xFFFFFFFDUL)     /* return to Thread mode, uses PSP after return                                */
+
+
+/* Interrupt Priorities are WORD accessible only under Armv6-M                  */
+/* The following MACROS handle generation of the register offset and byte masks */
+#define _BIT_SHIFT(IRQn)         (  ((((uint32_t)(int32_t)(IRQn))         )      &  0x03UL) * 8UL)
+#define _SHP_IDX(IRQn)           ( (((((uint32_t)(int32_t)(IRQn)) & 0x0FUL)-8UL) >>    2UL)      )
+#define _IP_IDX(IRQn)            (   (((uint32_t)(int32_t)(IRQn))                >>    2UL)      )
+
+#define __NVIC_SetPriorityGrouping(X) (void)(X)
+#define __NVIC_GetPriorityGrouping()  (0U)
+
+/**
+  \brief   Enable Interrupt
+  \details Enables a device specific interrupt in the NVIC interrupt controller.
+  \param [in]      IRQn  Device specific interrupt number.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_EnableIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->ISER[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+  }
+}
+
+
+/**
+  \brief   Get Interrupt Enable status
+  \details Returns a device specific interrupt enable status from the NVIC interrupt controller.
+  \param [in]      IRQn  Device specific interrupt number.
+  \return             0  Interrupt is not enabled.
+  \return             1  Interrupt is enabled.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE uint32_t __NVIC_GetEnableIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    return((uint32_t)(((NVIC->ISER[0U] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL));
+  }
+  else
+  {
+    return(0U);
+  }
+}
+
+
+/**
+  \brief   Disable Interrupt
+  \details Disables a device specific interrupt in the NVIC interrupt controller.
+  \param [in]      IRQn  Device specific interrupt number.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_DisableIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->ICER[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+    __DSB();
+    __ISB();
+  }
+}
+
+
+/**
+  \brief   Get Pending Interrupt
+  \details Reads the NVIC pending register and returns the pending bit for the specified device specific interrupt.
+  \param [in]      IRQn  Device specific interrupt number.
+  \return             0  Interrupt status is not pending.
+  \return             1  Interrupt status is pending.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE uint32_t __NVIC_GetPendingIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    return((uint32_t)(((NVIC->ISPR[0U] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL));
+  }
+  else
+  {
+    return(0U);
+  }
+}
+
+
+/**
+  \brief   Set Pending Interrupt
+  \details Sets the pending bit of a device specific interrupt in the NVIC pending register.
+  \param [in]      IRQn  Device specific interrupt number.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_SetPendingIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->ISPR[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+  }
+}
+
+
+/**
+  \brief   Clear Pending Interrupt
+  \details Clears the pending bit of a device specific interrupt in the NVIC pending register.
+  \param [in]      IRQn  Device specific interrupt number.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_ClearPendingIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->ICPR[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+  }
+}
+
+
+/**
+  \brief   Set Interrupt Priority
+  \details Sets the priority of a device specific interrupt or a processor exception.
+           The interrupt number can be positive to specify a device specific interrupt,
+           or negative to specify a processor exception.
+  \param [in]      IRQn  Interrupt number.
+  \param [in]  priority  Priority to set.
+  \note    The priority cannot be set for every processor exception.
+ */
+__STATIC_INLINE void __NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->IP[_IP_IDX(IRQn)]  = ((uint32_t)(NVIC->IP[_IP_IDX(IRQn)]  & ~(0xFFUL << _BIT_SHIFT(IRQn))) |
+       (((priority << (8U - __NVIC_PRIO_BITS)) & (uint32_t)0xFFUL) << _BIT_SHIFT(IRQn)));
+  }
+  else
+  {
+    SCB->SHP[_SHP_IDX(IRQn)] = ((uint32_t)(SCB->SHP[_SHP_IDX(IRQn)] & ~(0xFFUL << _BIT_SHIFT(IRQn))) |
+       (((priority << (8U - __NVIC_PRIO_BITS)) & (uint32_t)0xFFUL) << _BIT_SHIFT(IRQn)));
+  }
+}
+
+
+/**
+  \brief   Get Interrupt Priority
+  \details Reads the priority of a device specific interrupt or a processor exception.
+           The interrupt number can be positive to specify a device specific interrupt,
+           or negative to specify a processor exception.
+  \param [in]   IRQn  Interrupt number.
+  \return             Interrupt Priority.
+                      Value is aligned automatically to the implemented priority bits of the microcontroller.
+ */
+__STATIC_INLINE uint32_t __NVIC_GetPriority(IRQn_Type IRQn)
+{
+
+  if ((int32_t)(IRQn) >= 0)
+  {
+    return((uint32_t)(((NVIC->IP[ _IP_IDX(IRQn)] >> _BIT_SHIFT(IRQn) ) & (uint32_t)0xFFUL) >> (8U - __NVIC_PRIO_BITS)));
+  }
+  else
+  {
+    return((uint32_t)(((SCB->SHP[_SHP_IDX(IRQn)] >> _BIT_SHIFT(IRQn) ) & (uint32_t)0xFFUL) >> (8U - __NVIC_PRIO_BITS)));
+  }
+}
+
+
+/**
+  \brief   Encode Priority
+  \details Encodes the priority for an interrupt with the given priority group,
+           preemptive priority value, and subpriority value.
+           In case of a conflict between priority grouping and available
+           priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
+  \param [in]     PriorityGroup  Used priority group.
+  \param [in]   PreemptPriority  Preemptive priority value (starting from 0).
+  \param [in]       SubPriority  Subpriority value (starting from 0).
+  \return                        Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
+ */
+__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
+{
+  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07UL);   /* only values 0..7 are used          */
+  uint32_t PreemptPriorityBits;
+  uint32_t SubPriorityBits;
+
+  PreemptPriorityBits = ((7UL - PriorityGroupTmp) > (uint32_t)(__NVIC_PRIO_BITS)) ? (uint32_t)(__NVIC_PRIO_BITS) : (uint32_t)(7UL - PriorityGroupTmp);
+  SubPriorityBits     = ((PriorityGroupTmp + (uint32_t)(__NVIC_PRIO_BITS)) < (uint32_t)7UL) ? (uint32_t)0UL : (uint32_t)((PriorityGroupTmp - 7UL) + (uint32_t)(__NVIC_PRIO_BITS));
+
+  return (
+           ((PreemptPriority & (uint32_t)((1UL << (PreemptPriorityBits)) - 1UL)) << SubPriorityBits) |
+           ((SubPriority     & (uint32_t)((1UL << (SubPriorityBits    )) - 1UL)))
+         );
+}
+
+
+/**
+  \brief   Decode Priority
+  \details Decodes an interrupt priority value with a given priority group to
+           preemptive priority value and subpriority value.
+           In case of a conflict between priority grouping and available
+           priority bits (__NVIC_PRIO_BITS) the smallest possible priority group is set.
+  \param [in]         Priority   Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
+  \param [in]     PriorityGroup  Used priority group.
+  \param [out] pPreemptPriority  Preemptive priority value (starting from 0).
+  \param [out]     pSubPriority  Subpriority value (starting from 0).
+ */
+__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* const pPreemptPriority, uint32_t* const pSubPriority)
+{
+  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07UL);   /* only values 0..7 are used          */
+  uint32_t PreemptPriorityBits;
+  uint32_t SubPriorityBits;
+
+  PreemptPriorityBits = ((7UL - PriorityGroupTmp) > (uint32_t)(__NVIC_PRIO_BITS)) ? (uint32_t)(__NVIC_PRIO_BITS) : (uint32_t)(7UL - PriorityGroupTmp);
+  SubPriorityBits     = ((PriorityGroupTmp + (uint32_t)(__NVIC_PRIO_BITS)) < (uint32_t)7UL) ? (uint32_t)0UL : (uint32_t)((PriorityGroupTmp - 7UL) + (uint32_t)(__NVIC_PRIO_BITS));
+
+  *pPreemptPriority = (Priority >> SubPriorityBits) & (uint32_t)((1UL << (PreemptPriorityBits)) - 1UL);
+  *pSubPriority     = (Priority                   ) & (uint32_t)((1UL << (SubPriorityBits    )) - 1UL);
+}
+
+
+
+/**
+  \brief   Set Interrupt Vector
+  \details Sets an interrupt vector in SRAM based interrupt vector table.
+           The interrupt number can be positive to specify a device specific interrupt,
+           or negative to specify a processor exception.
+           Address 0 must be mapped to SRAM.
+  \param [in]   IRQn      Interrupt number
+  \param [in]   vector    Address of interrupt handler function
+ */
+__STATIC_INLINE void __NVIC_SetVector(IRQn_Type IRQn, uint32_t vector)
+{
+  uint32_t *vectors = (uint32_t *)0x0U;
+  vectors[(int32_t)IRQn + NVIC_USER_IRQ_OFFSET] = vector;
+}
+
+
+/**
+  \brief   Get Interrupt Vector
+  \details Reads an interrupt vector from interrupt vector table.
+           The interrupt number can be positive to specify a device specific interrupt,
+           or negative to specify a processor exception.
+  \param [in]   IRQn      Interrupt number.
+  \return                 Address of interrupt handler function
+ */
+__STATIC_INLINE uint32_t __NVIC_GetVector(IRQn_Type IRQn)
+{
+  uint32_t *vectors = (uint32_t *)0x0U;
+  return vectors[(int32_t)IRQn + NVIC_USER_IRQ_OFFSET];
+}
+
+
+/**
+  \brief   System Reset
+  \details Initiates a system reset request to reset the MCU.
+ */
+__NO_RETURN __STATIC_INLINE void __NVIC_SystemReset(void)
+{
+  __DSB();                                                          /* Ensure all outstanding memory accesses included
+                                                                       buffered write are completed before reset */
+  SCB->AIRCR  = ((0x5FAUL << SCB_AIRCR_VECTKEY_Pos) |
+                 SCB_AIRCR_SYSRESETREQ_Msk);
+  __DSB();                                                          /* Ensure completion of memory access */
+
+  for(;;)                                                           /* wait until reset */
+  {
+    __NOP();
+  }
+}
+
+/*@} end of CMSIS_Core_NVICFunctions */
+
+
+/* ##########################  FPU functions  #################################### */
+/**
+  \ingroup  CMSIS_Core_FunctionInterface
+  \defgroup CMSIS_Core_FpuFunctions FPU Functions
+  \brief    Function that provides FPU type.
+  @{
+ */
+
+/**
+  \brief   get FPU type
+  \details returns the FPU type
+  \returns
+   - \b  0: No FPU
+   - \b  1: Single precision FPU
+   - \b  2: Double + Single precision FPU
+ */
+__STATIC_INLINE uint32_t SCB_GetFPUType(void)
+{
+    return 0U;           /* No FPU */
+}
+
+
+/*@} end of CMSIS_Core_FpuFunctions */
+
+
+
+/* ##################################    SysTick function  ############################################ */
+/**
+  \ingroup  CMSIS_Core_FunctionInterface
+  \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
+  \brief    Functions that configure the System.
+  @{
+ */
+
+#if defined (__Vendor_SysTickConfig) && (__Vendor_SysTickConfig == 0U)
+
+/**
+  \brief   System Tick Configuration
+  \details Initializes the System Timer and its interrupt, and starts the System Tick Timer.
+           Counter is in free running mode to generate periodic interrupts.
+  \param [in]  ticks  Number of ticks between two interrupts.
+  \return          0  Function succeeded.
+  \return          1  Function failed.
+  \note    When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
+           function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
+           must contain a vendor-specific implementation of this function.
+ */
+__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
+{
+  if ((ticks - 1UL) > SysTick_LOAD_RELOAD_Msk)
+  {
+    return (1UL);                                                   /* Reload value impossible */
+  }
+
+  SysTick->LOAD  = (uint32_t)(ticks - 1UL);                         /* set reload register */
+  NVIC_SetPriority (SysTick_IRQn, (1UL << __NVIC_PRIO_BITS) - 1UL); /* set Priority for Systick Interrupt */
+  SysTick->VAL   = 0UL;                                             /* Load the SysTick Counter Value */
+  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |
+                   SysTick_CTRL_TICKINT_Msk   |
+                   SysTick_CTRL_ENABLE_Msk;                         /* Enable SysTick IRQ and SysTick Timer */
+  return (0UL);                                                     /* Function successful */
+}
+
+#endif
+
+/*@} end of CMSIS_Core_SysTickFunctions */
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CORE_CM0_H_DEPENDANT */
+
+#endif /* __CMSIS_GENERIC */

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Include/core_cm1.h

@@ -0,0 +1,976 @@
+/**************************************************************************//**
+ * @file     core_cm1.h
+ * @brief    CMSIS Cortex-M1 Core Peripheral Access Layer Header File
+ * @version  V1.0.0
+ * @date     23. July 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if   defined ( __ICCARM__ )
+  #pragma system_include         /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+  #pragma clang system_header   /* treat file as system include file */
+#endif
+
+#ifndef __CORE_CM1_H_GENERIC
+#define __CORE_CM1_H_GENERIC
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/**
+  \page CMSIS_MISRA_Exceptions  MISRA-C:2004 Compliance Exceptions
+  CMSIS violates the following MISRA-C:2004 rules:
+
+   \li Required Rule 8.5, object/function definition in header file.<br>
+     Function definitions in header files are used to allow 'inlining'.
+
+   \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
+     Unions are used for effective representation of core registers.
+
+   \li Advisory Rule 19.7, Function-like macro defined.<br>
+     Function-like macros are used to allow more efficient code.
+ */
+
+
+/*******************************************************************************
+ *                 CMSIS definitions
+ ******************************************************************************/
+/**
+  \ingroup Cortex_M1
+  @{
+ */
+
+#include "cmsis_version.h"
+ 
+/*  CMSIS CM1 definitions */
+#define __CM1_CMSIS_VERSION_MAIN  (__CM_CMSIS_VERSION_MAIN)              /*!< \deprecated [31:16] CMSIS HAL main version */
+#define __CM1_CMSIS_VERSION_SUB   (__CM_CMSIS_VERSION_SUB)               /*!< \deprecated [15:0]  CMSIS HAL sub version */
+#define __CM1_CMSIS_VERSION       ((__CM1_CMSIS_VERSION_MAIN << 16U) | \
+                                    __CM1_CMSIS_VERSION_SUB           )  /*!< \deprecated CMSIS HAL version number */
+
+#define __CORTEX_M                (1U)                                   /*!< Cortex-M Core */
+
+/** __FPU_USED indicates whether an FPU is used or not.
+    This core does not support an FPU at all
+*/
+#define __FPU_USED       0U
+
+#if defined ( __CC_ARM )
+  #if defined __TARGET_FPU_VFP
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
+  #if defined __ARM_PCS_VFP
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __GNUC__ )
+  #if defined (__VFP_FP__) && !defined(__SOFTFP__)
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __ICCARM__ )
+  #if defined __ARMVFP__
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __TI_ARM__ )
+  #if defined __TI_VFP_SUPPORT__
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __TASKING__ )
+  #if defined __FPU_VFP__
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#elif defined ( __CSMC__ )
+  #if ( __CSMC__ & 0x400U)
+    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
+  #endif
+
+#endif
+
+#include "cmsis_compiler.h"               /* CMSIS compiler specific defines */
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CORE_CM1_H_GENERIC */
+
+#ifndef __CMSIS_GENERIC
+
+#ifndef __CORE_CM1_H_DEPENDANT
+#define __CORE_CM1_H_DEPENDANT
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+/* check device defines and use defaults */
+#if defined __CHECK_DEVICE_DEFINES
+  #ifndef __CM1_REV
+    #define __CM1_REV               0x0100U
+    #warning "__CM1_REV not defined in device header file; using default!"
+  #endif
+
+  #ifndef __NVIC_PRIO_BITS
+    #define __NVIC_PRIO_BITS          2U
+    #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
+  #endif
+
+  #ifndef __Vendor_SysTickConfig
+    #define __Vendor_SysTickConfig    0U
+    #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
+  #endif
+#endif
+
+/* IO definitions (access restrictions to peripheral registers) */
+/**
+    \defgroup CMSIS_glob_defs CMSIS Global Defines
+
+    <strong>IO Type Qualifiers</strong> are used
+    \li to specify the access to peripheral variables.
+    \li for automatic generation of peripheral register debug information.
+*/
+#ifdef __cplusplus
+  #define   __I     volatile             /*!< Defines 'read only' permissions */
+#else
+  #define   __I     volatile const       /*!< Defines 'read only' permissions */
+#endif
+#define     __O     volatile             /*!< Defines 'write only' permissions */
+#define     __IO    volatile             /*!< Defines 'read / write' permissions */
+
+/* following defines should be used for structure members */
+#define     __IM     volatile const      /*! Defines 'read only' structure member permissions */
+#define     __OM     volatile            /*! Defines 'write only' structure member permissions */
+#define     __IOM    volatile            /*! Defines 'read / write' structure member permissions */
+
+/*@} end of group Cortex_M1 */
+
+
+
+/*******************************************************************************
+ *                 Register Abstraction
+  Core Register contain:
+  - Core Register
+  - Core NVIC Register
+  - Core SCB Register
+  - Core SysTick Register
+ ******************************************************************************/
+/**
+  \defgroup CMSIS_core_register Defines and Type Definitions
+  \brief Type definitions and defines for Cortex-M processor based devices.
+*/
+
+/**
+  \ingroup    CMSIS_core_register
+  \defgroup   CMSIS_CORE  Status and Control Registers
+  \brief      Core Register type definitions.
+  @{
+ */
+
+/**
+  \brief  Union type to access the Application Program Status Register (APSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t _reserved0:28;              /*!< bit:  0..27  Reserved */
+    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag */
+    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag */
+    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag */
+    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag */
+  } b;                                   /*!< Structure used for bit  access */
+  uint32_t w;                            /*!< Type      used for word access */
+} APSR_Type;
+
+/* APSR Register Definitions */
+#define APSR_N_Pos                         31U                                            /*!< APSR: N Position */
+#define APSR_N_Msk                         (1UL << APSR_N_Pos)                            /*!< APSR: N Mask */
+
+#define APSR_Z_Pos                         30U                                            /*!< APSR: Z Position */
+#define APSR_Z_Msk                         (1UL << APSR_Z_Pos)                            /*!< APSR: Z Mask */
+
+#define APSR_C_Pos                         29U                                            /*!< APSR: C Position */
+#define APSR_C_Msk                         (1UL << APSR_C_Pos)                            /*!< APSR: C Mask */
+
+#define APSR_V_Pos                         28U                                            /*!< APSR: V Position */
+#define APSR_V_Msk                         (1UL << APSR_V_Pos)                            /*!< APSR: V Mask */
+
+
+/**
+  \brief  Union type to access the Interrupt Program Status Register (IPSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number */
+    uint32_t _reserved0:23;              /*!< bit:  9..31  Reserved */
+  } b;                                   /*!< Structure used for bit  access */
+  uint32_t w;                            /*!< Type      used for word access */
+} IPSR_Type;
+
+/* IPSR Register Definitions */
+#define IPSR_ISR_Pos                        0U                                            /*!< IPSR: ISR Position */
+#define IPSR_ISR_Msk                       (0x1FFUL /*<< IPSR_ISR_Pos*/)                  /*!< IPSR: ISR Mask */
+
+
+/**
+  \brief  Union type to access the Special-Purpose Program Status Registers (xPSR).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number */
+    uint32_t _reserved0:15;              /*!< bit:  9..23  Reserved */
+    uint32_t T:1;                        /*!< bit:     24  Thumb bit        (read 0) */
+    uint32_t _reserved1:3;               /*!< bit: 25..27  Reserved */
+    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag */
+    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag */
+    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag */
+    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag */
+  } b;                                   /*!< Structure used for bit  access */
+  uint32_t w;                            /*!< Type      used for word access */
+} xPSR_Type;
+
+/* xPSR Register Definitions */
+#define xPSR_N_Pos                         31U                                            /*!< xPSR: N Position */
+#define xPSR_N_Msk                         (1UL << xPSR_N_Pos)                            /*!< xPSR: N Mask */
+
+#define xPSR_Z_Pos                         30U                                            /*!< xPSR: Z Position */
+#define xPSR_Z_Msk                         (1UL << xPSR_Z_Pos)                            /*!< xPSR: Z Mask */
+
+#define xPSR_C_Pos                         29U                                            /*!< xPSR: C Position */
+#define xPSR_C_Msk                         (1UL << xPSR_C_Pos)                            /*!< xPSR: C Mask */
+
+#define xPSR_V_Pos                         28U                                            /*!< xPSR: V Position */
+#define xPSR_V_Msk                         (1UL << xPSR_V_Pos)                            /*!< xPSR: V Mask */
+
+#define xPSR_T_Pos                         24U                                            /*!< xPSR: T Position */
+#define xPSR_T_Msk                         (1UL << xPSR_T_Pos)                            /*!< xPSR: T Mask */
+
+#define xPSR_ISR_Pos                        0U                                            /*!< xPSR: ISR Position */
+#define xPSR_ISR_Msk                       (0x1FFUL /*<< xPSR_ISR_Pos*/)                  /*!< xPSR: ISR Mask */
+
+
+/**
+  \brief  Union type to access the Control Registers (CONTROL).
+ */
+typedef union
+{
+  struct
+  {
+    uint32_t _reserved0:1;               /*!< bit:      0  Reserved */
+    uint32_t SPSEL:1;                    /*!< bit:      1  Stack to be used */
+    uint32_t _reserved1:30;              /*!< bit:  2..31  Reserved */
+  } b;                                   /*!< Structure used for bit  access */
+  uint32_t w;                            /*!< Type      used for word access */
+} CONTROL_Type;
+
+/* CONTROL Register Definitions */
+#define CONTROL_SPSEL_Pos                   1U                                            /*!< CONTROL: SPSEL Position */
+#define CONTROL_SPSEL_Msk                  (1UL << CONTROL_SPSEL_Pos)                     /*!< CONTROL: SPSEL Mask */
+
+/*@} end of group CMSIS_CORE */
+
+
+/**
+  \ingroup    CMSIS_core_register
+  \defgroup   CMSIS_NVIC  Nested Vectored Interrupt Controller (NVIC)
+  \brief      Type definitions for the NVIC Registers
+  @{
+ */
+
+/**
+  \brief  Structure type to access the Nested Vectored Interrupt Controller (NVIC).
+ */
+typedef struct
+{
+  __IOM uint32_t ISER[1U];               /*!< Offset: 0x000 (R/W)  Interrupt Set Enable Register */
+        uint32_t RESERVED0[31U];
+  __IOM uint32_t ICER[1U];               /*!< Offset: 0x080 (R/W)  Interrupt Clear Enable Register */
+        uint32_t RSERVED1[31U];
+  __IOM uint32_t ISPR[1U];               /*!< Offset: 0x100 (R/W)  Interrupt Set Pending Register */
+        uint32_t RESERVED2[31U];
+  __IOM uint32_t ICPR[1U];               /*!< Offset: 0x180 (R/W)  Interrupt Clear Pending Register */
+        uint32_t RESERVED3[31U];
+        uint32_t RESERVED4[64U];
+  __IOM uint32_t IP[8U];                 /*!< Offset: 0x300 (R/W)  Interrupt Priority Register */
+}  NVIC_Type;
+
+/*@} end of group CMSIS_NVIC */
+
+
+/**
+  \ingroup  CMSIS_core_register
+  \defgroup CMSIS_SCB     System Control Block (SCB)
+  \brief    Type definitions for the System Control Block Registers
+  @{
+ */
+
+/**
+  \brief  Structure type to access the System Control Block (SCB).
+ */
+typedef struct
+{
+  __IM  uint32_t CPUID;                  /*!< Offset: 0x000 (R/ )  CPUID Base Register */
+  __IOM uint32_t ICSR;                   /*!< Offset: 0x004 (R/W)  Interrupt Control and State Register */
+        uint32_t RESERVED0;
+  __IOM uint32_t AIRCR;                  /*!< Offset: 0x00C (R/W)  Application Interrupt and Reset Control Register */
+  __IOM uint32_t SCR;                    /*!< Offset: 0x010 (R/W)  System Control Register */
+  __IOM uint32_t CCR;                    /*!< Offset: 0x014 (R/W)  Configuration Control Register */
+        uint32_t RESERVED1;
+  __IOM uint32_t SHP[2U];                /*!< Offset: 0x01C (R/W)  System Handlers Priority Registers. [0] is RESERVED */
+  __IOM uint32_t SHCSR;                  /*!< Offset: 0x024 (R/W)  System Handler Control and State Register */
+} SCB_Type;
+
+/* SCB CPUID Register Definitions */
+#define SCB_CPUID_IMPLEMENTER_Pos          24U                                            /*!< SCB CPUID: IMPLEMENTER Position */
+#define SCB_CPUID_IMPLEMENTER_Msk          (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos)          /*!< SCB CPUID: IMPLEMENTER Mask */
+
+#define SCB_CPUID_VARIANT_Pos              20U                                            /*!< SCB CPUID: VARIANT Position */
+#define SCB_CPUID_VARIANT_Msk              (0xFUL << SCB_CPUID_VARIANT_Pos)               /*!< SCB CPUID: VARIANT Mask */
+
+#define SCB_CPUID_ARCHITECTURE_Pos         16U                                            /*!< SCB CPUID: ARCHITECTURE Position */
+#define SCB_CPUID_ARCHITECTURE_Msk         (0xFUL << SCB_CPUID_ARCHITECTURE_Pos)          /*!< SCB CPUID: ARCHITECTURE Mask */
+
+#define SCB_CPUID_PARTNO_Pos                4U                                            /*!< SCB CPUID: PARTNO Position */
+#define SCB_CPUID_PARTNO_Msk               (0xFFFUL << SCB_CPUID_PARTNO_Pos)              /*!< SCB CPUID: PARTNO Mask */
+
+#define SCB_CPUID_REVISION_Pos              0U                                            /*!< SCB CPUID: REVISION Position */
+#define SCB_CPUID_REVISION_Msk             (0xFUL /*<< SCB_CPUID_REVISION_Pos*/)          /*!< SCB CPUID: REVISION Mask */
+
+/* SCB Interrupt Control State Register Definitions */
+#define SCB_ICSR_NMIPENDSET_Pos            31U                                            /*!< SCB ICSR: NMIPENDSET Position */
+#define SCB_ICSR_NMIPENDSET_Msk            (1UL << SCB_ICSR_NMIPENDSET_Pos)               /*!< SCB ICSR: NMIPENDSET Mask */
+
+#define SCB_ICSR_PENDSVSET_Pos             28U                                            /*!< SCB ICSR: PENDSVSET Position */
+#define SCB_ICSR_PENDSVSET_Msk             (1UL << SCB_ICSR_PENDSVSET_Pos)                /*!< SCB ICSR: PENDSVSET Mask */
+
+#define SCB_ICSR_PENDSVCLR_Pos             27U                                            /*!< SCB ICSR: PENDSVCLR Position */
+#define SCB_ICSR_PENDSVCLR_Msk             (1UL << SCB_ICSR_PENDSVCLR_Pos)                /*!< SCB ICSR: PENDSVCLR Mask */
+
+#define SCB_ICSR_PENDSTSET_Pos             26U                                            /*!< SCB ICSR: PENDSTSET Position */
+#define SCB_ICSR_PENDSTSET_Msk             (1UL << SCB_ICSR_PENDSTSET_Pos)                /*!< SCB ICSR: PENDSTSET Mask */
+
+#define SCB_ICSR_PENDSTCLR_Pos             25U                                            /*!< SCB ICSR: PENDSTCLR Position */
+#define SCB_ICSR_PENDSTCLR_Msk             (1UL << SCB_ICSR_PENDSTCLR_Pos)                /*!< SCB ICSR: PENDSTCLR Mask */
+
+#define SCB_ICSR_ISRPREEMPT_Pos            23U                                            /*!< SCB ICSR: ISRPREEMPT Position */
+#define SCB_ICSR_ISRPREEMPT_Msk            (1UL << SCB_ICSR_ISRPREEMPT_Pos)               /*!< SCB ICSR: ISRPREEMPT Mask */
+
+#define SCB_ICSR_ISRPENDING_Pos            22U                                            /*!< SCB ICSR: ISRPENDING Position */
+#define SCB_ICSR_ISRPENDING_Msk            (1UL << SCB_ICSR_ISRPENDING_Pos)               /*!< SCB ICSR: ISRPENDING Mask */
+
+#define SCB_ICSR_VECTPENDING_Pos           12U                                            /*!< SCB ICSR: VECTPENDING Position */
+#define SCB_ICSR_VECTPENDING_Msk           (0x1FFUL << SCB_ICSR_VECTPENDING_Pos)          /*!< SCB ICSR: VECTPENDING Mask */
+
+#define SCB_ICSR_VECTACTIVE_Pos             0U                                            /*!< SCB ICSR: VECTACTIVE Position */
+#define SCB_ICSR_VECTACTIVE_Msk            (0x1FFUL /*<< SCB_ICSR_VECTACTIVE_Pos*/)       /*!< SCB ICSR: VECTACTIVE Mask */
+
+/* SCB Application Interrupt and Reset Control Register Definitions */
+#define SCB_AIRCR_VECTKEY_Pos              16U                                            /*!< SCB AIRCR: VECTKEY Position */
+#define SCB_AIRCR_VECTKEY_Msk              (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos)            /*!< SCB AIRCR: VECTKEY Mask */
+
+#define SCB_AIRCR_VECTKEYSTAT_Pos          16U                                            /*!< SCB AIRCR: VECTKEYSTAT Position */
+#define SCB_AIRCR_VECTKEYSTAT_Msk          (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos)        /*!< SCB AIRCR: VECTKEYSTAT Mask */
+
+#define SCB_AIRCR_ENDIANESS_Pos            15U                                            /*!< SCB AIRCR: ENDIANESS Position */
+#define SCB_AIRCR_ENDIANESS_Msk            (1UL << SCB_AIRCR_ENDIANESS_Pos)               /*!< SCB AIRCR: ENDIANESS Mask */
+
+#define SCB_AIRCR_SYSRESETREQ_Pos           2U                                            /*!< SCB AIRCR: SYSRESETREQ Position */
+#define SCB_AIRCR_SYSRESETREQ_Msk          (1UL << SCB_AIRCR_SYSRESETREQ_Pos)             /*!< SCB AIRCR: SYSRESETREQ Mask */
+
+#define SCB_AIRCR_VECTCLRACTIVE_Pos         1U                                            /*!< SCB AIRCR: VECTCLRACTIVE Position */
+#define SCB_AIRCR_VECTCLRACTIVE_Msk        (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos)           /*!< SCB AIRCR: VECTCLRACTIVE Mask */
+
+/* SCB System Control Register Definitions */
+#define SCB_SCR_SEVONPEND_Pos               4U                                            /*!< SCB SCR: SEVONPEND Position */
+#define SCB_SCR_SEVONPEND_Msk              (1UL << SCB_SCR_SEVONPEND_Pos)                 /*!< SCB SCR: SEVONPEND Mask */
+
+#define SCB_SCR_SLEEPDEEP_Pos               2U                                            /*!< SCB SCR: SLEEPDEEP Position */
+#define SCB_SCR_SLEEPDEEP_Msk              (1UL << SCB_SCR_SLEEPDEEP_Pos)                 /*!< SCB SCR: SLEEPDEEP Mask */
+
+#define SCB_SCR_SLEEPONEXIT_Pos             1U                                            /*!< SCB SCR: SLEEPONEXIT Position */
+#define SCB_SCR_SLEEPONEXIT_Msk            (1UL << SCB_SCR_SLEEPONEXIT_Pos)               /*!< SCB SCR: SLEEPONEXIT Mask */
+
+/* SCB Configuration Control Register Definitions */
+#define SCB_CCR_STKALIGN_Pos                9U                                            /*!< SCB CCR: STKALIGN Position */
+#define SCB_CCR_STKALIGN_Msk               (1UL << SCB_CCR_STKALIGN_Pos)                  /*!< SCB CCR: STKALIGN Mask */
+
+#define SCB_CCR_UNALIGN_TRP_Pos             3U                                            /*!< SCB CCR: UNALIGN_TRP Position */
+#define SCB_CCR_UNALIGN_TRP_Msk            (1UL << SCB_CCR_UNALIGN_TRP_Pos)               /*!< SCB CCR: UNALIGN_TRP Mask */
+
+/* SCB System Handler Control and State Register Definitions */
+#define SCB_SHCSR_SVCALLPENDED_Pos         15U                                            /*!< SCB SHCSR: SVCALLPENDED Position */
+#define SCB_SHCSR_SVCALLPENDED_Msk         (1UL << SCB_SHCSR_SVCALLPENDED_Pos)            /*!< SCB SHCSR: SVCALLPENDED Mask */
+
+/*@} end of group CMSIS_SCB */
+
+
+/**
+  \ingroup  CMSIS_core_register
+  \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)
+  \brief    Type definitions for the System Control and ID Register not in the SCB
+  @{
+ */
+
+/**
+  \brief  Structure type to access the System Control and ID Register not in the SCB.
+ */
+typedef struct
+{
+        uint32_t RESERVED0[2U];
+  __IOM uint32_t ACTLR;                  /*!< Offset: 0x008 (R/W)  Auxiliary Control Register */
+} SCnSCB_Type;
+
+/* Auxiliary Control Register Definitions */
+#define SCnSCB_ACTLR_ITCMUAEN_Pos            4U                                        /*!< ACTLR: Instruction TCM Upper Alias Enable Position */
+#define SCnSCB_ACTLR_ITCMUAEN_Msk           (1UL << SCnSCB_ACTLR_ITCMUAEN_Pos)         /*!< ACTLR: Instruction TCM Upper Alias Enable Mask */
+
+#define SCnSCB_ACTLR_ITCMLAEN_Pos            3U                                        /*!< ACTLR: Instruction TCM Lower Alias Enable Position */
+#define SCnSCB_ACTLR_ITCMLAEN_Msk           (1UL << SCnSCB_ACTLR_ITCMLAEN_Pos)         /*!< ACTLR: Instruction TCM Lower Alias Enable Mask */
+
+/*@} end of group CMSIS_SCnotSCB */
+
+
+/**
+  \ingroup  CMSIS_core_register
+  \defgroup CMSIS_SysTick     System Tick Timer (SysTick)
+  \brief    Type definitions for the System Timer Registers.
+  @{
+ */
+
+/**
+  \brief  Structure type to access the System Timer (SysTick).
+ */
+typedef struct
+{
+  __IOM uint32_t CTRL;                   /*!< Offset: 0x000 (R/W)  SysTick Control and Status Register */
+  __IOM uint32_t LOAD;                   /*!< Offset: 0x004 (R/W)  SysTick Reload Value Register */
+  __IOM uint32_t VAL;                    /*!< Offset: 0x008 (R/W)  SysTick Current Value Register */
+  __IM  uint32_t CALIB;                  /*!< Offset: 0x00C (R/ )  SysTick Calibration Register */
+} SysTick_Type;
+
+/* SysTick Control / Status Register Definitions */
+#define SysTick_CTRL_COUNTFLAG_Pos         16U                                            /*!< SysTick CTRL: COUNTFLAG Position */
+#define SysTick_CTRL_COUNTFLAG_Msk         (1UL << SysTick_CTRL_COUNTFLAG_Pos)            /*!< SysTick CTRL: COUNTFLAG Mask */
+
+#define SysTick_CTRL_CLKSOURCE_Pos          2U                                            /*!< SysTick CTRL: CLKSOURCE Position */
+#define SysTick_CTRL_CLKSOURCE_Msk         (1UL << SysTick_CTRL_CLKSOURCE_Pos)            /*!< SysTick CTRL: CLKSOURCE Mask */
+
+#define SysTick_CTRL_TICKINT_Pos            1U                                            /*!< SysTick CTRL: TICKINT Position */
+#define SysTick_CTRL_TICKINT_Msk           (1UL << SysTick_CTRL_TICKINT_Pos)              /*!< SysTick CTRL: TICKINT Mask */
+
+#define SysTick_CTRL_ENABLE_Pos             0U                                            /*!< SysTick CTRL: ENABLE Position */
+#define SysTick_CTRL_ENABLE_Msk            (1UL /*<< SysTick_CTRL_ENABLE_Pos*/)           /*!< SysTick CTRL: ENABLE Mask */
+
+/* SysTick Reload Register Definitions */
+#define SysTick_LOAD_RELOAD_Pos             0U                                            /*!< SysTick LOAD: RELOAD Position */
+#define SysTick_LOAD_RELOAD_Msk            (0xFFFFFFUL /*<< SysTick_LOAD_RELOAD_Pos*/)    /*!< SysTick LOAD: RELOAD Mask */
+
+/* SysTick Current Register Definitions */
+#define SysTick_VAL_CURRENT_Pos             0U                                            /*!< SysTick VAL: CURRENT Position */
+#define SysTick_VAL_CURRENT_Msk            (0xFFFFFFUL /*<< SysTick_VAL_CURRENT_Pos*/)    /*!< SysTick VAL: CURRENT Mask */
+
+/* SysTick Calibration Register Definitions */
+#define SysTick_CALIB_NOREF_Pos            31U                                            /*!< SysTick CALIB: NOREF Position */
+#define SysTick_CALIB_NOREF_Msk            (1UL << SysTick_CALIB_NOREF_Pos)               /*!< SysTick CALIB: NOREF Mask */
+
+#define SysTick_CALIB_SKEW_Pos             30U                                            /*!< SysTick CALIB: SKEW Position */
+#define SysTick_CALIB_SKEW_Msk             (1UL << SysTick_CALIB_SKEW_Pos)                /*!< SysTick CALIB: SKEW Mask */
+
+#define SysTick_CALIB_TENMS_Pos             0U                                            /*!< SysTick CALIB: TENMS Position */
+#define SysTick_CALIB_TENMS_Msk            (0xFFFFFFUL /*<< SysTick_CALIB_TENMS_Pos*/)    /*!< SysTick CALIB: TENMS Mask */
+
+/*@} end of group CMSIS_SysTick */
+
+
+/**
+  \ingroup  CMSIS_core_register
+  \defgroup CMSIS_CoreDebug       Core Debug Registers (CoreDebug)
+  \brief    Cortex-M1 Core Debug Registers (DCB registers, SHCSR, and DFSR) are only accessible over DAP and not via processor.
+            Therefore they are not covered by the Cortex-M1 header file.
+  @{
+ */
+/*@} end of group CMSIS_CoreDebug */
+
+
+/**
+  \ingroup    CMSIS_core_register
+  \defgroup   CMSIS_core_bitfield     Core register bit field macros
+  \brief      Macros for use with bit field definitions (xxx_Pos, xxx_Msk).
+  @{
+ */
+
+/**
+  \brief   Mask and shift a bit field value for use in a register bit range.
+  \param[in] field  Name of the register bit field.
+  \param[in] value  Value of the bit field. This parameter is interpreted as an uint32_t type.
+  \return           Masked and shifted value.
+*/
+#define _VAL2FLD(field, value)    (((uint32_t)(value) << field ## _Pos) & field ## _Msk)
+
+/**
+  \brief     Mask and shift a register value to extract a bit filed value.
+  \param[in] field  Name of the register bit field.
+  \param[in] value  Value of register. This parameter is interpreted as an uint32_t type.
+  \return           Masked and shifted bit field value.
+*/
+#define _FLD2VAL(field, value)    (((uint32_t)(value) & field ## _Msk) >> field ## _Pos)
+
+/*@} end of group CMSIS_core_bitfield */
+
+
+/**
+  \ingroup    CMSIS_core_register
+  \defgroup   CMSIS_core_base     Core Definitions
+  \brief      Definitions for base addresses, unions, and structures.
+  @{
+ */
+
+/* Memory mapping of Core Hardware */
+#define SCS_BASE            (0xE000E000UL)                            /*!< System Control Space Base Address */
+#define SysTick_BASE        (SCS_BASE +  0x0010UL)                    /*!< SysTick Base Address */
+#define NVIC_BASE           (SCS_BASE +  0x0100UL)                    /*!< NVIC Base Address */
+#define SCB_BASE            (SCS_BASE +  0x0D00UL)                    /*!< System Control Block Base Address */
+
+#define SCnSCB              ((SCnSCB_Type    *)     SCS_BASE      )   /*!< System control Register not in SCB */
+#define SCB                 ((SCB_Type       *)     SCB_BASE      )   /*!< SCB configuration struct */
+#define SysTick             ((SysTick_Type   *)     SysTick_BASE  )   /*!< SysTick configuration struct */
+#define NVIC                ((NVIC_Type      *)     NVIC_BASE     )   /*!< NVIC configuration struct */
+
+
+/*@} */
+
+
+
+/*******************************************************************************
+ *                Hardware Abstraction Layer
+  Core Function Interface contains:
+  - Core NVIC Functions
+  - Core SysTick Functions
+  - Core Register Access Functions
+ ******************************************************************************/
+/**
+  \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
+*/
+
+
+
+/* ##########################   NVIC functions  #################################### */
+/**
+  \ingroup  CMSIS_Core_FunctionInterface
+  \defgroup CMSIS_Core_NVICFunctions NVIC Functions
+  \brief    Functions that manage interrupts and exceptions via the NVIC.
+  @{
+ */
+
+#ifdef CMSIS_NVIC_VIRTUAL
+  #ifndef CMSIS_NVIC_VIRTUAL_HEADER_FILE
+    #define CMSIS_NVIC_VIRTUAL_HEADER_FILE "cmsis_nvic_virtual.h"
+  #endif
+  #include CMSIS_NVIC_VIRTUAL_HEADER_FILE
+#else
+  #define NVIC_SetPriorityGrouping    __NVIC_SetPriorityGrouping
+  #define NVIC_GetPriorityGrouping    __NVIC_GetPriorityGrouping
+  #define NVIC_EnableIRQ              __NVIC_EnableIRQ
+  #define NVIC_GetEnableIRQ           __NVIC_GetEnableIRQ
+  #define NVIC_DisableIRQ             __NVIC_DisableIRQ
+  #define NVIC_GetPendingIRQ          __NVIC_GetPendingIRQ
+  #define NVIC_SetPendingIRQ          __NVIC_SetPendingIRQ
+  #define NVIC_ClearPendingIRQ        __NVIC_ClearPendingIRQ
+/*#define NVIC_GetActive              __NVIC_GetActive             not available for Cortex-M1 */
+  #define NVIC_SetPriority            __NVIC_SetPriority
+  #define NVIC_GetPriority            __NVIC_GetPriority
+  #define NVIC_SystemReset            __NVIC_SystemReset
+#endif /* CMSIS_NVIC_VIRTUAL */
+
+#ifdef CMSIS_VECTAB_VIRTUAL
+  #ifndef CMSIS_VECTAB_VIRTUAL_HEADER_FILE
+    #define CMSIS_VECTAB_VIRTUAL_HEADER_FILE "cmsis_vectab_virtual.h"
+  #endif
+  #include CMSIS_VECTAB_VIRTUAL_HEADER_FILE
+#else
+  #define NVIC_SetVector              __NVIC_SetVector
+  #define NVIC_GetVector              __NVIC_GetVector
+#endif  /* (CMSIS_VECTAB_VIRTUAL) */
+
+#define NVIC_USER_IRQ_OFFSET          16
+
+
+/* The following EXC_RETURN values are saved the LR on exception entry */
+#define EXC_RETURN_HANDLER         (0xFFFFFFF1UL)     /* return to Handler mode, uses MSP after return                               */
+#define EXC_RETURN_THREAD_MSP      (0xFFFFFFF9UL)     /* return to Thread mode, uses MSP after return                                */
+#define EXC_RETURN_THREAD_PSP      (0xFFFFFFFDUL)     /* return to Thread mode, uses PSP after return                                */
+
+
+/* Interrupt Priorities are WORD accessible only under Armv6-M                  */
+/* The following MACROS handle generation of the register offset and byte masks */
+#define _BIT_SHIFT(IRQn)         (  ((((uint32_t)(int32_t)(IRQn))         )      &  0x03UL) * 8UL)
+#define _SHP_IDX(IRQn)           ( (((((uint32_t)(int32_t)(IRQn)) & 0x0FUL)-8UL) >>    2UL)      )
+#define _IP_IDX(IRQn)            (   (((uint32_t)(int32_t)(IRQn))                >>    2UL)      )
+
+#define __NVIC_SetPriorityGrouping(X) (void)(X)
+#define __NVIC_GetPriorityGrouping()  (0U)
+
+/**
+  \brief   Enable Interrupt
+  \details Enables a device specific interrupt in the NVIC interrupt controller.
+  \param [in]      IRQn  Device specific interrupt number.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_EnableIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->ISER[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+  }
+}
+
+
+/**
+  \brief   Get Interrupt Enable status
+  \details Returns a device specific interrupt enable status from the NVIC interrupt controller.
+  \param [in]      IRQn  Device specific interrupt number.
+  \return             0  Interrupt is not enabled.
+  \return             1  Interrupt is enabled.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE uint32_t __NVIC_GetEnableIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    return((uint32_t)(((NVIC->ISER[0U] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL));
+  }
+  else
+  {
+    return(0U);
+  }
+}
+
+
+/**
+  \brief   Disable Interrupt
+  \details Disables a device specific interrupt in the NVIC interrupt controller.
+  \param [in]      IRQn  Device specific interrupt number.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_DisableIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->ICER[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+    __DSB();
+    __ISB();
+  }
+}
+
+
+/**
+  \brief   Get Pending Interrupt
+  \details Reads the NVIC pending register and returns the pending bit for the specified device specific interrupt.
+  \param [in]      IRQn  Device specific interrupt number.
+  \return             0  Interrupt status is not pending.
+  \return             1  Interrupt status is pending.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE uint32_t __NVIC_GetPendingIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    return((uint32_t)(((NVIC->ISPR[0U] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL));
+  }
+  else
+  {
+    return(0U);
+  }
+}
+
+
+/**
+  \brief   Set Pending Interrupt
+  \details Sets the pending bit of a device specific interrupt in the NVIC pending register.
+  \param [in]      IRQn  Device specific interrupt number.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_SetPendingIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->ISPR[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+  }
+}
+
+
+/**
+  \brief   Clear Pending Interrupt
+  \details Clears the pending bit of a device specific interrupt in the NVIC pending register.
+  \param [in]      IRQn  Device specific interrupt number.
+  \note    IRQn must not be negative.
+ */
+__STATIC_INLINE void __NVIC_ClearPendingIRQ(IRQn_Type IRQn)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->ICPR[0U] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL));
+  }
+}
+
+
+/**
+  \brief   Set Interrupt Priority
+  \details Sets the priority of a device specific interrupt or a processor exception.
+           The interrupt number can be positive to specify a device specific interrupt,
+           or negative to specify a processor exception.
+  \param [in]      IRQn  Interrupt number.
+  \param [in]  priority  Priority to set.
+  \note    The priority cannot be set for every processor exception.
+ */
+__STATIC_INLINE void __NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
+{
+  if ((int32_t)(IRQn) >= 0)
+  {
+    NVIC->IP[_IP_IDX(IRQn)]  = ((uint32_t)(NVIC->IP[_IP_IDX(IRQn)]  & ~(0xFFUL << _BIT_SHIFT(IRQn))) |
+       (((priority << (8U - __NVIC_PRIO_BITS)) & (uint32_t)0xFFUL) << _BIT_SHIFT(IRQn)));
+  }
+  else
+  {
+    SCB->SHP[_SHP_IDX(IRQn)] = ((uint32_t)(SCB->SHP[_SHP_IDX(IRQn)] & ~(0xFFUL << _BIT_SHIFT(IRQn))) |
+       (((priority << (8U - __NVIC_PRIO_BITS)) & (uint32_t)0xFFUL) << _BIT_SHIFT(IRQn)));
+  }
+}
+
+
+/**
+  \brief   Get Interrupt Priority
+  \details Reads the priority of a device specific interrupt or a processor exception.
+           The interrupt number can be positive to specify a device specific interrupt,
+           or negative to specify a processor exception.
+  \param [in]   IRQn  Interrupt number.
+  \return             Interrupt Priority.
+                      Value is aligned automatically to the implemented priority bits of the microcontroller.
+ */
+__STATIC_INLINE uint32_t __NVIC_GetPriority(IRQn_Type IRQn)
+{
+
+  if ((int32_t)(IRQn) >= 0)
+  {
+    return((uint32_t)(((NVIC->IP[ _IP_IDX(IRQn)] >> _BIT_SHIFT(IRQn) ) & (uint32_t)0xFFUL) >> (8U - __NVIC_PRIO_BITS)));
+  }
+  else
+  {
+    return((uint32_t)(((SCB->SHP[_SHP_IDX(IRQn)] >> _BIT_SHIFT(IRQn) ) & (uint32_t)0xFFUL) >> (8U - __NVIC_PRIO_BITS)));
+  }
+}
+
+
+/**
+  \brief   Encode Priority
+  \details Encodes the priority for an interrupt with the given priority group,
+           preemptive priority value, and subpriority value.
+           In case of a conflict between priority grouping and available
+           priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
+  \param [in]     PriorityGroup  Used priority group.
+  \param [in]   PreemptPriority  Preemptive priority value (starting from 0).
+  \param [in]       SubPriority  Subpriority value (starting from 0).
+  \return                        Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
+ */
+__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
+{
+  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07UL);   /* only values 0..7 are used          */
+  uint32_t PreemptPriorityBits;
+  uint32_t SubPriorityBits;
+
+  PreemptPriorityBits = ((7UL - PriorityGroupTmp) > (uint32_t)(__NVIC_PRIO_BITS)) ? (uint32_t)(__NVIC_PRIO_BITS) : (uint32_t)(7UL - PriorityGroupTmp);
+  SubPriorityBits     = ((PriorityGroupTmp + (uint32_t)(__NVIC_PRIO_BITS)) < (uint32_t)7UL) ? (uint32_t)0UL : (uint32_t)((PriorityGroupTmp - 7UL) + (uint32_t)(__NVIC_PRIO_BITS));
+
+  return (
+           ((PreemptPriority & (uint32_t)((1UL << (PreemptPriorityBits)) - 1UL)) << SubPriorityBits) |
+           ((SubPriority     & (uint32_t)((1UL << (SubPriorityBits    )) - 1UL)))
+         );
+}
+
+
+/**
+  \brief   Decode Priority
+  \details Decodes an interrupt priority value with a given priority group to
+           preemptive priority value and subpriority value.
+           In case of a conflict between priority grouping and available
+           priority bits (__NVIC_PRIO_BITS) the smallest possible priority group is set.
+  \param [in]         Priority   Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
+  \param [in]     PriorityGroup  Used priority group.
+  \param [out] pPreemptPriority  Preemptive priority value (starting from 0).
+  \param [out]     pSubPriority  Subpriority value (starting from 0).
+ */
+__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* const pPreemptPriority, uint32_t* const pSubPriority)
+{
+  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07UL);   /* only values 0..7 are used          */
+  uint32_t PreemptPriorityBits;
+  uint32_t SubPriorityBits;
+
+  PreemptPriorityBits = ((7UL - PriorityGroupTmp) > (uint32_t)(__NVIC_PRIO_BITS)) ? (uint32_t)(__NVIC_PRIO_BITS) : (uint32_t)(7UL - PriorityGroupTmp);
+  SubPriorityBits     = ((PriorityGroupTmp + (uint32_t)(__NVIC_PRIO_BITS)) < (uint32_t)7UL) ? (uint32_t)0UL : (uint32_t)((PriorityGroupTmp - 7UL) + (uint32_t)(__NVIC_PRIO_BITS));
+
+  *pPreemptPriority = (Priority >> SubPriorityBits) & (uint32_t)((1UL << (PreemptPriorityBits)) - 1UL);
+  *pSubPriority     = (Priority                   ) & (uint32_t)((1UL << (SubPriorityBits    )) - 1UL);
+}
+
+
+
+/**
+  \brief   Set Interrupt Vector
+  \details Sets an interrupt vector in SRAM based interrupt vector table.
+           The interrupt number can be positive to specify a device specific interrupt,
+           or negative to specify a processor exception.
+           Address 0 must be mapped to SRAM.
+  \param [in]   IRQn      Interrupt number
+  \param [in]   vector    Address of interrupt handler function
+ */
+__STATIC_INLINE void __NVIC_SetVector(IRQn_Type IRQn, uint32_t vector)
+{
+  uint32_t *vectors = (uint32_t *)0x0U;
+  vectors[(int32_t)IRQn + NVIC_USER_IRQ_OFFSET] = vector;
+}
+
+
+/**
+  \brief   Get Interrupt Vector
+  \details Reads an interrupt vector from interrupt vector table.
+           The interrupt number can be positive to specify a device specific interrupt,
+           or negative to specify a processor exception.
+  \param [in]   IRQn      Interrupt number.
+  \return                 Address of interrupt handler function
+ */
+__STATIC_INLINE uint32_t __NVIC_GetVector(IRQn_Type IRQn)
+{
+  uint32_t *vectors = (uint32_t *)0x0U;
+  return vectors[(int32_t)IRQn + NVIC_USER_IRQ_OFFSET];
+}
+
+
+/**
+  \brief   System Reset
+  \details Initiates a system reset request to reset the MCU.
+ */
+__NO_RETURN __STATIC_INLINE void __NVIC_SystemReset(void)
+{
+  __DSB();                                                          /* Ensure all outstanding memory accesses included
+                                                                       buffered write are completed before reset */
+  SCB->AIRCR  = ((0x5FAUL << SCB_AIRCR_VECTKEY_Pos) |
+                 SCB_AIRCR_SYSRESETREQ_Msk);
+  __DSB();                                                          /* Ensure completion of memory access */
+
+  for(;;)                                                           /* wait until reset */
+  {
+    __NOP();
+  }
+}
+
+/*@} end of CMSIS_Core_NVICFunctions */
+
+
+/* ##########################  FPU functions  #################################### */
+/**
+  \ingroup  CMSIS_Core_FunctionInterface
+  \defgroup CMSIS_Core_FpuFunctions FPU Functions
+  \brief    Function that provides FPU type.
+  @{
+ */
+
+/**
+  \brief   get FPU type
+  \details returns the FPU type
+  \returns
+   - \b  0: No FPU
+   - \b  1: Single precision FPU
+   - \b  2: Double + Single precision FPU
+ */
+__STATIC_INLINE uint32_t SCB_GetFPUType(void)
+{
+    return 0U;           /* No FPU */
+}
+
+
+/*@} end of CMSIS_Core_FpuFunctions */
+
+
+
+/* ##################################    SysTick function  ############################################ */
+/**
+  \ingroup  CMSIS_Core_FunctionInterface
+  \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
+  \brief    Functions that configure the System.
+  @{
+ */
+
+#if defined (__Vendor_SysTickConfig) && (__Vendor_SysTickConfig == 0U)
+
+/**
+  \brief   System Tick Configuration
+  \details Initializes the System Timer and its interrupt, and starts the System Tick Timer.
+           Counter is in free running mode to generate periodic interrupts.
+  \param [in]  ticks  Number of ticks between two interrupts.
+  \return          0  Function succeeded.
+  \return          1  Function failed.
+  \note    When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
+           function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
+           must contain a vendor-specific implementation of this function.
+ */
+__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
+{
+  if ((ticks - 1UL) > SysTick_LOAD_RELOAD_Msk)
+  {
+    return (1UL);                                                   /* Reload value impossible */
+  }
+
+  SysTick->LOAD  = (uint32_t)(ticks - 1UL);                         /* set reload register */
+  NVIC_SetPriority (SysTick_IRQn, (1UL << __NVIC_PRIO_BITS) - 1UL); /* set Priority for Systick Interrupt */
+  SysTick->VAL   = 0UL;                                             /* Load the SysTick Counter Value */
+  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |
+                   SysTick_CTRL_TICKINT_Msk   |
+                   SysTick_CTRL_ENABLE_Msk;                         /* Enable SysTick IRQ and SysTick Timer */
+  return (0UL);                                                     /* Function successful */
+}
+
+#endif
+
+/*@} end of CMSIS_Core_SysTickFunctions */
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __CORE_CM1_H_DEPENDANT */
+
+#endif /* __CMSIS_GENERIC */

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Include/mpu_armv7.h

@@ -0,0 +1,270 @@
+/******************************************************************************
+ * @file     mpu_armv7.h
+ * @brief    CMSIS MPU API for Armv7-M MPU
+ * @version  V5.0.4
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2017-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+ 
+#if   defined ( __ICCARM__ )
+  #pragma system_include         /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+  #pragma clang system_header    /* treat file as system include file */
+#endif
+ 
+#ifndef ARM_MPU_ARMV7_H
+#define ARM_MPU_ARMV7_H
+
+#define ARM_MPU_REGION_SIZE_32B      ((uint8_t)0x04U) ///!< MPU Region Size 32 Bytes
+#define ARM_MPU_REGION_SIZE_64B      ((uint8_t)0x05U) ///!< MPU Region Size 64 Bytes
+#define ARM_MPU_REGION_SIZE_128B     ((uint8_t)0x06U) ///!< MPU Region Size 128 Bytes
+#define ARM_MPU_REGION_SIZE_256B     ((uint8_t)0x07U) ///!< MPU Region Size 256 Bytes
+#define ARM_MPU_REGION_SIZE_512B     ((uint8_t)0x08U) ///!< MPU Region Size 512 Bytes
+#define ARM_MPU_REGION_SIZE_1KB      ((uint8_t)0x09U) ///!< MPU Region Size 1 KByte
+#define ARM_MPU_REGION_SIZE_2KB      ((uint8_t)0x0AU) ///!< MPU Region Size 2 KBytes
+#define ARM_MPU_REGION_SIZE_4KB      ((uint8_t)0x0BU) ///!< MPU Region Size 4 KBytes
+#define ARM_MPU_REGION_SIZE_8KB      ((uint8_t)0x0CU) ///!< MPU Region Size 8 KBytes
+#define ARM_MPU_REGION_SIZE_16KB     ((uint8_t)0x0DU) ///!< MPU Region Size 16 KBytes
+#define ARM_MPU_REGION_SIZE_32KB     ((uint8_t)0x0EU) ///!< MPU Region Size 32 KBytes
+#define ARM_MPU_REGION_SIZE_64KB     ((uint8_t)0x0FU) ///!< MPU Region Size 64 KBytes
+#define ARM_MPU_REGION_SIZE_128KB    ((uint8_t)0x10U) ///!< MPU Region Size 128 KBytes
+#define ARM_MPU_REGION_SIZE_256KB    ((uint8_t)0x11U) ///!< MPU Region Size 256 KBytes
+#define ARM_MPU_REGION_SIZE_512KB    ((uint8_t)0x12U) ///!< MPU Region Size 512 KBytes
+#define ARM_MPU_REGION_SIZE_1MB      ((uint8_t)0x13U) ///!< MPU Region Size 1 MByte
+#define ARM_MPU_REGION_SIZE_2MB      ((uint8_t)0x14U) ///!< MPU Region Size 2 MBytes
+#define ARM_MPU_REGION_SIZE_4MB      ((uint8_t)0x15U) ///!< MPU Region Size 4 MBytes
+#define ARM_MPU_REGION_SIZE_8MB      ((uint8_t)0x16U) ///!< MPU Region Size 8 MBytes
+#define ARM_MPU_REGION_SIZE_16MB     ((uint8_t)0x17U) ///!< MPU Region Size 16 MBytes
+#define ARM_MPU_REGION_SIZE_32MB     ((uint8_t)0x18U) ///!< MPU Region Size 32 MBytes
+#define ARM_MPU_REGION_SIZE_64MB     ((uint8_t)0x19U) ///!< MPU Region Size 64 MBytes
+#define ARM_MPU_REGION_SIZE_128MB    ((uint8_t)0x1AU) ///!< MPU Region Size 128 MBytes
+#define ARM_MPU_REGION_SIZE_256MB    ((uint8_t)0x1BU) ///!< MPU Region Size 256 MBytes
+#define ARM_MPU_REGION_SIZE_512MB    ((uint8_t)0x1CU) ///!< MPU Region Size 512 MBytes
+#define ARM_MPU_REGION_SIZE_1GB      ((uint8_t)0x1DU) ///!< MPU Region Size 1 GByte
+#define ARM_MPU_REGION_SIZE_2GB      ((uint8_t)0x1EU) ///!< MPU Region Size 2 GBytes
+#define ARM_MPU_REGION_SIZE_4GB      ((uint8_t)0x1FU) ///!< MPU Region Size 4 GBytes
+
+#define ARM_MPU_AP_NONE 0U ///!< MPU Access Permission no access
+#define ARM_MPU_AP_PRIV 1U ///!< MPU Access Permission privileged access only
+#define ARM_MPU_AP_URO  2U ///!< MPU Access Permission unprivileged access read-only
+#define ARM_MPU_AP_FULL 3U ///!< MPU Access Permission full access
+#define ARM_MPU_AP_PRO  5U ///!< MPU Access Permission privileged access read-only
+#define ARM_MPU_AP_RO   6U ///!< MPU Access Permission read-only access
+
+/** MPU Region Base Address Register Value
+*
+* \param Region The region to be configured, number 0 to 15.
+* \param BaseAddress The base address for the region.
+*/
+#define ARM_MPU_RBAR(Region, BaseAddress) \
+  (((BaseAddress) & MPU_RBAR_ADDR_Msk) |  \
+   ((Region) & MPU_RBAR_REGION_Msk)    |  \
+   (MPU_RBAR_VALID_Msk))
+
+/**
+* MPU Memory Access Attributes
+* 
+* \param TypeExtField      Type extension field, allows you to configure memory access type, for example strongly ordered, peripheral.
+* \param IsShareable       Region is shareable between multiple bus masters.
+* \param IsCacheable       Region is cacheable, i.e. its value may be kept in cache.
+* \param IsBufferable      Region is bufferable, i.e. using write-back caching. Cacheable but non-bufferable regions use write-through policy.
+*/  
+#define ARM_MPU_ACCESS_(TypeExtField, IsShareable, IsCacheable, IsBufferable)   \
+  ((((TypeExtField ) << MPU_RASR_TEX_Pos) & MPU_RASR_TEX_Msk)                 | \
+   (((IsShareable ) << MPU_RASR_S_Pos) & MPU_RASR_S_Msk)                      | \
+   (((IsCacheable ) << MPU_RASR_C_Pos) & MPU_RASR_C_Msk)                      | \
+   (((IsBufferable ) << MPU_RASR_B_Pos) & MPU_RASR_B_Msk))
+
+/**
+* MPU Region Attribute and Size Register Value
+* 
+* \param DisableExec       Instruction access disable bit, 1= disable instruction fetches.
+* \param AccessPermission  Data access permissions, allows you to configure read/write access for User and Privileged mode.
+* \param AccessAttributes  Memory access attribution, see \ref ARM_MPU_ACCESS_.
+* \param SubRegionDisable  Sub-region disable field.
+* \param Size              Region size of the region to be configured, for example 4K, 8K.
+*/
+#define ARM_MPU_RASR_EX(DisableExec, AccessPermission, AccessAttributes, SubRegionDisable, Size)      \
+  ((((DisableExec ) << MPU_RASR_XN_Pos) & MPU_RASR_XN_Msk)                                          | \
+   (((AccessPermission) << MPU_RASR_AP_Pos) & MPU_RASR_AP_Msk)                                      | \
+   (((AccessAttributes) ) & (MPU_RASR_TEX_Msk | MPU_RASR_S_Msk | MPU_RASR_C_Msk | MPU_RASR_B_Msk)))
+  
+/**
+* MPU Region Attribute and Size Register Value
+* 
+* \param DisableExec       Instruction access disable bit, 1= disable instruction fetches.
+* \param AccessPermission  Data access permissions, allows you to configure read/write access for User and Privileged mode.
+* \param TypeExtField      Type extension field, allows you to configure memory access type, for example strongly ordered, peripheral.
+* \param IsShareable       Region is shareable between multiple bus masters.
+* \param IsCacheable       Region is cacheable, i.e. its value may be kept in cache.
+* \param IsBufferable      Region is bufferable, i.e. using write-back caching. Cacheable but non-bufferable regions use write-through policy.
+* \param SubRegionDisable  Sub-region disable field.
+* \param Size              Region size of the region to be configured, for example 4K, 8K.
+*/                         
+#define ARM_MPU_RASR(DisableExec, AccessPermission, TypeExtField, IsShareable, IsCacheable, IsBufferable, SubRegionDisable, Size) \
+  ARM_MPU_RASR_EX(DisableExec, AccessPermission, ARM_MPU_ACCESS_(TypeExtField, IsShareable, IsCacheable, IsBufferable), SubRegionDisable, Size)
+
+/**
+* MPU Memory Access Attribute for strongly ordered memory.
+*  - TEX: 000b
+*  - Shareable
+*  - Non-cacheable
+*  - Non-bufferable
+*/ 
+#define ARM_MPU_ACCESS_ORDERED ARM_MPU_ACCESS_(0U, 1U, 0U, 0U)
+
+/**
+* MPU Memory Access Attribute for device memory.
+*  - TEX: 000b (if non-shareable) or 010b (if shareable)
+*  - Shareable or non-shareable
+*  - Non-cacheable
+*  - Bufferable (if shareable) or non-bufferable (if non-shareable)
+*
+* \param IsShareable Configures the device memory as shareable or non-shareable.
+*/ 
+#define ARM_MPU_ACCESS_DEVICE(IsShareable) ((IsShareable) ? ARM_MPU_ACCESS_(0U, 1U, 0U, 1U) : ARM_MPU_ACCESS_(2U, 0U, 0U, 0U))
+
+/**
+* MPU Memory Access Attribute for normal memory.
+*  - TEX: 1BBb (reflecting outer cacheability rules)
+*  - Shareable or non-shareable
+*  - Cacheable or non-cacheable (reflecting inner cacheability rules)
+*  - Bufferable or non-bufferable (reflecting inner cacheability rules)
+*
+* \param OuterCp Configures the outer cache policy.
+* \param InnerCp Configures the inner cache policy.
+* \param IsShareable Configures the memory as shareable or non-shareable.
+*/ 
+#define ARM_MPU_ACCESS_NORMAL(OuterCp, InnerCp, IsShareable) ARM_MPU_ACCESS_((4U | (OuterCp)), IsShareable, ((InnerCp) & 2U), ((InnerCp) & 1U))
+
+/**
+* MPU Memory Access Attribute non-cacheable policy.
+*/
+#define ARM_MPU_CACHEP_NOCACHE 0U
+
+/**
+* MPU Memory Access Attribute write-back, write and read allocate policy.
+*/
+#define ARM_MPU_CACHEP_WB_WRA 1U
+
+/**
+* MPU Memory Access Attribute write-through, no write allocate policy.
+*/
+#define ARM_MPU_CACHEP_WT_NWA 2U
+
+/**
+* MPU Memory Access Attribute write-back, no write allocate policy.
+*/
+#define ARM_MPU_CACHEP_WB_NWA 3U
+
+
+/**
+* Struct for a single MPU Region
+*/
+typedef struct {
+  uint32_t RBAR; //!< The region base address register value (RBAR)
+  uint32_t RASR; //!< The region attribute and size register value (RASR) \ref MPU_RASR
+} ARM_MPU_Region_t;
+    
+/** Enable the MPU.
+* \param MPU_Control Default access permissions for unconfigured regions.
+*/
+__STATIC_INLINE void ARM_MPU_Enable(uint32_t MPU_Control)
+{
+  __DSB();
+  __ISB();
+  MPU->CTRL = MPU_Control | MPU_CTRL_ENABLE_Msk;
+#ifdef SCB_SHCSR_MEMFAULTENA_Msk
+  SCB->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk;
+#endif
+}
+
+/** Disable the MPU.
+*/
+__STATIC_INLINE void ARM_MPU_Disable(void)
+{
+  __DSB();
+  __ISB();
+#ifdef SCB_SHCSR_MEMFAULTENA_Msk
+  SCB->SHCSR &= ~SCB_SHCSR_MEMFAULTENA_Msk;
+#endif
+  MPU->CTRL  &= ~MPU_CTRL_ENABLE_Msk;
+}
+
+/** Clear and disable the given MPU region.
+* \param rnr Region number to be cleared.
+*/
+__STATIC_INLINE void ARM_MPU_ClrRegion(uint32_t rnr)
+{
+  MPU->RNR = rnr;
+  MPU->RASR = 0U;
+}
+
+/** Configure an MPU region.
+* \param rbar Value for RBAR register.
+* \param rsar Value for RSAR register.
+*/   
+__STATIC_INLINE void ARM_MPU_SetRegion(uint32_t rbar, uint32_t rasr)
+{
+  MPU->RBAR = rbar;
+  MPU->RASR = rasr;
+}
+
+/** Configure the given MPU region.
+* \param rnr Region number to be configured.
+* \param rbar Value for RBAR register.
+* \param rsar Value for RSAR register.
+*/   
+__STATIC_INLINE void ARM_MPU_SetRegionEx(uint32_t rnr, uint32_t rbar, uint32_t rasr)
+{
+  MPU->RNR = rnr;
+  MPU->RBAR = rbar;
+  MPU->RASR = rasr;
+}
+
+/** Memcopy with strictly ordered memory access, e.g. for register targets.
+* \param dst Destination data is copied to.
+* \param src Source data is copied from.
+* \param len Amount of data words to be copied.
+*/
+__STATIC_INLINE void orderedCpy(volatile uint32_t* dst, const uint32_t* __RESTRICT src, uint32_t len)
+{
+  uint32_t i;
+  for (i = 0U; i < len; ++i) 
+  {
+    dst[i] = src[i];
+  }
+}
+
+/** Load the given number of MPU regions from a table.
+* \param table Pointer to the MPU configuration table.
+* \param cnt Amount of regions to be configured.
+*/
+__STATIC_INLINE void ARM_MPU_Load(ARM_MPU_Region_t const* table, uint32_t cnt) 
+{
+  const uint32_t rowWordSize = sizeof(ARM_MPU_Region_t)/4U;
+  while (cnt > MPU_TYPE_RALIASES) {
+    orderedCpy(&(MPU->RBAR), &(table->RBAR), MPU_TYPE_RALIASES*rowWordSize);
+    table += MPU_TYPE_RALIASES;
+    cnt -= MPU_TYPE_RALIASES;
+  }
+  orderedCpy(&(MPU->RBAR), &(table->RBAR), cnt*rowWordSize);
+}
+
+#endif

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Include/mpu_armv8.h

@@ -0,0 +1,333 @@
+/******************************************************************************
+ * @file     mpu_armv8.h
+ * @brief    CMSIS MPU API for Armv8-M MPU
+ * @version  V5.0.4
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2017-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if   defined ( __ICCARM__ )
+  #pragma system_include         /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+  #pragma clang system_header    /* treat file as system include file */
+#endif
+
+#ifndef ARM_MPU_ARMV8_H
+#define ARM_MPU_ARMV8_H
+
+/** \brief Attribute for device memory (outer only) */
+#define ARM_MPU_ATTR_DEVICE                           ( 0U )
+
+/** \brief Attribute for non-cacheable, normal memory */
+#define ARM_MPU_ATTR_NON_CACHEABLE                    ( 4U )
+
+/** \brief Attribute for normal memory (outer and inner)
+* \param NT Non-Transient: Set to 1 for non-transient data.
+* \param WB Write-Back: Set to 1 to use write-back update policy.
+* \param RA Read Allocation: Set to 1 to use cache allocation on read miss.
+* \param WA Write Allocation: Set to 1 to use cache allocation on write miss.
+*/
+#define ARM_MPU_ATTR_MEMORY_(NT, WB, RA, WA) \
+  (((NT & 1U) << 3U) | ((WB & 1U) << 2U) | ((RA & 1U) << 1U) | (WA & 1U))
+
+/** \brief Device memory type non Gathering, non Re-ordering, non Early Write Acknowledgement */
+#define ARM_MPU_ATTR_DEVICE_nGnRnE (0U)
+
+/** \brief Device memory type non Gathering, non Re-ordering, Early Write Acknowledgement */
+#define ARM_MPU_ATTR_DEVICE_nGnRE  (1U)
+
+/** \brief Device memory type non Gathering, Re-ordering, Early Write Acknowledgement */
+#define ARM_MPU_ATTR_DEVICE_nGRE   (2U)
+
+/** \brief Device memory type Gathering, Re-ordering, Early Write Acknowledgement */
+#define ARM_MPU_ATTR_DEVICE_GRE    (3U)
+
+/** \brief Memory Attribute
+* \param O Outer memory attributes
+* \param I O == ARM_MPU_ATTR_DEVICE: Device memory attributes, else: Inner memory attributes
+*/
+#define ARM_MPU_ATTR(O, I) (((O & 0xFU) << 4U) | (((O & 0xFU) != 0U) ? (I & 0xFU) : ((I & 0x3U) << 2U)))
+
+/** \brief Normal memory non-shareable  */
+#define ARM_MPU_SH_NON   (0U)
+
+/** \brief Normal memory outer shareable  */
+#define ARM_MPU_SH_OUTER (2U)
+
+/** \brief Normal memory inner shareable  */
+#define ARM_MPU_SH_INNER (3U)
+
+/** \brief Memory access permissions
+* \param RO Read-Only: Set to 1 for read-only memory.
+* \param NP Non-Privileged: Set to 1 for non-privileged memory.
+*/
+#define ARM_MPU_AP_(RO, NP) (((RO & 1U) << 1U) | (NP & 1U))
+
+/** \brief Region Base Address Register value
+* \param BASE The base address bits [31:5] of a memory region. The value is zero extended. Effective address gets 32 byte aligned.
+* \param SH Defines the Shareability domain for this memory region.
+* \param RO Read-Only: Set to 1 for a read-only memory region.
+* \param NP Non-Privileged: Set to 1 for a non-privileged memory region.
+* \oaram XN eXecute Never: Set to 1 for a non-executable memory region.
+*/
+#define ARM_MPU_RBAR(BASE, SH, RO, NP, XN) \
+  ((BASE & MPU_RBAR_BASE_Msk) | \
+  ((SH << MPU_RBAR_SH_Pos) & MPU_RBAR_SH_Msk) | \
+  ((ARM_MPU_AP_(RO, NP) << MPU_RBAR_AP_Pos) & MPU_RBAR_AP_Msk) | \
+  ((XN << MPU_RBAR_XN_Pos) & MPU_RBAR_XN_Msk))
+
+/** \brief Region Limit Address Register value
+* \param LIMIT The limit address bits [31:5] for this memory region. The value is one extended.
+* \param IDX The attribute index to be associated with this memory region.
+*/
+#define ARM_MPU_RLAR(LIMIT, IDX) \
+  ((LIMIT & MPU_RLAR_LIMIT_Msk) | \
+  ((IDX << MPU_RLAR_AttrIndx_Pos) & MPU_RLAR_AttrIndx_Msk) | \
+  (MPU_RLAR_EN_Msk))
+
+/**
+* Struct for a single MPU Region
+*/
+typedef struct {
+  uint32_t RBAR;                   /*!< Region Base Address Register value */
+  uint32_t RLAR;                   /*!< Region Limit Address Register value */
+} ARM_MPU_Region_t;
+    
+/** Enable the MPU.
+* \param MPU_Control Default access permissions for unconfigured regions.
+*/
+__STATIC_INLINE void ARM_MPU_Enable(uint32_t MPU_Control)
+{
+  __DSB();
+  __ISB();
+  MPU->CTRL = MPU_Control | MPU_CTRL_ENABLE_Msk;
+#ifdef SCB_SHCSR_MEMFAULTENA_Msk
+  SCB->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk;
+#endif
+}
+
+/** Disable the MPU.
+*/
+__STATIC_INLINE void ARM_MPU_Disable(void)
+{
+  __DSB();
+  __ISB();
+#ifdef SCB_SHCSR_MEMFAULTENA_Msk
+  SCB->SHCSR &= ~SCB_SHCSR_MEMFAULTENA_Msk;
+#endif
+  MPU->CTRL  &= ~MPU_CTRL_ENABLE_Msk;
+}
+
+#ifdef MPU_NS
+/** Enable the Non-secure MPU.
+* \param MPU_Control Default access permissions for unconfigured regions.
+*/
+__STATIC_INLINE void ARM_MPU_Enable_NS(uint32_t MPU_Control)
+{
+  __DSB();
+  __ISB();
+  MPU_NS->CTRL = MPU_Control | MPU_CTRL_ENABLE_Msk;
+#ifdef SCB_SHCSR_MEMFAULTENA_Msk
+  SCB_NS->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk;
+#endif
+}
+
+/** Disable the Non-secure MPU.
+*/
+__STATIC_INLINE void ARM_MPU_Disable_NS(void)
+{
+  __DSB();
+  __ISB();
+#ifdef SCB_SHCSR_MEMFAULTENA_Msk
+  SCB_NS->SHCSR &= ~SCB_SHCSR_MEMFAULTENA_Msk;
+#endif
+  MPU_NS->CTRL  &= ~MPU_CTRL_ENABLE_Msk;
+}
+#endif
+
+/** Set the memory attribute encoding to the given MPU.
+* \param mpu Pointer to the MPU to be configured.
+* \param idx The attribute index to be set [0-7]
+* \param attr The attribute value to be set.
+*/
+__STATIC_INLINE void ARM_MPU_SetMemAttrEx(MPU_Type* mpu, uint8_t idx, uint8_t attr)
+{
+  const uint8_t reg = idx / 4U;
+  const uint32_t pos = ((idx % 4U) * 8U);
+  const uint32_t mask = 0xFFU << pos;
+  
+  if (reg >= (sizeof(mpu->MAIR) / sizeof(mpu->MAIR[0]))) {
+    return; // invalid index
+  }
+  
+  mpu->MAIR[reg] = ((mpu->MAIR[reg] & ~mask) | ((attr << pos) & mask));
+}
+
+/** Set the memory attribute encoding.
+* \param idx The attribute index to be set [0-7]
+* \param attr The attribute value to be set.
+*/
+__STATIC_INLINE void ARM_MPU_SetMemAttr(uint8_t idx, uint8_t attr)
+{
+  ARM_MPU_SetMemAttrEx(MPU, idx, attr);
+}
+
+#ifdef MPU_NS
+/** Set the memory attribute encoding to the Non-secure MPU.
+* \param idx The attribute index to be set [0-7]
+* \param attr The attribute value to be set.
+*/
+__STATIC_INLINE void ARM_MPU_SetMemAttr_NS(uint8_t idx, uint8_t attr)
+{
+  ARM_MPU_SetMemAttrEx(MPU_NS, idx, attr);
+}
+#endif
+
+/** Clear and disable the given MPU region of the given MPU.
+* \param mpu Pointer to MPU to be used.
+* \param rnr Region number to be cleared.
+*/
+__STATIC_INLINE void ARM_MPU_ClrRegionEx(MPU_Type* mpu, uint32_t rnr)
+{
+  mpu->RNR = rnr;
+  mpu->RLAR = 0U;
+}
+
+/** Clear and disable the given MPU region.
+* \param rnr Region number to be cleared.
+*/
+__STATIC_INLINE void ARM_MPU_ClrRegion(uint32_t rnr)
+{
+  ARM_MPU_ClrRegionEx(MPU, rnr);
+}
+
+#ifdef MPU_NS
+/** Clear and disable the given Non-secure MPU region.
+* \param rnr Region number to be cleared.
+*/
+__STATIC_INLINE void ARM_MPU_ClrRegion_NS(uint32_t rnr)
+{  
+  ARM_MPU_ClrRegionEx(MPU_NS, rnr);
+}
+#endif
+
+/** Configure the given MPU region of the given MPU.
+* \param mpu Pointer to MPU to be used.
+* \param rnr Region number to be configured.
+* \param rbar Value for RBAR register.
+* \param rlar Value for RLAR register.
+*/   
+__STATIC_INLINE void ARM_MPU_SetRegionEx(MPU_Type* mpu, uint32_t rnr, uint32_t rbar, uint32_t rlar)
+{
+  mpu->RNR = rnr;
+  mpu->RBAR = rbar;
+  mpu->RLAR = rlar;
+}
+
+/** Configure the given MPU region.
+* \param rnr Region number to be configured.
+* \param rbar Value for RBAR register.
+* \param rlar Value for RLAR register.
+*/   
+__STATIC_INLINE void ARM_MPU_SetRegion(uint32_t rnr, uint32_t rbar, uint32_t rlar)
+{
+  ARM_MPU_SetRegionEx(MPU, rnr, rbar, rlar);
+}
+
+#ifdef MPU_NS
+/** Configure the given Non-secure MPU region.
+* \param rnr Region number to be configured.
+* \param rbar Value for RBAR register.
+* \param rlar Value for RLAR register.
+*/   
+__STATIC_INLINE void ARM_MPU_SetRegion_NS(uint32_t rnr, uint32_t rbar, uint32_t rlar)
+{
+  ARM_MPU_SetRegionEx(MPU_NS, rnr, rbar, rlar);  
+}
+#endif
+
+/** Memcopy with strictly ordered memory access, e.g. for register targets.
+* \param dst Destination data is copied to.
+* \param src Source data is copied from.
+* \param len Amount of data words to be copied.
+*/
+__STATIC_INLINE void orderedCpy(volatile uint32_t* dst, const uint32_t* __RESTRICT src, uint32_t len)
+{
+  uint32_t i;
+  for (i = 0U; i < len; ++i) 
+  {
+    dst[i] = src[i];
+  }
+}
+
+/** Load the given number of MPU regions from a table to the given MPU.
+* \param mpu Pointer to the MPU registers to be used.
+* \param rnr First region number to be configured.
+* \param table Pointer to the MPU configuration table.
+* \param cnt Amount of regions to be configured.
+*/
+__STATIC_INLINE void ARM_MPU_LoadEx(MPU_Type* mpu, uint32_t rnr, ARM_MPU_Region_t const* table, uint32_t cnt) 
+{
+  const uint32_t rowWordSize = sizeof(ARM_MPU_Region_t)/4U;
+  if (cnt == 1U) {
+    mpu->RNR = rnr;
+    orderedCpy(&(mpu->RBAR), &(table->RBAR), rowWordSize);
+  } else {
+    uint32_t rnrBase   = rnr & ~(MPU_TYPE_RALIASES-1U);
+    uint32_t rnrOffset = rnr % MPU_TYPE_RALIASES;
+    
+    mpu->RNR = rnrBase;
+    while ((rnrOffset + cnt) > MPU_TYPE_RALIASES) {
+      uint32_t c = MPU_TYPE_RALIASES - rnrOffset;
+      orderedCpy(&(mpu->RBAR)+(rnrOffset*2U), &(table->RBAR), c*rowWordSize);
+      table += c;
+      cnt -= c;
+      rnrOffset = 0U;
+      rnrBase += MPU_TYPE_RALIASES;
+      mpu->RNR = rnrBase;
+    }
+    
+    orderedCpy(&(mpu->RBAR)+(rnrOffset*2U), &(table->RBAR), cnt*rowWordSize);
+  }
+}
+
+/** Load the given number of MPU regions from a table.
+* \param rnr First region number to be configured.
+* \param table Pointer to the MPU configuration table.
+* \param cnt Amount of regions to be configured.
+*/
+__STATIC_INLINE void ARM_MPU_Load(uint32_t rnr, ARM_MPU_Region_t const* table, uint32_t cnt) 
+{
+  ARM_MPU_LoadEx(MPU, rnr, table, cnt);
+}
+
+#ifdef MPU_NS
+/** Load the given number of MPU regions from a table to the Non-secure MPU.
+* \param rnr First region number to be configured.
+* \param table Pointer to the MPU configuration table.
+* \param cnt Amount of regions to be configured.
+*/
+__STATIC_INLINE void ARM_MPU_Load_NS(uint32_t rnr, ARM_MPU_Region_t const* table, uint32_t cnt) 
+{
+  ARM_MPU_LoadEx(MPU_NS, rnr, table, cnt);
+}
+#endif
+
+#endif
+

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Include/tz_context.h

@@ -0,0 +1,70 @@
+/******************************************************************************
+ * @file     tz_context.h
+ * @brief    Context Management for Armv8-M TrustZone
+ * @version  V1.0.1
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2017-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if   defined ( __ICCARM__ )
+  #pragma system_include         /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+  #pragma clang system_header   /* treat file as system include file */
+#endif
+
+#ifndef TZ_CONTEXT_H
+#define TZ_CONTEXT_H
+ 
+#include <stdint.h>
+ 
+#ifndef TZ_MODULEID_T
+#define TZ_MODULEID_T
+/// \details Data type that identifies secure software modules called by a process.
+typedef uint32_t TZ_ModuleId_t;
+#endif
+ 
+/// \details TZ Memory ID identifies an allocated memory slot.
+typedef uint32_t TZ_MemoryId_t;
+  
+/// Initialize secure context memory system
+/// \return execution status (1: success, 0: error)
+uint32_t TZ_InitContextSystem_S (void);
+ 
+/// Allocate context memory for calling secure software modules in TrustZone
+/// \param[in]  module   identifies software modules called from non-secure mode
+/// \return value != 0 id TrustZone memory slot identifier
+/// \return value 0    no memory available or internal error
+TZ_MemoryId_t TZ_AllocModuleContext_S (TZ_ModuleId_t module);
+ 
+/// Free context memory that was previously allocated with \ref TZ_AllocModuleContext_S
+/// \param[in]  id  TrustZone memory slot identifier
+/// \return execution status (1: success, 0: error)
+uint32_t TZ_FreeModuleContext_S (TZ_MemoryId_t id);
+ 
+/// Load secure context (called on RTOS thread context switch)
+/// \param[in]  id  TrustZone memory slot identifier
+/// \return execution status (1: success, 0: error)
+uint32_t TZ_LoadContext_S (TZ_MemoryId_t id);
+ 
+/// Store secure context (called on RTOS thread context switch)
+/// \param[in]  id  TrustZone memory slot identifier
+/// \return execution status (1: success, 0: error)
+uint32_t TZ_StoreContext_S (TZ_MemoryId_t id);
+ 
+#endif  // TZ_CONTEXT_H

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Template/ARMv8-M/main_s.c

@@ -0,0 +1,58 @@
+/******************************************************************************
+ * @file     main_s.c
+ * @brief    Code template for secure main function
+ * @version  V1.1.1
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2013-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/* Use CMSE intrinsics */
+#include <arm_cmse.h>
+ 
+#include "RTE_Components.h"
+#include CMSIS_device_header
+ 
+/* TZ_START_NS: Start address of non-secure application */
+#ifndef TZ_START_NS
+#define TZ_START_NS (0x200000U)
+#endif
+ 
+/* typedef for non-secure callback functions */
+typedef void (*funcptr_void) (void) __attribute__((cmse_nonsecure_call));
+ 
+/* Secure main() */
+int main(void) {
+  funcptr_void NonSecure_ResetHandler;
+ 
+  /* Add user setup code for secure part here*/
+ 
+  /* Set non-secure main stack (MSP_NS) */
+  __TZ_set_MSP_NS(*((uint32_t *)(TZ_START_NS)));
+ 
+  /* Get non-secure reset handler */
+  NonSecure_ResetHandler = (funcptr_void)(*((uint32_t *)((TZ_START_NS) + 4U)));
+ 
+  /* Start non-secure state software application */
+  NonSecure_ResetHandler();
+ 
+  /* Non-secure software does not return, this code is not executed */
+  while (1) {
+    __NOP();
+  }
+}

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core/Template/ARMv8-M/tz_context.c

@@ -0,0 +1,200 @@
+/******************************************************************************
+ * @file     tz_context.c
+ * @brief    Context Management for Armv8-M TrustZone - Sample implementation
+ * @version  V1.1.1
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2016-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "RTE_Components.h"
+#include CMSIS_device_header
+#include "tz_context.h"
+
+/// Number of process slots (threads may call secure library code)
+#ifndef TZ_PROCESS_STACK_SLOTS
+#define TZ_PROCESS_STACK_SLOTS     8U
+#endif
+
+/// Stack size of the secure library code
+#ifndef TZ_PROCESS_STACK_SIZE
+#define TZ_PROCESS_STACK_SIZE      256U
+#endif
+
+typedef struct {
+  uint32_t sp_top;      // stack space top
+  uint32_t sp_limit;    // stack space limit
+  uint32_t sp;          // current stack pointer
+} stack_info_t;
+
+static stack_info_t ProcessStackInfo  [TZ_PROCESS_STACK_SLOTS];
+static uint64_t     ProcessStackMemory[TZ_PROCESS_STACK_SLOTS][TZ_PROCESS_STACK_SIZE/8U];
+static uint32_t     ProcessStackFreeSlot = 0xFFFFFFFFU;
+
+
+/// Initialize secure context memory system
+/// \return execution status (1: success, 0: error)
+__attribute__((cmse_nonsecure_entry))
+uint32_t TZ_InitContextSystem_S (void) {
+  uint32_t n;
+
+  if (__get_IPSR() == 0U) {
+    return 0U;  // Thread Mode
+  }
+
+  for (n = 0U; n < TZ_PROCESS_STACK_SLOTS; n++) {
+    ProcessStackInfo[n].sp = 0U;
+    ProcessStackInfo[n].sp_limit = (uint32_t)&ProcessStackMemory[n];
+    ProcessStackInfo[n].sp_top   = (uint32_t)&ProcessStackMemory[n] + TZ_PROCESS_STACK_SIZE;
+    *((uint32_t *)ProcessStackMemory[n]) = n + 1U;
+  }
+  *((uint32_t *)ProcessStackMemory[--n]) = 0xFFFFFFFFU;
+
+  ProcessStackFreeSlot = 0U;
+
+  // Default process stack pointer and stack limit
+  __set_PSPLIM((uint32_t)ProcessStackMemory);
+  __set_PSP   ((uint32_t)ProcessStackMemory);
+
+  // Privileged Thread Mode using PSP
+  __set_CONTROL(0x02U);
+
+  return 1U;    // Success
+}
+
+
+/// Allocate context memory for calling secure software modules in TrustZone
+/// \param[in]  module   identifies software modules called from non-secure mode
+/// \return value != 0 id TrustZone memory slot identifier
+/// \return value 0    no memory available or internal error
+__attribute__((cmse_nonsecure_entry))
+TZ_MemoryId_t TZ_AllocModuleContext_S (TZ_ModuleId_t module) {
+  uint32_t slot;
+
+  (void)module; // Ignore (fixed Stack size)
+
+  if (__get_IPSR() == 0U) {
+    return 0U;  // Thread Mode
+  }
+
+  if (ProcessStackFreeSlot == 0xFFFFFFFFU) {
+    return 0U;  // No slot available
+  }
+
+  slot = ProcessStackFreeSlot;
+  ProcessStackFreeSlot = *((uint32_t *)ProcessStackMemory[slot]);
+
+  ProcessStackInfo[slot].sp = ProcessStackInfo[slot].sp_top;
+
+  return (slot + 1U);
+}
+
+
+/// Free context memory that was previously allocated with \ref TZ_AllocModuleContext_S
+/// \param[in]  id  TrustZone memory slot identifier
+/// \return execution status (1: success, 0: error)
+__attribute__((cmse_nonsecure_entry))
+uint32_t TZ_FreeModuleContext_S (TZ_MemoryId_t id) {
+  uint32_t slot;
+
+  if (__get_IPSR() == 0U) {
+    return 0U;  // Thread Mode
+  }
+
+  if ((id == 0U) || (id > TZ_PROCESS_STACK_SLOTS)) {
+    return 0U;  // Invalid ID
+  }
+
+  slot = id - 1U;
+
+  if (ProcessStackInfo[slot].sp == 0U) {
+    return 0U;  // Inactive slot
+  }
+  ProcessStackInfo[slot].sp = 0U;
+
+  *((uint32_t *)ProcessStackMemory[slot]) = ProcessStackFreeSlot;
+  ProcessStackFreeSlot = slot;
+
+  return 1U;    // Success
+}
+
+
+/// Load secure context (called on RTOS thread context switch)
+/// \param[in]  id  TrustZone memory slot identifier
+/// \return execution status (1: success, 0: error)
+__attribute__((cmse_nonsecure_entry))
+uint32_t TZ_LoadContext_S (TZ_MemoryId_t id) {
+  uint32_t slot;
+
+  if ((__get_IPSR() == 0U) || ((__get_CONTROL() & 2U) == 0U)) {
+    return 0U;  // Thread Mode or using Main Stack for threads
+  }
+
+  if ((id == 0U) || (id > TZ_PROCESS_STACK_SLOTS)) {
+    return 0U;  // Invalid ID
+  }
+
+  slot = id - 1U;
+
+  if (ProcessStackInfo[slot].sp == 0U) {
+    return 0U;  // Inactive slot
+  }
+
+  // Setup process stack pointer and stack limit
+  __set_PSPLIM(ProcessStackInfo[slot].sp_limit);
+  __set_PSP   (ProcessStackInfo[slot].sp);
+
+  return 1U;    // Success
+}
+
+
+/// Store secure context (called on RTOS thread context switch)
+/// \param[in]  id  TrustZone memory slot identifier
+/// \return execution status (1: success, 0: error)
+__attribute__((cmse_nonsecure_entry))
+uint32_t TZ_StoreContext_S (TZ_MemoryId_t id) {
+  uint32_t slot;
+  uint32_t sp;
+
+  if ((__get_IPSR() == 0U) || ((__get_CONTROL() & 2U) == 0U)) {
+    return 0U;  // Thread Mode or using Main Stack for threads
+  }
+
+  if ((id == 0U) || (id > TZ_PROCESS_STACK_SLOTS)) {
+    return 0U;  // Invalid ID
+  }
+
+  slot = id - 1U;
+
+  if (ProcessStackInfo[slot].sp == 0U) {
+    return 0U;  // Inactive slot
+  }
+
+  sp = __get_PSP();
+  if ((sp < ProcessStackInfo[slot].sp_limit) ||
+      (sp > ProcessStackInfo[slot].sp_top)) {
+    return 0U;  // SP out of range
+  }
+  ProcessStackInfo[slot].sp = sp;
+
+  // Default process stack pointer and stack limit
+  __set_PSPLIM((uint32_t)ProcessStackMemory);
+  __set_PSP   ((uint32_t)ProcessStackMemory);
+
+  return 1U;    // Success
+}

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core_A/Include/cmsis_armcc.h

@@ -0,0 +1,544 @@
+/**************************************************************************//**
+ * @file     cmsis_armcc.h
+ * @brief    CMSIS compiler specific macros, functions, instructions
+ * @version  V1.0.2
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __CMSIS_ARMCC_H
+#define __CMSIS_ARMCC_H
+
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 400677)
+  #error "Please use Arm Compiler Toolchain V4.0.677 or later!"
+#endif
+
+/* CMSIS compiler control architecture macros */
+#if (defined (__TARGET_ARCH_7_A ) && (__TARGET_ARCH_7_A  == 1))
+  #define __ARM_ARCH_7A__           1
+#endif
+
+/* CMSIS compiler specific defines */
+#ifndef   __ASM
+  #define __ASM                                  __asm
+#endif
+#ifndef   __INLINE
+  #define __INLINE                               __inline
+#endif
+#ifndef   __FORCEINLINE
+  #define __FORCEINLINE                          __forceinline
+#endif
+#ifndef   __STATIC_INLINE
+  #define __STATIC_INLINE                        static __inline
+#endif
+#ifndef   __STATIC_FORCEINLINE
+  #define __STATIC_FORCEINLINE                   static __forceinline
+#endif
+#ifndef   __NO_RETURN
+  #define __NO_RETURN                            __declspec(noreturn)
+#endif
+#ifndef   CMSIS_DEPRECATED
+  #define CMSIS_DEPRECATED                       __attribute__((deprecated))
+#endif
+#ifndef   __USED
+  #define __USED                                 __attribute__((used))
+#endif
+#ifndef   __WEAK
+  #define __WEAK                                 __attribute__((weak))
+#endif
+#ifndef   __PACKED
+  #define __PACKED                               __attribute__((packed))
+#endif
+#ifndef   __PACKED_STRUCT
+  #define __PACKED_STRUCT                        __packed struct
+#endif
+#ifndef   __UNALIGNED_UINT16_WRITE
+  #define __UNALIGNED_UINT16_WRITE(addr, val)    ((*((__packed uint16_t *)(addr))) = (val))
+#endif
+#ifndef   __UNALIGNED_UINT16_READ
+  #define __UNALIGNED_UINT16_READ(addr)          (*((const __packed uint16_t *)(addr)))
+#endif
+#ifndef   __UNALIGNED_UINT32_WRITE
+  #define __UNALIGNED_UINT32_WRITE(addr, val)    ((*((__packed uint32_t *)(addr))) = (val))
+#endif
+#ifndef   __UNALIGNED_UINT32_READ
+  #define __UNALIGNED_UINT32_READ(addr)          (*((const __packed uint32_t *)(addr)))
+#endif
+#ifndef   __ALIGNED
+  #define __ALIGNED(x)                           __attribute__((aligned(x)))
+#endif
+#ifndef   __PACKED
+  #define __PACKED                               __attribute__((packed))
+#endif
+
+/* ##########################  Core Instruction Access  ######################### */
+/**
+  \brief   No Operation
+ */
+#define __NOP                             __nop
+
+/**
+  \brief   Wait For Interrupt
+ */
+#define __WFI                             __wfi
+
+/**
+  \brief   Wait For Event
+ */
+#define __WFE                             __wfe
+
+/**
+  \brief   Send Event
+ */
+#define __SEV                             __sev
+
+/**
+  \brief   Instruction Synchronization Barrier
+ */
+#define __ISB() do {\
+                   __schedule_barrier();\
+                   __isb(0xF);\
+                   __schedule_barrier();\
+                } while (0U)
+
+/**
+  \brief   Data Synchronization Barrier
+ */
+#define __DSB() do {\
+                   __schedule_barrier();\
+                   __dsb(0xF);\
+                   __schedule_barrier();\
+                } while (0U)
+
+/**
+  \brief   Data Memory Barrier
+ */
+#define __DMB() do {\
+                   __schedule_barrier();\
+                   __dmb(0xF);\
+                   __schedule_barrier();\
+                } while (0U)
+
+/**
+  \brief   Reverse byte order (32 bit)
+  \details Reverses the byte order in unsigned integer value. For example, 0x12345678 becomes 0x78563412.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#define __REV                             __rev
+
+/**
+  \brief   Reverse byte order (16 bit)
+  \details Reverses the byte order within each halfword of a word. For example, 0x12345678 becomes 0x34127856.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)
+{
+  rev16 r0, r0
+  bx lr
+}
+#endif
+
+/**
+  \brief   Reverse byte order (16 bit)
+  \details Reverses the byte order in a 16-bit value and returns the signed 16-bit result. For example, 0x0080 becomes 0x8000.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int16_t __REVSH(int16_t value)
+{
+  revsh r0, r0
+  bx lr
+}
+#endif
+
+/**
+  \brief   Rotate Right in unsigned value (32 bit)
+  \param [in]    op1  Value to rotate
+  \param [in]    op2  Number of Bits to rotate
+  \return               Rotated value
+ */
+#define __ROR                             __ror
+
+/**
+  \brief   Breakpoint
+  \param [in]    value  is ignored by the processor.
+                 If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value)                     __breakpoint(value)
+
+/**
+  \brief   Reverse bit order of value
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#define __RBIT                            __rbit
+
+/**
+  \brief   Count leading zeros
+  \param [in]  value  Value to count the leading zeros
+  \return             number of leading zeros in value
+ */
+#define __CLZ                             __clz
+
+/**
+  \brief   LDR Exclusive (8 bit)
+  \details Executes a exclusive LDR instruction for 8 bit value.
+  \param [in]    ptr  Pointer to data
+  \return             value of type uint8_t at (*ptr)
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __LDREXB(ptr)                                                        ((uint8_t ) __ldrex(ptr))
+#else
+  #define __LDREXB(ptr)          _Pragma("push") _Pragma("diag_suppress 3731") ((uint8_t ) __ldrex(ptr))  _Pragma("pop")
+#endif
+
+/**
+  \brief   LDR Exclusive (16 bit)
+  \details Executes a exclusive LDR instruction for 16 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint16_t at (*ptr)
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __LDREXH(ptr)                                                        ((uint16_t) __ldrex(ptr))
+#else
+  #define __LDREXH(ptr)          _Pragma("push") _Pragma("diag_suppress 3731") ((uint16_t) __ldrex(ptr))  _Pragma("pop")
+#endif
+
+/**
+  \brief   LDR Exclusive (32 bit)
+  \details Executes a exclusive LDR instruction for 32 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint32_t at (*ptr)
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __LDREXW(ptr)                                                        ((uint32_t ) __ldrex(ptr))
+#else
+  #define __LDREXW(ptr)          _Pragma("push") _Pragma("diag_suppress 3731") ((uint32_t ) __ldrex(ptr))  _Pragma("pop")
+#endif
+
+/**
+  \brief   STR Exclusive (8 bit)
+  \details Executes a exclusive STR instruction for 8 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __STREXB(value, ptr)                                                 __strex(value, ptr)
+#else
+  #define __STREXB(value, ptr)   _Pragma("push") _Pragma("diag_suppress 3731") __strex(value, ptr)        _Pragma("pop")
+#endif
+
+/**
+  \brief   STR Exclusive (16 bit)
+  \details Executes a exclusive STR instruction for 16 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __STREXH(value, ptr)                                                 __strex(value, ptr)
+#else
+  #define __STREXH(value, ptr)   _Pragma("push") _Pragma("diag_suppress 3731") __strex(value, ptr)        _Pragma("pop")
+#endif
+
+/**
+  \brief   STR Exclusive (32 bit)
+  \details Executes a exclusive STR instruction for 32 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+#if defined(__ARMCC_VERSION) && (__ARMCC_VERSION < 5060020)
+  #define __STREXW(value, ptr)                                                 __strex(value, ptr)
+#else
+  #define __STREXW(value, ptr)   _Pragma("push") _Pragma("diag_suppress 3731") __strex(value, ptr)        _Pragma("pop")
+#endif
+
+/**
+  \brief   Remove the exclusive lock
+  \details Removes the exclusive lock which is created by LDREX.
+ */
+#define __CLREX                           __clrex
+
+
+/**
+  \brief   Signed Saturate
+  \details Saturates a signed value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (1..32)
+  \return             Saturated value
+ */
+#define __SSAT                            __ssat
+
+/**
+  \brief   Unsigned Saturate
+  \details Saturates an unsigned value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (0..31)
+  \return             Saturated value
+ */
+#define __USAT                            __usat
+
+/* ###########################  Core Function Access  ########################### */
+
+/**
+  \brief   Get FPSCR (Floating Point Status/Control)
+  \return               Floating Point Status/Control register value
+ */
+__STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+     (defined (__FPU_USED   ) && (__FPU_USED    == 1U))     )
+  register uint32_t __regfpscr         __ASM("fpscr");
+  return(__regfpscr);
+#else
+   return(0U);
+#endif
+}
+
+/**
+  \brief   Set FPSCR (Floating Point Status/Control)
+  \param [in]    fpscr  Floating Point Status/Control value to set
+ */
+__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+     (defined (__FPU_USED   ) && (__FPU_USED    == 1U))     )
+  register uint32_t __regfpscr         __ASM("fpscr");
+  __regfpscr = (fpscr);
+#else
+  (void)fpscr;
+#endif
+}
+
+/** \brief  Get CPSR (Current Program Status Register)
+    \return               CPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_CPSR(void)
+{
+  register uint32_t __regCPSR          __ASM("cpsr");
+  return(__regCPSR);
+}
+
+
+/** \brief  Set CPSR (Current Program Status Register)
+    \param [in]    cpsr  CPSR value to set
+ */
+__STATIC_INLINE void __set_CPSR(uint32_t cpsr)
+{
+  register uint32_t __regCPSR          __ASM("cpsr");
+  __regCPSR = cpsr;
+}
+
+/** \brief  Get Mode
+    \return                Processor Mode
+ */
+__STATIC_INLINE uint32_t __get_mode(void)
+{
+  return (__get_CPSR() & 0x1FU);
+}
+
+/** \brief  Set Mode
+    \param [in]    mode  Mode value to set
+ */
+__STATIC_INLINE __ASM void __set_mode(uint32_t mode)
+{
+  MOV  r1, lr
+  MSR  CPSR_C, r0
+  BX   r1
+}
+
+/** \brief  Get Stack Pointer
+    \return Stack Pointer
+ */
+__STATIC_INLINE __ASM uint32_t __get_SP(void)
+{
+  MOV  r0, sp
+  BX   lr
+}
+
+/** \brief  Set Stack Pointer
+    \param [in]    stack  Stack Pointer value to set
+ */
+__STATIC_INLINE __ASM void __set_SP(uint32_t stack)
+{
+  MOV  sp, r0
+  BX   lr
+}
+
+
+/** \brief  Get USR/SYS Stack Pointer
+    \return USR/SYSStack Pointer
+ */
+__STATIC_INLINE __ASM uint32_t __get_SP_usr(void)
+{
+  ARM
+  PRESERVE8
+
+  MRS     R1, CPSR
+  CPS     #0x1F       ;no effect in USR mode
+  MOV     R0, SP
+  MSR     CPSR_c, R1  ;no effect in USR mode
+  ISB
+  BX      LR
+}
+
+/** \brief  Set USR/SYS Stack Pointer
+    \param [in]    topOfProcStack  USR/SYS Stack Pointer value to set
+ */
+__STATIC_INLINE __ASM void __set_SP_usr(uint32_t topOfProcStack)
+{
+  ARM
+  PRESERVE8
+
+  MRS     R1, CPSR
+  CPS     #0x1F       ;no effect in USR mode
+  MOV     SP, R0
+  MSR     CPSR_c, R1  ;no effect in USR mode
+  ISB
+  BX      LR
+}
+
+/** \brief  Get FPEXC (Floating Point Exception Control Register)
+    \return               Floating Point Exception Control Register value
+ */
+__STATIC_INLINE uint32_t __get_FPEXC(void)
+{
+#if (__FPU_PRESENT == 1)
+  register uint32_t __regfpexc         __ASM("fpexc");
+  return(__regfpexc);
+#else
+  return(0);
+#endif
+}
+
+/** \brief  Set FPEXC (Floating Point Exception Control Register)
+    \param [in]    fpexc  Floating Point Exception Control value to set
+ */
+__STATIC_INLINE void __set_FPEXC(uint32_t fpexc)
+{
+#if (__FPU_PRESENT == 1)
+  register uint32_t __regfpexc         __ASM("fpexc");
+  __regfpexc = (fpexc);
+#endif
+}
+
+/*
+ * Include common core functions to access Coprocessor 15 registers
+ */
+
+#define __get_CP(cp, op1, Rt, CRn, CRm, op2) do { register volatile uint32_t tmp __ASM("cp" # cp ":" # op1 ":c" # CRn ":c" # CRm ":" # op2); (Rt) = tmp; } while(0)
+#define __set_CP(cp, op1, Rt, CRn, CRm, op2) do { register volatile uint32_t tmp __ASM("cp" # cp ":" # op1 ":c" # CRn ":c" # CRm ":" # op2); tmp = (Rt); } while(0)
+#define __get_CP64(cp, op1, Rt, CRm) \
+  do { \
+    uint32_t ltmp, htmp; \
+    __ASM volatile("MRRC p" # cp ", " # op1 ", ltmp, htmp, c" # CRm); \
+    (Rt) = ((((uint64_t)htmp) << 32U) | ((uint64_t)ltmp)); \
+  } while(0)
+
+#define __set_CP64(cp, op1, Rt, CRm) \
+  do { \
+    const uint64_t tmp = (Rt); \
+    const uint32_t ltmp = (uint32_t)(tmp); \
+    const uint32_t htmp = (uint32_t)(tmp >> 32U); \
+    __ASM volatile("MCRR p" # cp ", " # op1 ", ltmp, htmp, c" # CRm); \
+  } while(0)
+
+#include "cmsis_cp15.h"
+
+/** \brief  Enable Floating Point Unit
+
+  Critical section, called from undef handler, so systick is disabled
+ */
+__STATIC_INLINE __ASM void __FPU_Enable(void)
+{
+        ARM
+
+        //Permit access to VFP/NEON, registers by modifying CPACR
+        MRC     p15,0,R1,c1,c0,2
+        ORR     R1,R1,#0x00F00000
+        MCR     p15,0,R1,c1,c0,2
+
+        //Ensure that subsequent instructions occur in the context of VFP/NEON access permitted
+        ISB
+
+        //Enable VFP/NEON
+        VMRS    R1,FPEXC
+        ORR     R1,R1,#0x40000000
+        VMSR    FPEXC,R1
+
+        //Initialise VFP/NEON registers to 0
+        MOV     R2,#0
+
+        //Initialise D16 registers to 0
+        VMOV    D0, R2,R2
+        VMOV    D1, R2,R2
+        VMOV    D2, R2,R2
+        VMOV    D3, R2,R2
+        VMOV    D4, R2,R2
+        VMOV    D5, R2,R2
+        VMOV    D6, R2,R2
+        VMOV    D7, R2,R2
+        VMOV    D8, R2,R2
+        VMOV    D9, R2,R2
+        VMOV    D10,R2,R2
+        VMOV    D11,R2,R2
+        VMOV    D12,R2,R2
+        VMOV    D13,R2,R2
+        VMOV    D14,R2,R2
+        VMOV    D15,R2,R2
+
+  IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
+        //Initialise D32 registers to 0
+        VMOV    D16,R2,R2
+        VMOV    D17,R2,R2
+        VMOV    D18,R2,R2
+        VMOV    D19,R2,R2
+        VMOV    D20,R2,R2
+        VMOV    D21,R2,R2
+        VMOV    D22,R2,R2
+        VMOV    D23,R2,R2
+        VMOV    D24,R2,R2
+        VMOV    D25,R2,R2
+        VMOV    D26,R2,R2
+        VMOV    D27,R2,R2
+        VMOV    D28,R2,R2
+        VMOV    D29,R2,R2
+        VMOV    D30,R2,R2
+        VMOV    D31,R2,R2
+  ENDIF
+
+        //Initialise FPSCR to a known state
+        VMRS    R2,FPSCR
+        LDR     R3,=0x00086060 //Mask off all bits that do not have to be preserved. Non-preserved bits can/should be zero.
+        AND     R2,R2,R3
+        VMSR    FPSCR,R2
+
+        BX      LR
+}
+
+#endif /* __CMSIS_ARMCC_H */

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core_A/Include/cmsis_armclang.h

@@ -0,0 +1,503 @@
+/**************************************************************************//**
+ * @file     cmsis_armclang.h
+ * @brief    CMSIS compiler specific macros, functions, instructions
+ * @version  V1.0.2
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __CMSIS_ARMCLANG_H
+#define __CMSIS_ARMCLANG_H
+
+#pragma clang system_header   /* treat file as system include file */
+
+#ifndef __ARM_COMPAT_H
+#include <arm_compat.h>    /* Compatibility header for Arm Compiler 5 intrinsics */
+#endif
+
+/* CMSIS compiler specific defines */
+#ifndef   __ASM
+  #define __ASM                                  __asm
+#endif
+#ifndef   __INLINE
+  #define __INLINE                               __inline
+#endif
+#ifndef   __FORCEINLINE
+  #define __FORCEINLINE                          __attribute__((always_inline))
+#endif
+#ifndef   __STATIC_INLINE
+  #define __STATIC_INLINE                        static __inline
+#endif
+#ifndef   __STATIC_FORCEINLINE
+  #define __STATIC_FORCEINLINE                   __attribute__((always_inline)) static __inline
+#endif
+#ifndef   __NO_RETURN
+  #define __NO_RETURN                            __attribute__((__noreturn__))
+#endif
+#ifndef   CMSIS_DEPRECATED
+  #define CMSIS_DEPRECATED                       __attribute__((deprecated))
+#endif
+#ifndef   __USED
+  #define __USED                                 __attribute__((used))
+#endif
+#ifndef   __WEAK
+  #define __WEAK                                 __attribute__((weak))
+#endif
+#ifndef   __PACKED
+  #define __PACKED                               __attribute__((packed, aligned(1)))
+#endif
+#ifndef   __PACKED_STRUCT
+  #define __PACKED_STRUCT                        struct __attribute__((packed, aligned(1)))
+#endif
+#ifndef   __UNALIGNED_UINT16_WRITE
+  #pragma clang diagnostic push
+  #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT16_WRITE)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT16_WRITE */
+  __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+  #pragma clang diagnostic pop
+  #define __UNALIGNED_UINT16_WRITE(addr, val)    (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef   __UNALIGNED_UINT16_READ
+  #pragma clang diagnostic push
+  #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT16_READ)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT16_READ */
+  __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+  #pragma clang diagnostic pop
+  #define __UNALIGNED_UINT16_READ(addr)          (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+#endif
+#ifndef   __UNALIGNED_UINT32_WRITE
+  #pragma clang diagnostic push
+  #pragma clang diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT32_WRITE)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT32_WRITE */
+  __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+  #pragma clang diagnostic pop
+  #define __UNALIGNED_UINT32_WRITE(addr, val)    (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef   __UNALIGNED_UINT32_READ
+  #pragma clang diagnostic push
+  #pragma clang diagnostic ignored "-Wpacked"
+  __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+  #pragma clang diagnostic pop
+  #define __UNALIGNED_UINT32_READ(addr)          (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+#endif
+#ifndef   __ALIGNED
+  #define __ALIGNED(x)                           __attribute__((aligned(x)))
+#endif
+#ifndef   __PACKED
+  #define __PACKED                               __attribute__((packed))
+#endif
+
+/* ##########################  Core Instruction Access  ######################### */
+/**
+  \brief   No Operation
+ */
+#define __NOP                             __builtin_arm_nop
+
+/**
+  \brief   Wait For Interrupt
+ */
+#define __WFI                             __builtin_arm_wfi
+
+/**
+  \brief   Wait For Event
+ */
+#define __WFE                             __builtin_arm_wfe
+
+/**
+  \brief   Send Event
+ */
+#define __SEV                             __builtin_arm_sev
+
+/**
+  \brief   Instruction Synchronization Barrier
+ */
+#define __ISB() do {\
+                   __schedule_barrier();\
+                   __builtin_arm_isb(0xF);\
+                   __schedule_barrier();\
+                } while (0U)
+
+/**
+  \brief   Data Synchronization Barrier
+ */
+#define __DSB() do {\
+                   __schedule_barrier();\
+                   __builtin_arm_dsb(0xF);\
+                   __schedule_barrier();\
+                } while (0U)
+
+/**
+  \brief   Data Memory Barrier
+ */
+#define __DMB() do {\
+                   __schedule_barrier();\
+                   __builtin_arm_dmb(0xF);\
+                   __schedule_barrier();\
+                } while (0U)
+
+/**
+  \brief   Reverse byte order (32 bit)
+  \details Reverses the byte order in unsigned integer value. For example, 0x12345678 becomes 0x78563412.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#define __REV(value)   __builtin_bswap32(value)
+
+/**
+  \brief   Reverse byte order (16 bit)
+  \details Reverses the byte order within each halfword of a word. For example, 0x12345678 becomes 0x34127856.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#define __REV16(value) __ROR(__REV(value), 16)
+
+
+/**
+  \brief   Reverse byte order (16 bit)
+  \details Reverses the byte order in a 16-bit value and returns the signed 16-bit result. For example, 0x0080 becomes 0x8000.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#define __REVSH(value) (int16_t)__builtin_bswap16(value)
+
+
+/**
+  \brief   Rotate Right in unsigned value (32 bit)
+  \details Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+  \param [in]    op1  Value to rotate
+  \param [in]    op2  Number of Bits to rotate
+  \return               Rotated value
+ */
+__STATIC_FORCEINLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+  op2 %= 32U;
+  if (op2 == 0U)
+  {
+    return op1;
+  }
+  return (op1 >> op2) | (op1 << (32U - op2));
+}
+
+
+/**
+  \brief   Breakpoint
+  \param [in]    value  is ignored by the processor.
+                 If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value)   __ASM volatile ("bkpt "#value)
+
+/**
+  \brief   Reverse bit order of value
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#define __RBIT          __builtin_arm_rbit
+
+/**
+  \brief   Count leading zeros
+  \param [in]  value  Value to count the leading zeros
+  \return             number of leading zeros in value
+ */
+#define __CLZ           (uint8_t)__builtin_clz
+
+/**
+  \brief   LDR Exclusive (8 bit)
+  \details Executes a exclusive LDR instruction for 8 bit value.
+  \param [in]    ptr  Pointer to data
+  \return             value of type uint8_t at (*ptr)
+ */
+#define __LDREXB        (uint8_t)__builtin_arm_ldrex
+
+
+/**
+  \brief   LDR Exclusive (16 bit)
+  \details Executes a exclusive LDR instruction for 16 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint16_t at (*ptr)
+ */
+#define __LDREXH        (uint16_t)__builtin_arm_ldrex
+
+/**
+  \brief   LDR Exclusive (32 bit)
+  \details Executes a exclusive LDR instruction for 32 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint32_t at (*ptr)
+ */
+#define __LDREXW        (uint32_t)__builtin_arm_ldrex
+
+/**
+  \brief   STR Exclusive (8 bit)
+  \details Executes a exclusive STR instruction for 8 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+#define __STREXB        (uint32_t)__builtin_arm_strex
+
+/**
+  \brief   STR Exclusive (16 bit)
+  \details Executes a exclusive STR instruction for 16 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+#define __STREXH        (uint32_t)__builtin_arm_strex
+
+/**
+  \brief   STR Exclusive (32 bit)
+  \details Executes a exclusive STR instruction for 32 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+#define __STREXW        (uint32_t)__builtin_arm_strex
+
+/**
+  \brief   Remove the exclusive lock
+  \details Removes the exclusive lock which is created by LDREX.
+ */
+#define __CLREX             __builtin_arm_clrex
+
+/**
+  \brief   Signed Saturate
+  \details Saturates a signed value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (1..32)
+  \return             Saturated value
+ */
+#define __SSAT             __builtin_arm_ssat
+
+/**
+  \brief   Unsigned Saturate
+  \details Saturates an unsigned value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (0..31)
+  \return             Saturated value
+ */
+#define __USAT             __builtin_arm_usat
+
+
+/* ###########################  Core Function Access  ########################### */
+
+/**
+  \brief   Get FPSCR
+  \details Returns the current value of the Floating Point Status/Control register.
+  \return               Floating Point Status/Control register value
+ */
+#define __get_FPSCR      __builtin_arm_get_fpscr
+
+/**
+  \brief   Set FPSCR
+  \details Assigns the given value to the Floating Point Status/Control register.
+  \param [in]    fpscr  Floating Point Status/Control value to set
+ */
+#define __set_FPSCR      __builtin_arm_set_fpscr
+
+/** \brief  Get CPSR Register
+    \return               CPSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CPSR(void)
+{
+  uint32_t result;
+  __ASM volatile("MRS %0, cpsr" : "=r" (result) );
+  return(result);
+}
+
+/** \brief  Set CPSR Register
+    \param [in]    cpsr  CPSR value to set
+ */
+__STATIC_FORCEINLINE void __set_CPSR(uint32_t cpsr)
+{
+__ASM volatile ("MSR cpsr, %0" : : "r" (cpsr) : "memory");
+}
+
+/** \brief  Get Mode
+    \return                Processor Mode
+ */
+__STATIC_FORCEINLINE uint32_t __get_mode(void)
+{
+	return (__get_CPSR() & 0x1FU);
+}
+
+/** \brief  Set Mode
+    \param [in]    mode  Mode value to set
+ */
+__STATIC_FORCEINLINE void __set_mode(uint32_t mode)
+{
+  __ASM volatile("MSR  cpsr_c, %0" : : "r" (mode) : "memory");
+}
+
+/** \brief  Get Stack Pointer
+    \return Stack Pointer value
+ */
+__STATIC_FORCEINLINE uint32_t __get_SP()
+{
+  uint32_t result;
+  __ASM volatile("MOV  %0, sp" : "=r" (result) : : "memory");
+  return result;
+}
+
+/** \brief  Set Stack Pointer
+    \param [in]    stack  Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_SP(uint32_t stack)
+{
+  __ASM volatile("MOV  sp, %0" : : "r" (stack) : "memory");
+}
+
+/** \brief  Get USR/SYS Stack Pointer
+    \return USR/SYS Stack Pointer value
+ */
+__STATIC_FORCEINLINE uint32_t __get_SP_usr()
+{
+  uint32_t cpsr;
+  uint32_t result;
+  __ASM volatile(
+    "MRS     %0, cpsr   \n"
+    "CPS     #0x1F      \n" // no effect in USR mode
+    "MOV     %1, sp     \n"
+    "MSR     cpsr_c, %2 \n" // no effect in USR mode
+    "ISB" :  "=r"(cpsr), "=r"(result) : "r"(cpsr) : "memory"
+   );
+  return result;
+}
+
+/** \brief  Set USR/SYS Stack Pointer
+    \param [in]    topOfProcStack  USR/SYS Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_SP_usr(uint32_t topOfProcStack)
+{
+  uint32_t cpsr;
+  __ASM volatile(
+    "MRS     %0, cpsr   \n"
+    "CPS     #0x1F      \n" // no effect in USR mode
+    "MOV     sp, %1     \n"
+    "MSR     cpsr_c, %2 \n" // no effect in USR mode
+    "ISB" : "=r"(cpsr) : "r" (topOfProcStack), "r"(cpsr) : "memory"
+   );
+}
+
+/** \brief  Get FPEXC
+    \return               Floating Point Exception Control register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_FPEXC(void)
+{
+#if (__FPU_PRESENT == 1)
+  uint32_t result;
+  __ASM volatile("VMRS %0, fpexc" : "=r" (result) : : "memory");
+  return(result);
+#else
+  return(0);
+#endif
+}
+
+/** \brief  Set FPEXC
+    \param [in]    fpexc  Floating Point Exception Control value to set
+ */
+__STATIC_FORCEINLINE void __set_FPEXC(uint32_t fpexc)
+{
+#if (__FPU_PRESENT == 1)
+  __ASM volatile ("VMSR fpexc, %0" : : "r" (fpexc) : "memory");
+#endif
+}
+
+/*
+ * Include common core functions to access Coprocessor 15 registers
+ */
+
+#define __get_CP(cp, op1, Rt, CRn, CRm, op2) __ASM volatile("MRC p" # cp ", " # op1 ", %0, c" # CRn ", c" # CRm ", " # op2 : "=r" (Rt) : : "memory" )
+#define __set_CP(cp, op1, Rt, CRn, CRm, op2) __ASM volatile("MCR p" # cp ", " # op1 ", %0, c" # CRn ", c" # CRm ", " # op2 : : "r" (Rt) : "memory" )
+#define __get_CP64(cp, op1, Rt, CRm)         __ASM volatile("MRRC p" # cp ", " # op1 ", %Q0, %R0, c" # CRm  : "=r" (Rt) : : "memory" )
+#define __set_CP64(cp, op1, Rt, CRm)         __ASM volatile("MCRR p" # cp ", " # op1 ", %Q0, %R0, c" # CRm  : : "r" (Rt) : "memory" )
+
+#include "cmsis_cp15.h"
+
+/** \brief  Enable Floating Point Unit
+
+  Critical section, called from undef handler, so systick is disabled
+ */
+__STATIC_INLINE void __FPU_Enable(void)
+{
+  __ASM volatile(
+    //Permit access to VFP/NEON, registers by modifying CPACR
+    "        MRC     p15,0,R1,c1,c0,2  \n"
+    "        ORR     R1,R1,#0x00F00000 \n"
+    "        MCR     p15,0,R1,c1,c0,2  \n"
+
+    //Ensure that subsequent instructions occur in the context of VFP/NEON access permitted
+    "        ISB                       \n"
+
+    //Enable VFP/NEON
+    "        VMRS    R1,FPEXC          \n"
+    "        ORR     R1,R1,#0x40000000 \n"
+    "        VMSR    FPEXC,R1          \n"
+
+    //Initialise VFP/NEON registers to 0
+    "        MOV     R2,#0             \n"
+
+    //Initialise D16 registers to 0
+    "        VMOV    D0, R2,R2         \n"
+    "        VMOV    D1, R2,R2         \n"
+    "        VMOV    D2, R2,R2         \n"
+    "        VMOV    D3, R2,R2         \n"
+    "        VMOV    D4, R2,R2         \n"
+    "        VMOV    D5, R2,R2         \n"
+    "        VMOV    D6, R2,R2         \n"
+    "        VMOV    D7, R2,R2         \n"
+    "        VMOV    D8, R2,R2         \n"
+    "        VMOV    D9, R2,R2         \n"
+    "        VMOV    D10,R2,R2         \n"
+    "        VMOV    D11,R2,R2         \n"
+    "        VMOV    D12,R2,R2         \n"
+    "        VMOV    D13,R2,R2         \n"
+    "        VMOV    D14,R2,R2         \n"
+    "        VMOV    D15,R2,R2         \n"
+
+#if __ARM_NEON == 1
+    //Initialise D32 registers to 0
+    "        VMOV    D16,R2,R2         \n"
+    "        VMOV    D17,R2,R2         \n"
+    "        VMOV    D18,R2,R2         \n"
+    "        VMOV    D19,R2,R2         \n"
+    "        VMOV    D20,R2,R2         \n"
+    "        VMOV    D21,R2,R2         \n"
+    "        VMOV    D22,R2,R2         \n"
+    "        VMOV    D23,R2,R2         \n"
+    "        VMOV    D24,R2,R2         \n"
+    "        VMOV    D25,R2,R2         \n"
+    "        VMOV    D26,R2,R2         \n"
+    "        VMOV    D27,R2,R2         \n"
+    "        VMOV    D28,R2,R2         \n"
+    "        VMOV    D29,R2,R2         \n"
+    "        VMOV    D30,R2,R2         \n"
+    "        VMOV    D31,R2,R2         \n"
+#endif
+
+    //Initialise FPSCR to a known state
+    "        VMRS    R2,FPSCR          \n"
+    "        LDR     R3,=0x00086060    \n" //Mask off all bits that do not have to be preserved. Non-preserved bits can/should be zero.
+    "        AND     R2,R2,R3          \n"
+    "        VMSR    FPSCR,R2            "
+  );
+}
+
+#endif /* __CMSIS_ARMCLANG_H */

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core_A/Include/cmsis_compiler.h

@@ -0,0 +1,201 @@
+/**************************************************************************//**
+ * @file     cmsis_compiler.h
+ * @brief    CMSIS compiler specific macros, functions, instructions
+ * @version  V1.0.2
+ * @date     10. January 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __CMSIS_COMPILER_H
+#define __CMSIS_COMPILER_H
+
+#include <stdint.h>
+
+/*
+ * Arm Compiler 4/5
+ */
+#if   defined ( __CC_ARM )
+  #include "cmsis_armcc.h"
+
+
+/*
+ * Arm Compiler 6 (armclang)
+ */
+#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050)
+  #include "cmsis_armclang.h"
+
+
+/*
+ * GNU Compiler
+ */
+#elif defined ( __GNUC__ )
+  #include "cmsis_gcc.h"
+
+
+/*
+ * IAR Compiler
+ */
+#elif defined ( __ICCARM__ )
+  #include "cmsis_iccarm.h"
+
+
+/*
+ * TI Arm Compiler
+ */
+#elif defined ( __TI_ARM__ )
+  #include <cmsis_ccs.h>
+
+  #ifndef   __ASM
+    #define __ASM                     __asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                  inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE           static inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE           static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE      __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    #define __NO_RETURN               __attribute__((noreturn))
+  #endif
+  #ifndef   CMSIS_DEPRECATED
+    #define CMSIS_DEPRECATED          __attribute__((deprecated))
+  #endif
+  #ifndef   __USED
+    #define __USED                    __attribute__((used))
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                    __attribute__((weak))
+  #endif
+  #ifndef   __UNALIGNED_UINT32
+    struct __attribute__((packed)) T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)     (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #define __ALIGNED(x)              __attribute__((aligned(x)))
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                  __attribute__((packed))
+  #endif
+
+
+/*
+ * TASKING Compiler
+ */
+#elif defined ( __TASKING__ )
+  /*
+   * The CMSIS functions have been implemented as intrinsics in the compiler.
+   * Please use "carm -?i" to get an up to date list of all intrinsics,
+   * Including the CMSIS ones.
+   */
+
+  #ifndef   __ASM
+    #define __ASM                     __asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                  inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE           static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE      __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    #define __NO_RETURN               __attribute__((noreturn))
+  #endif
+  #ifndef   CMSIS_DEPRECATED
+    #define CMSIS_DEPRECATED          __attribute__((deprecated))
+  #endif
+  #ifndef   __USED
+    #define __USED                    __attribute__((used))
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                    __attribute__((weak))
+  #endif
+  #ifndef   __UNALIGNED_UINT32
+    struct __packed__ T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)     (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #define __ALIGNED(x)              __align(x)
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                  __packed__
+  #endif
+
+
+/*
+ * COSMIC Compiler
+ */
+#elif defined ( __CSMC__ )
+   #include <cmsis_csm.h>
+
+ #ifndef   __ASM
+    #define __ASM                     _asm
+  #endif
+  #ifndef   __INLINE
+    #define __INLINE                  inline
+  #endif
+  #ifndef   __STATIC_INLINE
+    #define __STATIC_INLINE           static inline
+  #endif
+  #ifndef   __STATIC_FORCEINLINE
+    #define __STATIC_FORCEINLINE      __STATIC_INLINE
+  #endif
+  #ifndef   __NO_RETURN
+    // NO RETURN is automatically detected hence no warning here
+    #define __NO_RETURN
+  #endif
+  #ifndef   __USED
+    #warning No compiler specific solution for __USED. __USED is ignored.
+    #define __USED
+  #endif
+  #ifndef   CMSIS_DEPRECATED
+    #warning No compiler specific solution for CMSIS_DEPRECATED. CMSIS_DEPRECATED is ignored.
+    #define CMSIS_DEPRECATED
+  #endif
+  #ifndef   __WEAK
+    #define __WEAK                    __weak
+  #endif
+  #ifndef   __UNALIGNED_UINT32
+    @packed struct T_UINT32 { uint32_t v; };
+    #define __UNALIGNED_UINT32(x)     (((struct T_UINT32 *)(x))->v)
+  #endif
+  #ifndef   __ALIGNED
+    #warning No compiler specific solution for __ALIGNED. __ALIGNED is ignored.
+    #define __ALIGNED(x)
+  #endif
+  #ifndef   __PACKED
+    #define __PACKED                  @packed
+  #endif
+
+
+#else
+  #error Unknown compiler.
+#endif
+
+
+#endif /* __CMSIS_COMPILER_H */
+

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core_A/Include/cmsis_cp15.h

@@ -0,0 +1,514 @@
+/**************************************************************************//**
+ * @file     cmsis_cp15.h
+ * @brief    CMSIS compiler specific macros, functions, instructions
+ * @version  V1.0.1
+ * @date     07. Sep 2017
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2017 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if   defined ( __ICCARM__ )
+  #pragma system_include         /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+  #pragma clang system_header   /* treat file as system include file */
+#endif
+
+#ifndef __CMSIS_CP15_H
+#define __CMSIS_CP15_H
+
+/** \brief  Get ACTLR
+    \return               Auxiliary Control register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_ACTLR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 1, 0, 1);
+  return(result);
+}
+
+/** \brief  Set ACTLR
+    \param [in]    actlr  Auxiliary Control value to set
+ */
+__STATIC_FORCEINLINE void __set_ACTLR(uint32_t actlr)
+{
+  __set_CP(15, 0, actlr, 1, 0, 1);
+}
+
+/** \brief  Get CPACR
+    \return               Coprocessor Access Control register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CPACR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 1, 0, 2);
+  return result;
+}
+
+/** \brief  Set CPACR
+    \param [in]    cpacr  Coprocessor Access Control value to set
+ */
+__STATIC_FORCEINLINE void __set_CPACR(uint32_t cpacr)
+{
+  __set_CP(15, 0, cpacr, 1, 0, 2);
+}
+
+/** \brief  Get DFSR
+    \return               Data Fault Status Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_DFSR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 5, 0, 0);
+  return result;
+}
+
+/** \brief  Set DFSR
+    \param [in]    dfsr  Data Fault Status value to set
+ */
+__STATIC_FORCEINLINE void __set_DFSR(uint32_t dfsr)
+{
+  __set_CP(15, 0, dfsr, 5, 0, 0);
+}
+
+/** \brief  Get IFSR
+    \return               Instruction Fault Status Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_IFSR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 5, 0, 1);
+  return result;
+}
+
+/** \brief  Set IFSR
+    \param [in]    ifsr  Instruction Fault Status value to set
+ */
+__STATIC_FORCEINLINE void __set_IFSR(uint32_t ifsr)
+{
+  __set_CP(15, 0, ifsr, 5, 0, 1);
+}
+
+/** \brief  Get ISR
+    \return               Interrupt Status Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_ISR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 12, 1, 0);
+  return result;
+}
+
+/** \brief  Get CBAR
+    \return               Configuration Base Address register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CBAR(void)
+{
+  uint32_t result;
+  __get_CP(15, 4, result, 15, 0, 0);
+  return result;
+}
+
+/** \brief  Get TTBR0
+
+    This function returns the value of the Translation Table Base Register 0.
+
+    \return               Translation Table Base Register 0 value
+ */
+__STATIC_FORCEINLINE uint32_t __get_TTBR0(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 2, 0, 0);
+  return result;
+}
+
+/** \brief  Set TTBR0
+
+    This function assigns the given value to the Translation Table Base Register 0.
+
+    \param [in]    ttbr0  Translation Table Base Register 0 value to set
+ */
+__STATIC_FORCEINLINE void __set_TTBR0(uint32_t ttbr0)
+{
+  __set_CP(15, 0, ttbr0, 2, 0, 0);
+}
+
+/** \brief  Get DACR
+
+    This function returns the value of the Domain Access Control Register.
+
+    \return               Domain Access Control Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_DACR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 3, 0, 0);
+  return result;
+}
+
+/** \brief  Set DACR
+
+    This function assigns the given value to the Domain Access Control Register.
+
+    \param [in]    dacr   Domain Access Control Register value to set
+ */
+__STATIC_FORCEINLINE void __set_DACR(uint32_t dacr)
+{
+  __set_CP(15, 0, dacr, 3, 0, 0);
+}
+
+/** \brief  Set SCTLR
+
+    This function assigns the given value to the System Control Register.
+
+    \param [in]    sctlr  System Control Register value to set
+ */
+__STATIC_FORCEINLINE void __set_SCTLR(uint32_t sctlr)
+{
+  __set_CP(15, 0, sctlr, 1, 0, 0);
+}
+
+/** \brief  Get SCTLR
+    \return               System Control Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_SCTLR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 1, 0, 0);
+  return result;
+}
+
+/** \brief  Set ACTRL
+    \param [in]    actrl  Auxiliary Control Register value to set
+ */
+__STATIC_FORCEINLINE void __set_ACTRL(uint32_t actrl)
+{
+  __set_CP(15, 0, actrl, 1, 0, 1);
+}
+
+/** \brief  Get ACTRL
+    \return               Auxiliary Control Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_ACTRL(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 1, 0, 1);
+  return result;
+}
+
+/** \brief  Get MPIDR
+
+    This function returns the value of the Multiprocessor Affinity Register.
+
+    \return               Multiprocessor Affinity Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_MPIDR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 0, 0, 5);
+  return result;
+}
+
+/** \brief  Get VBAR
+
+    This function returns the value of the Vector Base Address Register.
+
+    \return               Vector Base Address Register
+ */
+__STATIC_FORCEINLINE uint32_t __get_VBAR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 12, 0, 0);
+  return result;
+}
+
+/** \brief  Set VBAR
+
+    This function assigns the given value to the Vector Base Address Register.
+
+    \param [in]    vbar  Vector Base Address Register value to set
+ */
+__STATIC_FORCEINLINE void __set_VBAR(uint32_t vbar)
+{
+  __set_CP(15, 0, vbar, 12, 0, 0);
+}
+
+/** \brief  Get MVBAR
+
+    This function returns the value of the Monitor Vector Base Address Register.
+
+    \return               Monitor Vector Base Address Register
+ */
+__STATIC_FORCEINLINE uint32_t __get_MVBAR(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 12, 0, 1);
+  return result;
+}
+
+/** \brief  Set MVBAR
+
+    This function assigns the given value to the Monitor Vector Base Address Register.
+
+    \param [in]    mvbar  Monitor Vector Base Address Register value to set
+ */
+__STATIC_FORCEINLINE void __set_MVBAR(uint32_t mvbar)
+{
+  __set_CP(15, 0, mvbar, 12, 0, 1);
+}
+
+#if (defined(__CORTEX_A) && (__CORTEX_A == 7U) && \
+    defined(__TIM_PRESENT) && (__TIM_PRESENT == 1U)) || \
+    defined(DOXYGEN)
+
+/** \brief  Set CNTFRQ
+
+  This function assigns the given value to PL1 Physical Timer Counter Frequency Register (CNTFRQ).
+
+  \param [in]    value  CNTFRQ Register value to set
+*/
+__STATIC_FORCEINLINE void __set_CNTFRQ(uint32_t value)
+{
+  __set_CP(15, 0, value, 14, 0, 0);
+}
+
+/** \brief  Get CNTFRQ
+
+    This function returns the value of the PL1 Physical Timer Counter Frequency Register (CNTFRQ).
+
+    \return               CNTFRQ Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CNTFRQ(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 14, 0 , 0);
+  return result;
+}
+
+/** \brief  Set CNTP_TVAL
+
+  This function assigns the given value to PL1 Physical Timer Value Register (CNTP_TVAL).
+
+  \param [in]    value  CNTP_TVAL Register value to set
+*/
+__STATIC_FORCEINLINE void __set_CNTP_TVAL(uint32_t value)
+{
+  __set_CP(15, 0, value, 14, 2, 0);
+}
+
+/** \brief  Get CNTP_TVAL
+
+    This function returns the value of the PL1 Physical Timer Value Register (CNTP_TVAL).
+
+    \return               CNTP_TVAL Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CNTP_TVAL(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 14, 2, 0);
+  return result;
+}
+
+/** \brief  Get CNTPCT
+
+    This function returns the value of the 64 bits PL1 Physical Count Register (CNTPCT).
+
+    \return               CNTPCT Register value
+ */
+__STATIC_FORCEINLINE uint64_t __get_CNTPCT(void)
+{
+  uint64_t result;
+  __get_CP64(15, 0, result, 14);
+  return result;
+}
+
+/** \brief  Set CNTP_CVAL
+
+  This function assigns the given value to 64bits PL1 Physical Timer CompareValue Register (CNTP_CVAL).
+
+  \param [in]    value  CNTP_CVAL Register value to set
+*/
+__STATIC_FORCEINLINE void __set_CNTP_CVAL(uint64_t value)
+{
+  __set_CP64(15, 2, value, 14);
+}
+
+/** \brief  Get CNTP_CVAL
+
+    This function returns the value of the 64 bits PL1 Physical Timer CompareValue Register (CNTP_CVAL).
+
+    \return               CNTP_CVAL Register value
+ */
+__STATIC_FORCEINLINE uint64_t __get_CNTP_CVAL(void)
+{
+  uint64_t result;
+  __get_CP64(15, 2, result, 14);
+  return result;
+}
+
+/** \brief  Set CNTP_CTL
+
+  This function assigns the given value to PL1 Physical Timer Control Register (CNTP_CTL).
+
+  \param [in]    value  CNTP_CTL Register value to set
+*/
+__STATIC_FORCEINLINE void __set_CNTP_CTL(uint32_t value)
+{
+  __set_CP(15, 0, value, 14, 2, 1);
+}
+
+/** \brief  Get CNTP_CTL register
+    \return               CNTP_CTL Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CNTP_CTL(void)
+{
+  uint32_t result;
+  __get_CP(15, 0, result, 14, 2, 1);
+  return result;
+}
+
+#endif
+
+/** \brief  Set TLBIALL
+
+  TLB Invalidate All
+ */
+__STATIC_FORCEINLINE void __set_TLBIALL(uint32_t value)
+{
+  __set_CP(15, 0, value, 8, 7, 0);
+}
+
+/** \brief  Set BPIALL.
+
+  Branch Predictor Invalidate All
+ */
+__STATIC_FORCEINLINE void __set_BPIALL(uint32_t value)
+{
+  __set_CP(15, 0, value, 7, 5, 6);
+}
+
+/** \brief  Set ICIALLU
+
+  Instruction Cache Invalidate All
+ */
+__STATIC_FORCEINLINE void __set_ICIALLU(uint32_t value)
+{
+  __set_CP(15, 0, value, 7, 5, 0);
+}
+
+/** \brief  Set DCCMVAC
+
+  Data cache clean
+ */
+__STATIC_FORCEINLINE void __set_DCCMVAC(uint32_t value)
+{
+  __set_CP(15, 0, value, 7, 10, 1);
+}
+
+/** \brief  Set DCIMVAC
+
+  Data cache invalidate
+ */
+__STATIC_FORCEINLINE void __set_DCIMVAC(uint32_t value)
+{
+  __set_CP(15, 0, value, 7, 6, 1);
+}
+
+/** \brief  Set DCCIMVAC
+
+  Data cache clean and invalidate
+ */
+__STATIC_FORCEINLINE void __set_DCCIMVAC(uint32_t value)
+{
+  __set_CP(15, 0, value, 7, 14, 1);
+}
+
+/** \brief  Set CSSELR
+ */
+__STATIC_FORCEINLINE void __set_CSSELR(uint32_t value)
+{
+//  __ASM volatile("MCR p15, 2, %0, c0, c0, 0" : : "r"(value) : "memory");
+  __set_CP(15, 2, value, 0, 0, 0);
+}
+
+/** \brief  Get CSSELR
+    \return CSSELR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CSSELR(void)
+{
+  uint32_t result;
+//  __ASM volatile("MRC p15, 2, %0, c0, c0, 0" : "=r"(result) : : "memory");
+  __get_CP(15, 2, result, 0, 0, 0);
+  return result;
+}
+
+/** \brief  Set CCSIDR
+    \deprecated CCSIDR itself is read-only. Use __set_CSSELR to select cache level instead.
+ */
+CMSIS_DEPRECATED
+__STATIC_FORCEINLINE void __set_CCSIDR(uint32_t value)
+{
+  __set_CSSELR(value);
+}
+
+/** \brief  Get CCSIDR
+    \return CCSIDR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CCSIDR(void)
+{
+  uint32_t result;
+//  __ASM volatile("MRC p15, 1, %0, c0, c0, 0" : "=r"(result) : : "memory");
+  __get_CP(15, 1, result, 0, 0, 0);
+  return result;
+}
+
+/** \brief  Get CLIDR
+    \return CLIDR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CLIDR(void)
+{
+  uint32_t result;
+//  __ASM volatile("MRC p15, 1, %0, c0, c0, 1" : "=r"(result) : : "memory");
+  __get_CP(15, 1, result, 0, 0, 1);
+  return result;
+}
+
+/** \brief  Set DCISW
+ */
+__STATIC_FORCEINLINE void __set_DCISW(uint32_t value)
+{
+//  __ASM volatile("MCR p15, 0, %0, c7, c6, 2" : : "r"(value) : "memory")
+  __set_CP(15, 0, value, 7, 6, 2);
+}
+
+/** \brief  Set DCCSW
+ */
+__STATIC_FORCEINLINE void __set_DCCSW(uint32_t value)
+{
+//  __ASM volatile("MCR p15, 0, %0, c7, c10, 2" : : "r"(value) : "memory")
+  __set_CP(15, 0, value, 7, 10, 2);
+}
+
+/** \brief  Set DCCISW
+ */
+__STATIC_FORCEINLINE void __set_DCCISW(uint32_t value)
+{
+//  __ASM volatile("MCR p15, 0, %0, c7, c14, 2" : : "r"(value) : "memory")
+  __set_CP(15, 0, value, 7, 14, 2);
+}
+
+#endif

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core_A/Include/cmsis_gcc.h

@@ -0,0 +1,679 @@
+/**************************************************************************//**
+ * @file     cmsis_gcc.h
+ * @brief    CMSIS compiler specific macros, functions, instructions
+ * @version  V1.0.2
+ * @date     09. April 2018
+ ******************************************************************************/
+/*
+ * Copyright (c) 2009-2018 Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef __CMSIS_GCC_H
+#define __CMSIS_GCC_H
+
+/* ignore some GCC warnings */
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wsign-conversion"
+#pragma GCC diagnostic ignored "-Wconversion"
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+
+/* Fallback for __has_builtin */
+#ifndef __has_builtin
+  #define __has_builtin(x) (0)
+#endif
+
+/* CMSIS compiler specific defines */
+#ifndef   __ASM
+  #define __ASM                                  asm
+#endif
+#ifndef   __INLINE
+  #define __INLINE                               inline
+#endif
+#ifndef   __FORCEINLINE
+  #define __FORCEINLINE                          __attribute__((always_inline))
+#endif
+#ifndef   __STATIC_INLINE
+  #define __STATIC_INLINE                        static inline
+#endif
+#ifndef   __STATIC_FORCEINLINE
+  #define __STATIC_FORCEINLINE                   __attribute__((always_inline)) static inline
+#endif
+#ifndef   __NO_RETURN
+  #define __NO_RETURN                            __attribute__((__noreturn__))
+#endif
+#ifndef   CMSIS_DEPRECATED
+ #define  CMSIS_DEPRECATED                       __attribute__((deprecated))
+#endif
+#ifndef   __USED
+  #define __USED                                 __attribute__((used))
+#endif
+#ifndef   __WEAK
+  #define __WEAK                                 __attribute__((weak))
+#endif
+#ifndef   __PACKED
+  #define __PACKED                               __attribute__((packed, aligned(1)))
+#endif
+#ifndef   __PACKED_STRUCT
+  #define __PACKED_STRUCT                        struct __attribute__((packed, aligned(1)))
+#endif
+#ifndef   __UNALIGNED_UINT16_WRITE
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT16_WRITE)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT16_WRITE */
+  __PACKED_STRUCT T_UINT16_WRITE { uint16_t v; };
+  #pragma GCC diagnostic pop
+  #define __UNALIGNED_UINT16_WRITE(addr, val)    (void)((((struct T_UINT16_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef   __UNALIGNED_UINT16_READ
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT16_READ)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT16_READ */
+  __PACKED_STRUCT T_UINT16_READ { uint16_t v; };
+  #pragma GCC diagnostic pop
+  #define __UNALIGNED_UINT16_READ(addr)          (((const struct T_UINT16_READ *)(const void *)(addr))->v)
+#endif
+#ifndef   __UNALIGNED_UINT32_WRITE
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wpacked"
+/*lint -esym(9058, T_UINT32_WRITE)*/ /* disable MISRA 2012 Rule 2.4 for T_UINT32_WRITE */
+  __PACKED_STRUCT T_UINT32_WRITE { uint32_t v; };
+  #pragma GCC diagnostic pop
+  #define __UNALIGNED_UINT32_WRITE(addr, val)    (void)((((struct T_UINT32_WRITE *)(void *)(addr))->v) = (val))
+#endif
+#ifndef   __UNALIGNED_UINT32_READ
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wpacked"
+  __PACKED_STRUCT T_UINT32_READ { uint32_t v; };
+  #pragma GCC diagnostic pop
+  #define __UNALIGNED_UINT32_READ(addr)          (((const struct T_UINT32_READ *)(const void *)(addr))->v)
+#endif
+#ifndef   __ALIGNED
+  #define __ALIGNED(x)                           __attribute__((aligned(x)))
+#endif
+
+/* ##########################  Core Instruction Access  ######################### */
+/**
+  \brief   No Operation
+ */
+#define __NOP()                             __ASM volatile ("nop")
+
+/**
+  \brief   Wait For Interrupt
+ */
+#define __WFI()                             __ASM volatile ("wfi")
+
+/**
+  \brief   Wait For Event
+ */
+#define __WFE()                             __ASM volatile ("wfe")
+
+/**
+  \brief   Send Event
+ */
+#define __SEV()                             __ASM volatile ("sev")
+
+/**
+  \brief   Instruction Synchronization Barrier
+  \details Instruction Synchronization Barrier flushes the pipeline in the processor,
+           so that all instructions following the ISB are fetched from cache or memory,
+           after the instruction has been completed.
+ */
+__STATIC_FORCEINLINE  void __ISB(void)
+{
+  __ASM volatile ("isb 0xF":::"memory");
+}
+
+
+/**
+  \brief   Data Synchronization Barrier
+  \details Acts as a special kind of Data Memory Barrier.
+           It completes when all explicit memory accesses before this instruction complete.
+ */
+__STATIC_FORCEINLINE  void __DSB(void)
+{
+  __ASM volatile ("dsb 0xF":::"memory");
+}
+
+/**
+  \brief   Data Memory Barrier
+  \details Ensures the apparent order of the explicit memory operations before
+           and after the instruction, without ensuring their completion.
+ */
+__STATIC_FORCEINLINE  void __DMB(void)
+{
+  __ASM volatile ("dmb 0xF":::"memory");
+}
+
+/**
+  \brief   Reverse byte order (32 bit)
+  \details Reverses the byte order in unsigned integer value. For example, 0x12345678 becomes 0x78563412.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+__STATIC_FORCEINLINE  uint32_t __REV(uint32_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)
+  return __builtin_bswap32(value);
+#else
+  uint32_t result;
+
+  __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+  return result;
+#endif
+}
+
+/**
+  \brief   Reverse byte order (16 bit)
+  \details Reverses the byte order within each halfword of a word. For example, 0x12345678 becomes 0x34127856.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".rev16_text"))) __STATIC_INLINE uint32_t __REV16(uint32_t value)
+{
+  uint32_t result;
+  __ASM volatile("rev16 %0, %1" : "=r" (result) : "r" (value));
+  return result;
+}
+#endif
+
+/**
+  \brief   Reverse byte order (16 bit)
+  \details Reverses the byte order in a 16-bit value and returns the signed 16-bit result. For example, 0x0080 becomes 0x8000.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+__STATIC_FORCEINLINE  int16_t __REVSH(int16_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+  return (int16_t)__builtin_bswap16(value);
+#else
+  int16_t result;
+
+  __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+  return result;
+#endif
+}
+
+/**
+  \brief   Rotate Right in unsigned value (32 bit)
+  \details Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+  \param [in]    op1  Value to rotate
+  \param [in]    op2  Number of Bits to rotate
+  \return               Rotated value
+ */
+__STATIC_FORCEINLINE  uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+  op2 %= 32U;
+  if (op2 == 0U) {
+    return op1;
+  }
+  return (op1 >> op2) | (op1 << (32U - op2));
+}
+
+
+/**
+  \brief   Breakpoint
+  \param [in]    value  is ignored by the processor.
+                 If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value)                       __ASM volatile ("bkpt "#value)
+
+/**
+  \brief   Reverse bit order of value
+  \details Reverses the bit order of the given value.
+  \param [in]    value  Value to reverse
+  \return               Reversed value
+ */
+__STATIC_FORCEINLINE  uint32_t __RBIT(uint32_t value)
+{
+  uint32_t result;
+
+#if ((defined (__ARM_ARCH_7M__      ) && (__ARM_ARCH_7M__      == 1)) || \
+     (defined (__ARM_ARCH_7EM__     ) && (__ARM_ARCH_7EM__     == 1)) || \
+     (defined (__ARM_ARCH_8M_MAIN__ ) && (__ARM_ARCH_8M_MAIN__ == 1))    )
+   __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) );
+#else
+  int32_t s = (4U /*sizeof(v)*/ * 8U) - 1U; /* extra shift needed at end */
+
+  result = value;                      /* r will be reversed bits of v; first get LSB of v */
+  for (value >>= 1U; value; value >>= 1U)
+  {
+    result <<= 1U;
+    result |= value & 1U;
+    s--;
+  }
+  result <<= s;                        /* shift when v's highest bits are zero */
+#endif
+  return result;
+}
+
+/**
+  \brief   Count leading zeros
+  \param [in]  value  Value to count the leading zeros
+  \return             number of leading zeros in value
+ */
+#define __CLZ                             (uint8_t)__builtin_clz
+
+/**
+  \brief   LDR Exclusive (8 bit)
+  \details Executes a exclusive LDR instruction for 8 bit value.
+  \param [in]    ptr  Pointer to data
+  \return             value of type uint8_t at (*ptr)
+ */
+__STATIC_FORCEINLINE  uint8_t __LDREXB(volatile uint8_t *addr)
+{
+    uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+   __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+       accepted by assembler. So has to use following less efficient pattern.
+    */
+   __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+   return ((uint8_t) result);    /* Add explicit type cast here */
+}
+
+
+/**
+  \brief   LDR Exclusive (16 bit)
+  \details Executes a exclusive LDR instruction for 16 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint16_t at (*ptr)
+ */
+__STATIC_FORCEINLINE  uint16_t __LDREXH(volatile uint16_t *addr)
+{
+    uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+   __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+       accepted by assembler. So has to use following less efficient pattern.
+    */
+   __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+   return ((uint16_t) result);    /* Add explicit type cast here */
+}
+
+
+/**
+  \brief   LDR Exclusive (32 bit)
+  \details Executes a exclusive LDR instruction for 32 bit values.
+  \param [in]    ptr  Pointer to data
+  \return        value of type uint32_t at (*ptr)
+ */
+__STATIC_FORCEINLINE  uint32_t __LDREXW(volatile uint32_t *addr)
+{
+    uint32_t result;
+
+   __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) );
+   return(result);
+}
+
+
+/**
+  \brief   STR Exclusive (8 bit)
+  \details Executes a exclusive STR instruction for 8 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+__STATIC_FORCEINLINE  uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)
+{
+   uint32_t result;
+
+   __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" ((uint32_t)value) );
+   return(result);
+}
+
+
+/**
+  \brief   STR Exclusive (16 bit)
+  \details Executes a exclusive STR instruction for 16 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+__STATIC_FORCEINLINE  uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)
+{
+   uint32_t result;
+
+   __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" ((uint32_t)value) );
+   return(result);
+}
+
+
+/**
+  \brief   STR Exclusive (32 bit)
+  \details Executes a exclusive STR instruction for 32 bit values.
+  \param [in]  value  Value to store
+  \param [in]    ptr  Pointer to location
+  \return          0  Function succeeded
+  \return          1  Function failed
+ */
+__STATIC_FORCEINLINE  uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)
+{
+   uint32_t result;
+
+   __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+   return(result);
+}
+
+
+/**
+  \brief   Remove the exclusive lock
+  \details Removes the exclusive lock which is created by LDREX.
+ */
+__STATIC_FORCEINLINE  void __CLREX(void)
+{
+  __ASM volatile ("clrex" ::: "memory");
+}
+
+/**
+  \brief   Signed Saturate
+  \details Saturates a signed value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (1..32)
+  \return             Saturated value
+ */
+#define __SSAT(ARG1,ARG2) \
+__extension__ \
+({                          \
+  int32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("ssat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+
+/**
+  \brief   Unsigned Saturate
+  \details Saturates an unsigned value.
+  \param [in]  value  Value to be saturated
+  \param [in]    sat  Bit position to saturate to (0..31)
+  \return             Saturated value
+ */
+#define __USAT(ARG1,ARG2) \
+__extension__ \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("usat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+/* ###########################  Core Function Access  ########################### */
+
+/**
+  \brief   Enable IRQ Interrupts
+  \details Enables IRQ interrupts by clearing the I-bit in the CPSR.
+           Can only be executed in Privileged modes.
+ */
+__STATIC_FORCEINLINE void __enable_irq(void)
+{
+  __ASM volatile ("cpsie i" : : : "memory");
+}
+
+/**
+  \brief   Disable IRQ Interrupts
+  \details Disables IRQ interrupts by setting the I-bit in the CPSR.
+  Can only be executed in Privileged modes.
+ */
+__STATIC_FORCEINLINE  void __disable_irq(void)
+{
+  __ASM volatile ("cpsid i" : : : "memory");
+}
+
+/**
+  \brief   Get FPSCR
+  \details Returns the current value of the Floating Point Status/Control register.
+  \return Floating Point Status/Control register value
+*/
+__STATIC_FORCEINLINE  uint32_t __get_FPSCR(void)
+{
+  #if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+       (defined (__FPU_USED   ) && (__FPU_USED    == 1U))     )
+  #if __has_builtin(__builtin_arm_get_fpscr) 
+  // Re-enable using built-in when GCC has been fixed
+  // || (__GNUC__ > 7) || (__GNUC__ == 7 && __GNUC_MINOR__ >= 2)
+    /* see https://gcc.gnu.org/ml/gcc-patches/2017-04/msg00443.html */
+    return __builtin_arm_get_fpscr();
+  #else
+    uint32_t result;
+
+    __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
+    return(result);
+  #endif
+  #else
+    return(0U);
+  #endif
+}
+
+/**
+  \brief   Set FPSCR
+  \details Assigns the given value to the Floating Point Status/Control register.
+  \param [in] fpscr  Floating Point Status/Control value to set
+*/
+__STATIC_FORCEINLINE void __set_FPSCR(uint32_t fpscr)
+{
+  #if ((defined (__FPU_PRESENT) && (__FPU_PRESENT == 1U)) && \
+       (defined (__FPU_USED   ) && (__FPU_USED    == 1U))     )
+  #if __has_builtin(__builtin_arm_set_fpscr)
+  // Re-enable using built-in when GCC has been fixed
+  // || (__GNUC__ > 7) || (__GNUC__ == 7 && __GNUC_MINOR__ >= 2)
+    /* see https://gcc.gnu.org/ml/gcc-patches/2017-04/msg00443.html */
+    __builtin_arm_set_fpscr(fpscr);
+  #else
+    __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc", "memory");
+  #endif
+  #else
+    (void)fpscr;
+  #endif
+}
+
+/** \brief  Get CPSR Register
+    \return               CPSR Register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_CPSR(void)
+{
+  uint32_t result;
+  __ASM volatile("MRS %0, cpsr" : "=r" (result) );
+  return(result);
+}
+
+/** \brief  Set CPSR Register
+    \param [in]    cpsr  CPSR value to set
+ */
+__STATIC_FORCEINLINE void __set_CPSR(uint32_t cpsr)
+{
+__ASM volatile ("MSR cpsr, %0" : : "r" (cpsr) : "memory");
+}
+
+/** \brief  Get Mode
+    \return                Processor Mode
+ */
+__STATIC_FORCEINLINE uint32_t __get_mode(void)
+{
+    return (__get_CPSR() & 0x1FU);
+}
+
+/** \brief  Set Mode
+    \param [in]    mode  Mode value to set
+ */
+__STATIC_FORCEINLINE void __set_mode(uint32_t mode)
+{
+  __ASM volatile("MSR  cpsr_c, %0" : : "r" (mode) : "memory");
+}
+
+/** \brief  Get Stack Pointer
+    \return Stack Pointer value
+ */
+__STATIC_FORCEINLINE uint32_t __get_SP(void)
+{
+  uint32_t result;
+  __ASM volatile("MOV  %0, sp" : "=r" (result) : : "memory");
+  return result;
+}
+
+/** \brief  Set Stack Pointer
+    \param [in]    stack  Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_SP(uint32_t stack)
+{
+  __ASM volatile("MOV  sp, %0" : : "r" (stack) : "memory");
+}
+
+/** \brief  Get USR/SYS Stack Pointer
+    \return USR/SYS Stack Pointer value
+ */
+__STATIC_FORCEINLINE uint32_t __get_SP_usr(void)
+{
+  uint32_t cpsr = __get_CPSR();
+  uint32_t result;
+  __ASM volatile(
+    "CPS     #0x1F  \n"
+    "MOV     %0, sp   " : "=r"(result) : : "memory"
+   );
+  __set_CPSR(cpsr);
+  __ISB();
+  return result;
+}
+
+/** \brief  Set USR/SYS Stack Pointer
+    \param [in]    topOfProcStack  USR/SYS Stack Pointer value to set
+ */
+__STATIC_FORCEINLINE void __set_SP_usr(uint32_t topOfProcStack)
+{
+  uint32_t cpsr = __get_CPSR();
+  __ASM volatile(
+    "CPS     #0x1F  \n"
+    "MOV     sp, %0   " : : "r" (topOfProcStack) : "memory"
+   );
+  __set_CPSR(cpsr);
+  __ISB();
+}
+
+/** \brief  Get FPEXC
+    \return               Floating Point Exception Control register value
+ */
+__STATIC_FORCEINLINE uint32_t __get_FPEXC(void)
+{
+#if (__FPU_PRESENT == 1)
+  uint32_t result;
+  __ASM volatile("VMRS %0, fpexc" : "=r" (result) );
+  return(result);
+#else
+  return(0);
+#endif
+}
+
+/** \brief  Set FPEXC
+    \param [in]    fpexc  Floating Point Exception Control value to set
+ */
+__STATIC_FORCEINLINE void __set_FPEXC(uint32_t fpexc)
+{
+#if (__FPU_PRESENT == 1)
+  __ASM volatile ("VMSR fpexc, %0" : : "r" (fpexc) : "memory");
+#endif
+}
+
+/*
+ * Include common core functions to access Coprocessor 15 registers
+ */
+
+#define __get_CP(cp, op1, Rt, CRn, CRm, op2) __ASM volatile("MRC p" # cp ", " # op1 ", %0, c" # CRn ", c" # CRm ", " # op2 : "=r" (Rt) : : "memory" )
+#define __set_CP(cp, op1, Rt, CRn, CRm, op2) __ASM volatile("MCR p" # cp ", " # op1 ", %0, c" # CRn ", c" # CRm ", " # op2 : : "r" (Rt) : "memory" )
+#define __get_CP64(cp, op1, Rt, CRm) __ASM volatile("MRRC p" # cp ", " # op1 ", %Q0, %R0, c" # CRm  : "=r" (Rt) : : "memory" )
+#define __set_CP64(cp, op1, Rt, CRm) __ASM volatile("MCRR p" # cp ", " # op1 ", %Q0, %R0, c" # CRm  : : "r" (Rt) : "memory" )
+
+#include "cmsis_cp15.h"
+
+/** \brief  Enable Floating Point Unit
+
+  Critical section, called from undef handler, so systick is disabled
+ */
+__STATIC_INLINE void __FPU_Enable(void)
+{
+  __ASM volatile(
+    //Permit access to VFP/NEON, registers by modifying CPACR
+    "        MRC     p15,0,R1,c1,c0,2  \n"
+    "        ORR     R1,R1,#0x00F00000 \n"
+    "        MCR     p15,0,R1,c1,c0,2  \n"
+
+    //Ensure that subsequent instructions occur in the context of VFP/NEON access permitted
+    "        ISB                       \n"
+
+    //Enable VFP/NEON
+    "        VMRS    R1,FPEXC          \n"
+    "        ORR     R1,R1,#0x40000000 \n"
+    "        VMSR    FPEXC,R1          \n"
+
+    //Initialise VFP/NEON registers to 0
+    "        MOV     R2,#0             \n"
+
+    //Initialise D16 registers to 0
+    "        VMOV    D0, R2,R2         \n"
+    "        VMOV    D1, R2,R2         \n"
+    "        VMOV    D2, R2,R2         \n"
+    "        VMOV    D3, R2,R2         \n"
+    "        VMOV    D4, R2,R2         \n"
+    "        VMOV    D5, R2,R2         \n"
+    "        VMOV    D6, R2,R2         \n"
+    "        VMOV    D7, R2,R2         \n"
+    "        VMOV    D8, R2,R2         \n"
+    "        VMOV    D9, R2,R2         \n"
+    "        VMOV    D10,R2,R2         \n"
+    "        VMOV    D11,R2,R2         \n"
+    "        VMOV    D12,R2,R2         \n"
+    "        VMOV    D13,R2,R2         \n"
+    "        VMOV    D14,R2,R2         \n"
+    "        VMOV    D15,R2,R2         \n"
+
+#if (defined(__ARM_NEON) && (__ARM_NEON == 1))
+    //Initialise D32 registers to 0
+    "        VMOV    D16,R2,R2         \n"
+    "        VMOV    D17,R2,R2         \n"
+    "        VMOV    D18,R2,R2         \n"
+    "        VMOV    D19,R2,R2         \n"
+    "        VMOV    D20,R2,R2         \n"
+    "        VMOV    D21,R2,R2         \n"
+    "        VMOV    D22,R2,R2         \n"
+    "        VMOV    D23,R2,R2         \n"
+    "        VMOV    D24,R2,R2         \n"
+    "        VMOV    D25,R2,R2         \n"
+    "        VMOV    D26,R2,R2         \n"
+    "        VMOV    D27,R2,R2         \n"
+    "        VMOV    D28,R2,R2         \n"
+    "        VMOV    D29,R2,R2         \n"
+    "        VMOV    D30,R2,R2         \n"
+    "        VMOV    D31,R2,R2         \n"
+#endif
+
+    //Initialise FPSCR to a known state
+    "        VMRS    R2,FPSCR          \n"
+    "        LDR     R3,=0x00086060    \n" //Mask off all bits that do not have to be preserved. Non-preserved bits can/should be zero.
+    "        AND     R2,R2,R3          \n"
+    "        VMSR    FPSCR,R2            "
+  );
+}
+
+#pragma GCC diagnostic pop
+
+#endif /* __CMSIS_GCC_H */

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core_A/Include/cmsis_iccarm.h

@@ -0,0 +1,559 @@
+/**************************************************************************//**
+ * @file     cmsis_iccarm.h
+ * @brief    CMSIS compiler ICCARM (IAR Compiler for Arm) header file
+ * @version  V5.0.6
+ * @date     02. March 2018
+ ******************************************************************************/
+
+//------------------------------------------------------------------------------
+//
+// Copyright (c) 2017-2018 IAR Systems
+//
+// Licensed under the Apache License, Version 2.0 (the "License")
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+//------------------------------------------------------------------------------
+
+
+#ifndef __CMSIS_ICCARM_H__
+#define __CMSIS_ICCARM_H__
+
+#ifndef __ICCARM__
+  #error This file should only be compiled by ICCARM
+#endif
+
+#pragma system_include
+
+#define __IAR_FT _Pragma("inline=forced") __intrinsic
+
+#if (__VER__ >= 8000000)
+  #define __ICCARM_V8 1
+#else
+  #define __ICCARM_V8 0
+#endif
+
+#pragma language=extended
+
+#ifndef __ALIGNED
+  #if __ICCARM_V8
+    #define __ALIGNED(x) __attribute__((aligned(x)))
+  #elif (__VER__ >= 7080000)
+    /* Needs IAR language extensions */
+    #define __ALIGNED(x) __attribute__((aligned(x)))
+  #else
+    #warning No compiler specific solution for __ALIGNED.__ALIGNED is ignored.
+    #define __ALIGNED(x)
+  #endif
+#endif
+
+
+/* Define compiler macros for CPU architecture, used in CMSIS 5.
+ */
+#if __ARM_ARCH_7A__
+/* Macro already defined */
+#else
+  #if defined(__ARM7A__)
+    #define __ARM_ARCH_7A__ 1
+  #endif
+#endif
+
+#ifndef __ASM
+  #define __ASM __asm
+#endif
+
+#ifndef __INLINE
+  #define __INLINE inline
+#endif
+
+#ifndef   __NO_RETURN
+  #if __ICCARM_V8
+    #define __NO_RETURN __attribute__((__noreturn__))
+  #else
+    #define __NO_RETURN _Pragma("object_attribute=__noreturn")
+  #endif
+#endif
+
+#ifndef   __PACKED
+  /* Needs IAR language extensions */
+  #if __ICCARM_V8
+    #define __PACKED __attribute__((packed, aligned(1)))
+  #else
+    #define __PACKED __packed
+  #endif
+#endif
+
+#ifndef   __PACKED_STRUCT
+  /* Needs IAR language extensions */
+  #if __ICCARM_V8
+    #define __PACKED_STRUCT struct __attribute__((packed, aligned(1)))
+  #else
+    #define __PACKED_STRUCT __packed struct
+  #endif
+#endif
+
+#ifndef   __PACKED_UNION
+  /* Needs IAR language extensions */
+  #if __ICCARM_V8
+    #define __PACKED_UNION union __attribute__((packed, aligned(1)))
+  #else
+    #define __PACKED_UNION __packed union
+  #endif
+#endif
+
+#ifndef   __RESTRICT
+  #define __RESTRICT            __restrict
+#endif
+
+#ifndef   __STATIC_INLINE
+  #define __STATIC_INLINE       static inline
+#endif
+
+#ifndef   __FORCEINLINE
+  #define __FORCEINLINE         _Pragma("inline=forced")
+#endif
+
+#ifndef   __STATIC_FORCEINLINE
+  #define __STATIC_FORCEINLINE  __FORCEINLINE __STATIC_INLINE
+#endif
+
+#ifndef   CMSIS_DEPRECATED
+  #define CMSIS_DEPRECATED      __attribute__((deprecated))
+#endif
+
+#ifndef __UNALIGNED_UINT16_READ
+  #pragma language=save
+  #pragma language=extended
+  __IAR_FT uint16_t __iar_uint16_read(void const *ptr)
+  {
+    return *(__packed uint16_t*)(ptr);
+  }
+  #pragma language=restore
+  #define __UNALIGNED_UINT16_READ(PTR) __iar_uint16_read(PTR)
+#endif
+
+
+#ifndef __UNALIGNED_UINT16_WRITE
+  #pragma language=save
+  #pragma language=extended
+  __IAR_FT void __iar_uint16_write(void const *ptr, uint16_t val)
+  {
+    *(__packed uint16_t*)(ptr) = val;;
+  }
+  #pragma language=restore
+  #define __UNALIGNED_UINT16_WRITE(PTR,VAL) __iar_uint16_write(PTR,VAL)
+#endif
+
+#ifndef __UNALIGNED_UINT32_READ
+  #pragma language=save
+  #pragma language=extended
+  __IAR_FT uint32_t __iar_uint32_read(void const *ptr)
+  {
+    return *(__packed uint32_t*)(ptr);
+  }
+  #pragma language=restore
+  #define __UNALIGNED_UINT32_READ(PTR) __iar_uint32_read(PTR)
+#endif
+
+#ifndef __UNALIGNED_UINT32_WRITE
+  #pragma language=save
+  #pragma language=extended
+  __IAR_FT void __iar_uint32_write(void const *ptr, uint32_t val)
+  {
+    *(__packed uint32_t*)(ptr) = val;;
+  }
+  #pragma language=restore
+  #define __UNALIGNED_UINT32_WRITE(PTR,VAL) __iar_uint32_write(PTR,VAL)
+#endif
+
+#if 0
+#ifndef __UNALIGNED_UINT32   /* deprecated */
+  #pragma language=save
+  #pragma language=extended
+  __packed struct  __iar_u32 { uint32_t v; };
+  #pragma language=restore
+  #define __UNALIGNED_UINT32(PTR) (((struct __iar_u32 *)(PTR))->v)
+#endif
+#endif
+
+#ifndef   __USED
+  #if __ICCARM_V8
+    #define __USED __attribute__((used))
+  #else
+    #define __USED _Pragma("__root")
+  #endif
+#endif
+
+#ifndef   __WEAK
+  #if __ICCARM_V8
+    #define __WEAK __attribute__((weak))
+  #else
+    #define __WEAK _Pragma("__weak")
+  #endif
+#endif
+
+
+#ifndef __ICCARM_INTRINSICS_VERSION__
+  #define __ICCARM_INTRINSICS_VERSION__  0
+#endif
+
+#if __ICCARM_INTRINSICS_VERSION__ == 2
+
+  #if defined(__CLZ)
+    #undef __CLZ
+  #endif
+  #if defined(__REVSH)
+    #undef __REVSH
+  #endif
+  #if defined(__RBIT)
+    #undef __RBIT
+  #endif
+  #if defined(__SSAT)
+    #undef __SSAT
+  #endif
+  #if defined(__USAT)
+    #undef __USAT
+  #endif
+
+  #include "iccarm_builtin.h"
+
+  #define __enable_irq        __iar_builtin_enable_interrupt
+  #define __disable_irq       __iar_builtin_disable_interrupt
+  #define __enable_fault_irq    __iar_builtin_enable_fiq
+  #define __disable_fault_irq   __iar_builtin_disable_fiq
+  #define __arm_rsr           __iar_builtin_rsr
+  #define __arm_wsr           __iar_builtin_wsr
+
+  #if __FPU_PRESENT
+    #define __get_FPSCR()             (__arm_rsr("FPSCR"))
+  #else
+    #define __get_FPSCR()             ( 0 )
+  #endif
+
+  #define __set_FPSCR(VALUE)          (__arm_wsr("FPSCR", VALUE))
+
+  #define __get_CPSR()                (__arm_rsr("CPSR"))
+  #define __get_mode()                (__get_CPSR() & 0x1FU)
+
+  #define __set_CPSR(VALUE)           (__arm_wsr("CPSR", (VALUE)))
+  #define __set_mode(VALUE)           (__arm_wsr("CPSR_c", (VALUE)))
+
+
+  #define __get_FPEXC()       (__arm_rsr("FPEXC"))
+  #define __set_FPEXC(VALUE)    (__arm_wsr("FPEXC", VALUE))
+
+  #define __get_CP(cp, op1, RT, CRn, CRm, op2) \
+    ((RT) = __arm_rsr("p" # cp ":" # op1 ":c" # CRn ":c" # CRm ":" # op2))
+
+  #define __set_CP(cp, op1, RT, CRn, CRm, op2) \
+    (__arm_wsr("p" # cp ":" # op1 ":c" # CRn ":c" # CRm ":" # op2, (RT)))
+
+  #define __get_CP64(cp, op1, Rt, CRm) \
+    __ASM volatile("MRRC p" # cp ", " # op1 ", %Q0, %R0, c" # CRm  : "=r" (Rt) : : "memory" )
+
+  #define __set_CP64(cp, op1, Rt, CRm) \
+    __ASM volatile("MCRR p" # cp ", " # op1 ", %Q0, %R0, c" # CRm  : : "r" (Rt) : "memory" )
+
+  #include "cmsis_cp15.h"
+
+  #define __NOP     __iar_builtin_no_operation
+
+  #define __CLZ     __iar_builtin_CLZ
+  #define __CLREX   __iar_builtin_CLREX
+
+  #define __DMB     __iar_builtin_DMB
+  #define __DSB     __iar_builtin_DSB
+  #define __ISB     __iar_builtin_ISB
+
+  #define __LDREXB  __iar_builtin_LDREXB
+  #define __LDREXH  __iar_builtin_LDREXH
+  #define __LDREXW  __iar_builtin_LDREX
+
+  #define __RBIT    __iar_builtin_RBIT
+  #define __REV     __iar_builtin_REV
+  #define __REV16   __iar_builtin_REV16
+
+  __IAR_FT int16_t __REVSH(int16_t val)
+  {
+    return (int16_t) __iar_builtin_REVSH(val);
+  }
+
+  #define __ROR     __iar_builtin_ROR
+  #define __RRX     __iar_builtin_RRX
+
+  #define __SEV     __iar_builtin_SEV
+
+  #define __SSAT    __iar_builtin_SSAT
+
+  #define __STREXB  __iar_builtin_STREXB
+  #define __STREXH  __iar_builtin_STREXH
+  #define __STREXW  __iar_builtin_STREX
+
+  #define __USAT    __iar_builtin_USAT
+
+  #define __WFE     __iar_builtin_WFE
+  #define __WFI     __iar_builtin_WFI
+
+  #define __SADD8   __iar_builtin_SADD8
+  #define __QADD8   __iar_builtin_QADD8
+  #define __SHADD8  __iar_builtin_SHADD8
+  #define __UADD8   __iar_builtin_UADD8
+  #define __UQADD8  __iar_builtin_UQADD8
+  #define __UHADD8  __iar_builtin_UHADD8
+  #define __SSUB8   __iar_builtin_SSUB8
+  #define __QSUB8   __iar_builtin_QSUB8
+  #define __SHSUB8  __iar_builtin_SHSUB8
+  #define __USUB8   __iar_builtin_USUB8
+  #define __UQSUB8  __iar_builtin_UQSUB8
+  #define __UHSUB8  __iar_builtin_UHSUB8
+  #define __SADD16  __iar_builtin_SADD16
+  #define __QADD16  __iar_builtin_QADD16
+  #define __SHADD16 __iar_builtin_SHADD16
+  #define __UADD16  __iar_builtin_UADD16
+  #define __UQADD16 __iar_builtin_UQADD16
+  #define __UHADD16 __iar_builtin_UHADD16
+  #define __SSUB16  __iar_builtin_SSUB16
+  #define __QSUB16  __iar_builtin_QSUB16
+  #define __SHSUB16 __iar_builtin_SHSUB16
+  #define __USUB16  __iar_builtin_USUB16
+  #define __UQSUB16 __iar_builtin_UQSUB16
+  #define __UHSUB16 __iar_builtin_UHSUB16
+  #define __SASX    __iar_builtin_SASX
+  #define __QASX    __iar_builtin_QASX
+  #define __SHASX   __iar_builtin_SHASX
+  #define __UASX    __iar_builtin_UASX
+  #define __UQASX   __iar_builtin_UQASX
+  #define __UHASX   __iar_builtin_UHASX
+  #define __SSAX    __iar_builtin_SSAX
+  #define __QSAX    __iar_builtin_QSAX
+  #define __SHSAX   __iar_builtin_SHSAX
+  #define __USAX    __iar_builtin_USAX
+  #define __UQSAX   __iar_builtin_UQSAX
+  #define __UHSAX   __iar_builtin_UHSAX
+  #define __USAD8   __iar_builtin_USAD8
+  #define __USADA8  __iar_builtin_USADA8
+  #define __SSAT16  __iar_builtin_SSAT16
+  #define __USAT16  __iar_builtin_USAT16
+  #define __UXTB16  __iar_builtin_UXTB16
+  #define __UXTAB16 __iar_builtin_UXTAB16
+  #define __SXTB16  __iar_builtin_SXTB16
+  #define __SXTAB16 __iar_builtin_SXTAB16
+  #define __SMUAD   __iar_builtin_SMUAD
+  #define __SMUADX  __iar_builtin_SMUADX
+  #define __SMMLA   __iar_builtin_SMMLA
+  #define __SMLAD   __iar_builtin_SMLAD
+  #define __SMLADX  __iar_builtin_SMLADX
+  #define __SMLALD  __iar_builtin_SMLALD
+  #define __SMLALDX __iar_builtin_SMLALDX
+  #define __SMUSD   __iar_builtin_SMUSD
+  #define __SMUSDX  __iar_builtin_SMUSDX
+  #define __SMLSD   __iar_builtin_SMLSD
+  #define __SMLSDX  __iar_builtin_SMLSDX
+  #define __SMLSLD  __iar_builtin_SMLSLD
+  #define __SMLSLDX __iar_builtin_SMLSLDX
+  #define __SEL     __iar_builtin_SEL
+  #define __QADD    __iar_builtin_QADD
+  #define __QSUB    __iar_builtin_QSUB
+  #define __PKHBT   __iar_builtin_PKHBT
+  #define __PKHTB   __iar_builtin_PKHTB
+
+#else /* __ICCARM_INTRINSICS_VERSION__ == 2 */
+
+  #if !__FPU_PRESENT
+  #define __get_FPSCR __cmsis_iar_get_FPSR_not_active
+  #endif
+
+  #ifdef __INTRINSICS_INCLUDED
+  #error intrinsics.h is already included previously!
+  #endif
+
+  #include <intrinsics.h>
+
+  #if !__FPU_PRESENT
+  #define __get_FPSCR() (0)
+  #endif
+
+  #pragma diag_suppress=Pe940
+  #pragma diag_suppress=Pe177
+
+  #define __enable_irq        __enable_interrupt
+  #define __disable_irq       __disable_interrupt
+  #define __enable_fault_irq    __enable_fiq
+  #define __disable_fault_irq   __disable_fiq
+  #define __NOP               __no_operation
+
+  #define __get_xPSR          __get_PSR
+
+  __IAR_FT void __set_mode(uint32_t mode)
+  {
+    __ASM volatile("MSR  cpsr_c, %0" : : "r" (mode) : "memory");
+  }
+
+  __IAR_FT uint32_t __LDREXW(uint32_t volatile *ptr)
+  {
+    return __LDREX((unsigned long *)ptr);
+  }
+
+  __IAR_FT uint32_t __STREXW(uint32_t value, uint32_t volatile *ptr)
+  {
+    return __STREX(value, (unsigned long *)ptr);
+  }
+
+
+  __IAR_FT uint32_t __RRX(uint32_t value)
+  {
+    uint32_t result;
+    __ASM("RRX      %0, %1" : "=r"(result) : "r" (value) : "cc");
+    return(result);
+  }
+
+
+  __IAR_FT uint32_t __ROR(uint32_t op1, uint32_t op2)
+  {
+    return (op1 >> op2) | (op1 << ((sizeof(op1)*8)-op2));
+  }
+
+  __IAR_FT uint32_t __get_FPEXC(void)
+  {
+  #if (__FPU_PRESENT == 1)
+    uint32_t result;
+    __ASM volatile("VMRS %0, fpexc" : "=r" (result) : : "memory");
+    return(result);
+  #else
+    return(0);
+  #endif
+  }
+
+  __IAR_FT void __set_FPEXC(uint32_t fpexc)
+  {
+  #if (__FPU_PRESENT == 1)
+    __ASM volatile ("VMSR fpexc, %0" : : "r" (fpexc) : "memory");
+  #endif
+  }
+
+
+  #define __get_CP(cp, op1, Rt, CRn, CRm, op2) \
+    __ASM volatile("MRC p" # cp ", " # op1 ", %0, c" # CRn ", c" # CRm ", " # op2 : "=r" (Rt) : : "memory" )
+  #define __set_CP(cp, op1, Rt, CRn, CRm, op2) \
+    __ASM volatile("MCR p" # cp ", " # op1 ", %0, c" # CRn ", c" # CRm ", " # op2 : : "r" (Rt) : "memory" )
+  #define __get_CP64(cp, op1, Rt, CRm) \
+    __ASM volatile("MRRC p" # cp ", " # op1 ", %Q0, %R0, c" # CRm  : "=r" (Rt) : : "memory" )
+  #define __set_CP64(cp, op1, Rt, CRm) \
+    __ASM volatile("MCRR p" # cp ", " # op1 ", %Q0, %R0, c" # CRm  : : "r" (Rt) : "memory" )
+
+  #include "cmsis_cp15.h"
+
+#endif   /* __ICCARM_INTRINSICS_VERSION__ == 2 */
+
+#define __BKPT(value)    __asm volatile ("BKPT     %0" : : "i"(value))
+
+
+__IAR_FT uint32_t __get_SP_usr(void)
+{
+  uint32_t cpsr;
+  uint32_t result;
+  __ASM volatile(
+    "MRS     %0, cpsr   \n"
+    "CPS     #0x1F      \n" // no effect in USR mode
+    "MOV     %1, sp     \n"
+    "MSR     cpsr_c, %2 \n" // no effect in USR mode
+    "ISB" :  "=r"(cpsr), "=r"(result) : "r"(cpsr) : "memory"
+   );
+  return result;
+}
+
+__IAR_FT void __set_SP_usr(uint32_t topOfProcStack)
+{
+  uint32_t cpsr;
+  __ASM volatile(
+    "MRS     %0, cpsr   \n"
+    "CPS     #0x1F      \n" // no effect in USR mode
+    "MOV     sp, %1     \n"
+    "MSR     cpsr_c, %2 \n" // no effect in USR mode
+    "ISB" : "=r"(cpsr) : "r" (topOfProcStack), "r"(cpsr) : "memory"
+   );
+}
+
+#define __get_mode()                (__get_CPSR() & 0x1FU)
+
+__STATIC_INLINE
+void __FPU_Enable(void)
+{
+  __ASM volatile(
+    //Permit access to VFP/NEON, registers by modifying CPACR
+    "        MRC     p15,0,R1,c1,c0,2  \n"
+    "        ORR     R1,R1,#0x00F00000 \n"
+    "        MCR     p15,0,R1,c1,c0,2  \n"
+
+    //Ensure that subsequent instructions occur in the context of VFP/NEON access permitted
+    "        ISB                       \n"
+
+    //Enable VFP/NEON
+    "        VMRS    R1,FPEXC          \n"
+    "        ORR     R1,R1,#0x40000000 \n"
+    "        VMSR    FPEXC,R1          \n"
+
+    //Initialise VFP/NEON registers to 0
+    "        MOV     R2,#0             \n"
+
+    //Initialise D16 registers to 0
+    "        VMOV    D0, R2,R2         \n"
+    "        VMOV    D1, R2,R2         \n"
+    "        VMOV    D2, R2,R2         \n"
+    "        VMOV    D3, R2,R2         \n"
+    "        VMOV    D4, R2,R2         \n"
+    "        VMOV    D5, R2,R2         \n"
+    "        VMOV    D6, R2,R2         \n"
+    "        VMOV    D7, R2,R2         \n"
+    "        VMOV    D8, R2,R2         \n"
+    "        VMOV    D9, R2,R2         \n"
+    "        VMOV    D10,R2,R2         \n"
+    "        VMOV    D11,R2,R2         \n"
+    "        VMOV    D12,R2,R2         \n"
+    "        VMOV    D13,R2,R2         \n"
+    "        VMOV    D14,R2,R2         \n"
+    "        VMOV    D15,R2,R2         \n"
+
+#ifdef __ARM_ADVANCED_SIMD__
+    //Initialise D32 registers to 0
+    "        VMOV    D16,R2,R2         \n"
+    "        VMOV    D17,R2,R2         \n"
+    "        VMOV    D18,R2,R2         \n"
+    "        VMOV    D19,R2,R2         \n"
+    "        VMOV    D20,R2,R2         \n"
+    "        VMOV    D21,R2,R2         \n"
+    "        VMOV    D22,R2,R2         \n"
+    "        VMOV    D23,R2,R2         \n"
+    "        VMOV    D24,R2,R2         \n"
+    "        VMOV    D25,R2,R2         \n"
+    "        VMOV    D26,R2,R2         \n"
+    "        VMOV    D27,R2,R2         \n"
+    "        VMOV    D28,R2,R2         \n"
+    "        VMOV    D29,R2,R2         \n"
+    "        VMOV    D30,R2,R2         \n"
+    "        VMOV    D31,R2,R2         \n"
+#endif
+
+    //Initialise FPSCR to a known state
+    "        VMRS    R2,FPSCR          \n"
+    "        MOV32   R3,#0x00086060    \n" //Mask off all bits that do not have to be preserved. Non-preserved bits can/should be zero.
+    "        AND     R2,R2,R3          \n"
+    "        VMSR    FPSCR,R2          \n");
+}
+
+
+
+#undef __IAR_FT
+#undef __ICCARM_V8
+
+#pragma diag_default=Pe940
+#pragma diag_default=Pe177
+
+#endif /* __CMSIS_ICCARM_H__ */

2.Firmware/ICM42688P_HAL/Drivers/CMSIS/Core_A/Include/irq_ctrl.h

@@ -0,0 +1,186 @@
+/**************************************************************************//**
+ * @file     irq_ctrl.h
+ * @brief    Interrupt Controller API header file
+ * @version  V1.0.0
+ * @date     23. June 2017
+ ******************************************************************************/
+/*
+ * Copyright (c) 2017 ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#if   defined ( __ICCARM__ )
+  #pragma system_include         /* treat file as system include file for MISRA check */
+#elif defined (__clang__)
+  #pragma clang system_header   /* treat file as system include file */
+#endif
+
+#ifndef IRQ_CTRL_H_
+#define IRQ_CTRL_H_
+
+#include <stdint.h>
+
+#ifndef IRQHANDLER_T
+#define IRQHANDLER_T
+/// Interrupt handler data type
+typedef void (*IRQHandler_t) (void);
+#endif
+
+#ifndef IRQN_ID_T
+#define IRQN_ID_T
+/// Interrupt ID number data type
+typedef int32_t IRQn_ID_t;
+#endif
+
+/* Interrupt mode bit-masks */
+#define IRQ_MODE_TRIG_Pos           (0U)
+#define IRQ_MODE_TRIG_Msk           (0x07UL /*<< IRQ_MODE_TRIG_Pos*/)
+#define IRQ_MODE_TRIG_LEVEL         (0x00UL /*<< IRQ_MODE_TRIG_Pos*/) ///< Trigger: level triggered interrupt
+#define IRQ_MODE_TRIG_LEVEL_LOW     (0x01UL /*<< IRQ_MODE_TRIG_Pos*/) ///< Trigger: low level triggered interrupt
+#define IRQ_MODE_TRIG_LEVEL_HIGH    (0x02UL /*<< IRQ_MODE_TRIG_Pos*/) ///< Trigger: high level triggered interrupt
+#define IRQ_MODE_TRIG_EDGE          (0x04UL /*<< IRQ_MODE_TRIG_Pos*/) ///< Trigger: edge triggered interrupt
+#define IRQ_MODE_TRIG_EDGE_RISING   (0x05UL /*<< IRQ_MODE_TRIG_Pos*/) ///< Trigger: rising edge triggered interrupt
+#define IRQ_MODE_TRIG_EDGE_FALLING  (0x06UL /*<< IRQ_MODE_TRIG_Pos*/) ///< Trigger: falling edge triggered interrupt
+#define IRQ_MODE_TRIG_EDGE_BOTH     (0x07UL /*<< IRQ_MODE_TRIG_Pos*/) ///< Trigger: rising and falling edge triggered interrupt
+
+#define IRQ_MODE_TYPE_Pos           (3U)
+#define IRQ_MODE_TYPE_Msk           (0x01UL << IRQ_MODE_TYPE_Pos)
+#define IRQ_MODE_TYPE_IRQ           (0x00UL << IRQ_MODE_TYPE_Pos)     ///< Type: interrupt source triggers CPU IRQ line
+#define IRQ_MODE_TYPE_FIQ           (0x01UL << IRQ_MODE_TYPE_Pos)     ///< Type: interrupt source triggers CPU FIQ line
+
+#define IRQ_MODE_DOMAIN_Pos         (4U)
+#define IRQ_MODE_DOMAIN_Msk         (0x01UL << IRQ_MODE_DOMAIN_Pos)
+#define IRQ_MODE_DOMAIN_NONSECURE   (0x00UL << IRQ_MODE_DOMAIN_Pos)   ///< Domain: interrupt is targeting non-secure domain
+#define IRQ_MODE_DOMAIN_SECURE      (0x01UL << IRQ_MODE_DOMAIN_Pos)   ///< Domain: interrupt is targeting secure domain
+
+#define IRQ_MODE_CPU_Pos            (5U)
+#define IRQ_MODE_CPU_Msk            (0xFFUL << IRQ_MODE_CPU_Pos)
+#define IRQ_MODE_CPU_ALL            (0x00UL << IRQ_MODE_CPU_Pos)      ///< CPU: interrupt targets all CPUs
+#define IRQ_MODE_CPU_0              (0x01UL << IRQ_MODE_CPU_Pos)      ///< CPU: interrupt targets CPU 0
+#define IRQ_MODE_CPU_1              (0x02UL << IRQ_MODE_CPU_Pos)      ///< CPU: interrupt targets CPU 1
+#define IRQ_MODE_CPU_2              (0x04UL << IRQ_MODE_CPU_Pos)      ///< CPU: interrupt targets CPU 2
+#define IRQ_MODE_CPU_3              (0x08UL << IRQ_MODE_CPU_Pos)      ///< CPU: interrupt targets CPU 3
+#define IRQ_MODE_CPU_4              (0x10UL << IRQ_MODE_CPU_Pos)      ///< CPU: interrupt targets CPU 4
+#define IRQ_MODE_CPU_5              (0x20UL << IRQ_MODE_CPU_Pos)      ///< CPU: interrupt targets CPU 5
+#define IRQ_MODE_CPU_6              (0x40UL << IRQ_MODE_CPU_Pos)      ///< CPU: interrupt targets CPU 6
+#define IRQ_MODE_CPU_7              (0x80UL << IRQ_MODE_CPU_Pos)      ///< CPU: interrupt targets CPU 7
+
+#define IRQ_MODE_ERROR              (0x80000000UL)                    ///< Bit indicating mode value error
+
+/* Interrupt priority bit-masks */
+#define IRQ_PRIORITY_Msk            (0x0000FFFFUL)                    ///< Interrupt priority value bit-mask
+#define IRQ_PRIORITY_ERROR          (0x80000000UL)                    ///< Bit indicating priority value error
+
+/// Initialize interrupt controller.
+/// \return 0 on success, -1 on error.
+int32_t IRQ_Initialize (void);
+
+/// Register interrupt handler.
+/// \param[in]     irqn          interrupt ID number
+/// \param[in]     handler       interrupt handler function address
+/// \return 0 on success, -1 on error.
+int32_t IRQ_SetHandler (IRQn_ID_t irqn, IRQHandler_t handler);
+
+/// Get the registered interrupt handler.
+/// \param[in]     irqn          interrupt ID number
+/// \return registered interrupt handler function address.
+IRQHandler_t IRQ_GetHandler (IRQn_ID_t irqn);
+
+/// Enable interrupt.
+/// \param[in]     irqn          interrupt ID number
+/// \return 0 on success, -1 on error.
+int32_t IRQ_Enable (IRQn_ID_t irqn);
+
+/// Disable interrupt.
+/// \param[in]     irqn          interrupt ID number
+/// \return 0 on success, -1 on error.
+int32_t IRQ_Disable (IRQn_ID_t irqn);
+
+/// Get interrupt enable state.
+/// \param[in]     irqn          interrupt ID number
+/// \return 0 - interrupt is disabled, 1 - interrupt is enabled.
+uint32_t IRQ_GetEnableState (IRQn_ID_t irqn);
+
+/// Configure interrupt request mode.
+/// \param[in]     irqn          interrupt ID number
+/// \param[in]     mode          mode configuration
+/// \return 0 on success, -1 on error.
+int32_t IRQ_SetMode (IRQn_ID_t irqn, uint32_t mode);
+
+/// Get interrupt mode configuration.
+/// \param[in]     irqn          interrupt ID number
+/// \return current interrupt mode configuration with optional IRQ_MODE_ERROR bit set.
+uint32_t IRQ_GetMode (IRQn_ID_t irqn);
+
+/// Get ID number of current interrupt request (IRQ).
+/// \return interrupt ID number.
+IRQn_ID_t IRQ_GetActiveIRQ (void);
+
+/// Get ID number of current fast interrupt request (FIQ).
+/// \return interrupt ID number.
+IRQn_ID_t IRQ_GetActiveFIQ (void);
+
+/// Signal end of interrupt processing.
+/// \param[in]     irqn          interrupt ID number
+/// \return 0 on success, -1 on error.
+int32_t IRQ_EndOfInterrupt (IRQn_ID_t irqn);
+
+/// Set interrupt pending flag.
+/// \param[in]     irqn          interrupt ID number
+/// \return 0 on success, -1 on error.
+int32_t IRQ_SetPending (IRQn_ID_t irqn);
+
+/// Get interrupt pending flag.
+/// \param[in]     irqn          interrupt ID number
+/// \return 0 - interrupt is not pending, 1 - interrupt is pending.
+uint32_t IRQ_GetPending (IRQn_ID_t irqn);
+
+/// Clear interrupt pending flag.
+/// \param[in]     irqn          interrupt ID number
+/// \return 0 on success, -1 on error.
+int32_t IRQ_ClearPending (IRQn_ID_t irqn);
+
+/// Set interrupt priority value.
+/// \param[in]     irqn          interrupt ID number
+/// \param[in]     priority      interrupt priority value
+/// \return 0 on success, -1 on error.
+int32_t IRQ_SetPriority (IRQn_ID_t irqn, uint32_t priority);
+
+/// Get interrupt priority.
+/// \param[in]     irqn          interrupt ID number
+/// \return current interrupt priority value with optional IRQ_PRIORITY_ERROR bit set.
+uint32_t IRQ_GetPriority (IRQn_ID_t irqn);
+
+/// Set priority masking threshold.
+/// \param[in]     priority      priority masking threshold value
+/// \return 0 on success, -1 on error.
+int32_t IRQ_SetPriorityMask (uint32_t priority);
+
+/// Get priority masking threshold
+/// \return current priority masking threshold value with optional IRQ_PRIORITY_ERROR bit set.
+uint32_t IRQ_GetPriorityMask (void);
+
+/// Set priority grouping field split point
+/// \param[in]     bits          number of MSB bits included in the group priority field comparison
+/// \return 0 on success, -1 on error.
+int32_t IRQ_SetPriorityGroupBits (uint32_t bits);
+
+/// Get priority grouping field split point
+/// \return current number of MSB bits included in the group priority field comparison with
+///         optional IRQ_PRIORITY_ERROR bit set.
+uint32_t IRQ_GetPriorityGroupBits (void);
+
+#endif  // IRQ_CTRL_H_