main.ino
Arduino (C++) · 243 lines
#include <Wire.h>
#include <math.h>
// QMC5883P I2C address (datasheet default)
const uint8_t QMC5883P_ADDR = 0x2C;
// Registers
const uint8_t MODE_REG = 0x0A;
const uint8_t CONFIG_REG = 0x0B;
const uint8_t X_LSB_REG = 0x01;
const uint8_t STATUS_REG = 0x09;
// I2C pins for ESP32-C3 SuperMini
constexpr int I2C_SDA = 8;
constexpr int I2C_SCL = 9;
// Conversion for +/-8G: 3750 LSB/G and 1G = 100 uT => uT per LSB = 100/3750
constexpr float UT_PER_LSB = 100.0f / 3750.0f;
// Magnetic declination (degrees). Set to your local value for true-north heading.
constexpr float DECLINATION_DEG = 6.5f;
// How long to wait for a fresh sample before reporting a problem (ms)
constexpr unsigned long DRDY_TIMEOUT_MS = 2000;
// Is the sensor currently considered present and working?
bool sensorOK = false;
// ---------------------------------------------------------------------------
// Diagnostics
// ---------------------------------------------------------------------------
// Returns true if a device ACKs at the given address
bool devicePresent(uint8_t addr) {
Wire.beginTransmission(addr);
return Wire.endTransmission() == 0;
}
// Scans the bus and reports what it finds. Returns number of devices.
int scanBus() {
int found = 0;
Serial.println("Skanowanie magistrali I2C...");
for (uint8_t addr = 1; addr < 127; addr++) {
if (devicePresent(addr)) {
Serial.printf(" -> urzadzenie pod adresem 0x%02X", addr);
// Name the usual suspects so the user knows what they actually have
if (addr == 0x2C) Serial.print(" (QMC5883P)");
if (addr == 0x0D) Serial.print(" (QMC5883L - INNY UKLAD!)");
if (addr == 0x1E) Serial.print(" (HMC5883L - INNY UKLAD!)");
Serial.println();
found++;
}
}
if (found == 0) Serial.println(" (nic nie znaleziono)");
return found;
}
void printNotDetectedHelp(int devicesFound) {
Serial.println();
Serial.println("***************************************************");
Serial.println("* BLAD: nie wykryto modulu QMC5883P (0x2C) *");
Serial.println("***************************************************");
Serial.println();
if (devicesFound == 0) {
Serial.println("Na magistrali I2C nie odpowiada ZADNE urzadzenie.");
Serial.println("Sprawdz kolejno:");
Serial.println(" 1. VCC modulu -> pin 3V3 plytki");
Serial.println(" 2. GND modulu -> GND plytki");
Serial.printf (" 3. SDA modulu -> GPIO%d\n", I2C_SDA);
Serial.printf (" 4. SCL modulu -> GPIO%d\n", I2C_SCL);
Serial.println(" 5. Czy SDA i SCL nie sa zamienione miejscami?");
Serial.println(" 6. Czy goldpiny sa wlutowane? Sam drut wlozony");
Serial.println(" w otwor czesto nie daje kontaktu.");
Serial.println(" 7. Czy nie trzymasz wcisnietego przycisku BOOT?");
Serial.println(" Na SuperMini zwiera on linie SCL (GPIO9).");
} else {
Serial.println("Na magistrali sa inne urzadzenia, ale nie ma 0x2C.");
Serial.println("Najprawdopodobniej masz modul z INNYM ukladem:");
Serial.println(" 0x0D = QMC5883L -> wymaga biblioteki QMC5883LCompass");
Serial.println(" 0x1E = HMC5883L -> wymaga biblioteki Adafruit_HMC5883");
Serial.println("Ten szkic dziala wylacznie z QMC5883P pod adresem 0x2C.");
}
Serial.println();
Serial.println("Po poprawieniu polaczen nacisnij RESET na plytce.");
Serial.println();
}
// ---------------------------------------------------------------------------
// Sensor
// ---------------------------------------------------------------------------
bool initQMC5883P() {
// Continuous mode, ODR = 200 Hz
Wire.beginTransmission(QMC5883P_ADDR);
Wire.write(MODE_REG);
Wire.write(0xCF);
if (Wire.endTransmission() != 0) {
Serial.println("BLAD: zapis do MODE_REG nie powiodl sie.");
return false;
}
// Set/Reset ON, Range = +/-8G
Wire.beginTransmission(QMC5883P_ADDR);
Wire.write(CONFIG_REG);
Wire.write(0x08);
if (Wire.endTransmission() != 0) {
Serial.println("BLAD: zapis do CONFIG_REG nie powiodl sie.");
return false;
}
delay(50);
return true;
}
bool dataReady() {
Wire.beginTransmission(QMC5883P_ADDR);
Wire.write(STATUS_REG);
if (Wire.endTransmission(false) != 0) return false;
Wire.requestFrom(QMC5883P_ADDR, (uint8_t)1);
if (Wire.available() != 1) return false;
uint8_t st = Wire.read();
return (st & 0x01) != 0; // DRDY bit
}
bool readQMC5883PData(int16_t& x, int16_t& y, int16_t& z) {
Wire.beginTransmission(QMC5883P_ADDR);
Wire.write(X_LSB_REG);
if (Wire.endTransmission(false) != 0) return false;
uint8_t n = Wire.requestFrom(QMC5883P_ADDR, (uint8_t)6);
if (n != 6) return false;
uint8_t x_lsb = Wire.read();
uint8_t x_msb = Wire.read();
uint8_t y_lsb = Wire.read();
uint8_t y_msb = Wire.read();
uint8_t z_lsb = Wire.read();
uint8_t z_msb = Wire.read();
x = (int16_t)((x_msb << 8) | x_lsb);
y = (int16_t)((y_msb << 8) | y_lsb);
z = (int16_t)((z_msb << 8) | z_lsb);
return true;
}
float headingDegFromXY(float x_uT, float y_uT, float decl_deg) {
float hdg = atan2f(y_uT, x_uT) * 180.0f / (float)M_PI;
hdg += decl_deg;
if (hdg < 0) hdg += 360.0f;
if (hdg >= 360.0f) hdg -= 360.0f;
return hdg;
}
// ---------------------------------------------------------------------------
void setup() {
Serial.begin(115200);
delay(3000); // czas na enumeracje natywnego USB w ESP32-C3
Serial.println();
Serial.println("=== QMC5883P Compass ===");
Serial.printf("Piny I2C: SDA=GPIO%d SCL=GPIO%d\n", I2C_SDA, I2C_SCL);
Serial.println();
Wire.begin(I2C_SDA, I2C_SCL);
Wire.setClock(100000);
int devices = scanBus();
Serial.println();
if (!devicePresent(QMC5883P_ADDR)) {
printNotDetectedHelp(devices);
sensorOK = false;
return;
}
Serial.println("Modul QMC5883P wykryty pod adresem 0x2C.");
if (!initQMC5883P()) {
Serial.println("Modul odpowiada, ale konfiguracja sie nie powiodla.");
sensorOK = false;
return;
}
Serial.println("Konfiguracja OK - start pomiarow.");
Serial.println();
sensorOK = true;
}
void loop() {
// Sensor never came up - keep reminding instead of staying silent
if (!sensorOK) {
Serial.println("Czujnik niedostepny. Popraw polaczenia i nacisnij RESET.");
delay(5000);
return;
}
// Wait for fresh data, but give up after a timeout instead of hanging
unsigned long t0 = millis();
while (!dataReady()) {
if (millis() - t0 > DRDY_TIMEOUT_MS) {
if (!devicePresent(QMC5883P_ADDR)) {
Serial.println("BLAD: modul przestal odpowiadac (0x2C zniknal).");
Serial.println("Sprawdz polaczenia - luzny styk lub przerwa w zasilaniu.");
} else {
Serial.println("UWAGA: brak nowych danych (DRDY nie ustawia sie).");
Serial.println("Modul odpowiada, ale nie konczy pomiaru.");
}
delay(1000);
return;
}
delay(5);
}
int16_t x_raw, y_raw, z_raw;
if (!readQMC5883PData(x_raw, y_raw, z_raw)) {
Serial.println("BLAD: nie udalo sie odczytac danych z czujnika.");
delay(1000);
return;
}
float x_uT = x_raw * UT_PER_LSB;
float y_uT = y_raw * UT_PER_LSB;
float z_uT = z_raw * UT_PER_LSB;
float heading = headingDegFromXY(x_uT, y_uT, DECLINATION_DEG);
Serial.printf("X:%7.2f Y:%7.2f Z:%7.2f uT | Heading: %3.0f\n",
x_uT, y_uT, z_uT, heading);
delay(250);
}