HMC5883L三轴罗盘传感器取值异常，求3D测量C语言公式

Hey there! Let's work through your HMC5883L issues step by step—first fixing the wonky readings you're seeing, then diving into 3D heading calculations.

Troubleshooting Your Current Readings #

Your symptoms (fixed Y-value, erratic X/Z, incorrect ±320° heading when level) point to a mix of configuration errors and small code oversights. Let's tackle them:

1. Fix Register Configuration #

Looking at your code, you've set:

• 0x00 (Config A) to 0x70: This uses the reserved measurement mode (MA1=1, MA0=1), which isn't meant for normal operation. Switch to 0x00 for standard continuous measurement.
• 0x01 (Config B) to 0xA0: This sets gain to ±8.1Gauss—way too high for most environments, leading to magnetic saturation (which can lock readings like your fixed Y-value). Use 0x20 for the default ±1.3Gauss gain, which avoids saturation in typical indoor/outdoor settings.
• 0x02 (Mode Register) to 0x01 (single-shot) in your loop: If you set Config A to continuous mode, you don't need to re-trigger measurements every loop—just set 0x02 to 0x00 once at startup.

2. Correct Timing & Math Errors #

• Your final usleep(50000) is only 50ms, not 5 seconds—change it to usleep(5000000) for the 5-second delay you intended.
• You used 2*pi instead of 2*M_PI (the standard math library constant). Also, make sure to compile with -lm to link the math library (e.g., gcc your_code.c -o hmc -lwiringPi -lm).
• Your declination value is set to 1 (radians?), but magnetic declination depends on your location. For example, most of China has a west declination of ~-5 to -10 degrees—convert this to radians (e.g., -7.5 * M_PI / 180) for accurate heading.

3. Hardware Checks #

If Y-values stay fixed after fixing the code, check:

• Wiring: Ensure SDA/SCL/VCC/GND are securely connected to your Pi. Double-check that you didn't swap SDA and SCL.
• Sensor health: Try a different HMC5883L module to rule out a faulty sensor.

3D Heading Calculation (Tilt-Compensated) #

For 3D measurements (when the sensor isn't level), you need tilt data from an accelerometer (like ADXL345) to calculate roll (rotation around X-axis) and pitch (rotation around Y-axis). With those values, you can compensate the magnetic readings to get an accurate heading regardless of sensor orientation.

Here's the C code for tilt-compensated heading:

// Assume roll (rad) and pitch (rad) are obtained from an accelerometer
float roll = ...;   // Example: -0.3 radians (~-17°)
float pitch = ...;  // Example: 0.2 radians (~11°)

// Compensate magnetic readings for tilt
float x_compensated = x * cos(pitch) + z * sin(pitch);
float y_compensated = x * sin(roll) * sin(pitch) + y * cos(roll) - z * sin(roll) * cos(pitch);

// Calculate 3D heading
double heading_3d = atan2(y_compensated, x_compensated) + declination;

// Normalize to 0-360°
if (heading_3d < 0) heading_3d += 2 * M_PI;
if (heading_3d > 2 * M_PI) heading_3d -= 2 * M_PI;
float heading_3d_deg = heading_3d * 180 / M_PI;

Modified Working Code #

Here's your code with all fixes applied:

#include <unistd.h>
#include <stdio.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include <math.h>

int main() {
    unsigned short int xl, yl, xh, yh, zh, zl;
    short int x,y,z;
    int fd;
    // Replace with your local magnetic declination (radians)
    float declination = -7.5 * M_PI / 180;
    double angle;

    // Initialize WiringPi
    if (wiringPiSetup() == -1) {
        printf("WiringPi initialization failed!\n");
        return 1;
    }

    // Setup I2C connection to HMC5883L (address 0x1E)
    fd = wiringPiI2CSetup(0x1E);
    if (fd == -1) {
        printf("I2C connection failed!\n");
        return 1;
    }

    usleep(50000); // 50ms startup delay

    // Configure sensor registers
    wiringPiI2CWriteReg8(fd, 0x00, 0x00); // Config A: Normal continuous measurement
    wiringPiI2CWriteReg8(fd, 0x01, 0x20); // Config B: ±1.3Gauss gain (default)
    wiringPiI2CWriteReg8(fd, 0x02, 0x00); // Mode: Continuous measurement

    while(1) {
        usleep(50000); // Wait for measurement to complete

        // Read raw magnetic data
        xh = wiringPiI2CReadReg8(fd, 0x03);
        xl = wiringPiI2CReadReg8(fd, 0x04);
        zh = wiringPiI2CReadReg8(fd, 0x05);
        zl = wiringPiI2CReadReg8(fd, 0x06);
        yh = wiringPiI2CReadReg8(fd, 0x07);
        yl = wiringPiI2CReadReg8(fd, 0x08);

        // Combine MSB/LSB into signed 16-bit values
        x = (xh << 8) | xl;
        y = (yh << 8) | yl;
        z = (zh << 8) | zl;

        // Calculate level heading
        angle = atan2((double)y, (double)x) + declination;

        // Normalize to 0-2π range
        if (angle < 0) angle += 2 * M_PI;
        if (angle > 2 * M_PI) angle -= 2 * M_PI;

        float heading_deg = angle * 180 / M_PI;

        // Print results
        printf("\nHeading: %.2f°\n", heading_deg);
        printf("X: %d, Y: %d, Z: %d\n", x, y, z);

        usleep(5000000); // 5-second delay between readings
    }

    return 0;
}

Compile with:

gcc hmc_code.c -o hmc_reader -lwiringPi -lm

内容的提问来源于stack exchange，提问作者Liza Darwesh