Kategoria: Systemy embedded

QMC5883L to układ mierzący natężenie pola magnetycznego w 3 osiach, który umożliwia określenie orientacji (np. względem magnetycznej północy).

Kupiłem go na Allegro, myśląc że dostanę HMC5883L (taki napis widniał na płytce). Okazało się, że dostałem odrobinę inny układ: QMC5883L. Jest on niekompatybilnym zamiennikiem, ze względu na małe różnice w rejestrach I2C.

Podłączenie do STM32

Jednym z wyzwań było samo podłączenie układu do mikrokontrolera.

Wybrałem I2C1, pin SCL podłączyłem do PB8, a pin SDA do PB7 a prędkość GPIO ustawiłem na Bardzo szybką.

Połączenie I2C wymaga rezystorów pull-up, których płytka z układem nie posiadała. Spróbowałem użyć wbudowanych pull-upów w mój STM32U083MC, ale niestety połączenie często się rozrywało. Zdecydowałem się zbudować podłączyć własne pull-upy.

Ostatecznie, kod konfiguracyjny wygląda tak:

/**
  * @brief I2C MSP Initialization
  * @param hi2c: I2C handle pointer
  * @retval None
  */
void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
  if(hi2c->Instance==I2C1)
  {
  /** Initializes the peripherals clocks
  */
    PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C1;
    PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
    {
      Error_Handler();
    }

    __HAL_RCC_GPIOB_CLK_ENABLE();
    /**I2C1 GPIO Configuration
    PB7     ------> I2C1_SDA
    PB8     ------> I2C1_SCL
    */
    GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_8;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    /* Peripheral clock enable */
    __HAL_RCC_I2C1_CLK_ENABLE();
  }
}

Pull-upy

Pod ręką miałem rezystory 2,2 kΩ (choć słyszałem, że lepsze byłyby 4,7 kΩ), dlatego takich użyłem. Próbowałem podłączyć moduł z kontrolerem przez płytkę stykową, ale najwyraźniej rezystory słabo łączyły. Ulepiłem połączenie z kabli, podłączając pull-upy w środku przewodów do VCC (3.3V).

Po ulepieniu czegoś takiego, połączenie działało zadowalająco, nawet z I2C w trybie szybkim (400 kHz).

Azymut, kalibracja i deklinacja

QMC5883L nie daje nam gotowego azymutu, a wartość natężenia pola magnetycznego w trzech osiach. Azymut możemy obliczyć używając funkcji arcus tangens:

float QMC5883L_CalculateAzimuth(
const QMC5883L_Handle_t* hqmc, 
QMC5883L_Data_t* data)
{
    float azimuth = atan2f(data->y, data->x) * 180.0f / M_PI;
    
    // Uwzględnij korekcję wynikającą z deklinacji
    azimuth += hqmc->declination;
    
    // Normalizuj do zakresu 0-360
    // (zamiast -180 - 180)
    if (azimuth < 0) {
        azimuth += 360.0f;
    } else if (azimuth >= 360.0f) {
        azimuth -= 360.0f;
    }
    
    return azimuth;
}

Gdzie podziała się oś Z? Powyższa implementacja zakłada, że układ jest położony idealnie płasko. Nie miałem zamontowanego akcelerometru, którym mógłbym dodatkowo zmierzyć odchylenie układu. Z tego powodu nie stworzyłem implementacji, która uwzględniałaby oś Z (może jak zamontuję, to zaktualizuję post).

Niestety, bez kalibracji i korekcji wynikającej z deklinacji obliczony azymut będzie w najlepszym wypadku niedokładny – w moim kompletnie mijał się z prawdą.

Deklinację w stopniach możemy łatwo pozyskać z kalkulatorów online, np. tego od amerykańskiego NCEI.

Kalibracja QMC5883L

Skalibrowałem mój moduł obracając go we wszystkich osiach: X, Y oraz Z, jednocześnie notując minimalne i maksymalne wartości w każdej osi.

Pozwoliło mi to na obliczenie dwóch korekt: hard-iron oraz soft-iron. Więcej o tych korektach możesz przeczytać tutaj. W skrócie:

• hard-iron: korekta offsetowa, dodawana do odczytanej wartości
• soft-iron: korekta iloczynowa – mnożymy odczytaną wartość przez korektę

A to fragmenty kodu odpowiadające za kalibrację:

/**
 * @brief Dodaje próbkę kalibracyjną
 */
HAL_StatusTypeDef QMC5883L_AddCalibrationSample(
const QMC5883L_Handle_t* hqmc, 
QMC5883L_CalibrationData_t* cal_data)
{
    QMC5883L_RawData_t raw_data;
    HAL_StatusTypeDef status;
    
    status = QMC5883L_ReadRaw(hqmc, &raw_data);
    if (status != HAL_OK) return status;
    
    // Konwertuj na Gaussa
    float x = (float)raw_data.x / hqmc->scale_factor;
    float y = (float)raw_data.y / hqmc->scale_factor;
    float z = (float)raw_data.z / hqmc->scale_factor;
    
    if (x < cal_data->x_min) cal_data->x_min = x;
    if (x > cal_data->x_max) cal_data->x_max = x;
    if (y < cal_data->y_min) cal_data->y_min = y;
    if (y > cal_data->y_max) cal_data->y_max = y;
    if (z < cal_data->z_min) cal_data->z_min = z;
    if (z > cal_data->z_max) cal_data->z_max = z;
    
    cal_data->sample_count++;
    
    return HAL_OK;
}

/**
 * @brief Kończy kalibrację i oblicza korekty
 */
HAL_StatusTypeDef QMC5883L_FinishCalibration(
QMC5883L_Handle_t* hqmc, 
QMC5883L_CalibrationData_t* cal_data)
{
    if (cal_data->sample_count < 100) {
        // Za mało próbek
        return HAL_ERROR;
    }
    
    // Oblicz korektę hard-iron
    hqmc->calibration.x_offset = (cal_data->x_max + cal_data->x_min) / 2.0f;
    hqmc->calibration.y_offset = (cal_data->y_max + cal_data->y_min) / 2.0f;
    hqmc->calibration.z_offset = (cal_data->z_max + cal_data->z_min) / 2.0f;
    
    // Oblicz korektę soft-iron
    float x_range = cal_data->x_max - cal_data->x_min;
    float y_range = cal_data->y_max - cal_data->y_min;
    float z_range = cal_data->z_max - cal_data->z_min;
    
    float avg_range = (x_range + y_range + z_range) / 3.0f;
    
    hqmc->calibration.x_scale = avg_range / x_range;
    hqmc->calibration.y_scale = avg_range / y_range;
    hqmc->calibration.z_scale = avg_range / z_range;
    
    hqmc->calibration.is_calibrated = true;
    
    return HAL_OK;
}

/**
 * @brief Zaaplikuj dane kalibracyjne do odczytanych danych
 */
void QMC5883L_ApplyCalibration(
const QMC5883L_Handle_t* hqmc, 
QMC5883L_Data_t* data)
{
    if (!hqmc->calibration.is_calibrated) return;
    
    // Aplikuj korekcję hard-iron
    data->x -= hqmc->calibration.x_offset;
    data->y -= hqmc->calibration.y_offset;
    data->z -= hqmc->calibration.z_offset;
    
    // Aplikuj korekcję soft-iron
    data->x *= hqmc->calibration.x_scale;
    data->y *= hqmc->calibration.y_scale;
    data->z *= hqmc->calibration.z_scale;
}

Niezbyt idealny wynik

Po całej tej kalibracji i obliczeniach skierowałem oś X modułu QMC5883L na północ, wtedy mój azymut powinien wynosić 0 stopni. Ku mojemu zdziwieniu odczyt był całkiem dokładny (wahał się o 2, 3 stopnie). Obracając moduł, dalej dawał zadowalające wyniki.

Zatem czemu „niezbyt” idealny wynik? Gdy przymocowałem magnetometr do mojego samochodzika, który był blisko podłogi, odczyt nagle stał się niedokładny. Rozbiegał się o blisko 15, czasami 20 stopni. Okazało się, że coś w podłodze zakłóca pole magnetyczne Ziemi, przez co odczyt jest błędny.

Dodatkowo, na odczyt wpływa również temperatura, którą pominąłem w obliczeniach azymutu (moduł ma funkcję pomiaru temperatury).

Problemy z komunikacją

Oprócz niedokładnego pomiaru wystąpiły również problemy z samym modułem QMC5883L.

Gdy odczytywałem moduł zbyt często (biorąc pod uwagę stan pinu gotowości danych), przestawał on odczytywać natężenie. Ciągle zwracał te same wartości (sama komunikacja przez magistralę I2C działała poprawnie).

W związku z tym, odczytywałem dane z QMC5883L co 50 milisekund:

// [...]
while (1)
{
	  if (HAL_GetTick() >= next_read_tick && g_qmc_drdy) {
		  next_read_tick = HAL_GetTick() + 50;
		  if (QMC5883L_AppReadData()) {
			  // Jeśli QMC nadal utrzymuje wysoki poziom pinu lub operacja ponownego odczytu była zbyt szybka, aby EXTI mogło się uruchomić,
        // to po prostu ponownie odczytaj rejestry QMC (w przeciwnym razie g_qmc_drdy pozostanie fałszywe, podczas gdy pin będzie miał wysoki poziom).
			  g_qmc_drdy = HAL_GPIO_ReadPin(QMC_DRDY_GPIO_Port, QMC_DRDY_Pin);
		  }
		  else {
			  printf("i2c error\r\n");
			  uint32_t err = HAL_I2C_GetError(&hi2c1);
			  printf("error code: %lu\r\n", err);
		  }
	  }
}
// [...]

Tak czy inaczej, spróbuję ulepić lepsze pull-upy i dam update, czy to pomogło.

Podsumowanie

Wdrożenie magnetometru QMC5883L okazało się ciekawym wyzwaniem, obfitującym w szereg praktycznych problemów i naukę. Pomimo początkowych trudności, udało mi się osiągnąć w miarę działający system.

Moje wnioski:

1. Zamienniki: Kupiony moduł, oznaczony jako HMC5883L, w rzeczywistości zawierał układ QMC5883L, który jest niekompatybilny na poziomie rejestrów, co wymusiło samodzielną implementację sterownika.
2. Fizyczne podłączenie: Stabilna praca I2C jest niemożliwa bez odpowiednich rezystorów pull-up. Wbudowane w STM32 okazały się niewystarczające, a dopiero dodanie zewnętrznych rezystorów 2,2 kΩ zapewniło względnie stabilną komunikację nawet przy 400 kHz.
3. Kalibracja jest niezbędna: Samo odczytanie surowych wartości z osi X, Y i Z jest niewystarczające. Aby uzyskać wiarygodny azymut, konieczne było przeprowadzenie kalibracji (usuwającej zakłócenia hard-iron i soft-iron) oraz uwzględnienie lokalnej deklinacji magnetycznej.
4. Środowisko ma ogromne znaczenie: Nawet doskonała kalibracja może zostać unieważniona przez zewnętrzne zakłócenia magnetyczne (np. zbrojenie w podłodze), które znacząco wpływają na dokładność odczytu.
5. Opóźnienie odczytu: Układ wymagał ostrożnego zarządzania czasem odczytu. Zbyt częste pollowanie prowadziło do „zawieszenia” się czujnika, dlatego konieczne było wprowadzenie opóźnień i ręczny odczyt pinu DRDY w połączeniu z obsługą przerwania EXTI.