[STM32][MPU9250][AK8963] Problem z odczytem danych z magnetometru

[fasolus]

Cześć. 

Mam mały problem z modułem MPU9250, a konkretniej AK8963 (Magnetometr). Odczyt wartości zmierzonych przez akcelerometr i żyroskop nie sprawiły mi większych problemów i wszystko działa prawidłowo. Sprawa skomplikowała się kiedy zechciałem odczytać wartości zmierzone przez magnetometr. Ogólnie komunikacja z magnetometrem działa prawidłowo (WHO_AM_I itd.). Problem polega na tym, że po odczytaniu wartości z rejestrów HXL, HXH... cały czas otrzymuję wartość -1 dla każdej osi.  

Fragment kodu z inicjalizacją magnetometru: 

MPU9250_Error_code MPU9250_Magnetometer_Configuration(I2C_HandleTypeDef *I2Cx,
												      struct MPU9250 *DataStructure) {


	uint8_t Byte_temp = 0x00;
	uint8_t Bytes_temp[3] = {0};

	DataStructure->Magnetometer_addres = 0x0C << 1;

	// Case 2: Disable the I2C master interface
	Byte_temp = 0x00;

	if( HAL_I2C_Mem_Write(I2Cx, DataStructure->Device_addres, MPU9250_USER_CTRL, 1, &Byte_temp, 1, 1000) != HAL_OK ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Magnetometer_Config_FAIL;
	}

	// Case 3: Enable the bypass multiplexer
	Byte_temp = 0x02;

	if( HAL_I2C_Mem_Write(I2Cx, DataStructure->Device_addres, MPU9250_INT_PIN_CFG, 1, &Byte_temp, 1, 1000) != HAL_OK ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Magnetometer_Config_FAIL;
	}

	// Case 1: Is device connected ?
	if( HAL_I2C_IsDeviceReady(I2Cx, DataStructure->Magnetometer_addres, 1, 1000) != HAL_OK ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Magnetometer_Config_FAIL;
	}

	// Case 2: Who am i test
	if( HAL_I2C_Mem_Read(I2Cx, DataStructure->Magnetometer_addres, MPU9250_WIA, 1, &Byte_temp, 1, 1000) != HAL_OK ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Init_FAIL;
	}

	if( Byte_temp != 0x48 ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Init_FAIL;
	}

	// Case 4: Setup to fuse ROM access mode and 16-bit output
	Byte_temp = 0x1F;

	if( HAL_I2C_Mem_Write(I2Cx, DataStructure->Magnetometer_addres, MPU9250_CNTL1, 1, &Byte_temp, 1, 1000) != HAL_OK ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Magnetometer_Config_FAIL;
	}

	HAL_Delay(100);

	// Case 5: Read from the fuse ROM sensitivity adjustment values
	if( HAL_I2C_Mem_Read(I2Cx, DataStructure->Magnetometer_addres, MPU9250_ASAX | 0x80, 1, Bytes_temp, 3, 1000) != HAL_OK ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Magnetometer_Config_FAIL;
	}

	DataStructure->Magnetometer_ASAX = ( ( (Bytes_temp[0] - 128) * 0.5 ) / 128 ) + 1;
	DataStructure->Magnetometer_ASAY = ( ( (Bytes_temp[1] - 128) * 0.5 ) / 128 ) + 1;
	DataStructure->Magnetometer_ASAZ = ( ( (Bytes_temp[2] - 128) * 0.5 ) / 128 ) + 1;

	// Case 6: Reset to power down mode
	Byte_temp = 0x00;

	if( HAL_I2C_Mem_Write(I2Cx, DataStructure->Magnetometer_addres, MPU9250_CNTL1, 1, &Byte_temp, 1, 1000) != HAL_OK ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Magnetometer_Config_FAIL;
	}

	// Case 7: Enable continuous mode 2 and 16-bit output
	Byte_temp = 0x16;

	if( HAL_I2C_Mem_Write(I2Cx, DataStructure->Magnetometer_addres, MPU9250_CNTL1, 1, &Byte_temp, 1, 1000) != HAL_OK ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Magnetometer_Config_FAIL;
	}

	HAL_Delay(100);

	return MPU9250_Magnetometer_Config_OK;
}

 

Fragment kodu z odczytaniem zmierzonych wartości: 

MPU9250_Error_code MPU9250_Read_Magnetometer(I2C_HandleTypeDef *I2Cx,
										     struct MPU9250 *DataStructure) {

	uint8_t Bytes_temp[7] = { 0x00 };

	if( HAL_I2C_Mem_Read(I2Cx, DataStructure->Magnetometer_addres, MPU9250_HXL | 0x80, 1, Bytes_temp, 7, 1000) != HAL_OK ) {

		//HAL_GPIO_TogglePin(LD2_GPIO_Port, LD2_Pin);
		return MPU9250_Read_Magnetometer_FAIL;
	}

	DataStructure->Magnetometer_X = Bytes_temp[0] | Bytes_temp[1] << 8;
	DataStructure->Magnetometer_Y = Bytes_temp[2] | Bytes_temp[3] << 8;
	DataStructure->Magnetometer_Z = Bytes_temp[4] | Bytes_temp[5] << 8;

	return MPU9250_Read_Magnetometer_OK;
}

 

[Gość]

Też mam problem z magnetometrami scalonymi. Testowałem te tańsze z allegro i żaden nie działa w sposób umożliwiający praktyczne zastosowanie tj. albo wskazania są błędne (niewielka zmiana nachylenia względem poziomu powoduje np. przejechanie przez cały zakres pomiarowy) albo podobnie jak u Ciebie cały czas ta sama wartość. Testowałem LSM303D i HMC5883L i jeszcze jakiś na I2C i żaden z nich nie działał jak powinien gdzie z akcelerometrami nie ma problemów. Tak się zastanawiam, czy te scalone magnetometry wymagają jakiegoś szczególnego sposobu postępowania albo występują jakieś fluktuacje magnetyczne w mojej okolicy. Kompas analogowy jednak działa poprawnie.

Niestety nie zdołałem sobie z tym poradzić i dochodząc do wniosku, że to "takie badziewie" porzuciłem temat. Może te droższe sprawują się lepiej ale nie miałem okazji sprawdzić.

[fasolus]

1 minutę temu, atMegaTona napisał:

Też mam problem z magnetometrami scalonymi. Testowałem te tańsze z allegro i żaden nie działa w sposób umożliwiający praktyczne zastosowanie tj. albo wskazania są błędne (niewielka zmiana nachylenia względem poziomu powoduje np. przejechanie przez cały zakres pomiarowy) albo podobnie jak u Ciebie cały czas ta sama wartość. Testowałem LSM303D i HMC5883L i jeszcze jakiś na I2C i żaden z nich nie działał jak powinien gdzie z akcelerometrami nie ma problemów. Tak się zastanawiam, czy te scalone magnetometry wymagają jakiegoś szczególnego sposobu postępowania albo występują jakieś fluktuacje magnetyczne w mojej okolicy. Kompas analogowy jednak działa poprawnie.

Niestety nie zdołałem sobie z tym poradzić i dochodząc do wniosku, że to "takie badziewie" porzuciłem temat. Może te droższe sprawują się lepiej ale nie miałem okazji sprawdzić.

Problem rozwiązany. Przed odczytaniem wartości z rejestrów zawierających zmierzone wartości należy odczytać rejestr ST1, a po rejestr ST2.  Odchylenie kompasu od poziomu i "wariowanie wskazań" można zniwelować akcelerometrem stosując odpowiednie wzory. Dodatkowo jeśli w pobliżu znajdują się jakieś ferromagnetyki ich wpływ również można zniwelować stosując odpowiednią kalibrację. 

[InspektorGadzet]

12 minut temu, atMegaTona napisał:

Testowałem LSM303D i HMC5883L

Jest dostępna do nich dokumentacja?

[Polecacz 101]

Produkcja i montaż PCB - wybierz sprawdzone PCBWay!
   • Darmowe płytki dla studentów i projektów non-profit
   • Tylko 5$ za 10 prototypów PCB w 24 godziny
   • Usługa projektowania PCB na zlecenie
   • Montaż PCB od 30$ + bezpłatna dostawa i szablony
   • Darmowe narzędzie do podglądu plików Gerber
Zobacz również » Film z fabryki PCBWay

[Gość]

5 godzin temu, InspektorGadzet napisał:

Jest dostępna do nich dokumentacja?

• HMC5883L
• LSM303D

5 godzin temu, fasolus napisał:

Odchylenie kompasu od poziomu i "wariowanie wskazań" można zniwelować akcelerometrem stosując odpowiednie wzory

Masz może te wzory pod ręką? Może popróbuje jeszcze coś z tego zrobić. Póki co zadowoliłem się akcelerometrem ale fajnie by było dodać ten magnetometr dla zabawy i zobaczyć co z tego wyjdzie.

------------------------------------------

Znalazłem taką bibliotekę do arduino z której można wyciągnąć bebechy ale ile to warte to się okaże.   Może się komuś przyda.

------------------------------------------

@fasolus  możesz wyjaśnić po co to dzielenie przez 2 bo się chyba gdzieś pogubiłem?

DataStructure->Magnetometer_ASAX = ( ( (Bytes_temp[0] - 128) * 0.5 ) / 128 ) + 1;

 

Edytowano Lipiec 24, 2019 przez Gość
aktualizacja

[fasolus]

4 godziny temu, atMegaTona napisał:

• HMC5883L
• LSM303D

Masz może te wzory pod ręką? Może popróbuje jeszcze coś z tego zrobić. Póki co zadowoliłem się akcelerometrem ale fajnie by było dodać ten magnetometr dla zabawy i zobaczyć co z tego wyjdzie.

------------------------------------------

Znalazłem taką bibliotekę do arduino z której można wyciągnąć bebechy ale ile to warte to się okaże.   Może się komuś przyda.

------------------------------------------

@fasolus  możesz wyjaśnić po co to dzielenie przez 2 bo się chyba gdzieś pogubiłem?

DataStructure->Magnetometer_ASAX = ( ( (Bytes_temp[0] - 128) * 0.5 ) / 128 ) + 1;

 

Tutaj co nieco o fuzji sygnału z akcelerometru i żyroskopu oraz o kalibracji: http://students.iitk.ac.in/roboclub/2017/12/21/Beginners-Guide-to-IMU.html

A wzór o który pytasz pochodzi z dokumentacji modułu: 

[Gość]

Dzięki! I wszystko jasne. Mapa deklinacji magnetycznej też może się przydać.

[fasolus]

Ma ktoś z was pomysł, dlaczego taka próba kompensacji kąta wychylenia magnetometru nie przynosi rezultatu ? Algorytm działa prawidłowo tylko w przypadku, kiedy magnetometr nie jest odchylony od poziomu (Roll = 0, Pitch = 0). 

float Roll  = DataStructure->Accelerometer_Roll;
float Pitch = DataStructure->Accelerometer_Pitch;

float cosRoll = cosf(Roll * (M_PI / 180)), cosPitch = cosf(Pitch * (M_PI / 180));
float sinRoll = sinf(Roll * (M_PI / 180)), sinPitch = sinf(Pitch * (M_PI / 180));

float H_X = (DataStructure->Magnetometer_X_uT * cosPitch) + (DataStructure->Magnetometer_Y_uT * sinRoll * sinPitch) + (DataStructure->Magnetometer_Z_uT * cosRoll * sinPitch);
float H_Y = (DataStructure->Magnetometer_Y_uT * cosRoll)  - (DataStructure->Magnetometer_Z_uT * sinRoll);

DataStructure->Magnetometer_Yaw = atan2f(H_Y, H_X) * (180 / M_PI) + MAGNETIC_DECLINATION;

Zmienna Roll zawiera się w zakresie <-180, +180>, Pitch tak samo. Funkcje trygonometryczne również dają prawidłowe wartości <-1,1> w zależności od Roll i Pitch.