1010
1111#include "odometry.h"
1212#include "math.h"
13+ #include "mcu_config.h"
1314
1415uint8_t _i2c_reg_data ;
1516uint8_t _mag_buffer [6 ];
1617float _mag_heading_temp ;
1718
1819uint8_t _imu_addr_check ;
1920uint8_t _imu_buffer [6 ];
20- float tmp_x ,tmp_y ,tmp_z ;
2121
22- // ############################################################
23- // #################### HMC MAGNETOMETER ####################
24- // ############################################################
22+ static float _tmp_accx , _tmp_accy , _tmp_accz ;
23+ static float prev_gyr_x = 0.0f , prev_gyr_y = 0.0f , prev_gyr_z = 0.0f ;
24+ static float prev_acc_x = 0.0f , prev_acc_y = 0.0f , prev_acc_z = 0.0f ;
2525
26- float LRL_HMC5883L_SetDeclination (int16_t declination_degs , int16_t declination_mins , char declination_dir )
27- {
28- int8_t _dir = 0 ;
26+ float _tmp_cal_x = 0 , _tmp_cal_y = 0 , _tmp_cal_z = 0 ;
2927
30- if (declination_dir == 'E' || declination_dir == 'e' || declination_dir == 1 )
31- {
32- _dir = 1 ;
33- }
34- else if (declination_dir == 'W' || declination_dir == 'w' || declination_dir == -1 )
35- {
36- _dir = -1 ;
37- }
3828
39- return ((_dir )* ( declination_degs + (1 /60 * declination_mins )) * (M_PI / 180 ));
40- }
29+ // ############################################################
30+ // #################### HMC MAGNETOMETER ####################
31+ // ############################################################
4132
4233void LRL_HMC5883L_Init (odom_cfgType * odom )
4334{
35+ _LRL_MPU6050_EnableBypass (odom , 1 );
4436 // write CONFIG_A register
4537 HAL_I2C_Master_Transmit (odom -> imu .hi2c , HMC5883L_ADDRESS , (uint8_t * )HMC5883L_ADDRESS_WRITE , 1 , DELAY_TIMEOUT );
4638 _i2c_reg_data = 0x10 ;
@@ -58,8 +50,25 @@ void LRL_HMC5883L_Init(odom_cfgType * odom)
5850 HAL_Delay (10 );
5951}
6052
53+ float LRL_HMC5883L_SetDeclination (int16_t declination_degs , int16_t declination_mins , char declination_dir )
54+ {
55+ int8_t _dir = 0 ;
56+
57+ if (declination_dir == 'E' || declination_dir == 'e' || declination_dir == 1 )
58+ {
59+ _dir = 1 ;
60+ }
61+ else if (declination_dir == 'W' || declination_dir == 'w' || declination_dir == -1 )
62+ {
63+ _dir = -1 ;
64+ }
65+
66+ return ((_dir )* ( declination_degs + (1 /60 * declination_mins )) * (M_PI / 180 ));
67+ }
68+
6169void LRL_HMC5883L_ReadHeading (odom_cfgType * odom )
6270{
71+ _LRL_MPU6050_EnableBypass (odom , 1 );
6372 HAL_I2C_Master_Transmit (odom -> imu .hi2c , HMC5883L_ADDRESS , (uint8_t * )HMC5883L_ADDRESS_READ , 1 , DELAY_TIMEOUT );
6473 HAL_I2C_Mem_Read (odom -> imu .hi2c , HMC5883L_ADDRESS , HMC5883L_RA_DATAX_H , 1 , (uint8_t * )& _mag_buffer , 6 , DELAY_TIMEOUT );
6574
@@ -88,6 +97,7 @@ void LRL_HMC5883L_ReadHeading(odom_cfgType * odom)
8897
8998void LRL_MPU6050_Init (odom_cfgType * odom )
9099{
100+ _LRL_MPU6050_EnableBypass (odom , 0 );
91101 HAL_I2C_Mem_Read (odom -> imu .hi2c , MPU_ADDR , WHO_AM_I , 1 , & _imu_addr_check , 1 , DELAY_TIMEOUT );
92102
93103 if (_imu_addr_check == 0x68 ) // 0x68 will be returned by the sensor if everything goes well
@@ -110,10 +120,39 @@ void LRL_MPU6050_Init(odom_cfgType * odom)
110120 _i2c_reg_data = 0x00 ;
111121 HAL_I2C_Mem_Write (odom -> imu .hi2c , MPU_ADDR , GYRO_CONFIG , 1 , & _i2c_reg_data , 1 , DELAY_TIMEOUT );
112122 }
123+
124+ odom -> imu .offset_calibration_x = 0 ;
125+ odom -> imu .offset_calibration_y = 0 ;
126+ odom -> imu .offset_calibration_z = 0 ;
127+
128+ for (int i = 0 ; i < 500 ; i ++ )
129+ {
130+ LRL_MPU6050_ReadAll (odom );
131+ LRL_MPU6050_ComplementaryFilter (odom );
132+
133+ _tmp_cal_x += odom -> angle .x ;
134+ _tmp_cal_y += odom -> angle .y ;
135+ _tmp_cal_z += odom -> angle .z ;
136+
137+ HAL_Delay (5 );
138+ }
139+
140+ odom -> imu .offset_calibration_x = (_tmp_cal_x /500 );
141+ odom -> imu .offset_calibration_y = (_tmp_cal_y /500 );
142+ odom -> imu .offset_calibration_z = (_tmp_cal_z /500 );
143+
144+ prev_acc_x = 0 ;
145+ prev_acc_y = 0 ;
146+ prev_acc_z = 0 ;
147+ prev_gyr_x = 0 ;
148+ prev_gyr_y = 0 ;
149+ prev_gyr_z = 0 ;
113150}
114151
115152void LRL_MPU6050_ReadAccel (odom_cfgType * odom )
116153{
154+
155+ _LRL_MPU6050_EnableBypass (odom , 0 );
117156 // Read 6 BYTES of data starting from ACCEL_XOUT_H register
118157 HAL_I2C_Mem_Read (odom -> imu .hi2c , MPU_ADDR , ACCEL_XOUT_H , 1 , (uint8_t * )& _imu_buffer , 6 , DELAY_TIMEOUT );
119158
@@ -135,6 +174,7 @@ void LRL_MPU6050_ReadAccel(odom_cfgType * odom)
135174
136175void LRL_MPU6050_ReadGyro (odom_cfgType * odom )
137176{
177+ _LRL_MPU6050_EnableBypass (odom , 0 );
138178 HAL_I2C_Mem_Read (odom -> imu .hi2c , MPU_ADDR , GYRO_XOUT_H , 1 , (uint8_t * )_imu_buffer , 6 , DELAY_TIMEOUT );
139179
140180 odom -> gyro .x = (int16_t )(_imu_buffer [0 ] << 8 | _imu_buffer [1 ]);
@@ -146,7 +186,14 @@ void LRL_MPU6050_ReadGyro(odom_cfgType *odom)
146186 odom -> gyro .z /= (GYRO_CORRECTOR / FLOAT_SCALING );
147187}
148188
149- void LRL_MPU6050_EnableBypass (odom_cfgType * odom , uint8_t enable )
189+ void LRL_MPU6050_ReadAll (odom_cfgType * odom )
190+ {
191+ _LRL_MPU6050_EnableBypass (odom , 0 );
192+ LRL_MPU6050_ReadAccel (odom );
193+ LRL_MPU6050_ReadGyro (odom );
194+ }
195+
196+ void _LRL_MPU6050_EnableBypass (odom_cfgType * odom , uint8_t enable )
150197{
151198 _i2c_reg_data = 0x00 ;
152199 HAL_I2C_Mem_Write (odom -> imu .hi2c , MPU_ADDR , USER_CTRL , 1 , & _i2c_reg_data , 1 , DELAY_TIMEOUT );
@@ -156,6 +203,56 @@ void LRL_MPU6050_EnableBypass(odom_cfgType * odom, uint8_t enable)
156203 HAL_I2C_Mem_Write (odom -> imu .hi2c , MPU_ADDR , PWR_MGMT_1 , 1 , & _i2c_reg_data , 1 , DELAY_TIMEOUT );
157204}
158205
206+ void LRL_MPU6050_ComplementaryFilter (odom_cfgType * odom )
207+ {
208+ _LRL_MPU6050_EnableBypass (odom , 0 );
209+ float dt = 0.01 ;
210+
211+ // Low-pass filter accelerometer data
212+ _tmp_accx = ALPHA * prev_acc_x + (1 - ALPHA ) * odom -> accel .x /FLOAT_SCALING ;
213+ _tmp_accy = ALPHA * prev_acc_y + (1 - ALPHA ) * odom -> accel .y /FLOAT_SCALING ;
214+ _tmp_accz = ALPHA * prev_acc_z + (1 - ALPHA ) * odom -> accel .z /FLOAT_SCALING ;
215+
216+ // Normalize accelerometer data
217+ float acc_norm = sqrtf (_tmp_accx * _tmp_accx + _tmp_accy * _tmp_accy + _tmp_accz * _tmp_accz );
218+ _tmp_accx /= acc_norm ;
219+ _tmp_accy /= acc_norm ;
220+ _tmp_accz /= acc_norm ;
221+
222+ // Update angle using accelerometer
223+ float acc_angle_x = atan2f (_tmp_accy , _tmp_accz ) * (180.0f / M_PI );
224+ float acc_angle_y = atan2f (_tmp_accx , _tmp_accz ) * (180.0f / M_PI );
225+ float acc_angle_z = atan2f (_tmp_accy , _tmp_accx ) * (180.0f / M_PI );
226+
227+ // Low-pass filter gyroscope data
228+ float gyr_x_filtered = ALPHA * prev_gyr_x + (1 - ALPHA ) * odom -> gyro .x /FLOAT_SCALING ;
229+ float gyr_y_filtered = ALPHA * prev_gyr_y + (1 - ALPHA ) * odom -> gyro .y /FLOAT_SCALING ;
230+ float gyr_z_filtered = ALPHA * prev_gyr_z + (1 - ALPHA ) * odom -> gyro .z /FLOAT_SCALING ;
231+
232+ // Update angle using gyroscope
233+ odom -> imu .roll_temp += gyr_x_filtered * dt ;
234+ odom -> imu .pitch_temp -= gyr_y_filtered * dt ;
235+ odom -> imu .yaw_temp += gyr_z_filtered * dt ;
236+
237+ // Complementary filter
238+ odom -> imu .roll_temp = (ALPHA * odom -> imu .roll_temp + (1 - ALPHA ) * acc_angle_x ) ;
239+ odom -> imu .pitch_temp = ((ALPHA * odom -> imu .pitch_temp ) - (1 - ALPHA ) * acc_angle_y );
240+ odom -> imu .yaw_temp = (ALPHA * odom -> imu .yaw_temp + (1 - ALPHA ) * acc_angle_z );
241+
242+ odom -> angle .x = odom -> imu .roll_temp - odom -> imu .offset_calibration_x ;
243+ odom -> angle .y = odom -> imu .pitch_temp - odom -> imu .offset_calibration_y ;
244+ odom -> angle .z = odom -> imu .yaw_temp - odom -> imu .offset_calibration_z ;
245+
246+ // Update previous values
247+ prev_acc_x = _tmp_accx ;
248+ prev_acc_y = _tmp_accy ;
249+ prev_acc_z = _tmp_accz ;
250+ prev_gyr_x = gyr_x_filtered ;
251+ prev_gyr_y = gyr_y_filtered ;
252+ prev_gyr_z = gyr_z_filtered ;
253+ }
254+
255+
159256// #########################################################
160257// #################### MOTOR ENCODER ####################
161258// #########################################################
0 commit comments