Skip to content

Commit

Permalink
Update UWBNode_Application_mahonyFilter 01/19
Browse files Browse the repository at this point in the history
  • Loading branch information
@Hom-Wang
Hom-Wang committed Jan 19, 2017
1 parent 7032ca5 commit d13f75b
Show file tree
Hide file tree
Showing 32 changed files with 5,876 additions and 337 deletions.
124 changes: 72 additions & 52 deletions Software/UWBNode_Application_mahonyFilter/Program/algorithms/mahonyFilter.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,14 @@

/* Private typedef -------------------------------------------------------------------------*/
/* Private define --------------------------------------------------------------------------*/
#define MF_KP 0.4f
//#define MF_KI 0.001f
#define MF_A_THR 0.4f
#define MF_M_THR 0.1f

#define MF_KPA 0.4f
//#define MF_KIA 0.001f

#define MF_KPM 0.01f
//#define MF_KIM 0.001f

//#define MAHONY_FILTER_9DOF

Expand All @@ -39,7 +45,7 @@
* @param imu:
* @retval None
*/
void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t sampleRate )
void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t sampleTime )
{
mf->angE.pitch = 0.0f;
mf->angE.roll = 0.0f;
Expand All @@ -49,7 +55,7 @@ void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t samp

mf->imu = imu;

mf->sampleRate = sampleRate;
mf->sampleTime = sampleTime;
}

/**
Expand All @@ -59,17 +65,20 @@ void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t samp
*/
void MahonyFilter_Update( MahonyFilter_t *mf )
{
float32_t err[3];

float32_t gVect[3];
float32_t gyr[3], acc[3];
#if defined(MF_KIA)
static float32_t errIntA[3] = {0};
#endif

#if defined(MAHONY_FILTER_9DOF)
float32_t hVect[3], wVect[3];
float32_t mag[3];
#if defined(MF_KIM)
static float32_t errIntM[3] = {0};
#endif

float32_t err[3];
#if defined(MF_KI)
static float32_t errInt[3] = {0};
#endif

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

/* Normalise accelerometer measurement */
const float32_t normAcc = invSqrtf(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
acc[0] *= normAcc;
acc[1] *= normAcc;
acc[2] *= normAcc;
const float32_t normAcc = sqrtf(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
if ((normAcc < (mf->imu->accStrength * (1.0f + MF_A_THR))) && (normAcc > (mf->imu->accStrength * (1.0f - MF_A_THR)))) {
acc[0] /= normAcc;
acc[1] /= normAcc;
acc[2] /= normAcc;

/* Estimated direction of gravity */
gVect[0] = mf->numQ.rMat[0][2];
gVect[1] = mf->numQ.rMat[1][2];
gVect[2] = mf->numQ.rMat[2][2];

err[0] = acc[1] * gVect[2] - acc[2] * gVect[1];
err[1] = acc[2] * gVect[0] - acc[0] * gVect[2];
err[2] = acc[0] * gVect[1] - acc[1] * gVect[0];

#if defined(MF_KIA)
/* Compute and apply integral feedback */
errIntA[0] += MF_KIA * err[0];
errIntA[1] += MF_KIA * err[1];
errIntA[2] += MF_KIA * err[2];

/* Apply proportional feedback */
gyr[0] += MF_KPA * err[0] + errIntA[0];
gyr[1] += MF_KPA * err[1] + errIntA[1];
gyr[2] += MF_KPA * err[2] + errIntA[2];

/* Estimated direction of gravity */
gVect[0] = mf->numQ.rMat[0][2];
gVect[1] = mf->numQ.rMat[1][2];
gVect[2] = mf->numQ.rMat[2][2];
#else
gyr[0] += MF_KPA * err[0];
gyr[1] += MF_KPA * err[1];
gyr[2] += MF_KPA * err[2];

err[0] = acc[1] * gVect[2] - acc[2] * gVect[1];
err[1] = acc[2] * gVect[0] - acc[0] * gVect[2];
err[2] = acc[0] * gVect[1] - acc[1] * gVect[0];
#endif
}

#if defined(MAHONY_FILTER_9DOF)
mag[0] = mf->imu->magData[0];
mag[1] = mf->imu->magData[1];
mag[2] = mf->imu->magData[2];

// const float32_t normMag = invSqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
// if ((normMag < fusionAhrs->minMagneticFieldSquared) ||
// (normMag > fusionAhrs->maxMagneticFieldSquared)) {
// break;
// }
const float32_t normMag = sqrtf(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
if ((normMag < (mf->imu->magStrength * (1.0f + MF_M_THR))) && (normMag > (mf->imu->magStrength * (1.0f - MF_M_THR)))) {
}

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

/* Normalise magnetometer measurement */
const float32_t normVect = invSqrtf(wVect[0] * wVect[0] + wVect[1] * wVect[1] + wVect[2] * wVect[2]);
wVect[2] *= normVect;
wVect[2] *= normVect;
wVect[0] *= normVect;
wVect[1] *= normVect;
wVect[2] *= normVect;

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

/* Error is sum of cross product between estimated direction and measured direction of field vectors */
err[0] += wVect[1] * hVect[2] - wVect[2] * hVect[1];
err[1] += wVect[2] * hVect[0] - wVect[0] * hVect[2];
err[2] += wVect[0] * hVect[1] - wVect[1] * hVect[0];

#endif
err[0] = wVect[1] * hVect[2] - wVect[2] * hVect[1];
err[1] = wVect[2] * hVect[0] - wVect[0] * hVect[2];
err[2] = wVect[0] * hVect[1] - wVect[1] * hVect[0];

#if defined(MF_KI)
/* Compute and apply integral feedback */
errInt[0] += (SAMPLE_RATE * MF_KI) * err[0];
errInt[1] += (SAMPLE_RATE * MF_KI) * err[1];
errInt[2] += (SAMPLE_RATE * MF_KI) * err[2];
#if defined(MF_KIA)
/* Compute and apply integral feedback */
errIntA[0] += MF_KIM * err[0];
errIntA[1] += MF_KIM * err[1];
errIntA[2] += MF_KIM * err[2];

/* Apply proportional feedback */
gyr[0] += MF_KP * err[0] + errInt[0];
gyr[1] += MF_KP * err[1] + errInt[1];
gyr[2] += MF_KP * err[2] + errInt[2];
/* Apply proportional feedback */
gyr[0] += MF_KPM * err[0] + errIntM[0];
gyr[1] += MF_KPM * err[1] + errIntM[1];
gyr[2] += MF_KPM * err[2] + errIntM[2];

#else
gyr[0] += MF_KP * err[0];
gyr[1] += MF_KP * err[1];
gyr[2] += MF_KP * err[2];
gyr[0] += MF_KPM * err[0];
gyr[1] += MF_KPM * err[1];
gyr[2] += MF_KPM * err[2];

#endif

#endif

/* Integrate rate of change of quaternion */
// Quaternion_Conjugate(&mf->numQ, &mf->numQ);
Quaternion_RungeKutta(&mf->numQ, gyr, mf->sampleRate * 0.5f);
Quaternion_RungeKutta(&mf->numQ, gyr, mf->sampleTime * 0.5f);
Quaternion_Normalize(&mf->numQ, &mf->numQ);
Quaternion_UpdateRotMatrix(&mf->numQ);
Quaternion_ToAngE(&mf->angE, &mf->numQ);

// // Get Euler Angle */
// mf->angE.pitch = -toDeg(mf->angE.pitch);
// mf->angE.roll = toDeg(mf->angE.roll);
// mf->angE.yaw = -toDeg(mf->angE.yaw);
// Quaternion_ToAngE(&mf->angE, &mf->numQ);
}

/*************************************** END OF FILE ****************************************/
4 changes: 2 additions & 2 deletions Software/UWBNode_Application_mahonyFilter/Program/algorithms/mahonyFilter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,13 +30,13 @@
typedef struct {
quaternion_t numQ;
eulerAngle_t angE;
float32_t sampleRate;
float32_t sampleTime;
IMU_DataTypeDef *imu;
} __attribute__((aligned)) MahonyFilter_t;

/* Exported constants ----------------------------------------------------------------------*/
/* Exported functions ----------------------------------------------------------------------*/
void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t sampleRate );
void MahonyFilter_Init( MahonyFilter_t *mf, IMU_DataTypeDef *imu, float32_t sampleTime );
void MahonyFilter_Update( MahonyFilter_t *mf );

#ifdef __cplusplus
Expand Down
54 changes: 27 additions & 27 deletions Software/UWBNode_Application_mahonyFilter/Program/algorithms/quaternion.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,9 +93,9 @@ __INLINE void Quaternion_ToNumQ( quaternion_t *pNumQ, const eulerAngle_t *pAngE
*/
__INLINE void Quaternion_ToAngE( eulerAngle_t *pAngE, const quaternion_t *pNumQ )
{
pAngE->pitch = toDeg(-asinf(pNumQ->rMat[0][2]));
pAngE->roll = toDeg(atan2f(pNumQ->rMat[1][2], pNumQ->rMat[2][2]));
pAngE->yaw = toDeg(atan2f(pNumQ->rMat[0][1], pNumQ->rMat[0][0]));
pAngE->pitch = toDeg(asinf(pNumQ->rMat[0][2]));
pAngE->roll = toDeg(atan2f(-pNumQ->rMat[1][2], pNumQ->rMat[2][2]));
pAngE->yaw = toDeg(atan2f(-pNumQ->rMat[0][1], pNumQ->rMat[0][0]));
}

/**
Expand Down Expand Up @@ -158,13 +158,13 @@ __INLINE void Quaternion_Sub( quaternion_t *pNumQ, const quaternion_t *pNumA, co
}

/**
* @brief Quaternion_Multiply
* @brief Quaternion_Mult
* @param pNumQ:
* @param pNumA:
* @param pNumB:
* @retval None
*/
__INLINE void Quaternion_Multiply( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB )
__INLINE void Quaternion_Mult( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB )
{
if (pNumQ == pNumA) {
quaternion_t tmpQ;
Expand All @@ -179,38 +179,38 @@ __INLINE void Quaternion_Multiply( quaternion_t *pNumQ, const quaternion_t *pNum
}
else {
pNumQ->q0 = pNumA->q0 * pNumB->q0 - pNumA->q1 * pNumB->q1 - pNumA->q2 * pNumB->q2 - pNumA->q3 * pNumB->q3;
pNumQ->q1 = pNumA->q1 * pNumB->q0 + pNumA->q0 * pNumB->q1 - pNumA->q3 * pNumB->q2 + pNumA->q2 * pNumB->q3;
pNumQ->q2 = pNumA->q2 * pNumB->q0 + pNumA->q3 * pNumB->q1 + pNumA->q0 * pNumB->q2 - pNumA->q1 * pNumB->q3;
pNumQ->q3 = pNumA->q3 * pNumB->q0 - pNumA->q2 * pNumB->q1 + pNumA->q1 * pNumB->q2 + pNumA->q0 * pNumB->q3;
pNumQ->q1 = pNumA->q1 * pNumB->q0 + pNumA->q0 * pNumB->q1 + pNumA->q3 * pNumB->q2 - pNumA->q2 * pNumB->q3;
pNumQ->q2 = pNumA->q2 * pNumB->q0 - pNumA->q3 * pNumB->q1 + pNumA->q0 * pNumB->q2 + pNumA->q1 * pNumB->q3;
pNumQ->q3 = pNumA->q3 * pNumB->q0 + pNumA->q2 * pNumB->q1 - pNumA->q1 * pNumB->q2 + pNumA->q0 * pNumB->q3;
}
}

/**
* @brief Quaternion_MultiplyVector
* @brief Quaternion_MultVect
* @param pNumQ:
* @param pNumA:
* @param pVect:
* @retval None
*/
__INLINE void Quaternion_MultiplyVector( quaternion_t *pNumQ, const quaternion_t *pNumA, const float32_t *pVect )
__INLINE void Quaternion_MultVect( quaternion_t *pNumQ, const quaternion_t *pNumA, const float32_t *pVect )
{
if (pNumQ == pNumA) {
quaternion_t tmpQ;
tmpQ.q0 = pNumA->q0;
tmpQ.q1 = pNumA->q1;
tmpQ.q2 = pNumA->q2;
tmpQ.q3 = pNumA->q3;
pNumQ->q0 = -tmpQ.q1 * pVect[0] - tmpQ.q2 * pVect[1] - tmpQ.q3 * pVect[2];
pNumQ->q1 = tmpQ.q0 * pVect[0] - tmpQ.q3 * pVect[1] + tmpQ.q2 * pVect[2];
pNumQ->q2 = tmpQ.q3 * pVect[0] + tmpQ.q0 * pVect[1] - tmpQ.q1 * pVect[2];
pNumQ->q3 = -tmpQ.q2 * pVect[0] + tmpQ.q1 * pVect[1] + tmpQ.q0 * pVect[2];
}
else {
pNumQ->q0 = -pNumA->q1 * pVect[0] - pNumA->q2 * pVect[1] - pNumA->q3 * pVect[2];
pNumQ->q1 = pNumA->q0 * pVect[0] - pNumA->q3 * pVect[1] + pNumA->q2 * pVect[2];
pNumQ->q2 = pNumA->q3 * pVect[0] + pNumA->q0 * pVect[1] - pNumA->q1 * pVect[2];
pNumQ->q3 = -pNumA->q2 * pVect[0] + pNumA->q1 * pVect[1] + pNumA->q0 * pVect[2];
}
// if (pNumQ == pNumA) {
// quaternion_t tmpQ;
// tmpQ.q0 = pNumA->q0;
// tmpQ.q1 = pNumA->q1;
// tmpQ.q2 = pNumA->q2;
// tmpQ.q3 = pNumA->q3;
// pNumQ->q0 = -tmpQ.q1 * pVect[0] - tmpQ.q2 * pVect[1] - tmpQ.q3 * pVect[2];
// pNumQ->q1 = tmpQ.q0 * pVect[0] - tmpQ.q3 * pVect[1] + tmpQ.q2 * pVect[2];
// pNumQ->q2 = tmpQ.q3 * pVect[0] + tmpQ.q0 * pVect[1] - tmpQ.q1 * pVect[2];
// pNumQ->q3 = -tmpQ.q2 * pVect[0] + tmpQ.q1 * pVect[1] + tmpQ.q0 * pVect[2];
// }
// else {
// pNumQ->q0 = -pNumA->q1 * pVect[0] - pNumA->q2 * pVect[1] - pNumA->q3 * pVect[2];
// pNumQ->q1 = pNumA->q0 * pVect[0] - pNumA->q3 * pVect[1] + pNumA->q2 * pVect[2];
// pNumQ->q2 = pNumA->q3 * pVect[0] + pNumA->q0 * pVect[1] - pNumA->q1 * pVect[2];
// pNumQ->q3 = -pNumA->q2 * pVect[0] + pNumA->q1 * pVect[1] + pNumA->q0 * pVect[2];
// }
}

/**
Expand Down
4 changes: 2 additions & 2 deletions Software/UWBNode_Application_mahonyFilter/Program/algorithms/quaternion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,8 @@ void Quaternion_ToAngE( eulerAngle_t *pAngE, const quaternion_t *pNumQ );
void Quaternion_UpdateRotMatrix( quaternion_t *pNumQ );
void Quaternion_Add( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB );
void Quaternion_Sub( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB );
void Quaternion_Multiply( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB );
void Quaternion_MultiplyVector( quaternion_t *pNumQ, const quaternion_t *pNumA, const float32_t *pVect );
void Quaternion_Mult( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB );
void Quaternion_MultVect( quaternion_t *pNumQ, const quaternion_t *pNumA, const float32_t *pVect );
void Quaternion_Conjugate( quaternion_t *pNumQ, const quaternion_t *pNumC );
void Quaternion_Normalize( quaternion_t *pNumQ, const quaternion_t *pNumN );
void Quaternion_NormalizeFast( quaternion_t *pNumQ, const quaternion_t *pNumN );
Expand Down
Loading

0 comments on commit d13f75b

Please sign in to comment.