Mercurial > public > ostc4

comparison Small_CPU/Src/compass.c @ 180:9ecc2e60418d Cleanup_Compass_Wireless

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Removed not used compass code Code was prepared for usage of LSM303DLHCTR but at the end the OSTC4 was never equipped with this chip
author ideenmodellierer
date Wed, 13 Mar 2019 21:49:25 +0100
parents 14e4c83a7559
children 9baecc0c24b2
comparison
equal deleted inserted replaced
179:a492a7a457b7 180:9ecc2e60418d
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
277 hardwareCompass = LSM303D; 275 hardwareCompass = LSM303D;
278 else 276 else
279 hardwareCompass = HMC5883L; 277 hardwareCompass = HMC5883L;
280 } 278 }
281 279
282 280 /* 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) 281 if(hardwareCompass == 0)
311 { 282 {
312 uint8_t data = WHO_AM_I; 283 uint8_t data = WHO_AM_I;
313 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); 284 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
314 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); 285 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
316 hardwareCompass = LSM303D; 287 hardwareCompass = LSM303D;
317 else 288 else
318 hardwareCompass = HMC5883L; 289 hardwareCompass = HMC5883L;
319 } 290 }
320 291
321 // was in else before ! 292 /* Assume that a HMC5883L is equipped by default if detection still failed */
322 if(hardwareCompass == 0) 293 if(hardwareCompass == 0)
323 hardwareCompass = HMC5883L; 294 hardwareCompass = HMC5883L;
324 295
325 #ifdef TEST_IF_HMC5883L 296 #ifdef TEST_IF_HMC5883L
326 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT; 297 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
339 testCompassTypeDebug = 0xEC; 310 testCompassTypeDebug = 0xEC;
340 } 311 }
341 } 312 }
342 #endif 313 #endif
343 314
344
345 if(hardwareCompass == LSM303DLHC)
346 {
347 compass_init_LSM303DLHC(fast, gain);
348 }
349 else
350 if(hardwareCompass == LSM303D) 315 if(hardwareCompass == LSM303D)
351 { 316 {
352 compass_init_LSM303D(fast, gain); 317 compass_init_LSM303D(fast, gain);
353 } 318 }
354 else 319 else
372 // compass_calib 337 // compass_calib
373 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D 338 /// @brief with onchip_lowpass_filter configuration for accelerometer of LSM303D
374 // =============================================================================== 339 // ===============================================================================
375 int compass_calib(void) 340 int compass_calib(void)
376 { 341 {
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) 342 if(hardwareCompass == LSM303D)
383 { 343 {
384 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773); 344 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(773);
385 int out = compass_calib_common(); 345 int out = compass_calib_common();
386 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); 346 LSM303D_accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
404 // compass_sleep 364 // compass_sleep
405 /// @brief low power mode 365 /// @brief low power mode
406 // =============================================================================== 366 // ===============================================================================
407 void compass_sleep(void) 367 void compass_sleep(void)
408 { 368 {
409 if(hardwareCompass == LSM303DLHC)
410 {
411 compass_sleep_LSM303DLHC();
412 }
413 else
414 if(hardwareCompass == LSM303D) 369 if(hardwareCompass == LSM303D)
415 { 370 {
416 compass_sleep_LSM303D(); 371 compass_sleep_LSM303D();
417 } 372 }
418 else 373 else
428 /// @brief reads magnetometer and accelerometer for LSM303D, 383 /// @brief reads magnetometer and accelerometer for LSM303D,
429 /// otherwise magnetometer only 384 /// otherwise magnetometer only
430 // =============================================================================== 385 // ===============================================================================
431 void compass_read(void) 386 void compass_read(void)
432 { 387 {
433 if(hardwareCompass == LSM303DLHC)
434 {
435 compass_read_LSM303DLHC();
436 }
437 else
438 if(hardwareCompass == LSM303D) 388 if(hardwareCompass == LSM303D)
439 { 389 {
440 compass_read_LSM303D(); 390 compass_read_LSM303D();
441 } 391 }
442 else 392 else
451 // accelerator_init 401 // accelerator_init
452 /// @brief empty for for LSM303D 402 /// @brief empty for for LSM303D
453 // =============================================================================== 403 // ===============================================================================
454 void accelerator_init(void) 404 void accelerator_init(void)
455 { 405 {
456 // if((hardwareCompass != LSM303D) && (hardwareCompass != LSM303DLHC))
457 if(hardwareCompass == HMC5883L) 406 if(hardwareCompass == HMC5883L)
458 accelerator_init_MMA8452Q(); 407 accelerator_init_MMA8452Q();
459 } 408 }
460 409
461 410
463 // accelerator_sleep 412 // accelerator_sleep
464 /// @brief empty for for LSM303D 413 /// @brief empty for for LSM303D
465 // =============================================================================== 414 // ===============================================================================
466 void accelerator_sleep(void) 415 void accelerator_sleep(void)
467 { 416 {
468 // if((hardwareCompass != LSM303D) && (hardwareCompass != LSM303DLHC))
469 if(hardwareCompass == HMC5883L) 417 if(hardwareCompass == HMC5883L)
470 accelerator_sleep_MMA8452Q(); 418 accelerator_sleep_MMA8452Q();
471 } 419 }
472 420
473 421
475 // acceleration_read 423 // acceleration_read
476 /// @brief empty for for LSM303D 424 /// @brief empty for for LSM303D
477 // =============================================================================== 425 // ===============================================================================
478 void acceleration_read(void) 426 void acceleration_read(void)
479 { 427 {
480 if(hardwareCompass == LSM303DLHC)
481 {
482 acceleration_read_LSM303DLHC();
483 }
484 else
485 if(hardwareCompass == LSM303D) 428 if(hardwareCompass == LSM303D)
486 { 429 {
487 acceleration_read_LSM303D(); 430 acceleration_read_LSM303D();
488 } 431 }
489 else 432 else
545 488
546 val = LSM303D_read_reg(reg); 489 val = LSM303D_read_reg(reg);
547 val &= ~clearbits; 490 val &= ~clearbits;
548 val |= setbits; 491 val |= setbits;
549 LSM303D_write_checked_reg(reg, val); 492 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 } 493 }
581 494
582 /* 495 /*
583 // =============================================================================== 496 // ===============================================================================
584 // LSM303D_accel_set_range 497 // LSM303D_accel_set_range
1126 1039
1127 compass_DX_f = (int16_t)(mag_report_x * 1000.0f); // 1000.0 is just a wild guess by hw 1040 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); 1041 compass_DY_f = (int16_t)(mag_report_y * 1000.0f);
1129 compass_DZ_f = (int16_t)(mag_report_z * 1000.0f); 1042 compass_DZ_f = (int16_t)(mag_report_z * 1000.0f);
1130 */ 1043 */
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 } 1044 }
1330 1045
1331 1046
1332 // -------------------------------------------------------------------------------- 1047 // --------------------------------------------------------------------------------
1333 // ----------EARLIER COMPONENTS --------------------------------------------------- 1048 // ----------EARLIER COMPONENTS ---------------------------------------------------