@@ -45,79 +45,89 @@ void mpu9250_read_reg(uint8_t reg, uint8_t *data, uint8_t len)
4545
4646void mpu9250_setup ()
4747{
48- mpu9250_write_reg (26 , 0x05 ); //enable digital low pass filter
49- mpu9250_write_reg (28 , 0x10 ); //set accelerometer full scale to +-8g
50- mpu9250_write_reg (27 , 0x08 ); //set gyroscope full scale full scale to +-500deg
51- mpu9250_calibrateGyro (1500 );
48+ mpu9250_write_reg (MPU9250_CONFIG , 0x05 ); //enable digital low pass filter
49+ mpu9250_write_reg (MPU9250_ACCEL_CONFIG , 0x10 ); //set accelerometer full scale to +-8g
50+ mpu9250_write_reg (MPU9250_GYRO_CONFIG , 0x08 ); //set gyroscope full scale full scale to +-500deg
51+
52+
53+ //
54+ // // magnetometer setup
55+ mpu9250_write_reg (MPU9250_USER_CTRL , 0x20 );
56+ mpu9250_write_reg (MPU9250_I2C_MST_CTRL , 0x0D );
57+ mpu9250_write_reg (MPU9250_I2C_SLV0_ADDR , 0x8C );
58+ mpu9250_write_reg (MPU9250_I2C_SLV0_REG , 0x03 );
59+
60+ mpu9250_init_ak8963 ();
61+ mpu9250_calibrateIMU (1500 );
5262 quat [0 ] = 1.0f ;
5363 quat [1 ] = 0.0f ;
5464 quat [2 ] = 0.0f ;
5565 quat [3 ] = 0.0f ;
56- //
57- // // magnetometer setup
58- // mpu9250_write_reg(0x6A, 0x20);
59- // mpu9250_write_reg(0x24, 0x0D);
60- // mpu9250_write_reg(0x25, 0x8C);
61- // mpu9250_write_reg(0x26, 0x03);
6266}
6367
6468void mpu9250_init_ak8963 ()
6569{
6670 uint8_t calibData [3 ]; // buffer for factory calibration data
6771
68- mpu9250_write_reg (0x27 , 0x00 ); // disable I2C_SLV0_CTRL temporarily
72+ mpu9250_write_reg (MPU9250_I2C_SLV0_CTRL , 0x00 ); // disable I2C_SLV0_CTRL temporarily
6973
70- mpu9250_write_reg (0x25 , 0x0C );
74+ mpu9250_write_reg (MPU9250_I2C_SLV0_ADDR , AK8963_I2C_ADDR );
7175 // I2C_SLV0_REG (0x26): Point to AK8963_CNTL (0x0A)
72- mpu9250_write_reg (0x26 , 0x0A );
76+ mpu9250_write_reg (MPU9250_I2C_SLV0_REG , AK8963_CNTL1 );
7377 // I2C_SLV0_DO (0x63): Data to write: 0x00 (power down)
74- mpu9250_write_reg (0x63 , 0x00 );
78+ mpu9250_write_reg (MPU9250_I2C_SLV0_DO , AK8963_POWER_DOWN );
7579 // I2C_SLV0_CTRL (0x27): Enable 1-byte write (0x80 | 1)
76- mpu9250_write_reg (0x27 , 0x81 );
80+ mpu9250_write_reg (MPU9250_I2C_SLV0_CTRL , 0x81 );
7781 HAL_Delay (10 );
7882
79- mpu9250_write_reg (0x63 , 0x0F );
80- mpu9250_write_reg (0x27 , 0x81 );
83+ mpu9250_write_reg (MPU9250_I2C_SLV0_DO , AK8963_FUSE_ROM_ACCESS );
84+ mpu9250_write_reg (MPU9250_I2C_SLV0_CTRL , 0x81 );
8185 HAL_Delay (10 );
8286
83- mpu9250_write_reg (0x25 , 0x0C | 0x80 );
87+ mpu9250_write_reg (MPU9250_I2C_SLV0_ADDR , AK8963_I2C_ADDR | 0x80 );
8488 // Set I2C_SLV0_REG to AK8963_ASAX (starting register for calibration data)
85- mpu9250_write_reg (0x26 , 0x10 );
89+ mpu9250_write_reg (MPU9250_I2C_SLV0_REG , AK8963_ASAX );
8690 // Enable reading 3 bytes (0x80 | 3)
87- mpu9250_write_reg (0x27 , 0x83 );
91+ mpu9250_write_reg (MPU9250_I2C_SLV0_CTRL , 0x83 );
8892 HAL_Delay (10 );
8993
90- mpu9250_read_reg (0x49 , calibData , 3 );
94+ mpu9250_read_reg (MPU9250_EXT_SENS_DATA_00 , calibData , 3 );
9195
9296 mag_calibration_data .calibData1 = (((float )calibData [0 ] - 128.0f ) / 256.0f ) + 1.0f ;
9397 mag_calibration_data .calibData2 = (((float )calibData [1 ] - 128.0f ) / 256.0f ) + 1.0f ;
9498 mag_calibration_data .calibData3 = (((float )calibData [2 ] - 128.0f ) / 256.0f ) + 1.0f ;
9599
96- mpu9250_write_reg (0x25 , 0x0C );
97- mpu9250_write_reg (0x26 , 0x0A );
98- mpu9250_write_reg (0x63 , 0x00 ); // Power down command
99- mpu9250_write_reg (0x27 , 0x81 );
100+ mpu9250_write_reg (MPU9250_I2C_SLV0_ADDR , AK8963_I2C_ADDR );
101+ mpu9250_write_reg (MPU9250_I2C_SLV0_REG , AK8963_CNTL1 );
102+ mpu9250_write_reg (MPU9250_I2C_SLV0_DO , AK8963_POWER_DOWN ); // Power down command
103+ mpu9250_write_reg (MPU9250_I2C_SLV0_CTRL , 0x81 );
100104 HAL_Delay (10 );
101105
102- uint8_t ctrlValue = (1 << 4 ) | 0 ;
103- mpu9250_write_reg (0x63 , ctrlValue );
104- mpu9250_write_reg (0x27 , 0x81 );
106+ // MADE ONE CHANGE HERE --> ADDED CONTINUOUS 100 HZ SAMPLING MODE
107+ uint8_t ctrlValue = (1 << 4 ) | AK8963_CONTINUOUS_100HZ ;
108+ mpu9250_write_reg (MPU9250_I2C_SLV0_DO , ctrlValue );
109+ mpu9250_write_reg (MPU9250_I2C_SLV0_CTRL , 0x81 );
105110 HAL_Delay (10 );
106111
107112 // ---- Step 6. Restore Automatic Continuous Reading ----
108113 // Reconfigure the I2C slave to read 7 bytes from the magnetometer starting at register 0x03 (HXL)
109- mpu9250_write_reg (0x25 , 0x0C | 0x80 ); // Set to read mode
110- mpu9250_write_reg (0x26 , 0x03 ); // Start at HXL register
111- mpu9250_write_reg (0x27 , 0x87 ); // Enable reading 7 bytes (0x80 | 7)
114+ mpu9250_write_reg (MPU9250_I2C_SLV0_ADDR , AK8963_I2C_ADDR | 0x80 ); // Set to read mode
115+ mpu9250_write_reg (MPU9250_I2C_SLV0_REG , AK8963_HXL ); // Start at HXL register
116+ mpu9250_write_reg (MPU9250_I2C_SLV0_CTRL , 0x87 ); // Enable reading 7 bytes (0x80 | 7)
112117}
113118
114- void mpu9250_calibrateGyro (uint16_t numCalPoints )
119+ void mpu9250_calibrateIMU (uint16_t numCalPoints )
115120{
116121 // Init
117122 int32_t x = 0 ;
118123 int32_t y = 0 ;
119124 int32_t z = 0 ;
120125
126+ int32_t mag_x , mag_y , mag_z ;
127+ int32_t max_x = INT32_MAX ,min_x = INT32_MIN ;
128+ int32_t max_y = INT32_MAX , min_y = INT32_MIN ;
129+ int32_t max_z = INT32_MAX , min_z = INT32_MIN ;
130+
121131 // Zero guard
122132 if (numCalPoints == 0 )
123133 {
@@ -131,30 +141,68 @@ void mpu9250_calibrateGyro(uint16_t numCalPoints)
131141 x += imu_raw_data .gyro_x ;
132142 y += imu_raw_data .gyro_y ;
133143 z += imu_raw_data .gyro_z ;
144+
145+ mag_x = imu_raw_data .mag_x ;
146+ mag_y = imu_raw_data .mag_y ;
147+ mag_z = imu_raw_data .mag_z ;
148+
149+ // Used for magnetometer hard-iron/soft-iron scaling
150+ if (mag_x > max_x ) max_x = mag_x ;
151+ if (mag_x < min_x ) min_x = mag_x ;
152+ if (mag_y > max_y ) max_y = mag_y ;
153+ if (mag_y < min_y ) min_y = mag_y ;
154+ if (mag_z > max_z ) max_z = mag_z ;
155+ if (mag_z < min_z ) min_z = mag_z ;
134156 HAL_Delay (3 );
135157 }
136158
137159 // Average the saved data points to find the gyroscope offset
138160 imu_processed_data .gyro_offX = (float )x / (float )numCalPoints ;
139161 imu_processed_data .gyro_offY = (float )y / (float )numCalPoints ;
140162 imu_processed_data .gyro_offZ = (float )z / (float )numCalPoints ;
141- }
142163
164+ imu_processed_data .mag_offX = (float )(max_x + min_x ) / 2.0f ;
165+ imu_processed_data .mag_offY = (float )(max_y + min_y ) / 2.0f ;
166+ imu_processed_data .mag_offZ = (float )(max_z + min_z ) / 2.0f ;
167+
168+ if (APPLY_SOFT_IRON_SCALING ) {
169+ float half_x = (float )(max_x - min_x ) / 2.0f ;
170+ float half_y = (float )(max_y - min_y ) / 2.0f ;
171+ float half_z = (float )(max_z - min_z ) / 2.0f ;
172+
173+ float r_max = fmaxf (fmaxf (half_x , half_y ), half_z );
174+
175+ imu_processed_data .mag_scaleX = r_max / half_x ;
176+ imu_processed_data .mag_scaleY = r_max / half_y ;
177+ imu_processed_data .mag_scaleZ = r_max / half_z ;
178+ } else {
179+ imu_processed_data .mag_scaleX = 1.0f ;
180+ imu_processed_data .mag_scaleY = 1.0f ;
181+ imu_processed_data .mag_scaleZ = 1.0f ;
182+ }
183+ }
143184
144185
145186void mpu9250_getRawData ()
146187{
147188 uint8_t imu_data [6 ];
148189
149- mpu9250_read_reg (59 , imu_data , sizeof (imu_data ));
190+ // addr # 59
191+ mpu9250_read_reg (MPU9250_ACCEL_XOUT_H , imu_data , sizeof (imu_data ));
150192 imu_raw_data .accel_x = ((int16_t )imu_data [0 ]<<8 ) | imu_data [1 ];
151193 imu_raw_data .accel_y = ((int16_t )imu_data [2 ]<<8 ) | imu_data [3 ];
152194 imu_raw_data .accel_z = ((int16_t )imu_data [4 ]<<8 ) | imu_data [5 ];
153195
154- mpu9250_read_reg (67 , imu_data , sizeof (imu_data ));
196+ // addr # 67
197+ mpu9250_read_reg (MPU9250_GYRO_XOUT_H , imu_data , sizeof (imu_data ));
155198 imu_raw_data .gyro_x = ((int16_t )imu_data [0 ]<<8 ) | imu_data [1 ];
156199 imu_raw_data .gyro_y = ((int16_t )imu_data [2 ]<<8 ) | imu_data [3 ];
157200 imu_raw_data .gyro_z = ((int16_t )imu_data [4 ]<<8 ) | imu_data [5 ];
201+
202+ mpu9250_read_reg (MPU9250_EXT_SENS_DATA_00 , imu_data , sizeof (imu_data ));
203+ imu_raw_data .mag_x = ((int16_t )imu_data [0 ]<<8 ) | imu_data [1 ];
204+ imu_raw_data .mag_y = ((int16_t )imu_data [2 ]<<8 ) | imu_data [3 ];
205+ imu_raw_data .mag_z = ((int16_t )imu_data [4 ]<<8 ) | imu_data [5 ];
158206}
159207
160208void mpu9250_getProcessedAngle ()
@@ -170,21 +218,21 @@ void mpu9250_getProcessedAngle()
170218 imu_processed_data .gyro_y = ((float )imu_raw_data .gyro_y - imu_processed_data .gyro_offY )/65.5 * M_PI /180.0f ;;
171219 imu_processed_data .gyro_z = ((float )imu_raw_data .gyro_z - imu_processed_data .gyro_offZ )/65.5 * M_PI /180.0f ;;
172220
173- // mpu9250_read_reg(0x49, imu_data, sizeof(imu_data));
174- // imu_raw_data.mag_x = ((int16_t)imu_data[0]<<8) | imu_data[1];
175- // imu_raw_data.mag_y = ((int16_t)imu_data[2]<<8) | imu_data[3];
176- // imu_raw_data.mag_z = ((int16_t)imu_data[4]<<8) | imu_data[5];
221+ imu_processed_data .mag_x = (((float )imu_raw_data .mag_x * mag_calibration_data .calibData1 ) - imu_processed_data .mag_offX ) * imu_processed_data .mag_scaleX ;
222+ imu_processed_data .mag_y = (((float )imu_raw_data .mag_y * mag_calibration_data .calibData2 ) - imu_processed_data .mag_offY ) * imu_processed_data .mag_scaleY ;
223+ imu_processed_data .mag_z = (((float )imu_raw_data .mag_z * mag_calibration_data .calibData3 ) - imu_processed_data .mag_offZ ) * imu_processed_data .mag_scaleZ ;
177224
178- MahonyAHRSupdateIMU (quat , imu_processed_data .gyro_x , imu_processed_data .gyro_y , imu_processed_data .gyro_z , imu_processed_data .accel_x , imu_processed_data .accel_y ,imu_processed_data .accel_z );
225+ MahonyAHRSupdate (quat , imu_processed_data .gyro_x , imu_processed_data .gyro_y , imu_processed_data .gyro_z , imu_processed_data .accel_x ,
226+ imu_processed_data .accel_y ,imu_processed_data .accel_z , imu_processed_data .mag_x , imu_processed_data .mag_y , imu_processed_data .mag_z );
179227
180228 /* Quternion to Euler */
181229 float radPitch = asinf (-2.0f * (quat [1 ] * quat [3 ] - quat [0 ] * quat [2 ]));
182230 float radRoll = atan2f (2.0f * (quat [0 ] * quat [1 ] + quat [2 ] * quat [3 ]), 2.0f * (quat [0 ] * quat [0 ] + quat [3 ] * quat [3 ]) - 1.0f );
183231 float radYaw = atan2f (2.0f * (quat [0 ] * quat [3 ] + quat [1 ] * quat [2 ]), 2.0f * (quat [0 ] * quat [0 ] + quat [1 ] * quat [1 ]) - 1.0f );
184- /* Radian to Degree*/
232+
233+ /* Radian to Degree*/
185234 imu_angles .pitch = radPitch * RAD_TO_DEG ;
186235 imu_angles .roll = radRoll * RAD_TO_DEG ;
187236 imu_angles .yaw = radYaw * RAD_TO_DEG ;
188-
189237}
190238
0 commit comments