424 lines
13 KiB
Arduino
424 lines
13 KiB
Arduino
|
/**
|
||
|
|
*
|
||
|
|
* @license MIT License
|
||
|
|
*
|
||
|
|
* Copyright (c) 2026 lewis he
|
||
|
|
*
|
||
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
|
|
* of this software and associated documentation files (the "Software"), to deal
|
||
|
|
* in the Software without restriction, including without limitation the rights
|
||
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
|
|
* copies of the Software, and to permit persons to whom the Software is
|
||
|
|
* furnished to do so, subject to the following conditions:
|
||
|
|
*
|
||
|
|
* The above copyright notice and this permission notice shall be included in all
|
||
|
|
* copies or substantial portions of the Software.
|
||
|
|
*
|
||
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||
|
|
* SOFTWARE.
|
||
|
|
*
|
||
|
|
* @file QMC6310_CalibrateExample.ino
|
||
|
|
* @author Lewis He (lewishe@outlook.com)
|
||
|
|
* @date 2026-01-26
|
||
|
|
*
|
||
|
|
* Modified 2026-04-21 to compute mean-based offsets from CALRAW samples
|
||
|
|
* instead of midpoint-of-extrema offsets.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
#include <Wire.h>
|
||
|
|
#include <SPI.h>
|
||
|
|
#include <Arduino.h>
|
||
|
|
#include "SensorQMC6310.hpp"
|
||
|
|
#define ARDUINO_T_BEAM_S3_SUPREME
|
||
|
|
#ifdef ARDUINO_T_BEAM_S3_SUPREME
|
||
|
|
#include <XPowersAXP2101.tpp> //PMU Library https://github.com/lewisxhe/XPowersLib.git
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#ifndef SENSOR_SDA
|
||
|
|
#define SENSOR_SDA 17
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#ifndef SENSOR_SCL
|
||
|
|
#define SENSOR_SCL 18
|
||
|
|
#endif
|
||
|
|
|
||
|
|
SensorQMC6310 magnetometer;
|
||
|
|
|
||
|
|
void beginPower()
|
||
|
|
{
|
||
|
|
#if defined(ARDUINO_T_BEAM_S3_SUPREME)
|
||
|
|
XPowersAXP2101 power;
|
||
|
|
power.begin(Wire1, AXP2101_SLAVE_ADDRESS, 42, 41);
|
||
|
|
power.disableALDO1();
|
||
|
|
power.disableALDO2();
|
||
|
|
delay(250);
|
||
|
|
power.setALDO1Voltage(3300);
|
||
|
|
power.enableALDO1();
|
||
|
|
power.setALDO2Voltage(3300);
|
||
|
|
power.enableALDO2();
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
void calibrate()
|
||
|
|
{
|
||
|
|
if (!magnetometer.setOutputDataRate(200.0f)) {
|
||
|
|
Serial.println("Failed to set output data rate");
|
||
|
|
return ;
|
||
|
|
}
|
||
|
|
|
||
|
|
Serial.println("========================================");
|
||
|
|
Serial.println("Calibration Instructions:");
|
||
|
|
Serial.println("1. Rotate sensor in FIGURE-8 pattern");
|
||
|
|
Serial.println("2. Cover all axes (X, Y, Z directions)");
|
||
|
|
Serial.println("3. Rotate slowly and completely");
|
||
|
|
Serial.println("4. Wait for progress bar to complete");
|
||
|
|
Serial.println("5. Expected: Magnetic Strength ~25-65 uT");
|
||
|
|
Serial.println("========================================");
|
||
|
|
Serial.println();
|
||
|
|
|
||
|
|
Serial.println("Place the sensor on the plane and slowly rotate the sensor...");
|
||
|
|
Serial.println("Rotate in FIGURE-8 pattern to cover all directions!");
|
||
|
|
Serial.println();
|
||
|
|
|
||
|
|
int32_t x_min = 65535;
|
||
|
|
int32_t x_max = -65535;
|
||
|
|
int32_t y_min = 65535;
|
||
|
|
int32_t y_max = -65535;
|
||
|
|
int32_t z_min = 65535;
|
||
|
|
int32_t z_max = -65535;
|
||
|
|
|
||
|
|
int32_t range = 2000;
|
||
|
|
int32_t i = 0;
|
||
|
|
int32_t raw_x = 0, raw_y = 0, raw_z = 0;
|
||
|
|
int16_t x_offset = 0;
|
||
|
|
int16_t y_offset = 0;
|
||
|
|
int16_t z_offset = 0;
|
||
|
|
|
||
|
|
uint32_t sample_counter = 0;
|
||
|
|
|
||
|
|
// Mean-based accumulation of raw calibration samples.
|
||
|
|
int64_t x_sum = 0;
|
||
|
|
int64_t y_sum = 0;
|
||
|
|
int64_t z_sum = 0;
|
||
|
|
|
||
|
|
MagnetometerData data;
|
||
|
|
while (i < range) {
|
||
|
|
i += 1;
|
||
|
|
|
||
|
|
if (magnetometer.isDataReady()) {
|
||
|
|
|
||
|
|
magnetometer.readData(data);
|
||
|
|
sample_counter++;
|
||
|
|
|
||
|
|
raw_x = data.raw.x;
|
||
|
|
raw_y = data.raw.y;
|
||
|
|
raw_z = data.raw.z;
|
||
|
|
|
||
|
|
x_sum += raw_x;
|
||
|
|
y_sum += raw_y;
|
||
|
|
z_sum += raw_z;
|
||
|
|
|
||
|
|
Serial.print("CALRAW ");
|
||
|
|
Serial.print("X:");
|
||
|
|
Serial.print(raw_x);
|
||
|
|
Serial.print(" Y:");
|
||
|
|
Serial.print(raw_y);
|
||
|
|
Serial.print(" Z:");
|
||
|
|
Serial.println(raw_z);
|
||
|
|
|
||
|
|
// Track extrema on the direct raw sample, not on a running average.
|
||
|
|
if (raw_x < x_min) {
|
||
|
|
x_min = raw_x;
|
||
|
|
i = 0;
|
||
|
|
}
|
||
|
|
if (raw_x > x_max) {
|
||
|
|
x_max = raw_x;
|
||
|
|
i = 0;
|
||
|
|
}
|
||
|
|
if (raw_y < y_min) {
|
||
|
|
y_min = raw_y;
|
||
|
|
i = 0;
|
||
|
|
}
|
||
|
|
if (raw_y > y_max) {
|
||
|
|
y_max = raw_y;
|
||
|
|
i = 0;
|
||
|
|
}
|
||
|
|
if (raw_z < z_min) {
|
||
|
|
z_min = raw_z;
|
||
|
|
i = 0;
|
||
|
|
}
|
||
|
|
if (raw_z > z_max) {
|
||
|
|
z_max = raw_z;
|
||
|
|
i = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
if ((sample_counter % 100) == 0) {
|
||
|
|
Serial.print("CALSTAT i=");
|
||
|
|
Serial.print(i);
|
||
|
|
Serial.print(" samples=");
|
||
|
|
Serial.print(sample_counter);
|
||
|
|
Serial.print(" x_min=");
|
||
|
|
Serial.print(x_min);
|
||
|
|
Serial.print(" x_max=");
|
||
|
|
Serial.print(x_max);
|
||
|
|
Serial.print(" y_min=");
|
||
|
|
Serial.print(y_min);
|
||
|
|
Serial.print(" y_max=");
|
||
|
|
Serial.print(y_max);
|
||
|
|
Serial.print(" z_min=");
|
||
|
|
Serial.print(z_min);
|
||
|
|
Serial.print(" z_max=");
|
||
|
|
Serial.println(z_max);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
delay(5);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (sample_counter == 0) {
|
||
|
|
Serial.println("No calibration samples were collected.");
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Mean/centroid-based offsets.
|
||
|
|
x_offset = (int16_t)lround((double)x_sum / (double)sample_counter);
|
||
|
|
y_offset = (int16_t)lround((double)y_sum / (double)sample_counter);
|
||
|
|
z_offset = (int16_t)lround((double)z_sum / (double)sample_counter);
|
||
|
|
|
||
|
|
// Also compute the old midpoint offsets for comparison/debugging.
|
||
|
|
int16_t x_midpoint_offset = (x_max + x_min) / 2;
|
||
|
|
int16_t y_midpoint_offset = (y_max + y_min) / 2;
|
||
|
|
int16_t z_midpoint_offset = (z_max + z_min) / 2;
|
||
|
|
|
||
|
|
Serial.print("samples:");
|
||
|
|
Serial.println(sample_counter);
|
||
|
|
|
||
|
|
Serial.print("x_min:");
|
||
|
|
Serial.println(x_min);
|
||
|
|
|
||
|
|
Serial.print("x_max:");
|
||
|
|
Serial.println(x_max);
|
||
|
|
|
||
|
|
Serial.print("y_min:");
|
||
|
|
Serial.println(y_min);
|
||
|
|
|
||
|
|
Serial.print("y_max:");
|
||
|
|
Serial.println(y_max);
|
||
|
|
|
||
|
|
Serial.print("z_min:");
|
||
|
|
Serial.println(z_min);
|
||
|
|
|
||
|
|
Serial.print("z_max:");
|
||
|
|
Serial.println(z_max);
|
||
|
|
|
||
|
|
Serial.print("x_midpoint_offset:");
|
||
|
|
Serial.println(x_midpoint_offset);
|
||
|
|
|
||
|
|
Serial.print("y_midpoint_offset:");
|
||
|
|
Serial.println(y_midpoint_offset);
|
||
|
|
|
||
|
|
Serial.print("z_midpoint_offset:");
|
||
|
|
Serial.println(z_midpoint_offset);
|
||
|
|
|
||
|
|
Serial.print("x_mean_offset:");
|
||
|
|
Serial.println(x_offset);
|
||
|
|
|
||
|
|
Serial.print("y_mean_offset:");
|
||
|
|
Serial.println(y_offset);
|
||
|
|
|
||
|
|
Serial.print("z_mean_offset:");
|
||
|
|
Serial.println(z_offset);
|
||
|
|
|
||
|
|
// Set the mean-based calibration values.
|
||
|
|
magnetometer.setOffset(x_offset, y_offset, z_offset);
|
||
|
|
|
||
|
|
Serial.println();
|
||
|
|
Serial.println("Calibration complete!");
|
||
|
|
Serial.println("Offsets applied using raw-sample means.");
|
||
|
|
Serial.println("Check if Magnetic Strength is ~25-65 uT");
|
||
|
|
Serial.println("If too low, repeat calibration with better rotation");
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void setup()
|
||
|
|
{
|
||
|
|
Serial.begin(115200);
|
||
|
|
while (!Serial);
|
||
|
|
|
||
|
|
// LilyGo T-Beam-Supreme sensor requires a power source to function.
|
||
|
|
beginPower();
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Supports QMC6310U and QMC6310N; simply pass the corresponding device address
|
||
|
|
* during initialization.
|
||
|
|
* - QMC6310U_SLAVE_ADDRESS
|
||
|
|
* - QMC6310N_SLAVE_ADDRESS
|
||
|
|
*/
|
||
|
|
uint8_t address = QMC6310U_SLAVE_ADDRESS;
|
||
|
|
// uint8_t address = QMC6310N_SLAVE_ADDRESS;
|
||
|
|
|
||
|
|
if (!magnetometer.begin(Wire, address, SENSOR_SDA, SENSOR_SCL)) {
|
||
|
|
while (1) {
|
||
|
|
Serial.println("Failed to find QMC6310 - check your wiring!");
|
||
|
|
delay(1000);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// The desired output data rate in Hz. Allowed values are 10.0, 50.0, 100.0 and 200.0HZ.
|
||
|
|
float data_rate_hz = 200.0f;
|
||
|
|
// op_mode: Allowed values are SUSPEND, NORMAL, SINGLE_MEASUREMENT, CONTINUOUS_MEASUREMENT
|
||
|
|
OperationMode op_mode = OperationMode::CONTINUOUS_MEASUREMENT;
|
||
|
|
// full_scale: Allowed values are FS_2G, FS_8G, FS_12G ,FS_30G
|
||
|
|
MagFullScaleRange full_scale = MagFullScaleRange::FS_8G;
|
||
|
|
// over_sample_ratio: Allowed values are OSR_1, OSR_2, OSR_4, OSR_8
|
||
|
|
MagOverSampleRatio over_sample_ratio = MagOverSampleRatio::OSR_1;
|
||
|
|
// down_sample_ratio: Allowed values are DSR_1, DSR_2, DSR_4, DSR_8
|
||
|
|
MagDownSampleRatio down_sample_ratio = MagDownSampleRatio::DSR_1;
|
||
|
|
|
||
|
|
/* Config Magnetometer */
|
||
|
|
if (magnetometer.configMagnetometer(
|
||
|
|
op_mode,
|
||
|
|
full_scale,
|
||
|
|
data_rate_hz,
|
||
|
|
over_sample_ratio,
|
||
|
|
down_sample_ratio)) {
|
||
|
|
Serial.println("Magnetometer configured successfully.");
|
||
|
|
} else {
|
||
|
|
Serial.println("Magnetometer configuration failed.");
|
||
|
|
while (1);
|
||
|
|
}
|
||
|
|
|
||
|
|
calibrate();
|
||
|
|
Serial.println("Calibration done.");
|
||
|
|
|
||
|
|
Serial.println();
|
||
|
|
|
||
|
|
// Print Sensor ID and basic configuration.
|
||
|
|
SensorInfo info = magnetometer.getSensorInfo();
|
||
|
|
Serial.print("Manufacturer: ");
|
||
|
|
Serial.println(info.manufacturer);
|
||
|
|
|
||
|
|
Serial.print("Model: ");
|
||
|
|
Serial.println(info.model);
|
||
|
|
|
||
|
|
Serial.print("I2C Address: 0x");
|
||
|
|
Serial.println(info.i2c_address, HEX);
|
||
|
|
|
||
|
|
Serial.print("Version: ");
|
||
|
|
Serial.println(info.version);
|
||
|
|
|
||
|
|
Serial.print("UID: 0x");
|
||
|
|
Serial.println(info.uid, HEX);
|
||
|
|
|
||
|
|
Serial.print("Type: ");
|
||
|
|
Serial.print("Type: "); Serial.println(SensorUtils::typeToString(info.type));
|
||
|
|
Serial.println();
|
||
|
|
|
||
|
|
/* Print Sensor settings. */
|
||
|
|
// TODO: Add getSensorSettings() method to retrieve current sensor settings and print them here.
|
||
|
|
// auto settings = magnetometer.getSensorSettings();
|
||
|
|
// Serial.print("DataRate: ");
|
||
|
|
// Serial.println(settings.outputDataRate);
|
||
|
|
|
||
|
|
// Serial.print("FullScaleRange: ");
|
||
|
|
// Serial.println(settings.fullScaleRange);
|
||
|
|
|
||
|
|
// Serial.print("Mode: ");
|
||
|
|
// Serial.println(settings.mode);
|
||
|
|
|
||
|
|
// Serial.println();
|
||
|
|
|
||
|
|
// User's location magnetic declination offset.
|
||
|
|
//float declination_offset_deg = -3.33f; // Lewis's
|
||
|
|
float declination_offset_deg = 14.28f; // Salem, Oregon
|
||
|
|
magnetometer.setDeclination(declination_offset_deg);
|
||
|
|
Serial.print(" Magnetic Declination: ");
|
||
|
|
Serial.print(declination_offset_deg);
|
||
|
|
Serial.println("°");
|
||
|
|
|
||
|
|
// Print current sensitivity.
|
||
|
|
// Full scale 2G: 0.000067 Gauss/LSB
|
||
|
|
// Full scale 8G: 0.000267 Gauss/LSB
|
||
|
|
// Full scale 12G: 0.000400 Gauss/LSB
|
||
|
|
// Full scale 30G: 0.001000 Gauss/LSB
|
||
|
|
Serial.print(" Sensitivity: ");
|
||
|
|
Serial.print(magnetometer.getSensitivity(), 6);
|
||
|
|
Serial.println(" Gauss/LSB");
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void loop()
|
||
|
|
{
|
||
|
|
MagnetometerData data;
|
||
|
|
|
||
|
|
if (magnetometer.readData(data)) {
|
||
|
|
|
||
|
|
// Gauss to μT
|
||
|
|
float x = MagnetometerUtils::gaussToMicroTesla(data.magnetic_field.x);
|
||
|
|
float y = MagnetometerUtils::gaussToMicroTesla(data.magnetic_field.y);
|
||
|
|
float z = MagnetometerUtils::gaussToMicroTesla(data.magnetic_field.z);
|
||
|
|
|
||
|
|
Serial.print("Mag:");
|
||
|
|
Serial.print(" X:"); Serial.print(x);
|
||
|
|
Serial.print(" Y:"); Serial.print(y);
|
||
|
|
Serial.print(" Z:"); Serial.print(z);
|
||
|
|
Serial.print(" μT");
|
||
|
|
|
||
|
|
Serial.print(" Sensitivity: ");
|
||
|
|
Serial.print(magnetometer.getSensitivity(), 6);
|
||
|
|
Serial.print(" Gauss/LSB");
|
||
|
|
|
||
|
|
Serial.print(" Metadata:");
|
||
|
|
Serial.print(" X:");
|
||
|
|
Serial.print(data.raw.x);
|
||
|
|
Serial.print(" Y:");
|
||
|
|
Serial.print(data.raw.y);
|
||
|
|
Serial.print(" Z:");
|
||
|
|
Serial.print(data.raw.z);
|
||
|
|
|
||
|
|
//Serial.print(" Heading (rad): ");
|
||
|
|
//Serial.print(data.heading, 6);
|
||
|
|
//Serial.print(" rad");
|
||
|
|
|
||
|
|
//Serial.print(" Heading (deg): ");
|
||
|
|
//Serial.print(data.heading_degrees, 2);
|
||
|
|
//Serial.print("°");
|
||
|
|
// Use raw values (already offset-corrected by calibration)
|
||
|
|
float Xc = data.raw.x;
|
||
|
|
float Yc = data.raw.y;
|
||
|
|
|
||
|
|
// Compute corrected heading
|
||
|
|
//float heading_rad = atan2(Yc, Xc); // this was mirrored
|
||
|
|
float heading_rad = atan2(-Yc, Xc);
|
||
|
|
// Convert to degrees
|
||
|
|
float heading_deg = heading_rad * 180.0 / PI;
|
||
|
|
if (heading_deg < 0) heading_deg += 360;
|
||
|
|
|
||
|
|
// Print corrected values
|
||
|
|
Serial.print(" HeadingCorr (rad): ");
|
||
|
|
Serial.print(heading_rad, 6);
|
||
|
|
Serial.print(" rad");
|
||
|
|
|
||
|
|
Serial.print(" HeadingCorr (deg): ");
|
||
|
|
Serial.print(heading_deg, 2);
|
||
|
|
Serial.print("°");
|
||
|
|
|
||
|
|
float strength = MagnetometerUtils::calculateMagneticStrength(data);
|
||
|
|
strength = MagnetometerUtils::gaussToMicroTesla(strength);
|
||
|
|
Serial.print(" Magnetic Strength: ");
|
||
|
|
Serial.print(strength, 2);
|
||
|
|
Serial.println(" μT");
|
||
|
|
|
||
|
|
if (data.overflow) {
|
||
|
|
Serial.println("\tWarning: Data Overflow occurred!");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
delay(10);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|