Mercurial > public > ostc4

comparison Small_CPU/Src/compass.c @ 358:c6a084d1433f

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Add Support for new end-2019 hardware: support LSM303AGR compass (Now working) Make a build file for testing
author heinrichsweikamp
date Sun, 24 Nov 2019 16:08:29 +0100
parents c3d511365552
children f9458e979154
comparison
equal deleted inserted replaced
357:c3d511365552 358:c6a084d1433f
37 #include "spi.h" 37 #include "spi.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 TEST_IF_HMC5883L
43 //#define COMPASS_DEACTIVATE
44
45 /// split byte to bits 42 /// split byte to bits
46 typedef struct{ 43 typedef struct{
47 uint8_t bit0:1; ///< split byte to bits 44 uint8_t bit0:1; ///< split byte to bits
48 uint8_t bit1:1; ///< split byte to bits 45 uint8_t bit1:1; ///< split byte to bits
49 uint8_t bit2:1; ///< split byte to bits 46 uint8_t bit2:1; ///< split byte to bits
231 /// On first call it figures out which hardware is integrated 228 /// On first call it figures out which hardware is integrated
232 /// 229 ///
233 /// @param gain: 7 is max gain, compass_calib() might reduce it 230 /// @param gain: 7 is max gain, compass_calib() might reduce it
234 // =============================================================================== 231 // ===============================================================================
235 232
236 uint8_t testCompassTypeDebug = 0xFF;
237
238 void compass_init(uint8_t fast, uint8_t gain) 233 void compass_init(uint8_t fast, uint8_t gain)
239 { 234 {
240 // quick off
241 #ifdef COMPASS_DEACTIVATE
242 hardwareCompass = COMPASS_NOT_RECOGNIZED;
243 #endif
244 235
245 // don't call again with fast, gain in calib mode etc. 236 // don't call again with fast, gain in calib mode etc.
246 // if unknown 237 // if unknown
247 if(hardwareCompass == COMPASS_NOT_RECOGNIZED) 238 if(hardwareCompass == COMPASS_NOT_RECOGNIZED)
248 { 239 {
255 uint8_t data = WHO_AM_I; 246 uint8_t data = WHO_AM_I;
256 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); 247 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
257 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); 248 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
258 if(data == WHOIAM_VALUE_LSM303D) 249 if(data == WHOIAM_VALUE_LSM303D)
259 hardwareCompass = compass_generation2; //LSM303D; 250 hardwareCompass = compass_generation2; //LSM303D;
260 else 251 data = WHO_AM_I;
261 hardwareCompass = compass_generation1; //HMC5883L 252 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
253 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &data, 1);
262 if(data == WHOIAM_VALUE_LSM303AGR) 254 if(data == WHOIAM_VALUE_LSM303AGR)
263 hardwareCompass = compass_generation3; //LSM303AGR; 255 hardwareCompass = compass_generation3; //LSM303AGR;
264 } 256 }
265 257
266 /* No compass identified => Retry */ 258 /* No compass identified => Retry */
269 uint8_t data = WHO_AM_I; 261 uint8_t data = WHO_AM_I;
270 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1); 262 I2C_Master_Transmit( DEVICE_COMPASS_303D, &data, 1);
271 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1); 263 I2C_Master_Receive( DEVICE_COMPASS_303D, &data, 1);
272 if(data == WHOIAM_VALUE_LSM303D) 264 if(data == WHOIAM_VALUE_LSM303D)
273 hardwareCompass = compass_generation2; //LSM303D; 265 hardwareCompass = compass_generation2; //LSM303D;
274 else 266 data = WHO_AM_I;
275 hardwareCompass = compass_generation1; //HMC5883L; 267 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
268 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &data, 1);
276 if(data == WHOIAM_VALUE_LSM303AGR) 269 if(data == WHOIAM_VALUE_LSM303AGR)
277 hardwareCompass = compass_generation3; //LSM303AGR; 270 hardwareCompass = compass_generation3; //LSM303AGR;
278 } 271 }
279 272
280 /* Assume that a HMC5883L is equipped by default if detection still failed */ 273 /* Assume that a HMC5883L is equipped by default if detection still failed */
281 if(hardwareCompass == 0) 274 if(hardwareCompass == 0)
282 hardwareCompass = compass_generation1; //HMC5883L; 275 hardwareCompass = compass_generation1; //HMC5883L;
283 276
284 #ifdef TEST_IF_HMC5883L
285 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT; 277 HAL_StatusTypeDef resultOfOperationHMC_MMA = HAL_TIMEOUT;
286 278
279 // test if both chips of the two-chip solution (gen 1) are present
287 if(hardwareCompass == compass_generation1) // HMC5883L) 280 if(hardwareCompass == compass_generation1) // HMC5883L)
288 { 281 {
282 HAL_Delay(10);
283 MX_I2C1_Init();
289 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q 284 uint8_t data = 0x2A; // CTRL_REG1 of DEVICE_ACCELARATOR_MMA8452Q
290 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1); 285 resultOfOperationHMC_MMA = I2C_Master_Transmit( DEVICE_ACCELARATOR_MMA8452Q, &data, 1);
291 if(resultOfOperationHMC_MMA == HAL_OK) 286 if(resultOfOperationHMC_MMA == HAL_OK)
292 { 287 {
293 hardwareCompass = compass_generation1; //HMC5883L; // all fine, keep it 288 hardwareCompass = compass_generation1; //HMC5883L; // all fine, keep it
294 } 289 }
295 else 290 else
296 { 291 {
297 hardwareCompass = COMPASS_NOT_RECOGNIZED; 292 hardwareCompass = COMPASS_NOT_RECOGNIZED;
298 testCompassTypeDebug = 0xEC;
299 } 293 }
300 } 294 }
301 #endif
302 295
303 if(hardwareCompass == compass_generation2) //LSM303D) 296 if(hardwareCompass == compass_generation2) //LSM303D)
304 compass_init_LSM303D(fast, gain); 297 compass_init_LSM303D(fast, gain);
305 if(hardwareCompass == compass_generation3) //LSM303AGR) 298 if(hardwareCompass == compass_generation3) //LSM303AGR)
306 compass_init_LSM303AGR(fast, gain); 299 compass_init_LSM303AGR(fast, gain);
749 742
750 void compass_init_LSM303AGR(uint8_t fast, uint8_t gain) 743 void compass_init_LSM303AGR(uint8_t fast, uint8_t gain)
751 { 744 {
752 if(fast == 0) 745 if(fast == 0)
753 { 746 {
747 // init compass
754 LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz 748 LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz
755 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M 749 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
756 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M 750 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
751 LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_M
752
753 // init accel (Same chip, but different address...)
754 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // TEMP_CFG_REG_A (Temp sensor off)
755 LSM303AGR_acc_write_checked_reg(0x20, 0x4F); // CTRL_REG1_A (50Hz, x,y,z = ON)
756 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A
757 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A
758 LSM303AGR_acc_write_checked_reg(0x23, 0x08); // CTRL_REG4_A, High Resolution Mode enabled
757 } 759 }
758 else 760 else
759 { 761 {
760 LSM303AGR_write_checked_reg(0x60, 0x80); // 10Hz 762 // init compass
763 LSM303AGR_write_checked_reg(0x60, 0x84); // 20Hz
761 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M 764 LSM303AGR_write_checked_reg(0x61, 0x03); // CFG_REG_B_M
762 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M 765 LSM303AGR_write_checked_reg(0x62, 0x10); // CFG_REG_C_M
763 } 766 LSM303AGR_write_checked_reg(0x63, 0x00); // INT_CTRL_REG_M
764 // init accel (Same chip, but different address...) 767
765 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // TEMP_CFG_REG_A (Temp sensor off) 768 // init accel (Same chip, but different address...)
766 LSM303AGR_acc_write_checked_reg(0x20, 0x4F); // CTRL_REG1_A (10Hz, x,y,z = ON) 769 LSM303AGR_acc_write_checked_reg(0x1F, 0x00); // TEMP_CFG_REG_A (Temp sensor off)
767 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A 770 LSM303AGR_acc_write_checked_reg(0x20, 0x4F); // CTRL_REG1_A (50Hz, x,y,z = ON)
768 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A 771 LSM303AGR_acc_write_checked_reg(0x21, 0x00); // CTRL_REG2_A
769 LSM303AGR_acc_write_checked_reg(0x23, 0x04); // CTRL_REG4_A 772 LSM303AGR_acc_write_checked_reg(0x22, 0x00); // CTRL_REG3_A
773 LSM303AGR_acc_write_checked_reg(0x23, 0x0); // CTRL_REG4_A, High Resolution Mode enabled
774 }
770 775
771 return; 776 return;
772 } 777 }
773 778
774 779
805 accel_DY_f = 0; 810 accel_DY_f = 0;
806 accel_DZ_f = 0; 811 accel_DZ_f = 0;
807 812
808 for(int i=0;i<6;i++) 813 for(int i=0;i<6;i++)
809 { 814 {
810 data = ADDR_OUT_X_L_A + i; // ADDR_OUT_X_L_A is the same as in the LSM303D (luckily) 815 data = 0x28 + i; // OUT_X_L_A
811 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1); 816 I2C_Master_Transmit( DEVICE_ACCELARATOR_303AGR, &data, 1);
812 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &accDataBuffer[i], 1); 817 I2C_Master_Receive( DEVICE_ACCELARATOR_303AGR, &accDataBuffer[i], 1);
813 } 818 }
814 819
815 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0])))); 820 xraw_f = ((float)( (int16_t)((accDataBuffer[1] << 8) | (accDataBuffer[0]))));
819 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f); 824 rotate_accel_3f(&xraw_f, &yraw_f, &zraw_f);
820 825
821 // mh 826 // mh
822 accel_report_x = xraw_f; 827 accel_report_x = xraw_f;
823 accel_report_y = yraw_f; 828 accel_report_y = yraw_f;
824 accel_report_z = zraw_f; 829 accel_report_z = -zraw_f; // flip Z in gen 2 hardware
825 830
826 accel_DX_f = ((int16_t)(accel_report_x)); 831 accel_DX_f = ((int16_t)(accel_report_x));
827 accel_DY_f = ((int16_t)(accel_report_y)); 832 accel_DY_f = ((int16_t)(accel_report_y));
828 accel_DZ_f = ((int16_t)(accel_report_z)); 833 accel_DZ_f = ((int16_t)(accel_report_z));
829 } 834 }
855 860
856 // mh 160620 flip x and y if flip display 861 // mh 160620 flip x and y if flip display
857 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0])))); 862 compass_DX_f = (((int16_t)((magDataBuffer[1] << 8) | (magDataBuffer[0]))));
858 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2])))); 863 compass_DY_f = (((int16_t)((magDataBuffer[3] << 8) | (magDataBuffer[2]))));
859 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4])))); 864 compass_DZ_f = (((int16_t)((magDataBuffer[5] << 8) | (magDataBuffer[4]))));
860 // no rotation 865
866 // align axis in gen 2 hardware
867 compass_DZ_f *= -1;
868
861 return; 869 return;
862 } 870 }
863 871
864 872
865 // -------------------------------------------------------------------------------- 873 // --------------------------------------------------------------------------------