Skip to content

Commit d13f75b

Browse files
Hom-WangHom-Wang
committed
Update UWBNode_Application_mahonyFilter 01/19
1 parent 7032ca5 commit d13f75b

32 files changed

+5876
-337
lines changed

‎Software/UWBNode_Application_mahonyFilter/Program/algorithms/mahonyFilter.c

Lines changed: 72 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -23,8 +23,14 @@
2323

2424
/* Private typedef -------------------------------------------------------------------------*/
2525
/* Private define --------------------------------------------------------------------------*/
26-
#define MF_KP 0.4f
27-
//#define MF_KI 0.001f
26+
#define MF_A_THR 0.4f
27+
#define MF_M_THR 0.1f
28+
29+
#define MF_KPA 0.4f
30+
//#define MF_KIA 0.001f
31+
32+
#define MF_KPM 0.01f
33+
//#define MF_KIM 0.001f
2834

2935
//#define MAHONY_FILTER_9DOF
3036

@@ -39,7 +45,7 @@
3945
* @param imu:
4046
* @retval None
4147
*/
42-
void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t sampleRate )
48+
void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t sampleTime )
4349
{
4450
mf->angE.pitch = 0.0f;
4551
mf->angE.roll = 0.0f;
@@ -49,7 +55,7 @@ void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t samp
4955

5056
mf->imu = imu;
5157

52-
mf->sampleRate = sampleRate;
58+
mf->sampleTime = sampleTime;
5359
}
5460

5561
/**
@@ -59,17 +65,20 @@ void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t samp
5965
*/
6066
void MahonyFilter_Update( MahonyFilter_t *mf )
6167
{
68+
float32_t err[3];
69+
6270
float32_t gVect[3];
6371
float32_t gyr[3], acc[3];
72+
#if defined(MF_KIA)
73+
static float32_t errIntA[3] = {0};
74+
#endif
6475

6576
#if defined(MAHONY_FILTER_9DOF)
6677
float32_t hVect[3], wVect[3];
6778
float32_t mag[3];
79+
#if defined(MF_KIM)
80+
static float32_t errIntM[3] = {0};
6881
#endif
69-
70-
float32_t err[3];
71-
#if defined(MF_KI)
72-
static float32_t errInt[3] = {0};
7382
#endif
7483

7584
gyr[0] = toRad(mf->imu->gyrData[0]);
@@ -80,30 +89,47 @@ void MahonyFilter_Update( MahonyFilter_t *mf )
8089
acc[2] = mf->imu->accData[2];
8190

8291
/* Normalise accelerometer measurement */
83-
const float32_t normAcc = invSqrtf(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
84-
acc[0] *= normAcc;
85-
acc[1] *= normAcc;
86-
acc[2] *= normAcc;
92+
const float32_t normAcc = sqrtf(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
93+
if ((normAcc < (mf->imu->accStrength * (1.0f + MF_A_THR))) && (normAcc > (mf->imu->accStrength * (1.0f - MF_A_THR)))) {
94+
acc[0] /= normAcc;
95+
acc[1] /= normAcc;
96+
acc[2] /= normAcc;
97+
98+
/* Estimated direction of gravity */
99+
gVect[0] = mf->numQ.rMat[0][2];
100+
gVect[1] = mf->numQ.rMat[1][2];
101+
gVect[2] = mf->numQ.rMat[2][2];
102+
103+
err[0] = acc[1] * gVect[2] - acc[2] * gVect[1];
104+
err[1] = acc[2] * gVect[0] - acc[0] * gVect[2];
105+
err[2] = acc[0] * gVect[1] - acc[1] * gVect[0];
106+
107+
#if defined(MF_KIA)
108+
/* Compute and apply integral feedback */
109+
errIntA[0] += MF_KIA * err[0];
110+
errIntA[1] += MF_KIA * err[1];
111+
errIntA[2] += MF_KIA * err[2];
112+
113+
/* Apply proportional feedback */
114+
gyr[0] += MF_KPA * err[0] + errIntA[0];
115+
gyr[1] += MF_KPA * err[1] + errIntA[1];
116+
gyr[2] += MF_KPA * err[2] + errIntA[2];
87117

88-
/* Estimated direction of gravity */
89-
gVect[0] = mf->numQ.rMat[0][2];
90-
gVect[1] = mf->numQ.rMat[1][2];
91-
gVect[2] = mf->numQ.rMat[2][2];
118+
#else
119+
gyr[0] += MF_KPA * err[0];
120+
gyr[1] += MF_KPA * err[1];
121+
gyr[2] += MF_KPA * err[2];
92122

93-
err[0] = acc[1] * gVect[2] - acc[2] * gVect[1];
94-
err[1] = acc[2] * gVect[0] - acc[0] * gVect[2];
95-
err[2] = acc[0] * gVect[1] - acc[1] * gVect[0];
123+
#endif
124+
}
96125

97126
#if defined(MAHONY_FILTER_9DOF)
98127
mag[0] = mf->imu->magData[0];
99128
mag[1] = mf->imu->magData[1];
100129
mag[2] = mf->imu->magData[2];
101-
102-
// const float32_t normMag = invSqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
103-
// if ((normMag < fusionAhrs->minMagneticFieldSquared) ||
104-
// (normMag > fusionAhrs->maxMagneticFieldSquared)) {
105-
// break;
106-
// }
130+
const float32_t normMag = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
131+
if ((normMag < (mf->imu->magStrength * (1.0f + MF_M_THR))) && (normMag > (mf->imu->magStrength * (1.0f - MF_M_THR)))) {
132+
}
107133

108134
/* Reference direction of Earth's magnetic field */
109135
wVect[0] = acc[1] * mag[2] - acc[2] * mag[1];
@@ -112,8 +138,8 @@ void MahonyFilter_Update( MahonyFilter_t *mf )
112138

113139
/* Normalise magnetometer measurement */
114140
const float32_t normVect = invSqrtf(wVect[0] * wVect[0] + wVect[1] * wVect[1] + wVect[2] * wVect[2]);
115-
wVect[2] *= normVect;
116-
wVect[2] *= normVect;
141+
wVect[0] *= normVect;
142+
wVect[1] *= normVect;
117143
wVect[2] *= normVect;
118144

119145
/* Estimated direction of Earth's magnetic field */
@@ -122,41 +148,35 @@ void MahonyFilter_Update( MahonyFilter_t *mf )
122148
hVect[2] = mf->numQ.rMat[2][1];
123149

124150
/* Error is sum of cross product between estimated direction and measured direction of field vectors */
125-
err[0] += wVect[1] * hVect[2] - wVect[2] * hVect[1];
126-
err[1] += wVect[2] * hVect[0] - wVect[0] * hVect[2];
127-
err[2] += wVect[0] * hVect[1] - wVect[1] * hVect[0];
128-
129-
#endif
151+
err[0] = wVect[1] * hVect[2] - wVect[2] * hVect[1];
152+
err[1] = wVect[2] * hVect[0] - wVect[0] * hVect[2];
153+
err[2] = wVect[0] * hVect[1] - wVect[1] * hVect[0];
130154

131-
#if defined(MF_KI)
132-
/* Compute and apply integral feedback */
133-
errInt[0] += (SAMPLE_RATE * MF_KI) * err[0];
134-
errInt[1] += (SAMPLE_RATE * MF_KI) * err[1];
135-
errInt[2] += (SAMPLE_RATE * MF_KI) * err[2];
155+
#if defined(MF_KIA)
156+
/* Compute and apply integral feedback */
157+
errIntA[0] += MF_KIM * err[0];
158+
errIntA[1] += MF_KIM * err[1];
159+
errIntA[2] += MF_KIM * err[2];
136160

137-
/* Apply proportional feedback */
138-
gyr[0] += MF_KP * err[0] + errInt[0];
139-
gyr[1] += MF_KP * err[1] + errInt[1];
140-
gyr[2] += MF_KP * err[2] + errInt[2];
161+
/* Apply proportional feedback */
162+
gyr[0] += MF_KPM * err[0] + errIntM[0];
163+
gyr[1] += MF_KPM * err[1] + errIntM[1];
164+
gyr[2] += MF_KPM * err[2] + errIntM[2];
141165

142166
#else
143-
gyr[0] += MF_KP * err[0];
144-
gyr[1] += MF_KP * err[1];
145-
gyr[2] += MF_KP * err[2];
167+
gyr[0] += MF_KPM * err[0];
168+
gyr[1] += MF_KPM * err[1];
169+
gyr[2] += MF_KPM * err[2];
170+
171+
#endif
146172

147173
#endif
148174

149175
/* Integrate rate of change of quaternion */
150-
// Quaternion_Conjugate(&mf->numQ, &mf->numQ);
151-
Quaternion_RungeKutta(&mf->numQ, gyr, mf->sampleRate * 0.5f);
176+
Quaternion_RungeKutta(&mf->numQ, gyr, mf->sampleTime * 0.5f);
152177
Quaternion_Normalize(&mf->numQ, &mf->numQ);
153178
Quaternion_UpdateRotMatrix(&mf->numQ);
154-
Quaternion_ToAngE(&mf->angE, &mf->numQ);
155-
156-
// // Get Euler Angle */
157-
// mf->angE.pitch = -toDeg(mf->angE.pitch);
158-
// mf->angE.roll = toDeg(mf->angE.roll);
159-
// mf->angE.yaw = -toDeg(mf->angE.yaw);
179+
// Quaternion_ToAngE(&mf->angE, &mf->numQ);
160180
}
161181

162182
/*************************************** END OF FILE ****************************************/

‎Software/UWBNode_Application_mahonyFilter/Program/algorithms/mahonyFilter.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -30,13 +30,13 @@
3030
typedef struct {
3131
quaternion_t numQ;
3232
eulerAngle_t angE;
33-
float32_t sampleRate;
33+
float32_t sampleTime;
3434
IMU_DataTypeDef *imu;
3535
} __attribute__((aligned)) MahonyFilter_t;
3636

3737
/* Exported constants ----------------------------------------------------------------------*/
3838
/* Exported functions ----------------------------------------------------------------------*/
39-
void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t sampleRate );
39+
void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t sampleTime );
4040
void MahonyFilter_Update( MahonyFilter_t *mf );
4141

4242
#ifdef __cplusplus

‎Software/UWBNode_Application_mahonyFilter/Program/algorithms/quaternion.c

Lines changed: 27 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -93,9 +93,9 @@ __INLINE void Quaternion_ToNumQ( quaternion_t *pNumQ, const eulerAngle_t *pAngE
9393
*/
9494
__INLINE void Quaternion_ToAngE( eulerAngle_t *pAngE, const quaternion_t *pNumQ )
9595
{
96-
pAngE->pitch = toDeg(-asinf(pNumQ->rMat[0][2]));
97-
pAngE->roll = toDeg(atan2f(pNumQ->rMat[1][2], pNumQ->rMat[2][2]));
98-
pAngE->yaw = toDeg(atan2f(pNumQ->rMat[0][1], pNumQ->rMat[0][0]));
96+
pAngE->pitch = toDeg(asinf(pNumQ->rMat[0][2]));
97+
pAngE->roll = toDeg(atan2f(-pNumQ->rMat[1][2], pNumQ->rMat[2][2]));
98+
pAngE->yaw = toDeg(atan2f(-pNumQ->rMat[0][1], pNumQ->rMat[0][0]));
9999
}
100100

101101
/**
@@ -158,13 +158,13 @@ __INLINE void Quaternion_Sub( quaternion_t *pNumQ, const quaternion_t *pNumA, co
158158
}
159159

160160
/**
161-
* @brief Quaternion_Multiply
161+
* @brief Quaternion_Mult
162162
* @param pNumQ:
163163
* @param pNumA:
164164
* @param pNumB:
165165
* @retval None
166166
*/
167-
__INLINE void Quaternion_Multiply( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB )
167+
__INLINE void Quaternion_Mult( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB )
168168
{
169169
if (pNumQ == pNumA) {
170170
quaternion_t tmpQ;
@@ -179,38 +179,38 @@ __INLINE void Quaternion_Multiply( quaternion_t *pNumQ, const quaternion_t *pNum
179179
}
180180
else {
181181
pNumQ->q0 = pNumA->q0 * pNumB->q0 - pNumA->q1 * pNumB->q1 - pNumA->q2 * pNumB->q2 - pNumA->q3 * pNumB->q3;
182-
pNumQ->q1 = pNumA->q1 * pNumB->q0 + pNumA->q0 * pNumB->q1 - pNumA->q3 * pNumB->q2 + pNumA->q2 * pNumB->q3;
183-
pNumQ->q2 = pNumA->q2 * pNumB->q0 + pNumA->q3 * pNumB->q1 + pNumA->q0 * pNumB->q2 - pNumA->q1 * pNumB->q3;
184-
pNumQ->q3 = pNumA->q3 * pNumB->q0 - pNumA->q2 * pNumB->q1 + pNumA->q1 * pNumB->q2 + pNumA->q0 * pNumB->q3;
182+
pNumQ->q1 = pNumA->q1 * pNumB->q0 + pNumA->q0 * pNumB->q1 + pNumA->q3 * pNumB->q2 - pNumA->q2 * pNumB->q3;
183+
pNumQ->q2 = pNumA->q2 * pNumB->q0 - pNumA->q3 * pNumB->q1 + pNumA->q0 * pNumB->q2 + pNumA->q1 * pNumB->q3;
184+
pNumQ->q3 = pNumA->q3 * pNumB->q0 + pNumA->q2 * pNumB->q1 - pNumA->q1 * pNumB->q2 + pNumA->q0 * pNumB->q3;
185185
}
186186
}
187187

188188
/**
189-
* @brief Quaternion_MultiplyVector
189+
* @brief Quaternion_MultVect
190190
* @param pNumQ:
191191
* @param pNumA:
192192
* @param pVect:
193193
* @retval None
194194
*/
195-
__INLINE void Quaternion_MultiplyVector( quaternion_t *pNumQ, const quaternion_t *pNumA, const float32_t *pVect )
195+
__INLINE void Quaternion_MultVect( quaternion_t *pNumQ, const quaternion_t *pNumA, const float32_t *pVect )
196196
{
197-
if (pNumQ == pNumA) {
198-
quaternion_t tmpQ;
199-
tmpQ.q0 = pNumA->q0;
200-
tmpQ.q1 = pNumA->q1;
201-
tmpQ.q2 = pNumA->q2;
202-
tmpQ.q3 = pNumA->q3;
203-
pNumQ->q0 = -tmpQ.q1 * pVect[0] - tmpQ.q2 * pVect[1] - tmpQ.q3 * pVect[2];
204-
pNumQ->q1 = tmpQ.q0 * pVect[0] - tmpQ.q3 * pVect[1] + tmpQ.q2 * pVect[2];
205-
pNumQ->q2 = tmpQ.q3 * pVect[0] + tmpQ.q0 * pVect[1] - tmpQ.q1 * pVect[2];
206-
pNumQ->q3 = -tmpQ.q2 * pVect[0] + tmpQ.q1 * pVect[1] + tmpQ.q0 * pVect[2];
207-
}
208-
else {
209-
pNumQ->q0 = -pNumA->q1 * pVect[0] - pNumA->q2 * pVect[1] - pNumA->q3 * pVect[2];
210-
pNumQ->q1 = pNumA->q0 * pVect[0] - pNumA->q3 * pVect[1] + pNumA->q2 * pVect[2];
211-
pNumQ->q2 = pNumA->q3 * pVect[0] + pNumA->q0 * pVect[1] - pNumA->q1 * pVect[2];
212-
pNumQ->q3 = -pNumA->q2 * pVect[0] + pNumA->q1 * pVect[1] + pNumA->q0 * pVect[2];
213-
}
197+
// if (pNumQ == pNumA) {
198+
// quaternion_t tmpQ;
199+
// tmpQ.q0 = pNumA->q0;
200+
// tmpQ.q1 = pNumA->q1;
201+
// tmpQ.q2 = pNumA->q2;
202+
// tmpQ.q3 = pNumA->q3;
203+
// pNumQ->q0 = -tmpQ.q1 * pVect[0] - tmpQ.q2 * pVect[1] - tmpQ.q3 * pVect[2];
204+
// pNumQ->q1 = tmpQ.q0 * pVect[0] - tmpQ.q3 * pVect[1] + tmpQ.q2 * pVect[2];
205+
// pNumQ->q2 = tmpQ.q3 * pVect[0] + tmpQ.q0 * pVect[1] - tmpQ.q1 * pVect[2];
206+
// pNumQ->q3 = -tmpQ.q2 * pVect[0] + tmpQ.q1 * pVect[1] + tmpQ.q0 * pVect[2];
207+
// }
208+
// else {
209+
// pNumQ->q0 = -pNumA->q1 * pVect[0] - pNumA->q2 * pVect[1] - pNumA->q3 * pVect[2];
210+
// pNumQ->q1 = pNumA->q0 * pVect[0] - pNumA->q3 * pVect[1] + pNumA->q2 * pVect[2];
211+
// pNumQ->q2 = pNumA->q3 * pVect[0] + pNumA->q0 * pVect[1] - pNumA->q1 * pVect[2];
212+
// pNumQ->q3 = -pNumA->q2 * pVect[0] + pNumA->q1 * pVect[1] + pNumA->q0 * pVect[2];
213+
// }
214214
}
215215

216216
/**

‎Software/UWBNode_Application_mahonyFilter/Program/algorithms/quaternion.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -42,8 +42,8 @@ void Quaternion_ToAngE( eulerAngle_t *pAngE, const quaternion_t *pNumQ );
4242
void Quaternion_UpdateRotMatrix( quaternion_t *pNumQ );
4343
void Quaternion_Add( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB );
4444
void Quaternion_Sub( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB );
45-
void Quaternion_Multiply( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB );
46-
void Quaternion_MultiplyVector( quaternion_t *pNumQ, const quaternion_t *pNumA, const float32_t *pVect );
45+
void Quaternion_Mult( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB );
46+
void Quaternion_MultVect( quaternion_t *pNumQ, const quaternion_t *pNumA, const float32_t *pVect );
4747
void Quaternion_Conjugate( quaternion_t *pNumQ, const quaternion_t *pNumC );
4848
void Quaternion_Normalize( quaternion_t *pNumQ, const quaternion_t *pNumN );
4949
void Quaternion_NormalizeFast( quaternion_t *pNumQ, const quaternion_t *pNumN );

0 commit comments

Comments
 (0)