99
1010
1111#define FUSION_PI 3.14159265358979323846f
12+ #define FUSION_UNUSED (x ) ((void)x)
1213
1314#define FUSION_RADIANS (degree ) (degree) * FUSION_PI / 180.0f
1415#define FUSION_DEGREES (radian ) (radian) * 180.0f / FUSION_PI
1516
16-
1717#define FUSION_MAX (item1 , item2 ) (item1) >= (item2) ? (item1) : (item2)
1818#define FUSION_MIN (item1 , item2 ) (item1) <= (item2) ? (item1) : (item2)
1919
@@ -38,10 +38,8 @@ static mp_obj_t fusion_make_new(const mp_obj_type_t *type, size_t n_args, size_t
3838
3939 self -> beta = 0.6045997880780725842169464404f ;
4040
41- float declination ;
42-
4341 if (args [ARG_declination ].u_obj == mp_const_none ) {
44- self -> declination = 0.0f
42+ self -> declination = 0.0f ;
4543 } else {
4644 self -> declination = mp_obj_get_float_to_f (args [ARG_declination ].u_obj );
4745 }
@@ -50,7 +48,8 @@ static mp_obj_t fusion_make_new(const mp_obj_type_t *type, size_t n_args, size_t
5048}
5149
5250
53- static mp_obj_t calibrate (up_obj_t self_in , mp_obj_t getxyz , mp_obj_t stopfunc ):
51+ static mp_obj_t calibrate (mp_obj_t self_in , mp_obj_t getxyz , mp_obj_t stopfunc )
52+ {
5453
5554 mp_fusion_obj_t * self = MP_OBJ_TO_PTR (self_in );
5655
@@ -85,7 +84,7 @@ static mp_obj_t calibrate(up_obj_t self_in, mp_obj_t getxyz, mp_obj_t stopfunc):
8584 }
8685
8786 return mp_const_none ;
88-
87+ }
8988
9089static MP_DEFINE_CONST_FUN_OBJ_3 (calibrate_obj , calibrate ) ;
9190
@@ -95,13 +94,13 @@ static float delta_T(mp_fusion_obj_t *self)
9594 float res ;
9695 uint32_t ts = mp_hal_ticks_us ();
9796
98- if (self -> start_time == 0 ) {
97+ if (self -> start_ts == 0 ) {
9998 res = 0.0001f ;
10099 } else {
101- res = (float )(ts - self -> start_time ) * 0.000001f ;
100+ res = (float )(ts - self -> start_ts ) * 0.000001f ;
102101 }
103102
104- self -> start_time = ts ;
103+ self -> start_ts = ts ;
105104 return res ;
106105}
107106
@@ -133,10 +132,14 @@ mp_obj_t calculate(mp_fusion_obj_t *self, float accel[3], float gyro[3], float *
133132 ay *= norm ;
134133 az *= norm ;
135134
135+ float mx ;
136+ float my ;
137+ float mz ;
138+
136139 if (mag != NULL ) {
137- float mx = mag [0 ] - self -> mag_bias [0 ];
138- float my = mag [1 ] - self -> mag_bias [1 ];
139- float mz = mag [2 ] - self -> mag_bias [2 ];
140+ mx = mag [0 ] - self -> mag_bias [0 ];
141+ my = mag [1 ] - self -> mag_bias [1 ];
142+ mz = mag [2 ] - self -> mag_bias [2 ];
140143
141144 // Normalise magnetometer measurement
142145 norm = sqrtf ((mx * mx ) + (my * my ) + (mz * mz ));
@@ -148,6 +151,10 @@ mp_obj_t calculate(mp_fusion_obj_t *self, float accel[3], float gyro[3], float *
148151 mx *= norm ;
149152 my *= norm ;
150153 mz *= norm ;
154+ } else {
155+ mx = 0.0f ;
156+ my = 0.0f ;
157+ mz = 0.0f ;
151158 }
152159
153160 float gx = FUSION_RADIANS (gyro [0 ]);
@@ -160,10 +167,10 @@ mp_obj_t calculate(mp_fusion_obj_t *self, float accel[3], float gyro[3], float *
160167 float q4 = self -> q [3 ];
161168
162169 // Auxiliary variables to avoid repeated arithmetic
163- float _2q1 = 2.0f * q1
164- float _2q2 = 2.0f * q2
165- float _2q3 = 2.0f * q3
166- float _2q4 = 2.0f * q4
170+ float _2q1 = 2.0f * q1 ;
171+ float _2q2 = 2.0f * q2 ;
172+ float _2q3 = 2.0f * q3 ;
173+ float _2q4 = 2.0f * q4 ;
167174
168175 float q1sq = q1 * q1 ;
169176 float q2sq = q2 * q2 ;
@@ -261,7 +268,7 @@ mp_obj_t calculate(mp_fusion_obj_t *self, float accel[3], float gyro[3], float *
261268
262269 q1sq = q1 * q1 ;
263270 q2sq = q2 * q2 ;
264- s3sq = s3 * s3 ;
271+ float s3sq = s3 * s3 ;
265272 q4sq = q4 * q4 ;
266273
267274 self -> q [0 ] = q1 ;
@@ -275,6 +282,8 @@ mp_obj_t calculate(mp_fusion_obj_t *self, float accel[3], float gyro[3], float *
275282 if (mag != NULL ) {
276283 yaw = FUSION_DEGREES (atan2f (2.0f * ((q2 * q3 ) + (q1 * q4 )), q1sq + q2sq - s3sq - q4sq ));
277284 yaw += self -> declination ;
285+ } else {
286+ yaw = 0.0f ;
278287 }
279288
280289 tuple [0 ] = mp_obj_new_float ((mp_float_t )roll );
@@ -365,7 +374,7 @@ static MP_DEFINE_CONST_DICT(fusion_globals, fusion_globals_table);
365374
366375const mp_obj_module_t mp_module_fusion = {
367376 .base = {& mp_type_module },
368- .globals = (mp_obj_dict_t * )fusion_globals ,
377+ .globals = (mp_obj_dict_t * )& fusion_globals ,
369378};
370379
371380
0 commit comments