Fixed CMake Issue in include directory, header files should be included now. (e0cc7aac) · Commits · Wenxi XU / Mambo

include/ares/board/CMakeLists.txt

+4 −1

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

		zephyr_library()

		file(GLOB INIT_SOURCES "*.c")
		set(INIT_SOURCES
		init.c
		)

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

include/ares/board/init.c

+1 −0

Original line number	Diff line number	Diff line
		@@ -11,6 +11,7 @@
		#include <zephyr/kernel/thread.h>
		#include <zephyr/logging/log.h>
		#include <zephyr/debug/thread_analyzer.h>
		#include "init.h"

		#ifdef CONFIG_BOARD_DM_MC02
		#define XT30_1_NODE DT_NODELABEL(power1)

include/ares/board/init.h

+14 −0

Original line number	Diff line number	Diff line
		// init.h
		#ifndef INIT_H
		#define INIT_H

		#include <zephyr/kernel.h>
		#include <zephyr/drivers/led.h>

		struct led_rgb;

		void led_set_rgb(struct led_rgb *color);
		void led_serivce_func(void p1, void p2, void *p3);
		void board_init(void);

		#endif /* INIT_H */
		No newline at end of file

include/ares/ekf/QuaternionEKF.c

+13 −13

Original line number	Diff line number	Diff line
		@@ -219,15 +219,6 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		QEKF_INS.IMU_QuaternionEKF.R_data[4] = QEKF_INS.R;
		QEKF_INS.IMU_QuaternionEKF.R_data[8] = QEKF_INS.R;

		// 调用kalman_filter.c封装好的函数,注意几个User_Funcx_f的调用
		Kalman_Filter_Update(&QEKF_INS.IMU_QuaternionEKF);

		// 获取融合后的数据,包括四元数和xy零飘值
		QEKF_INS.q[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[0];
		QEKF_INS.q[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[1];
		QEKF_INS.q[2] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[2];
		QEKF_INS.q[3] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[3];

		#if DT_HAS_CHOSEN(ares_bias) && CONFIG_AUTO_PROBE_GYRO_BIAS
		#error Do not use bias specified in dts and auto probe at the same time!!
		#endif // DT_HAS_CHOSEN(ares_bias) && CONFIG_AUTO_PROBE_BIAS
		@@ -240,14 +231,14 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		QEKF_INS.GyroBias[2] = QEKF_INS.GyroBias[2] * 0.9992f + gz * 0.0008f;
		float bias[3];
		#ifndef PHY_G
		QEKF_INS.g = QEKF_INS.g * 0.92f + NormOf3d(acc) * 0.08f;
		QEKF_INS.g = QEKF_INS.g * 0.992f + NormOf3d(acc) * 0.008f;
		#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.92f + bias[0] * 0.08f;
		QEKF_INS.AccelBias[1] = QEKF_INS.AccelBias[1] * 0.92f + bias[1] * 0.08f;
		QEKF_INS.AccelBias[2] = QEKF_INS.AccelBias[2] * 0.92f + bias[2] * 0.08f;
		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

		@@ -257,6 +248,15 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		QEKF_INS.GyroBias[2] = 0; // 大部分时候z轴通天,无法观测yaw的漂移
		#endif // DT_HAS_CHOSEN(ares_bias)

		// 调用kalman_filter.c封装好的函数,注意几个User_Funcx_f的调用
		Kalman_Filter_Update(&QEKF_INS.IMU_QuaternionEKF);

		// 获取融合后的数据,包括四元数和xy零飘值
		QEKF_INS.q[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[0];
		QEKF_INS.q[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[1];
		QEKF_INS.q[2] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[2];
		QEKF_INS.q[3] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[3];

		// 利用四元数反解欧拉角
		QEKF_INS.Yaw =
		atan2f(2.0f * (QEKF_INS.q[0] * QEKF_INS.q[3] + QEKF_INS.q[1] * QEKF_INS.q[2]),

include/ares/ekf/algorithm.h

+2 −0

Original line number	Diff line number	Diff line
		@@ -107,6 +107,8 @@ float Dot3d(float v1, float v2);

		float AverageFilter(float new_data, float *buf, uint8_t len);

		void GetGroundAccel(float q, float accel, float g, float *ground_accel);

		#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)

		#endif
		No newline at end of file

Admin message