123

.mxproject

@@ -6,6 +6,11 @@ SourceFiles=..\Core\Src\main.c;..\Core\Src\gpio.c;..\Core\Src\freertos.c;..\Core
 HeaderPath=..\Drivers\STM32F4xx_HAL_Driver\Inc;..\Drivers\STM32F4xx_HAL_Driver\Inc\Legacy;..\Middlewares\Third_Party\FreeRTOS\Source\include;..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2;..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F;..\Drivers\CMSIS\Device\ST\STM32F4xx\Include;..\Drivers\CMSIS\Include;..\Core\Inc;
 CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER;
 
+[PreviousUsedCMakes]
+SourceFiles=Core\Src\main.c;Core\Src\gpio.c;Core\Src\freertos.c;Core\Src\dma.c;Core\Src\i2c.c;Core\Src\spi.c;Core\Src\tim.c;Core\Src\usart.c;Core\Src\stm32f4xx_it.c;Core\Src\stm32f4xx_hal_msp.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_spi.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_spi.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;;;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;
+HeaderPath=Drivers\STM32F4xx_HAL_Driver\Inc;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy;Middlewares\Third_Party\FreeRTOS\Source\include;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F;Drivers\CMSIS\Device\ST\STM32F4xx\Include;Drivers\CMSIS\Include;Core\Inc;
+CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER;
+
 [PreviousGenFiles]
 AdvancedFolderStructure=true
 HeaderFileListSize=10
@@ -37,8 +42,3 @@ SourceFolderListSize=1
 SourcePath#0=..\Core\Src
 SourceFiles=;
 
-[PreviousUsedCMakes]
-SourceFiles=Core\Src\main.c;Core\Src\gpio.c;Core\Src\freertos.c;Core\Src\dma.c;Core\Src\i2c.c;Core\Src\spi.c;Core\Src\tim.c;Core\Src\usart.c;Core\Src\stm32f4xx_it.c;Core\Src\stm32f4xx_hal_msp.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_spi.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_i2c_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_spi.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;;;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;
-HeaderPath=Drivers\STM32F4xx_HAL_Driver\Inc;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy;Middlewares\Third_Party\FreeRTOS\Source\include;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F;Drivers\CMSIS\Device\ST\STM32F4xx\Include;Drivers\CMSIS\Include;Core\Inc;
-CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER;
-

Core/Src/tim.c

@@ -109,6 +109,7 @@ void MX_TIM4_Init(void)
 
   /* USER CODE END TIM4_Init 0 */
 
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
   TIM_MasterConfigTypeDef sMasterConfig = {0};
   TIM_OC_InitTypeDef sConfigOC = {0};
 
@@ -121,6 +122,15 @@ void MX_TIM4_Init(void)
   htim4.Init.Period = 65535;
   htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
   if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)
   {
     Error_Handler();
@@ -153,6 +163,7 @@ void MX_TIM5_Init(void)
 
   /* USER CODE END TIM5_Init 0 */
 
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
   TIM_MasterConfigTypeDef sMasterConfig = {0};
   TIM_OC_InitTypeDef sConfigOC = {0};
 
@@ -165,6 +176,15 @@ void MX_TIM5_Init(void)
   htim5.Init.Period = 65535;
   htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim5) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
   if (HAL_TIM_PWM_Init(&htim5) != HAL_OK)
   {
     Error_Handler();
@@ -253,7 +273,12 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* tim_pwmHandle)
 
   /* USER CODE END TIM1_MspInit 1 */
   }
-  else if(tim_pwmHandle->Instance==TIM4)
+}
+
+void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
+{
+
+  if(tim_baseHandle->Instance==TIM4)
   {
   /* USER CODE BEGIN TIM4_MspInit 0 */
 
@@ -264,7 +289,7 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* tim_pwmHandle)
 
   /* USER CODE END TIM4_MspInit 1 */
   }
-  else if(tim_pwmHandle->Instance==TIM5)
+  else if(tim_baseHandle->Instance==TIM5)
   {
   /* USER CODE BEGIN TIM5_MspInit 0 */
 
@@ -275,12 +300,7 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* tim_pwmHandle)
 
   /* USER CODE END TIM5_MspInit 1 */
   }
-}
+  else if(tim_baseHandle->Instance==TIM10)
-
-void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
-{
-
-  if(tim_baseHandle->Instance==TIM10)
   {
   /* USER CODE BEGIN TIM10_MspInit 0 */
 
@@ -401,7 +421,12 @@ void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* tim_pwmHandle)
 
   /* USER CODE END TIM1_MspDeInit 1 */
   }
-  else if(tim_pwmHandle->Instance==TIM4)
+}
+
+void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
+{
+
+  if(tim_baseHandle->Instance==TIM4)
   {
   /* USER CODE BEGIN TIM4_MspDeInit 0 */
 
@@ -412,7 +437,7 @@ void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* tim_pwmHandle)
 
   /* USER CODE END TIM4_MspDeInit 1 */
   }
-  else if(tim_pwmHandle->Instance==TIM5)
+  else if(tim_baseHandle->Instance==TIM5)
   {
   /* USER CODE BEGIN TIM5_MspDeInit 0 */
 
@@ -423,12 +448,7 @@ void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* tim_pwmHandle)
 
   /* USER CODE END TIM5_MspDeInit 1 */
   }
-}
+  else if(tim_baseHandle->Instance==TIM10)
-
-void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
-{
-
-  if(tim_baseHandle->Instance==TIM10)
   {
   /* USER CODE BEGIN TIM10_MspDeInit 0 */
 

DveC.ioc

@@ -122,14 +122,16 @@ Mcu.Pin29=PB0
 Mcu.Pin3=PA14
 Mcu.Pin30=VP_FREERTOS_VS_CMSIS_V2
 Mcu.Pin31=VP_SYS_VS_Systick
-Mcu.Pin32=VP_TIM10_VS_ClockSourceINT
+Mcu.Pin32=VP_TIM4_VS_ClockSourceINT
+Mcu.Pin33=VP_TIM5_VS_ClockSourceINT
+Mcu.Pin34=VP_TIM10_VS_ClockSourceINT
 Mcu.Pin4=PA13
 Mcu.Pin5=PC11
 Mcu.Pin6=PC10
 Mcu.Pin7=PG9
 Mcu.Pin8=PF0
 Mcu.Pin9=PC9
-Mcu.PinsNb=33
+Mcu.PinsNb=35
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F407IGHx
@@ -409,5 +411,9 @@ VP_SYS_VS_Systick.Mode=SysTick
 VP_SYS_VS_Systick.Signal=SYS_VS_Systick
 VP_TIM10_VS_ClockSourceINT.Mode=Enable_Timer
 VP_TIM10_VS_ClockSourceINT.Signal=TIM10_VS_ClockSourceINT
+VP_TIM4_VS_ClockSourceINT.Mode=Internal
+VP_TIM4_VS_ClockSourceINT.Signal=TIM4_VS_ClockSourceINT
+VP_TIM5_VS_ClockSourceINT.Mode=Internal
+VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT
 board=custom
 rtos.0.ip=FREERTOS

MDK-ARM/DveC.uvoptx

@@ -271,7 +271,7 @@
 
   <Group>
     <GroupName>Application/User/Core</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1191,7 +1191,7 @@
 
   <Group>
     <GroupName>User/component</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1319,7 +1319,7 @@
 
   <Group>
     <GroupName>User/module</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1423,7 +1423,7 @@
 
   <Group>
     <GroupName>User/task</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>

User/module/HeightEstimation.c

@@ -72,8 +72,8 @@ int8_t HeightEstimation_Init(float freq)
 {
     SPL06_Init();
 	BSP_TIME_Delay(100);
-    LowPassFilter2p_Init(&accl_z_filter, freq, 10.0f);
+    LowPassFilter2p_Init(&accl_z_filter, freq, 30.0f);
-	LowPassFilter2p_Init(&relative_height_filter, freq, 5.0f);
+	LowPassFilter2p_Init(&relative_height_filter, freq, 20.0f);
     AltKalman_Init(&alt_kf);
 	
 	while(initializeCounter<INITIALIZE_SAMPLE_COUNT)

User/module/cmd.c

@@ -0,0 +1,85 @@
+/*
+  控制命令
+*/
+#include "module/cmd.h"
+#include <string.h>
+
+/*************************************************************************/
+/*********************************仲裁器**********************************/
+/*************************************************************************/
+
+
+
+
+/*************************************************************************/
+/**********************************RC*************************************/
+/*************************************************************************/
+
+/* Includes ----------------------------------------------------------------- */
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+#define RC_SELECT_Index 1
+
+/* 扩展接口 */
+#if RC_SELECT_Index == 0
+#define FOR_EACH_RC(_) _(dr16, DR16)
+#elif RC_SELECT_Index == 1
+#define FOR_EACH_RC(_) _(at9s, AT9S)
+#endif
+
+#if RC_SELECT_Index == 0
+#include "device/dr16.h"
+#elif RC_SELECT_Index == 1
+#include "device/at9s_pro.h"
+#endif
+
+#define X_FIELD(name, NAME) DEVICE_##NAME##_t name;
+#define X_EXTERN(name, NAME) extern DEVICE_##NAME##_t name##_out;
+#define X_COPY(name, NAME) \
+    static void copy_##name(rc_u *dst) { dst->name = name##_out; }
+#define X_REF(name, NAME) copy_##name,
+
+union rc_u{
+    FOR_EACH_RC(X_FIELD)
+};
+FOR_EACH_RC(X_EXTERN)
+FOR_EACH_RC(X_COPY)
+
+/*静态缓冲区，供返回使用*/
+static rc_u rc_buffer;
+
+CMD_RCInputData_t rc;
+
+int8_t Cmd_get_rc(CMD_RCInputData_t *dst)
+{
+    FOR_EACH_RC(X_REF)(&rc_buffer); 
+    dst->rc = &rc_buffer;                     
+    dst->rc_type = RC_SELECT_Index;
+    return CMD_OK;
+}
+
+/*************************************************************************/
+/**********************************PC*************************************/
+/*************************************************************************/
+
+
+
+/*************************************************************************/
+/**********************************NUC*************************************/
+/*************************************************************************/
+
+
+
+/*************************************************************************/
+/**********************************REF*************************************/
+/*************************************************************************/
+
+
+/*************************************************************************/
+/*********************************分发命令*********************************/
+/*************************************************************************/

User/module/config.c

@@ -23,13 +23,13 @@ Config_RobotParam_t robot_config = {
             .att_omg=1000
         },
 
-        .baseThrottle=0.3,
+        .baseThrottle=0.25,
 
         .pid = {
             .pit_agl = {
                 .k = 1.0f,
-                .p = 10.0f,
+                .p = 6.5f,
-                .i = 3.0f,
+                .i = 1.5f,
                 .d = 0.0f,
                 .i_limit = 0.1f,
                 .out_limit = 0.1745f,
@@ -38,8 +38,8 @@ Config_RobotParam_t robot_config = {
             },
             .rol_agl = {
                 .k = 1.0f,
-                .p = 10.0f,
+                .p = 6.5f,
-                .i = 3.0f,
+                .i = 1.5f,
                 .d = 0.0f,
                 .i_limit = 0.1f,
                 .out_limit = 0.1745f,
@@ -48,8 +48,8 @@ Config_RobotParam_t robot_config = {
             },
             .yaw_agl = {
                 .k = 1.0f,
-                .p = 10.0f,
+                .p = 6.5f,
-                .i = 3.0f,
+                .i = 1.5f,
                 .d = 0.0f,
                 .i_limit = 0.1f,
                 .out_limit = 0.1745f,
@@ -97,13 +97,13 @@ Config_RobotParam_t robot_config = {
                 .range = 0.0f
             },
 			.alt_vel = {
-                .k = 0.08f,
+                .k = 0.2f,
-                .p = 2.2f, 
+                .p = 4.0f, 
-                .i = 0.0f,          
+                .i = 0.3f,          
-                .d = 0.01f,          
+                .d = 0.02f,          
-                .i_limit = 0.0f,
+                .i_limit = 0.2f,
                 .out_limit = 0.4f,   
-                .d_cutoff_freq = 30.0f,
+                .d_cutoff_freq = 20.0f,
                 .range = 0.0f
             },
 			

User/module/xm_quadctrl.c

@@ -197,7 +197,9 @@ int8_t Quad_UpdateStatus(Quad_t *q) {
 
 int8_t Quad_SetTargetStates(Quad_t *q, Quad_CMD_t *cmd) {
 
-    q->expect_status.vel     = cmd->throttle;
+//    q->expect_status.vel     = 0.1*cmd->throttle;
+    q->output.throttle=q->param->baseThrottle
+                        +0.5*cmd->throttle;
     q->expect_status.yaw_omg = cmd->yaw;
     q->expect_status.rol_agl = cmd->rol+q->calibration_status.eulr.rol;
     q->expect_status.pit_agl = cmd->pit+q->calibration_status.eulr.pit;
@@ -222,15 +224,15 @@ int8_t Quad_PosCtrl(Quad_t *q,float dt) {
 //                                    q->current_status.alt, 
 //                                    0, 
 //                                    dt);
-    q->expect_status.thrust=PID_Calc(&q->pid.alt_vel, 
+//  q->expect_status.thrust=PID_Calc(&q->pid.alt_vel, 
-                                    q->expect_status.vel,              
+//                                  q->expect_status.vel,              
-                                    q->current_status.vel_z, 
+//                                  q->current_status.vel_z, 
-                                    0, 
+//                                  0, 
-                                    dt);
+//                                  dt);
-    Quad_Math_LimitFloat(&q->expect_status.thrust, -1.0f, 1.0f);
+//  Quad_Math_LimitFloat(&q->expect_status.thrust, -1.0f, 1.0f);
-    q->output.throttle=q->param->baseThrottle
+//  q->output.throttle=q->param->baseThrottle
-                        +q->expect_status.thrust;
+//                      +q->expect_status.thrust;
-    return 0; 
+  return 0; 
 }
 //姿态控制
 int8_t Quad_AttCtrl_agl(Quad_t *q, float dt) {

ozone/quadcopterDveC.jdebug

@@ -0,0 +1,344 @@
+/*********************************************************************
+*                 (c) SEGGER Microcontroller GmbH                    *
+*                      The Embedded Experts                          *
+*                         www.segger.com                             *
+**********************************************************************
+
+File          : 
+Created       : 29. Oct 2025 12:49
+Ozone Version : V3.40b
+*/
+
+/*********************************************************************
+*
+*       OnProjectLoad
+*
+* Function description
+*   Project load routine. Required.
+*
+**********************************************************************
+*/
+void OnProjectLoad (void) {
+  //
+  // Dialog-generated settings
+  //
+  Project.AddPathSubstitute ("D:/CUBEMX/Quadcopter/DveC/ozone", "$(ProjectDir)");
+  Project.AddPathSubstitute ("d:/cubemx/quadcopter/dvec/ozone", "$(ProjectDir)");
+  Project.SetDevice ("STM32F407IG");
+  Project.SetHostIF ("USB", "20750720");
+  Project.SetTargetIF ("SWD");
+  Project.SetTIFSpeed ("4 MHz");
+  Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
+  //
+  // User settings
+  //
+  File.Open ("D:/CUBEMX/Quadcopter/DveC/build/Debug/DveC.elf");
+}
+
+/*********************************************************************
+*
+*       OnStartupComplete
+*
+* Function description
+*   Called when program execution has reached/passed
+*   the startup completion point. Optional.
+*
+**********************************************************************
+*/
+//void OnStartupComplete (void) {
+//}
+
+/*********************************************************************
+*
+*      TargetReset
+*
+* Function description
+*   Replaces the default target device reset routine. Optional.
+*
+* Notes
+*   This example demonstrates the usage when
+*   debugging an application in RAM on a Cortex-M target device.
+*
+**********************************************************************
+*/
+//void TargetReset (void) {
+//
+//  unsigned int SP;
+//  unsigned int PC;
+//  unsigned int VectorTableAddr;
+//
+//  VectorTableAddr = Elf.GetBaseAddr();
+//  //
+//  // Set up initial stack pointer
+//  //
+//  if (VectorTableAddr != 0xFFFFFFFF) {
+//    SP = Target.ReadU32(VectorTableAddr);
+//    Target.SetReg("SP", SP);
+//  }
+//  //
+//  // Set up entry point PC
+//  //
+//  PC = Elf.GetEntryPointPC();
+//
+//  if (PC != 0xFFFFFFFF) {
+//    Target.SetReg("PC", PC);
+//  } else if (VectorTableAddr != 0xFFFFFFFF) {
+//    PC = Target.ReadU32(VectorTableAddr + 4);
+//    Target.SetReg("PC", PC);
+//  } else {
+//    Util.Error("Project file error: failed to set entry point PC", 1);
+//  }
+//}
+
+/*********************************************************************
+*
+*       BeforeTargetReset
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetReset (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterTargetReset
+*
+* Function description
+*   Event handler routine. Optional.
+*   The default implementation initializes SP and PC to reset values.
+**
+**********************************************************************
+*/
+void AfterTargetReset (void) {
+  _SetupTarget();
+}
+
+/*********************************************************************
+*
+*       DebugStart
+*
+* Function description
+*   Replaces the default debug session startup routine. Optional.
+*
+**********************************************************************
+*/
+//void DebugStart (void) {
+//}
+
+/*********************************************************************
+*
+*       TargetConnect
+*
+* Function description
+*   Replaces the default target IF connection routine. Optional.
+*
+**********************************************************************
+*/
+//void TargetConnect (void) {
+//}
+
+/*********************************************************************
+*
+*       BeforeTargetConnect
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetConnect (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterTargetConnect
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void AfterTargetConnect (void) {
+//}
+
+/*********************************************************************
+*
+*       TargetDownload
+*
+* Function description
+*   Replaces the default program download routine. Optional.
+*
+**********************************************************************
+*/
+//void TargetDownload (void) {
+//}
+
+/*********************************************************************
+*
+*       BeforeTargetDownload
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetDownload (void) {
+//}
+
+/*********************************************************************
+*
+*      AfterTargetDownload
+*
+* Function description
+*   Event handler routine. Optional.
+*   The default implementation initializes SP and PC to reset values.
+*
+**********************************************************************
+*/
+void AfterTargetDownload (void) {
+  _SetupTarget();
+}
+
+/*********************************************************************
+*
+*       BeforeTargetDisconnect
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetDisconnect (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterTargetDisconnect
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void AfterTargetDisconnect (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterTargetHalt
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void AfterTargetHalt (void) {
+//}
+
+/*********************************************************************
+*
+*       BeforeTargetResume
+*
+* Function description
+*   Event handler routine. Optional.
+*
+**********************************************************************
+*/
+//void BeforeTargetResume (void) {
+//}
+
+/*********************************************************************
+*
+*       OnSnapshotLoad
+*
+* Function description
+*   Called upon loading a snapshot. Optional.
+*
+* Additional information
+*   This function is used to restore the target state in cases
+*   where values cannot simply be written to the target.
+*   Typical use: GPIO clock needs to be enabled, before
+*   GPIO is configured.
+*
+**********************************************************************
+*/
+//void OnSnapshotLoad (void) {
+//}
+
+/*********************************************************************
+*
+*       OnSnapshotSave
+*
+* Function description
+*   Called upon saving a snapshot. Optional.
+*
+* Additional information
+*   This function is usually used to save values of the target
+*   state which can either not be trivially read,
+*   or need to be restored in a specific way or order.
+*   Typically use: Memory Mapped Registers,
+*   such as PLL and GPIO configuration.
+*
+**********************************************************************
+*/
+//void OnSnapshotSave (void) {
+//}
+
+/*********************************************************************
+*
+*       OnError
+*
+* Function description
+*   Called when an error ocurred. Optional.
+*
+**********************************************************************
+*/
+//void OnError (void) {
+//}
+
+/*********************************************************************
+*
+*       AfterProjectLoad
+*
+* Function description
+*   After Project load routine. Optional.
+*
+**********************************************************************
+*/
+//void AfterProjectLoad (void) {
+//}
+
+/*********************************************************************
+*
+*       OnDebugStartBreakSymbolReached
+*
+* Function description
+*   Called when program execution has reached/passed
+*   the symbol to be breaked at during debug start. Optional.
+*
+**********************************************************************
+*/
+//void OnDebugStartBreakSymReached (void) {
+//}
+
+/*********************************************************************
+*
+*       _SetupTarget
+*
+* Function description
+*   Setup the target.
+*   Called by AfterTargetReset() and AfterTargetDownload().
+*
+*   Auto-generated function. May be overridden by Ozone.
+*
+**********************************************************************
+*/
+void _SetupTarget(void) {
+  //
+  // this function is intentionally empty because both inital PC and
+  // initial SP were chosen not to be set
+  //
+}

ozone/quadcopterDveC.jdebug.user

@@ -0,0 +1,37 @@
+
+
+GraphedExpression="((q).current_status).vel_z", Color=#e56a6f
+GraphedExpression="((q).expect_status).vel", Color=#35792b
+GraphedExpression="((((q).pid).alt_vel).last).out", Color=#769dda, Show=0
+GraphedExpression="((((q).pid).alt_vel).out).pout", Color=#b14f0d, Show=0
+GraphedExpression="((((q).pid).alt_vel).out).iout", Color=#b3c38e, Show=0
+GraphedExpression="((((q).pid).alt_vel).out).dout", Color=#ab7b05, Show=0
+OpenDocument="time.c", FilePath="D:/CUBEMX/Quadcopter/DveC/User/bsp/time.c", Line=21
+OpenDocument="stm32f4xx_hal_gpio.c", FilePath="D:/CUBEMX/Quadcopter/DveC/Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c", Line=378
+OpenDocument="config.h", FilePath="D:/CUBEMX/Quadcopter/DveC/User/module/config.h", Line=0
+OpenDocument="mpu_wrappers.h", FilePath="D:/CUBEMX/Quadcopter/DveC/Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h", Line=0
+OpenDocument="quad_ctrl.c", FilePath="D:/CUBEMX/Quadcopter/DveC/User/task/quad_ctrl.c", Line=0
+OpenDocument="HeightEstimation.c", FilePath="D:/CUBEMX/Quadcopter/DveC/User/module/HeightEstimation.c", Line=0
+OpenDocument="startup_stm32f407xx.s", FilePath="D:/CUBEMX/Quadcopter/DveC/startup_stm32f407xx.s", Line=48
+OpenDocument="main.c", FilePath="D:/CUBEMX/Quadcopter/DveC/Core/Src/main.c", Line=48
+OpenToolbar="Debug", Floating=0, x=0, y=0
+OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=1, w=729, h=713, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1
+OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=729, h=205, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=1, w=729, h=713, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=301, h=919, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+OpenWindow="Data Sampling", DockArea=BOTTOM, x=1, y=0, w=841, h=536, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0
+OpenWindow="Timeline", DockArea=BOTTOM, x=0, y=0, w=1718, h=555, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="2 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="0;112", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="2040;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1990;0"
+OpenWindow="Console", DockArea=BOTTOM, x=1, y=0, w=841, h=536, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0
+SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1"
+TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[100;144;485]
+TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Source"], ColWidths=[1594;104;100;100;100]
+TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100]
+TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[]
+TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1014]
+TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh"], ColWidths=[250;282;91;100]
+TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";" ((q).current_status).vel_z";" ((q).expect_status).vel";" ((((q).pid).alt_vel).last).out";" ((((q).pid).alt_vel).out).pout";" ((((q).pid).alt_vel).out).iout";" ((((q).pid).alt_vel).out).dout"], ColWidths=[100;100;100;100;100;100;100;100]
+TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[253;100;144;114;114;144;110;126;126]
+TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;366]
+WatchedExpression="pressure", RefreshRate=5, Window=Watched Data 1
+WatchedExpression="q", RefreshRate=5, Window=Watched Data 1