添加imu标定存储 (8c508245) · Commits · Wenxi XU / Mambo

boards/damiao/dm_mc02/dm_mc02_defconfig

+21 −7

Original line number	Diff line number	Diff line
		@@ -5,10 +5,18 @@
		CONFIG_ARM_MPU=n

		# LOG CONFIG
		CONFIG_LOG=y
		# Enable UART console
		CONFIG_SERIAL=y
		CONFIG_UART_ASYNC_API=y
		CONFIG_UART_INTERRUPT_DRIVEN=y

		# 启用 Shell 支持
		CONFIG_SHELL=n
		CONFIG_SHELL_BACKENDS=n
		CONFIG_SHELL_BACKEND_SERIAL=n

		# 启用 UART 控制台
		CONFIG_UART_CONSOLE=y
		CONFIG_CONSOLE=y
		CONFIG_STDOUT_CONSOLE=y
		CONFIG_CBPRINTF_FP_SUPPORT=y

		# Enable HW stack protection
		@@ -32,11 +40,13 @@ CONFIG_SPI=y
		CONFIG_DMA=y

		CONFIG_USB_DEVICE_STACK=y
		CONFIG_USB_DEVICE_PRODUCT="Zephyr CDC ACM sample"
		CONFIG_USB_DEVICE_MANUFACTURER="ARES"
		CONFIG_USB_DEVICE_PRODUCT="ARES USB Transfer Device"
		CONFIG_USB_DEVICE_SN="520131499992025A"
		CONFIG_USB_DEVICE_VID=0x1209
		CONFIG_USB_DEVICE_PID=0x0001
		CONFIG_USB_CDC_ACM=y

		CONFIG_USB_DEVICE_INITIALIZE_AT_BOOT=y
		CONFIG_USB_DEVICE_INITIALIZE_AT_BOOT=n

		# CONFIG_MEM_ATTR=y
		# CONFIG_MEM_ATTR_HEAP=y
		@@ -74,6 +84,7 @@ CONFIG_BMI08X_I2C_WRITE_BURST_SIZE=16

		CONFIG_IMU_PWM_TEMP_CTRL=y
		CONFIG_PWM=y
		CONFIG_PID=y

		CONFIG_CRC=y

		@@ -81,3 +92,6 @@ CONFIG_CMSIS_DSP=y
		CONFIG_CMSIS_DSP_MATRIX=y
		CONFIG_CMSIS_DSP_CONTROLLER=y
		CONFIG_CMSIS_DSP_FASTMATH=y

		CONFIG_ISR_STACK_SIZE=3200
		CONFIG_MAIN_STACK_SIZE=3200
		No newline at end of file

include/ares/CMakeLists.txt

+2 −0

Original line number	Diff line number	Diff line
		@@ -3,4 +3,6 @@ add_subdirectory_ifdef(CONFIG_VOFA_LIB vofa)
		add_subdirectory_ifdef(CONFIG_MASTER_BOARD interboard)
		add_subdirectory_ifdef(CONFIG_SLAVE_BOARD interboard)
		add_subdirectory_ifdef(CONFIG_UART_TRANS_LIB connectivity)
		add_subdirectory_ifdef(CONFIG_EXPERIMENTAL_BULK_LIB usb_bulk_trans)
		add_subdirectory_ifdef(CONFIG_USBD_BULK usbd_bulk_trans)
		add_subdirectory(board)
		No newline at end of file

include/ares/Kconfig

+8 −2

Original line number	Diff line number	Diff line
		@@ -4,6 +4,7 @@
		# SPDX-License-Identifier: Apache-2.0

		rsource "connectivity/Kconfig"
		rsource "usbd_bulk_trans/Kconfig"

		menuconfig ARES
		bool "ARES options"
		@@ -14,10 +15,10 @@ module = ARES
		module-str = ARES

		config IMU_PWM_TEMP_CTRL
		select PID
		bool "IMU_PWM_TEMP_CTRL"
		help
		Enable IMU PWM TEMP CTRL
		select PID

		config AUTO_PROBE_GYRO_BIAS
		bool "AUTO_PROBE_GYRO_BIAS"
		@@ -50,3 +51,8 @@ config UART_TRANS_LIB
		bool "UART_TRANS_LIB"
		help
		Enable UART_TRANS_LIB

		config EXPERIMENTAL_BULK_LIB
		bool "EXPERIMENTAL_BULK_LIB"
		help
		Enable EXPERIMENTAL_BULK_LIB
		No newline at end of file

include/ares/ekf/CMakeLists.txt

+2 −2

Original line number	Diff line number	Diff line
		@@ -3,8 +3,8 @@

		zephyr_library()

		set(EKF_SOURCES
		imu_task.c
		file(GLOB EKF_SOURCES
		"${CMAKE_CURRENT_SOURCE_DIR}/*.c"
		)

		zephyr_library_sources(${EKF_SOURCES})
		No newline at end of file

include/ares/ekf/QuaternionEKF.c

+19 −20

Original line number	Diff line number	Diff line
		@@ -17,7 +17,6 @@
		#include "QuaternionEKF.h"
		#include "kalman_filter.h"
		#include <math.h>
		#include "kalman_filter.c"
		#include <zephyr/kernel.h>
		#include "algorithm.h"
		#include <string.h>
		@@ -27,6 +26,11 @@ __ccm_bss_section QEKF_INS_t QEKF_INS;
		__ccm_data_section float IMU_QuaternionEKF_P[16] = default_EKF_P;
		__ccm_bss_section float IMU_QuaternionEKF_K[12];
		__ccm_bss_section float IMU_QuaternionEKF_H[12];
		#elif DT_HAS_CHOSEN(zephyr_dtcm)
		__dtcm_bss_section QEKF_INS_t QEKF_INS;
		__dtcm_data_section float IMU_QuaternionEKF_P[16] = default_EKF_P;
		__dtcm_bss_section float IMU_QuaternionEKF_K[12];
		__dtcm_bss_section float IMU_QuaternionEKF_H[12];
		#else
		QEKF_INS_t QEKF_INS;
		float IMU_QuaternionEKF_P[16] = default_EKF_P;
		@@ -108,21 +112,22 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		12* 13* 14* 15
		*/
		QEKF_INS.dt = gyro_dt;
		QEKF_INS.accel_dt = accel_dt;

		if (!QEKF_INS.hasStoredBias) {
		gx -= QEKF_INS.GyroBias[0];
		gy -= QEKF_INS.GyroBias[1];
		gz -= QEKF_INS.GyroBias[2];
		}

		if (QEKF_INS.UpdateCount == 0) {
		QEKF_INS.Gyro[0] = gx;
		QEKF_INS.Gyro[1] = gy;
		QEKF_INS.Gyro[2] = gz;
		}

		QEKF_INS.Gyro[0] = gx - QEKF_INS.GyroBias[0];
		QEKF_INS.Gyro[1] = gy - QEKF_INS.GyroBias[1];
		QEKF_INS.Gyro[2] = gz - QEKF_INS.GyroBias[2];

		// set F
		halfgxdt = 0.5f * QEKF_INS.Gyro[0] * gyro_dt;
		halfgydt = 0.5f * QEKF_INS.Gyro[1] * gyro_dt;
		halfgzdt = 0.5f * QEKF_INS.Gyro[2] * gyro_dt;
		halfgxdt = 0.5f * gx * gyro_dt;
		halfgydt = 0.5f * gy * gyro_dt;
		halfgzdt = 0.5f * gz * gyro_dt;

		// 此部分设定状态转移矩阵F的左上角部分
		// 4x4子矩阵,即0.5(Ohm-Ohm^bias)*deltaT,右下角有一个2x2单位阵已经初始化好了
		@@ -157,7 +162,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		ax * accel_dt / (accel_dt + QEKF_INS.accLPFcoef);
		QEKF_INS.Accel[1] =
		QEKF_INS.Accel[1] * QEKF_INS.accLPFcoef / (accel_dt + QEKF_INS.accLPFcoef) +
		ay * accel_dt / (QEKF_INS.dt + QEKF_INS.accLPFcoef);
		ay * accel_dt / (accel_dt + QEKF_INS.accLPFcoef);
		QEKF_INS.Accel[2] =
		QEKF_INS.Accel[2] * QEKF_INS.accLPFcoef / (accel_dt + QEKF_INS.accLPFcoef) +
		az * accel_dt / (accel_dt + QEKF_INS.accLPFcoef);
		@@ -172,9 +177,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		}

		// get body state
		QEKF_INS.gyro_norm = 1.0f / invSqrt(QEKF_INS.Gyro[0] * QEKF_INS.Gyro[0] +
		QEKF_INS.Gyro[1] * QEKF_INS.Gyro[1] +
		QEKF_INS.Gyro[2] * QEKF_INS.Gyro[2]);
		QEKF_INS.gyro_norm = 1.0f / invSqrt(gx * gx + gy * gy + gz * gz);
		QEKF_INS.accl_norm = 1.0f / accelInvNorm;

		// 如果角速度小于阈值且加速度处于设定范围内,认为运动稳定,加速度可以用于修正角速度
		@@ -203,7 +206,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		#endif // DT_HAS_CHOSEN(ares_bias) && CONFIG_AUTO_PROBE_BIAS

		#if CONFIG_AUTO_PROBE_GYRO_BIAS
		if (BiasFlag) {
		if (BiasFlag && !QEKF_INS.hasStoredBias) {
		// We update gyro bias with a low pass filter
		if (QEKF_INS.UpdateCount == 0) {
		QEKF_INS.GyroBias[0] = gx;
		@@ -218,10 +221,6 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		#else
		QEKF_INS.g = PHY_G;
		#endif // PHY_G
		// CalcBias(QEKF_INS.q, acc, QEKF_INS.g, bias);
		// QEKF_INS.AccelBias[0] = QEKF_INS.AccelBias[0] * 0.992f + bias[0] * 0.008f;
		// QEKF_INS.AccelBias[1] = QEKF_INS.AccelBias[1] * 0.992f + bias[1] * 0.008f;
		// QEKF_INS.AccelBias[2] = QEKF_INS.AccelBias[2] * 0.992f + bias[2] * 0.008f;
		}
		#endif // CONFIG_AUTO_PROBE_GYRO_BIAS

Admin message