Skip to content

Commit feae505

Browse files
Hom-WangHom-Wang
committed
Update UWBNode_Application_mahonyFilter 12/13
1 parent 3b5176c commit feae505

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

41 files changed

+702
-4155
lines changed

‎Software/UWBNode_Application_mahonyFilter/Libraries/CMSIS/Device/system_stm32f4xx.c

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -67,7 +67,7 @@
6767
#include "stm32f4xx.h"
6868

6969
#if !defined (HSE_VALUE)
70-
#define HSE_VALUE ((uint32_t)16000000) /*!< Default value of the External oscillator in Hz */
70+
#define HSE_VALUE ((uint32_t)25000000) /*!< Default value of the External oscillator in Hz */
7171
#endif /* HSE_VALUE */
7272

7373
#if !defined (HSI_VALUE)

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,8 @@
2828

2929
/* Exported types --------------------------------------------------------------------------*/
3030
typedef struct {
31-
Quaternion_t numQ;
32-
EulerAngle_t angE;
31+
quaternion_t numQ;
32+
eulerAngle_t angE;
3333
float32_t sampleRate;
3434
IMU_DataTypeDef *imu;
3535
} __attribute__((aligned)) MahonyFilter_t;

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

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
*
99
* @file mathUnit.h
1010
* @author KitSprout
11-
* @date 6-Oct-2016
11+
* @date 16-Nov-2016
1212
* @brief
1313
*
1414
*/
@@ -22,24 +22,24 @@
2222
#endif
2323

2424
/* Includes --------------------------------------------------------------------------------*/
25-
#include "stm32f4xx.h"
25+
#include "core_cm4.h"
2626
#include "arm_math.h"
2727

2828
/* Exported types --------------------------------------------------------------------------*/
2929

3030
/**
31-
* @brief euler angle structure definition
31+
* @brief euler angle structure definition
3232
*/
3333
typedef struct {
3434
float32_t pitch;
3535
float32_t roll;
3636
float32_t yaw;
37-
} __attribute__((aligned(4))) EulerAngle_t;
37+
} __attribute__((aligned(4))) eulerAngle_t;
3838

3939
/* Exported constants ----------------------------------------------------------------------*/
4040
/* Exported macro --------------------------------------------------------------------------*/
41-
#define invSqrtf( iSq ) (1.0f / sqrtf((float32_t)(iSq)))
42-
#define squa( Sq ) (((float32_t)Sq) * ((float32_t)(Sq)))
41+
#define invSqrtf( _iSq ) (1.0f / sqrtf((float32_t)(_iSq)))
42+
#define squa( _Sq ) (((float32_t)_Sq) * ((float32_t)(_Sq)))
4343
#define toRad( _mathD ) ((_mathD) * 0.0174532925f)
4444
#define toDeg( _mathR ) ((_mathR) * 57.2957795f)
4545

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

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@
3535
* @param pNumQ:
3636
* @retval None
3737
*/
38-
void Quaternion_Clear( Quaternion_t *pNumQ )
38+
void Quaternion_Clear( quaternion_t *pNumQ )
3939
{
4040
pNumQ->q0 = 1.0f;
4141
pNumQ->q1 = 0.0f;
@@ -55,7 +55,7 @@ void Quaternion_Clear( Quaternion_t *pNumQ )
5555
* @param pAngE:
5656
* @retval None
5757
*/
58-
__INLINE void Quaternion_ToNumQ( Quaternion_t *pNumQ, const EulerAngle_t *pAngE )
58+
__INLINE void Quaternion_ToNumQ( quaternion_t *pNumQ, const eulerAngle_t *pAngE )
5959
{
6060
const float32_t halfP = toRad(pAngE->pitch * 0.5f);
6161
const float32_t halfR = toRad(pAngE->roll * 0.5f);
@@ -91,7 +91,7 @@ __INLINE void Quaternion_ToNumQ( Quaternion_t *pNumQ, const EulerAngle_t *pAngE
9191
* @param pAngE:
9292
* @retval None
9393
*/
94-
__INLINE void Quaternion_ToAngE( EulerAngle_t *pAngE, const Quaternion_t *pNumQ )
94+
__INLINE void Quaternion_ToAngE( eulerAngle_t *pAngE, const quaternion_t *pNumQ )
9595
{
9696
pAngE->pitch = toDeg(-asinf(pNumQ->rMat[0][2]));
9797
pAngE->roll = toDeg(atan2f(pNumQ->rMat[1][2], pNumQ->rMat[2][2]));
@@ -103,7 +103,7 @@ __INLINE void Quaternion_ToAngE( EulerAngle_t *pAngE, const Quaternion_t *pNumQ
103103
* @param pNumQ:
104104
* @retval None
105105
*/
106-
__INLINE void Quaternion_UpdateRotMatrix( Quaternion_t *pNumQ )
106+
__INLINE void Quaternion_UpdateRotMatrix( quaternion_t *pNumQ )
107107
{
108108
const float32_t Mq0q0 = pNumQ->q0 * pNumQ->q0;
109109
const float32_t Mq1q1 = pNumQ->q1 * pNumQ->q1;
@@ -134,7 +134,7 @@ __INLINE void Quaternion_UpdateRotMatrix( Quaternion_t *pNumQ )
134134
* @param pNumB:
135135
* @retval None
136136
*/
137-
__INLINE void Quaternion_Add( Quaternion_t *pNumQ, const Quaternion_t *pNumA, const Quaternion_t *pNumB )
137+
__INLINE void Quaternion_Add( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB )
138138
{
139139
pNumQ->q0 = pNumA->q0 + pNumB->q0;
140140
pNumQ->q1 = pNumA->q1 + pNumB->q1;
@@ -149,7 +149,7 @@ __INLINE void Quaternion_Add( Quaternion_t *pNumQ, const Quaternion_t *pNumA, co
149149
* @param pNumB:
150150
* @retval None
151151
*/
152-
__INLINE void Quaternion_Sub( Quaternion_t *pNumQ, const Quaternion_t *pNumA, const Quaternion_t *pNumB )
152+
__INLINE void Quaternion_Sub( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB )
153153
{
154154
pNumQ->q0 = pNumA->q0 - pNumB->q0;
155155
pNumQ->q1 = pNumA->q1 - pNumB->q1;
@@ -164,10 +164,10 @@ __INLINE void Quaternion_Sub( Quaternion_t *pNumQ, const Quaternion_t *pNumA, co
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_Multiply( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB )
168168
{
169169
if (pNumQ == pNumA) {
170-
Quaternion_t tmpQ;
170+
quaternion_t tmpQ;
171171
tmpQ.q0 = pNumA->q0;
172172
tmpQ.q1 = pNumA->q1;
173173
tmpQ.q2 = pNumA->q2;
@@ -192,10 +192,10 @@ __INLINE void Quaternion_Multiply( Quaternion_t *pNumQ, const Quaternion_t *pNum
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_MultiplyVector( quaternion_t *pNumQ, const quaternion_t *pNumA, const float32_t *pVect )
196196
{
197197
if (pNumQ == pNumA) {
198-
Quaternion_t tmpQ;
198+
quaternion_t tmpQ;
199199
tmpQ.q0 = pNumA->q0;
200200
tmpQ.q1 = pNumA->q1;
201201
tmpQ.q2 = pNumA->q2;
@@ -219,7 +219,7 @@ __INLINE void Quaternion_MultiplyVector( Quaternion_t *pNumQ, const Quaternion_t
219219
* @param pNumC:
220220
* @retval None
221221
*/
222-
__INLINE void Quaternion_Conjugate( Quaternion_t *pNumQ, const Quaternion_t *pNumC )
222+
__INLINE void Quaternion_Conjugate( quaternion_t *pNumQ, const quaternion_t *pNumC )
223223
{
224224
pNumQ->q0 = pNumC->q0;
225225
pNumQ->q1 = -pNumC->q1;
@@ -233,7 +233,7 @@ __INLINE void Quaternion_Conjugate( Quaternion_t *pNumQ, const Quaternion_t *pNu
233233
* @param pNumN:
234234
* @retval None
235235
*/
236-
__INLINE void Quaternion_Normalize( Quaternion_t *pNumQ, const Quaternion_t *pNumN )
236+
__INLINE void Quaternion_Normalize( quaternion_t *pNumQ, const quaternion_t *pNumN )
237237
{
238238
const float32_t norm = invSqrtf(squa(pNumQ->q0) + squa(pNumQ->q1) + squa(pNumQ->q2) + squa(pNumQ->q3));
239239

@@ -249,7 +249,7 @@ __INLINE void Quaternion_Normalize( Quaternion_t *pNumQ, const Quaternion_t *pNu
249249
* @param pNumN:
250250
* @retval None
251251
*/
252-
__INLINE void Quaternion_NormalizeFast( Quaternion_t *pNumQ, const Quaternion_t *pNumN )
252+
__INLINE void Quaternion_NormalizeFast( quaternion_t *pNumQ, const quaternion_t *pNumN )
253253
{
254254
const float32_t sqrtNumber = squa(pNumQ->q0) + squa(pNumQ->q1) + squa(pNumQ->q2) + squa(pNumQ->q3);
255255

@@ -274,7 +274,7 @@ __INLINE void Quaternion_NormalizeFast( Quaternion_t *pNumQ, const Quaternion_t
274274
* @param helfTime:
275275
* @retval None
276276
*/
277-
__INLINE void Quaternion_RungeKutta( Quaternion_t *pNumQ, const float32_t *gyro, const float32_t helfTime )
277+
__INLINE void Quaternion_RungeKutta( quaternion_t *pNumQ, const float32_t *gyro, const float32_t helfTime )
278278
{
279279
float32_t q[4];
280280
float32_t gyr[3];

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

Lines changed: 13 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -32,22 +32,22 @@ typedef struct {
3232
float32_t q2;
3333
float32_t q3;
3434
float32_t rMat[3][3];
35-
} __attribute__((aligned(4))) Quaternion_t;
35+
} __attribute__((aligned(4))) quaternion_t;
3636

3737
/* Exported constants ----------------------------------------------------------------------*/
3838
/* Exported functions ----------------------------------------------------------------------*/
39-
void Quaternion_Clear( Quaternion_t *pNumQ );
40-
void Quaternion_ToNumQ( Quaternion_t *pNumQ, const EulerAngle_t *pAngE );
41-
void Quaternion_ToAngE( EulerAngle_t *pAngE, const Quaternion_t *pNumQ );
42-
void Quaternion_UpdateRotMatrix( Quaternion_t *pNumQ );
43-
void Quaternion_Add( Quaternion_t *pNumQ, const Quaternion_t *pNumA, const Quaternion_t *pNumB );
44-
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 );
47-
void Quaternion_Conjugate( Quaternion_t *pNumQ, const Quaternion_t *pNumC );
48-
void Quaternion_Normalize( Quaternion_t *pNumQ, const Quaternion_t *pNumN );
49-
void Quaternion_NormalizeFast( Quaternion_t *pNumQ, const Quaternion_t *pNumN );
50-
void Quaternion_RungeKutta( Quaternion_t *pNumQ, const float32_t *gyro, const float32_t helfTime );
39+
void Quaternion_Clear( quaternion_t *pNumQ );
40+
void Quaternion_ToNumQ( quaternion_t *pNumQ, const eulerAngle_t *pAngE );
41+
void Quaternion_ToAngE( eulerAngle_t *pAngE, const quaternion_t *pNumQ );
42+
void Quaternion_UpdateRotMatrix( quaternion_t *pNumQ );
43+
void Quaternion_Add( quaternion_t *pNumQ, const quaternion_t *pNumA, const quaternion_t *pNumB );
44+
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 );
47+
void Quaternion_Conjugate( quaternion_t *pNumQ, const quaternion_t *pNumC );
48+
void Quaternion_Normalize( quaternion_t *pNumQ, const quaternion_t *pNumN );
49+
void Quaternion_NormalizeFast( quaternion_t *pNumQ, const quaternion_t *pNumN );
50+
void Quaternion_RungeKutta( quaternion_t *pNumQ, const float32_t *gyro, const float32_t helfTime );
5151

5252
#ifdef __cplusplus
5353
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
*
99
* @file string.c
1010
* @author KitSprout
11-
* @date 6-Oct-2016
11+
* @date 16-Nov-2016
1212
* @brief
1313
*
1414
*/

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
*
99
* @file string.h
1010
* @author KitSprout
11-
* @date 6-Oct-2016
11+
* @date 16-Nov-2016
1212
* @brief
1313
*
1414
*/
@@ -23,7 +23,7 @@
2323

2424
/* Includes --------------------------------------------------------------------------------*/
2525
#include <string.h>
26-
#include "stm32f4xx.h"
26+
#include <stdint.h>
2727

2828
/* Exported types --------------------------------------------------------------------------*/
2929
typedef enum {

‎Software/UWBNode_Application_mahonyFilter/Program/drivers/stm32f4_delay.c

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
*
99
* @file stm32f4_delay.c
1010
* @author KitSprout
11-
* @date 4-Nov-2016
11+
* @date 16-Nov-2016
1212
* @brief
1313
*
1414
*/
@@ -30,14 +30,14 @@
3030

3131
/**
3232
* @brief delay_us
33-
* @param vCnt_us: delay us time.
33+
* @param __us: delay us time.
3434
* @retval None
3535
*/
36-
void delay_us( __IO uint32_t vCnt_us )
36+
void delay_us( __IO uint32_t __us )
3737
{
38-
__IO uint32_t vCnt;
39-
while (vCnt_us--) {
40-
for (vCnt = 7; vCnt != 0; vCnt--);
38+
__IO uint32_t count;
39+
while (__us--) {
40+
for (count = 7; count != 0; count--);
4141
}
4242
}
4343

‎Software/UWBNode_Application_mahonyFilter/Program/drivers/stm32f4_delay.h

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
*
99
* @file stm32f4_delay.h
1010
* @author KitSprout
11-
* @date 4-Nov-2016
11+
* @date 16-Nov-2016
1212
* @brief
1313
*
1414
*/
@@ -22,14 +22,14 @@
2222
#endif
2323

2424
/* Includes --------------------------------------------------------------------------------*/
25-
#include "stm32f4xx.h"
25+
#include "core_cm4.h"
2626

2727
/* Exported types --------------------------------------------------------------------------*/
2828
/* Exported constants ----------------------------------------------------------------------*/
2929
#define delay_ms(__ms) HAL_Delay(__ms)
3030

3131
/* Exported functions ----------------------------------------------------------------------*/
32-
void delay_us( __IO uint32_t vCnt_us );
32+
void delay_us( __IO uint32_t __us );
3333

3434
#ifdef __cplusplus
3535
}

‎Software/UWBNode_Application_mahonyFilter/Program/drivers/stm32f4_msp.c

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -8,29 +8,27 @@
88
*
99
* @file stm32f4_msp.c
1010
* @author KitSprout
11-
* @date 13-Nov-2016
11+
* @date 19-Nov-2016
1212
* @brief
1313
*
1414
*/
1515

1616
/* Includes --------------------------------------------------------------------------------*/
1717
#include "stm32f4xx.h"
18+
#include "boardConfig.h"
1819

1920
/** @addtogroup STM32_Driver
2021
* @{
2122
*/
2223

2324
/* Private typedef -------------------------------------------------------------------------*/
2425
/* Private define --------------------------------------------------------------------------*/
25-
#define USE_CLOCK_SOUCE_HSE
26-
//#define USE_CLOCK_SOUCE_HSI
27-
2826
/* Private macro ---------------------------------------------------------------------------*/
2927
/* Private variables -----------------------------------------------------------------------*/
3028
/* Private function prototypes -------------------------------------------------------------*/
3129
/* Private functions -----------------------------------------------------------------------*/
3230

33-
#if defined(USE_CLOCK_SOUCE_HSE)
31+
#if defined(KS_HW_CLOCK_SOUCE_HSE)
3432
/*
3533
System Clock source = PLL (HSE)
3634
SYSCLK(Hz) = 100000000
@@ -78,7 +76,8 @@ void HAL_MspInit( void )
7876
while(1) { ; }
7977
}
8078

81-
#elif defined(USE_CLOCK_SOUCE_HSI)
79+
80+
#elif defined(KS_HW_CLOCK_SOUCE_HSI)
8281
/*
8382
System Clock source = PLL (HSI)
8483
SYSCLK(Hz) = 100000000

0 commit comments

Comments
 (0)