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.

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:

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

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

Co sądzicie o li-po 7,4v 180 mAh 2s do robota minisumo z silnikami pololu hp 30:1 i dwoma sharpami ? Nie będzie to zbyt mała pojemność ?