Added two functions (2bf7fd6b) · Commits · Wenxi XU / Mambo

app @ 7434b9c6

Original line number	Diff line number	Diff line
		Subproject commit 09d40c672e3dc6ac2c79d42366fd535965d49ca0
		Subproject commit 7434b9c626270ecf61c028bfdfd25a737f4ab175

boards/dji/robomaster_board_c/robomaster_board_c.dts

+1 −10

Original line number	Diff line number	Diff line
		@@ -23,7 +23,6 @@
		zephyr,canbus = &can1;
		zephyr,ccm = &ccm0;
		ares,pwm = &heater;
		ares,bias = &gyro_bias;
		sbus,uart = &usart3;
		};

		@@ -63,9 +62,7 @@
		#controller-cells = <0>;
		compatible = "pid,single";
		k_p = "6000000";
		k_i = "5";
		k_d = "0.0000002";
		i_max = "100000000";
		k_d = "0.02";
		out_max = "20000000.000000";
		offset = "6750000.000000";
		};
		@@ -90,12 +87,6 @@
		can_device = <&can2>;
		};
		};

		gyro_bias: gyro_bias {
		compatible = "gyro-bias";
		status = "okay";
		gyro-bias = "-0.000330", "-0.000053", "0.004050";
		};
		};

		&clk_lsi {

include/ares/Kconfig

+6 −1

Original line number	Diff line number	Diff line
		@@ -15,3 +15,8 @@ config IMU_PWM_TEMP_CTRL
		bool "IMU_PWM_TEMP_CTRL"
		help
		Enable IMU PWM TEMP CTRL

		config AUTO_PROBE_GYRO_BIAS
		bool "AUTO_PROBE_GYRO_BIAS"
		help
		Enable AUTO PROBE GYRO BIAS
		No newline at end of file

include/ares/ekf/QuaternionEKF.c

+62 −8

Original line number	Diff line number	Diff line
		@@ -16,8 +16,11 @@
		*/
		#include "QuaternionEKF.h"
		#include "kalman_filter.h"
		#include <math.h>
		#include "kalman_filter.c"
		#include <zephyr/kernel.h>
		#include "algorithm.h"
		#include <string.h>

		#if DT_HAS_CHOSEN(zephyr_ccm)
		__ccm_bss_section QEKF_INS_t QEKF_INS;
		@@ -39,6 +42,35 @@ static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf);
		static void IMU_QuaternionEKF_SetH(KalmanFilter_t *kf);
		static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf);

		void CalcBias(float q, float accel, float g, float *bias)
		{
		// 理想情况下，静止时加速度计受力只有重力
		// 将重力从地面坐标系(0,0,-g)旋转到设备坐标系得到理想值
		float R[3][3];
		R[0][0] = 1 - 2 * (q[2] * q[2] + q[3] * q[3]);
		R[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]);
		R[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]);

		R[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
		R[1][1] = 1 - 2 * (q[1] * q[1] + q[3] * q[3]);
		R[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]);

		R[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]);
		R[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]);
		R[2][2] = 1 - 2 * (q[1] * q[1] + q[2] * q[2]);

		// 计算理想的加速度计读数
		float ideal_accel[3];
		ideal_accel[0] = g * R[2][0]; // 重力在设备x轴的分量
		ideal_accel[1] = g * R[2][1]; // 重力在设备y轴的分量
		ideal_accel[2] = g * R[2][2]; // 重力在设备z轴的分量

		// 零飘 = 实际读数 - 理想值
		bias[0] = accel[0] - ideal_accel[0];
		bias[1] = accel[1] - ideal_accel[1];
		bias[2] = accel[2] - ideal_accel[2];
		}

		/**
		* @brief Quaternion EKF initialization and some reference value
		* @param[in] process_noise1 quaternion process noise 10
		@@ -57,7 +89,6 @@ void IMU_QuaternionEKF_Init(float *init_quaternion, float process_noise1, float
		QEKF_INS.ChiSquareTestThreshold = 1e-8;
		QEKF_INS.ConvergeFlag = 0;
		QEKF_INS.ErrorCount = 0;
		QEKF_INS.UpdateCount = 0;
		if (lambda > 1) {
		lambda = 1;
		}
		@@ -141,7 +172,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		QEKF_INS.IMU_QuaternionEKF.F_data[20] = -halfgxdt;

		// accel low pass filter,加速度过一下低通滤波平滑数据,降低撞击和异常的影响
		if (QEKF_INS.UpdateCount == 0) // 如果是第一次进入,需要初始化低通滤波
		if (QEKF_INS.UpdateCount == 200) // 如果是第一次进入,需要初始化低通滤波
		{
		QEKF_INS.Accel[0] = ax;
		QEKF_INS.Accel[1] = ay;
		@@ -149,13 +180,13 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		}
		QEKF_INS.Accel[0] =
		QEKF_INS.Accel[0] * QEKF_INS.accLPFcoef / (accel_dt + QEKF_INS.accLPFcoef) +
		ax * accel_dt / (accel_dt + QEKF_INS.accLPFcoef);
		(ax - QEKF_INS.AccelBias[0]) * 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 - QEKF_INS.AccelBias[1]) * accel_dt / (QEKF_INS.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);
		(az - QEKF_INS.AccelBias[2]) * accel_dt / (accel_dt + QEKF_INS.accLPFcoef);

		// set z,单位化重力加速度向量
		accelInvNorm = invSqrt(QEKF_INS.Accel[0] * QEKF_INS.Accel[0] +
		@@ -174,8 +205,8 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,

		// 如果角速度小于阈值且加速度处于设定范围内,认为运动稳定,加速度可以用于修正角速度
		// 稍后在最后的姿态更新部分会利用StableFlag来确定
		if (QEKF_INS.gyro_norm < 0.3f && QEKF_INS.accl_norm > 9.8f - 0.5f &&
		QEKF_INS.accl_norm < 9.8f + 0.5f) {
		float acc[3] = {ax, ay, az};
		if (QEKF_INS.gyro_norm < 0.002f && fabsf(NormOf3d(acc) - 9.8f) < 0.25f) {
		QEKF_INS.StableFlag = 1;
		} else {
		QEKF_INS.StableFlag = 0;
		@@ -200,7 +231,30 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay,
		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)
		#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

		#if CONFIG_AUTO_PROBE_GYRO_BIAS
		if (QEKF_INS.StableFlag) {
		// We update gyro bias with a low pass filter
		QEKF_INS.GyroBias[0] = QEKF_INS.GyroBias[0] * 0.9992f + gx * 0.0008f;
		QEKF_INS.GyroBias[1] = QEKF_INS.GyroBias[1] * 0.9992f + gy * 0.0008f;
		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;
		#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;
		}
		#endif // CONFIG_AUTO_PROBE_GYRO_BIAS

		#if !DT_HAS_CHOSEN(ares_bias) && !CONFIG_AUTO_PROBE_GYRO_BIAS
		QEKF_INS.GyroBias[0] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[4];
		QEKF_INS.GyroBias[1] = QEKF_INS.IMU_QuaternionEKF.FilteredValue[5];
		QEKF_INS.GyroBias[2] = 0; // 大部分时候z轴通天,无法观测yaw的漂移

include/ares/ekf/QuaternionEKF.h

+8 −2

Original line number	Diff line number	Diff line
		@@ -24,6 +24,8 @@
		#define FALSE 0 /*< boolean fails /
		#endif

		#define PHY_G 9.8161f

		typedef struct {
		uint8_t Initialized;
		KalmanFilter_t IMU_QuaternionEKF;
		@@ -34,6 +36,9 @@ typedef struct {

		float q[4]; // 四元数估计值
		float GyroBias[3]; // 陀螺仪零偏估计值
		float AccelBias[3]; // 加速度计零偏估计值

		float g; // 重力加速度

		float Gyro[3];
		float Accel[3];
		@@ -76,6 +81,7 @@ void IMU_QuaternionEKF_Init(float *init_quaternion, float process_noise1, float
		void IMU_QuaternionEKF_UpdateIMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax,
		float ay, float az, float accel_dt,
		float gyro_dt);
		void CalcBias(float q, float accel, float g, float *bias);

		#define default_EKF_F \
		{1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, \

Admin message