Skocz do zawartości
fasolus

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

Pomocna odpowiedź

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;
}

 

Udostępnij ten post


Link to post
Share on other sites

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ć.

  • Lubię! 1

Udostępnij ten post


Link to post
Share on other sites
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ę. 

  • Lubię! 1

Udostępnij ten post


Link to post
Share on other sites
12 minut temu, atMegaTona napisał:

Testowałem LSM303D i HMC5883L

Jest dostępna do nich dokumentacja?

Udostępnij ten post


Link to post
Share on other sites
(edytowany)
5 godzin temu, InspektorGadzet napisał:

Jest dostępna do nich dokumentacja?

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 przez atMegaTona
aktualizacja

Udostępnij ten post


Link to post
Share on other sites
4 godziny temu, atMegaTona napisał:

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: 

188355543_Zrzutekranu2019-07-24o17_33_22.thumb.png.10b5780b872a9d81a351cabc48bb7583.png

  • Lubię! 1
  • Pomogłeś! 1

Udostępnij ten post


Link to post
Share on other sites

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.

Udostępnij ten post


Link to post
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Gość
Napisz odpowiedź...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.


×
×
  • Utwórz nowe...