Mercurial > public > ostc4

comparison Small_CPU/Src/compass.c @ 185:1bb24fd3fc92

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Merged in Ideenmodellierer/ostc4/Cleanup_Compass_Wireless (pull request #4) Cleanup Compass Wireless
author heinrichsweikamp <bitbucket@heinrichsweikamp.com>
date Fri, 15 Mar 2019 08:22:16 +0000
parents 9baecc0c24b2
children a61b46a5265b
comparison
equal deleted inserted replaced
178:d36596281501 185:1bb24fd3fc92
37 #include "i2c.h" 37 #include "i2c.h"
38 #include "RTE_FlashAccess.h" // to store compass_calib_data 38 #include "RTE_FlashAccess.h" // to store compass_calib_data
39 39
40 #include "stm32f4xx_hal.h" 40 #include "stm32f4xx_hal.h"
41 41
42 #define MODE_LSM303DLHC
43 #define TEST_IF_HMC5883L 42 #define TEST_IF_HMC5883L
44 //#define COMPASS_DEACTIVATE 43 //#define COMPASS_DEACTIVATE
45 44
46 /// split byte to bits 45 /// split byte to bits
47 typedef struct{ 46 typedef struct{
110 #define Q_PI (18000) 109 #define Q_PI (18000)
111 #define Q_PIO2 (9000) 110 #define Q_PIO2 (9000)
112 111
113 #define HMC5883L (1) ///< id used with hardwareCompass 112 #define HMC5883L (1) ///< id used with hardwareCompass
114 #define LSM303D (2) ///< id used with hardwareCompass 113 #define LSM303D (2) ///< id used with hardwareCompass
115 #define LSM303DLHC (3) ///< id used with hardwareCompass
116 #define COMPASS_NOT_RECOGNIZED (4) ///< id used with hardwareCompass 114 #define COMPASS_NOT_RECOGNIZED (4) ///< id used with hardwareCompass
117 115
118 116
119 ////////////////////////////////////////////////////////////////////////////// 117 //////////////////////////////////////////////////////////////////////////////
120 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error 118 // fifth order of polynomial approximation of atan(), giving 0.05 deg max error
223 void compass_init_LSM303D(uint8_t fast, uint8_t gain); 221 void compass_init_LSM303D(uint8_t fast, uint8_t gain);
224 void compass_sleep_LSM303D(void); 222 void compass_sleep_LSM303D(void);
225 void compass_read_LSM303D(void); 223 void compass_read_LSM303D(void);
226 void acceleration_read_LSM303D(void); 224 void acceleration_read_LSM303D(void);
227 225
228 void compass_init_LSM303DLHC(uint8_t fast, uint8_t gain);
229 void compass_sleep_LSM303DLHC(void);
230 void compass_read_LSM303DLHC(void);
231 void acceleration_read_LSM303DLHC(void);
232
233
234 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); 226 int LSM303D_accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
235 int compass_calib_common(void); 227 int compass_calib_common(void);
236 228
237 void compass_calc_roll_pitch_only(void); 229 void compass_calc_roll_pitch_only(void);
238 230
277 hardwareCompass = LSM303D; 269 hardwareCompass = LSM303D;
278 else 270 else
279 hardwareCompass = HMC5883L; 271 hardwareCompass = HMC5883L;
280 } 272 }
281 273
282 274 /* No compass identified => Retry */
283 // k�nnte Probleme mit altem Chip machen
284 // beim 303D f�hrt dieser Code dazu, dass WHOIAM_VALUE nicht geschickt wird!!!
285
286 #ifdef MODE_LSM303DLHC
287 HAL_StatusTypeDef resultOfOperation = HAL_TIMEOUT;
288
289 if(hardwareCompass == 0)
290 {
291 uint8_t data = DLHC_CTRL_REG1_A;
292 resultOfOperation = I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, &data, 1);
293 if(resultOfOperation == HAL_OK)
294 {
295 I2C_Master_Receive( DEVICE_ACCELARATOR_303DLHC, &data, 1);
296 testCompassTypeDebug = data;
297 if((data & 0x0f) == 0x07)
298 {
299 hardwareCompass = LSM303DLHC;
300 }
301 }
302 else
303 {
304 testCompassTypeDebug = 0xEE;
305 }
306 }
307
308 #endif
309
310 if(hardwareCompass == 0) 275 if(hardwareCompass == 0)
311 { 276 {
312 uint8_t data = WHO_AM_I; 277 uint8_t data = WHO_AM_I;
313 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); 278 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
314 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); 279 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
316 hardwareCompass = LSM303D; 281 hardwareCompass = LSM303D;
317 else 282 else
318 hardwareCompass = HMC5883L; 283 hardwareCompass = HMC5883L;
319 } 284 }
320 285
321 // was in else before ! 286 /* Assume that a HMC5883L is equipped by default if detection still failed */
322 if(hardwareCompass == 0) 287 if(hardwareCompass == 0)
323 hardwareCompass = HMC5883L; 288 hardwareCompass = HMC5883L;
324 289
325 #ifdef TEST_IF_HMC5883L 290 #ifdef TEST_IF_HMC5883L
326 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT; 291 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
339 testCompassTypeDebug = 0xEC; 304 testCompassTypeDebug = 0xEC;
340 } 305 }
341 } 306 }
342 #endif 307 #endif
343 308
344
345 if(hardwareCompass == LSM303DLHC)
346 {
347 compass_init_LSM303DLHC(fast, gain);
348 }
349 else
350 if(hardwareCompass == LSM303D) 309 if(hardwareCompass == LSM303D)
351 { 310 {
352 compass_init_LSM303D(fast, gain); 311 compass_init_LSM303D(fast, gain);
353 } 312 }
354 else 313 else
372 // compass_calib 331 // compass_calib
373 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D 332 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D
374 // =============================================================================== 333 // ===============================================================================
375 int compass_calib(void) 334 int compass_calib(void)
376 { 335 {
377 if(hardwareCompass == LSM303DLHC)
378 {
379 return compass_calib_common(); // 170821 zur Zeit kein lowpass filtering gefunden, nur high pass auf dem Register ohne Erkl�rung
380 }
381 else
382 if(hardwareCompass == LSM303D) 336 if(hardwareCompass == LSM303D)
383 { 337 {
384 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); 338 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);
385 int out = compass_calib_common(); 339 int out = compass_calib_common();
386 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); 340 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
404 // compass_sleep 358 // compass_sleep
405 /// @brief low power mode 359 /// @brief low power mode
406 // =============================================================================== 360 // ===============================================================================
407 void compass_sleep(void) 361 void compass_sleep(void)
408 { 362 {
409 if(hardwareCompass == LSM303DLHC)
410 {
411 compass_sleep_LSM303DLHC();
412 }
413 else
414 if(hardwareCompass == LSM303D) 363 if(hardwareCompass == LSM303D)
415 { 364 {
416 compass_sleep_LSM303D(); 365 compass_sleep_LSM303D();
417 } 366 }
418 else 367 else
428 /// @brief reads magnetometer and accelerometer for LSM303D, 377 /// @brief reads magnetometer and accelerometer for LSM303D,
429 /// otherwise magnetometer only 378 /// otherwise magnetometer only
430 // =============================================================================== 379 // ===============================================================================
431 void compass_read(void) 380 void compass_read(void)
432 { 381 {
433 if(hardwareCompass == LSM303DLHC)
434 {
435 compass_read_LSM303DLHC();
436 }
437 else
438 if(hardwareCompass == LSM303D) 382 if(hardwareCompass == LSM303D)
439 { 383 {
440 compass_read_LSM303D(); 384 compass_read_LSM303D();
441 } 385 }
442 else 386 else
451 // accelerator_init 395 // accelerator_init
452 /// @brief empty for for LSM303D 396 /// @brief empty for for LSM303D
453 // =============================================================================== 397 // ===============================================================================
454 void accelerator_init(void) 398 void accelerator_init(void)
455 { 399 {
456 // if((hardwareCompass != LSM303D) && (hardwareCompass != LSM303DLHC))
457 if(hardwareCompass == HMC5883L) 400 if(hardwareCompass == HMC5883L)
458 accelerator_init_MMA8452Q(); 401 accelerator_init_MMA8452Q();
459 } 402 }
460 403
461 404
463 // accelerator_sleep 406 // accelerator_sleep
464 /// @brief empty for for LSM303D 407 /// @brief empty for for LSM303D
465 // =============================================================================== 408 // ===============================================================================
466 void accelerator_sleep(void) 409 void accelerator_sleep(void)
467 { 410 {
468 // if((hardwareCompass != LSM303D) && (hardwareCompass != LSM303DLHC))
469 if(hardwareCompass == HMC5883L) 411 if(hardwareCompass == HMC5883L)
470 accelerator_sleep_MMA8452Q(); 412 accelerator_sleep_MMA8452Q();
471 } 413 }
472 414
473 415
475 // acceleration_read 417 // acceleration_read
476 /// @brief empty for for LSM303D 418 /// @brief empty for for LSM303D
477 // =============================================================================== 419 // ===============================================================================
478 void acceleration_read(void) 420 void acceleration_read(void)
479 { 421 {
480 if(hardwareCompass == LSM303DLHC)
481 {
482 acceleration_read_LSM303DLHC();
483 }
484 else
485 if(hardwareCompass == LSM303D) 422 if(hardwareCompass == LSM303D)
486 { 423 {
487 acceleration_read_LSM303D(); 424 acceleration_read_LSM303D();
488 } 425 }
489 else 426 else
545 482
546 val = LSM303D_read_reg(reg); 483 val = LSM303D_read_reg(reg);
547 val &= ~clearbits; 484 val &= ~clearbits;
548 val |= setbits; 485 val |= setbits;
549 LSM303D_write_checked_reg(reg, val); 486 LSM303D_write_checked_reg(reg, val);
550 }
551
552
553
554 // ===============================================================================
555 // LSM303DLHC_accelerator_read_req
556 /// @brief
557 // ===============================================================================
558 uint8_t LSM303DLHC_accelerator_read_req(uint8_t addr)
559 {
560 uint8_t data;
561
562 I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, &addr, 1);
563 I2C_Master_Receive( DEVICE_ACCELARATOR_303DLHC, &data, 1);
564 return data;
565 }
566
567
568 // ===============================================================================
569 // LSM303DLHC_accelerator_write_req
570 /// @brief
571 // ===============================================================================
572 void LSM303DLHC_accelerator_write_req(uint8_t addr, uint8_t value)
573 {
574 uint8_t data[2];
575
576 /* enable accel*/
577 data[0] = addr;
578 data[1] = value;
579 I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, data, 2);
580 } 487 }
581 488
582 /* 489 /*
583 // =============================================================================== 490 // ===============================================================================
584 // LSM303D_accel_set_range 491 // LSM303D_accel_set_range
1126 1033
1127 compass_DX_f = (int16_t)(mag_report_x * 1000.0f); // 1000.0 is just a wild guess by hw 1034 compass_DX_f = (int16_t)(mag_report_x * 1000.0f); // 1000.0 is just a wild guess by hw
1128 compass_DY_f = (int16_t)(mag_report_y * 1000.0f); 1035 compass_DY_f = (int16_t)(mag_report_y * 1000.0f);
1129 compass_DZ_f = (int16_t)(mag_report_z * 1000.0f); 1036 compass_DZ_f = (int16_t)(mag_report_z * 1000.0f);
1130 */ 1037 */
1131 }
1132
1133
1134 // ===============================================================================
1135 // compass_init_LSM303DLHC
1136 /// @brief The new ST 303DLHC 2017/2018
1137 /// This might be called several times with different gain values during calibration
1138 /// but gain change is not supported at the moment.
1139 /// parts from KOMPASS LSM303DLH-compass-app-note.pdf
1140 ///
1141 /// @param gain:
1142 /// @param fast:
1143 // ===============================================================================
1144
1145
1146
1147 void compass_init_LSM303DLHC(uint8_t fast, uint8_t gain)
1148 {
1149 // acceleration
1150 // todo : BDU an (wie 303D) und high res, beides in REG4
1151 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier k�nnte filter sein 0x8?80, cutoff freq. not beschrieben
1152
1153 if(fast == 0)
1154 {
1155 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x27); // 10 hz
1156 }
1157 else
1158 {
1159 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x57); // 100 hz
1160 }
1161 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x88); // 0x88: BDU + HighRes, BDU ist doof!
1162 LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x00); // 0x00 little-endian, ist's immer
1163 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x08); // 0x08: HighRes
1164 //LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x80); //
1165
1166
1167 // compass
1168 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x10); // 15 Hz
1169
1170 if(fast == 0)
1171 {
1172 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x10); // 15 Hz
1173 }
1174 else
1175 {
1176 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x18); // 75 Hz
1177 }
1178 LSM303D_write_checked_reg(DLHC_CRB_REG_M,0x20); // 0x60: 2.5 Gauss ,0x40: +/-1.9 Gauss,0x20: +/-1.3 Gauss
1179 LSM303D_write_checked_reg(DLHC_MR_REG_M,0x00); //continuous conversation
1180
1181
1182
1183 return;
1184
1185
1186 // LSM303D_write_checked_reg(,);
1187 // LSM303D_write_checked_reg(DLHC_CTRL_REG1_A, 0x27); // 0x27 = acc. normal mode with ODR 50Hz - passt nicht mit datenblatt!!
1188 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x40); // 0x40 = full scale range �2 gauss in continuous data update mode and change the little-endian to a big-endian structure.
1189
1190 if(fast == 0)
1191 {
1192 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x27); // 0x27 = acc. normal mode, all axes, with ODR 10HZ laut LSM303DLHC, page 25/42
1193 //
1194 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier k�nnte filter sein 0x8?80, cutoff freq. not beschrieben
1195 //LSM303D_write_checked_reg(DLHC_CTRL_REG3_A,0x00); // 0x00 default
1196 //
1197 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG4_A, 0x00); // 0x00 = ich glaube little-endian ist gut
1198 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x40); // 0x00 = ich glaube little-endian ist gut
1199 //
1200 //LSM303D_write_checked_reg(DLHC_CTRL_REG5_A,0x00); // 0x00 default
1201 //LSM303D_write_checked_reg(DLHC_CTRL_REG6_A,0x00); // 0x00 default
1202 // magnetic sensor
1203 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x10); // 15 Hz
1204 }
1205 else
1206 {
1207 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x57); // 0x57 = acc. normal mode, all axes, with ODR 100HZ, LSM303DLHC, page 25/42
1208 //
1209 //LSM303D_write_checked_reg(DLHC_CTRL_REG2_A,0x00); // 0x00 default, hier k�nnte filter sein 0x8?80, cutoff freq. not beschrieben
1210 //LSM303D_write_checked_reg(DLHC_CTRL_REG3_A,0x00); // 0x00 default
1211 //
1212 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG4_A, 0x00); // 0x00 = ich glaube little-endian ist gut
1213 // LSM303D_write_checked_reg(DLHC_CTRL_REG4_A, 0x40); // 0x00 = ich glaube little-endian ist gut
1214 //
1215 //LSM303D_write_checked_reg(DLHC_CTRL_REG5_A,0x00); // 0x00 default
1216 //LSM303D_write_checked_reg(DLHC_CTRL_REG6_A,0x00); // 0x00 default
1217 // magnetic sensor
1218 LSM303D_write_checked_reg(DLHC_CRA_REG_M,0x18); // 75 Hz
1219 }
1220 LSM303D_write_checked_reg(DLHC_CRB_REG_M,0x02); // +/-1.9 Gauss
1221 LSM303D_write_checked_reg(DLHC_MR_REG_M,0x00); //continuous conversation
1222
1223
1224 /*
1225 // matthias version 160620
1226 if(fast == 0)
1227 {
1228 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
1229 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x3F); // mod 12,5 Hz 3 instead of 6,25 Hz 2
1230 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0); // anti alias 50 Hz (minimum)
1231 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
1232 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
1233 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x68); // mod 12,5 Hz 8 instead of 6,25 Hz 4
1234 }
1235 else
1236 {
1237 LSM303D_write_checked_reg(ADDR_CTRL_REG0, 0x00);
1238 LSM303D_write_checked_reg(ADDR_CTRL_REG1, 0x6F); // 100 Hz
1239 LSM303D_write_checked_reg(ADDR_CTRL_REG2, 0xC0);
1240 LSM303D_write_checked_reg(ADDR_CTRL_REG3, 0x00);
1241 LSM303D_write_checked_reg(ADDR_CTRL_REG4, 0x00);
1242 LSM303D_write_checked_reg(ADDR_CTRL_REG5, 0x74); // 100 Hz
1243 }
1244 LSM303D_write_checked_reg(ADDR_CTRL_REG6, 0x00);
1245 LSM303D_write_checked_reg(ADDR_CTRL_REG7, 0x00);
1246 */
1247 return;
1248 }
1249
1250 // ===============================================================================
1251 // compass_sleep_LSM303DLHC
1252 /// @brief The new 2017/2018 compass chip.
1253 // ===============================================================================
1254 void compass_sleep_LSM303DLHC(void)
1255 {
1256 LSM303DLHC_accelerator_write_req(DLHC_CTRL_REG1_A, 0x07); // CTRL_REG1_A: linear acceleration Power-down mode
1257 LSM303D_write_checked_reg(DLHC_MR_REG_M, 0x02); // MR_REG_M: magnetic sensor Power-down mode
1258 }
1259
1260
1261 // ===============================================================================
1262 // compass_read_LSM303DLHC
1263 /// @brief The new 2017/2018 compass chip.
1264 // ===============================================================================
1265 void compass_read_LSM303DLHC(void)
1266 {
1267 uint8_t data;
1268
1269 memset(magDataBuffer,0,6);
1270
1271 compass_DX_f = 0;
1272 compass_DY_f = 0;
1273 compass_DZ_f = 0;
1274
1275 for(int i=0;i<6;i++)
1276 {
1277 data = DLHC_OUT_X_L_M + i;
1278 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
1279 I2C_Master_Receive( DEVICE_COMPASS_303D, &magDataBuffer[i], 1);
1280 }
1281
1282 // 303DLHC new order
1283 compass_DX_f = (((int16_t)((magDataBuffer[0] << 8) | (magDataBuffer[1]))));
1284 compass_DZ_f = (((int16_t)((magDataBuffer[2] << 8) | (magDataBuffer[3]))));
1285 compass_DY_f = (((int16_t)((magDataBuffer[4] << 8) | (magDataBuffer[5]))));
1286
1287 // no rotation, otherwise see compass_read_LSM303D()
1288 return;
1289 }
1290
1291
1292 // ===============================================================================
1293 // acceleration_read_LSM303DLHC
1294 /// @brief The new 2017/2018 compass chip.
1295 // ===============================================================================
1296 void acceleration_read_LSM303DLHC(void)
1297 {
1298 uint8_t data;
1299 float xraw_f, yraw_f, zraw_f;
1300 float accel_report_x, accel_report_y, accel_report_z;
1301
1302 memset(accDataBuffer,0,6);
1303
1304 accel_DX_f = 0;
1305 accel_DY_f = 0;
1306 accel_DZ_f = 0;
1307
1308 for(int i=0;i<6;i++)
1309 {
1310 data = DLHC_OUT_X_L_A + i;
1311 I2C_Master_Transmit( DEVICE_ACCELARATOR_303DLHC, &data, 1);
1312 I2C_Master_Receive( DEVICE_ACCELARATOR_303DLHC, &accDataBuffer[i], 1);
1313 }
1314
1315 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
1316 yraw_f = ((float)( (int16_t)((accDataBuffer[3] << 8) | (accDataBuffer[2]))));
1317 zraw_f = ((float)( (int16_t)((accDataBuffer[5] << 8) | (accDataBuffer[4]))));
1318
1319 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
1320
1321 // mh f�r 303D
1322 accel_report_x = xraw_f;
1323 accel_report_y = yraw_f;
1324 accel_report_z = zraw_f;
1325
1326 accel_DX_f = ((int16_t)(accel_report_x));
1327 accel_DY_f = ((int16_t)(accel_report_y));
1328 accel_DZ_f = ((int16_t)(accel_report_z));
1329 } 1038 }
1330 1039
1331 1040
1332 // -------------------------------------------------------------------------------- 1041 // --------------------------------------------------------------------------------
1333 // ----------EARLIER COMPONENTS --------------------------------------------------- 1042 // ----------EARLIER COMPONENTS ---------------------------------------------------