adam/LoraSA
1
0
Fork 0
mirror of https://github.com/Genaker/LoraSA.git synced 2026-08-07 17:33:00 +02:00
Code Issues Packages Projects Releases Wiki Activity

Move HMC5883 to Compass

Browse Source
This commit is contained in:
Sassa NF
2025-04-13 16:11:00 +01:00
parent 726a5e797f
commit e9a09eec58
3 changed files with 326 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files
lib/heading/Compass.cpp
+265
View File
@@ -1,6 +1,109 @@
#include "heading.h"
#include <bus.h>
enum MountingOrientation
{
XY, // X forward, Y right, Z down
X_Y, // X forward, Y left, Z up
XZ, // X forward, Z right, Y up
X_Z, // X forward, Z left, Y down
YX, // Y forward, X right, Z up
Y_X, // Y forward, X left, Z down
YZ, // Y forward, Z right, X down
Y_Z, // Y forward, Z left, X up
ZX, // Z forward, X right, Y down
Z_X, // Z forward, X left, Y up
ZY, // Z forward, Y right, X down
Z_Y // Z forward, Y left, X up
};
// Produces CompassXYZ in a canonical mounting orientation: X forward, Y left, Z up
CompassXYZ _orientation(MountingOrientation o, int16_t x, int16_t y, int16_t z)
{
CompassXYZ res;
res.status = 0;
switch (o)
{
case XY:
case X_Y:
case XZ:
case X_Z:
res.x = x;
break;
case YX:
case Y_X:
case YZ:
case Y_Z:
res.x = y;
break;
case ZY:
case Z_Y:
case ZX:
case Z_X:
res.x = z;
break;
}
switch (o)
{
case X_Y:
case Z_Y:
res.y = y;
break;
case XY:
case ZY:
res.y = -y;
break;
case Y_X:
case Z_X:
res.y = x;
break;
case YX:
case ZX:
res.y = -x;
break;
case X_Z:
case Y_Z:
res.y = z;
break;
case XZ:
case YZ:
res.y = -z;
break;
}
switch (o)
{
case X_Y:
case YX:
res.z = z;
break;
case XY:
case Y_X:
res.z = -z;
break;
case XZ:
case Z_X:
res.z = y;
break;
case X_Z:
case ZX:
res.z = -y;
break;
case Y_Z:
case Z_Y:
res.z = x;
break;
case YZ:
case ZY:
res.z = -x;
break;
}
return res;
}
/*
* QMC5883L Registers:
*
@@ -211,7 +314,15 @@ int16_t Compass::heading()
return -999;
}
// heading for canonical mounting orientation: X forward, Y left, Z up
float heading = atan2(xyz.y, xyz.x);
// Once you have your heading, you must then add your 'Declination Angle', which
// is the 'Error' of the magnetic field in your location. Find yours here:
// http://www.magnetic-declination.com/ Mine is: -13* 2' W, which is ~13 Degrees,
// or (which we need) 0.22 radians If you cannot find your Declination, comment
// out these two lines, your compass will be slightly off.
float declinationAngle = 0.22;
heading += declinationAngle;
@@ -228,3 +339,157 @@ int16_t Compass::heading()
return headingDegrees;
}
bool UninitializedCompass::begin()
{
_lastErr = CompassStatus::COMPASS_UNINITIALIZED;
return false;
}
String UninitializedCompass::selfTest() { return "No compass is attached"; }
uint8_t UninitializedCompass::setMode(CompassMode m) { return 4; }
int8_t UninitializedCompass::readXYZ() { return 4; }
#ifdef COMPASS_ENABLED
Adafruit_HMC5883_Unified _mag = Adafruit_HMC5883_Unified(12345);
#else
UninitializedCompass _mag;
#endif
bool HMC5883LCompass::begin()
{
_lastErr = CompassStatus::COMPASS_UNINITIALIZED;
#ifdef COMPASS_ENABLED
if (!mag.begin())
{
return false;
}
String err = selfTest();
if (!err.startsWith("OK\n"))
{
return false;
}
_lastErr = CompassStatus::COMPASS_OK;
return true;
#else
return mag.begin();
#endif
}
String HMC5883LCompass::selfTest()
{
#ifdef COMPASS_ENABLED
sensor_t sensor;
mag.getSensor(&sensor);
return "OK\nSensor: " + String(sensor.name) +
"\nDriver Ver: " + String(sensor.version) +
"\nUnique ID: " + String(sensor.sensor_id) +
"\nMax Value: " + String(sensor.max_value) + " uT" +
"\nMin Value: " + String(sensor.min_value) + " uT" +
"\nResolution: " + String(sensor.resolution) + " uT";
#else
return mag.selfTest();
#endif
}
uint8_t HMC5883LCompass::setMode(CompassMode m)
{
#ifdef COMPASS_ENABLED
_lastErr = m;
return 0;
#else
return 1;
#endif
}
int8_t HMC5883LCompass::readXYZ()
{
#ifdef COMPASS_ENABLED
if (calStart == 0)
{
calStart = millis();
}
/* Get a new sensor event */
sensors_event_t event2;
mag.getEvent(&event2);
sensors_event_t event3;
mag.getEvent(&event3);
#ifdef COMPASS_DEBUG
/* Display the results (magnetic vector values are in micro-Tesla (uT)) */
Serial.print("X: ");
Serial.print(event2.magnetic.x);
Serial.print(" ");
Serial.print("Y: ");
Serial.print(event2.magnetic.y);
Serial.print(" ");
Serial.print("Z: ");
Serial.print(event2.magnetic.z);
Serial.print(" ");
Serial.println("uT");
#endif
// Hold the module so that Z is pointing 'up' and you can measure the heading with
// x&y Calculate heading when the magnetometer is level, then correct for signs of
// axis. float heading = atan2(event.magnetic.y, event.magnetic.x); Use Y as the
// forward axis float heading = atan2(event.magnetic.x, event.magnetic.y);
/// If Z-axis is forward and Y-axis points upward:
// float heading = atan2(event.magnetic.x, event.magnetic.y);
// If Z-axis is forward and X-axis points upward:
// float heading = atan2(event.magnetic.y, -event.magnetic.x);
// heading based on the magnetic readings from the Z-axis (forward) and the X-axis
// (perpendicular to Z, horizontal).
// float heading = atan2(event.magnetic.z, event.magnetic.x);
// Dynamicly Calibrated out
// Read raw magnetometer data
float x = (event2.magnetic.x + event3.magnetic.x) / 2;
float y = (event2.magnetic.y + event3.magnetic.y) / 2;
float z = (event2.magnetic.z + event3.magnetic.z) / 2;
// Doing calibration first 1 minute
if (millis() - calStart < 60000)
{
// Update min/max values dynamically
x_min = min(x_min, x);
x_max = max(x_max, x);
y_min = min(y_min, y);
y_max = max(y_max, y);
z_min = min(z_min, z);
z_max = max(z_max, z);
}
#ifdef COMPASS_DEBUG
Serial.println("x_min:" + String(x_min) + " x_max: " + String(x_max) +
" y_min: " + String(y_min));
#endif
// Calculate offsets and scales in real-time
float x_offset = (x_max + x_min) / 2;
float y_offset = (y_max + y_min) / 2;
float z_offset = (z_max + z_min) / 2;
float x_scale = (x_max - x_min) / 2;
float y_scale = (y_max - y_min) / 2;
float z_scale = (z_max - z_min) / 2;
// Apply calibration to raw data
float calibrated_x = (x - x_offset) / x_scale;
float calibrated_y = (y - y_offset) / y_scale;
float calibrated_z = (z - z_offset) / z_scale;
xyz = _orientation(ZX, calibrated_x, calibrated_y, calibrated_z);
return 0;
#else
return mag.readXYZ();
#endif
}
lib/heading/heading.h
+39 -1
View File
@@ -68,12 +68,50 @@ struct QMC5883LCompass : Compass
int8_t readXYZ() override;
};
struct UninitializedCompass : Compass
{
UninitializedCompass() : Compass() {}
bool begin() override;
String selfTest() override;
uint8_t setMode(CompassMode m) override;
int8_t readXYZ() override;
};
#ifdef COMPASS_ENABLED
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_Sensor.h>
#define HMC5883Type Adafruit_HMC5883_Unified
#else
#define HMC5883Type UninitializedCompass
#endif
extern HMC5883Type _mag;
struct HMC5883LCompass : Compass
{
HMC5883Type &mag;
long calStart = 0;
// Variables for dynamic calibration
float x_min = 1000, x_max = -1000;
float y_min = 1000, y_max = -1000;
float z_min = 1000, z_max = -1000;
HMC5883LCompass() : Compass(), mag(_mag) {}
bool begin() override;
String selfTest() override;
uint8_t setMode(CompassMode m) override;
int8_t readXYZ() override;
};
struct DroneHeading : HeadingSensor
{
int64_t _lastRead;
int16_t _heading;
DroneHeading() : HeadingSensor(), _lastRead(-1) {}
DroneHeading() : HeadingSensor(), _lastRead(-1), _heading(-999) {}
void setHeading(int64_t now, int16_t heading);
src/main.cpp
+22 -170
View File
@@ -304,6 +304,7 @@ void sendBTData(float heading, float rssi)
DroneHeading droneHeading;
Compass *compass = NULL;
HeadingSensor &headingSensor = droneHeading;
RadioModule *radio2;
@@ -390,41 +391,14 @@ DFRobot_OSD osd(OSD_CS);
#include "global_config.h"
#include "ui.h"
#ifdef COMPASS_ENABLED
#include <Adafruit_HMC5883_U.h>
#include <Adafruit_Sensor.h>
/* Assign a unique ID to this sensor at the same time */
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
void displaySensorDetails(void)
{
sensor_t sensor;
mag.getSensor(&sensor);
Serial.println("------------------------------------");
Serial.print("Sensor: ");
Serial.println(sensor.name);
Serial.print("Driver Ver: ");
Serial.println(sensor.version);
Serial.print("Unique ID: ");
Serial.println(sensor.sensor_id);
Serial.print("Max Value: ");
Serial.print(sensor.max_value);
Serial.println(" uT");
Serial.print("Min Value: ");
Serial.print(sensor.min_value);
Serial.println(" uT");
Serial.print("Resolution: ");
Serial.print(sensor.resolution);
Serial.println(" uT");
Serial.println("------------------------------------");
Serial.println("");
if (compass == NULL)
return;
Serial.println(compass->selfTest());
heltec_delay(500);
}
// Variables for dynamic calibration
float x_min = 1000, x_max = -1000;
float y_min = 1000, y_max = -1000;
float z_min = 1000, z_max = -1000;
#endif
// -----------------------------------------------------------------
// CONFIGURATION OPTIONS
// -----------------------------------------------------------------
@@ -1198,15 +1172,9 @@ void eventListenerForReport(void *arg, Event &e)
frequency_scan_result.rssi = e.detected.rssi;
}
HeadingSensor *sensor = &droneHeading;
if (compass != NULL && compass->lastRead() > sensor->lastRead())
int16_t heading = headingSensor.heading();
if (heading > -999)
{
sensor = compass;
}
if (sensor->lastRead() > -1)
{
int16_t heading = sensor->heading();
if (frequency_scan_result.dump.heading_min == -999)
{
frequency_scan_result.dump.heading_min = heading;
@@ -1875,32 +1843,14 @@ void setup(void)
r.addEventListener(ALL_EVENTS, eventListenerForReport, NULL);
#ifdef COMPASS_ENABLED
Serial.println("Compass Init Start");
Wire1.end();
Wire1.begin(46, 42);
Serial.println("Compass BEGIN");
Serial.println("HMC5883 Magnetometer Test");
/* Initialise the sensor */
if (!mag.begin())
{
/* There was a problem detecting the HMC5883 ... check your connections */
Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
}
/* Display some basic information on this sensor */
displaySensorDetails();
Serial.println("Compass Success!!!");
#endif
if (wireDevices & QMC5883L || wire1Devices & QMC5883L)
{
compass = new QMC5883LCompass(wireDevices & QMC5883L ? Wire : Wire1);
}
else if (wireDevices & HMC5883L)
{
compass = new HMC5883LCompass();
}
if (compass)
{
@@ -1913,6 +1863,7 @@ void setup(void)
if (err.startsWith("OK\n"))
{
Serial.printf("Compass self-test passed: %s\n", err.c_str());
headingSensor = *compass;
}
else
{
@@ -2413,111 +2364,8 @@ int max_rssi_x = 999;
void doScan();
void reportScan();
long calStart = 0;
#ifdef COMPASS_ENABLED
float getCompassHeading()
{
if (calStart == 0)
{
calStart = millis();
}
/* Get a new sensor event */
sensors_event_t event2;
mag.getEvent(&event2);
sensors_event_t event3;
mag.getEvent(&event3);
#ifdef COMPASS_DEBUG
/* Display the results (magnetic vector values are in micro-Tesla (uT)) */
Serial.print("X: ");
Serial.print(event2.magnetic.x);
Serial.print(" ");
Serial.print("Y: ");
Serial.print(event2.magnetic.y);
Serial.print(" ");
Serial.print("Z: ");
Serial.print(event2.magnetic.z);
Serial.print(" ");
Serial.println("uT");
#endif
// Hold the module so that Z is pointing 'up' and you can measure the heading with
// x&y Calculate heading when the magnetometer is level, then correct for signs of
// axis. float heading = atan2(event.magnetic.y, event.magnetic.x); Use Y as the
// forward axis float heading = atan2(event.magnetic.x, event.magnetic.y);
/// If Z-axis is forward and Y-axis points upward:
// float heading = atan2(event.magnetic.x, event.magnetic.y);
// If Z-axis is forward and X-axis points upward:
// float heading = atan2(event.magnetic.y, -event.magnetic.x);
// heading based on the magnetic readings from the Z-axis (forward) and the X-axis
// (perpendicular to Z, horizontal).
// float heading = atan2(event.magnetic.z, event.magnetic.x);
// Dynamicly Calibrated out
// Read raw magnetometer data
float x = (event2.magnetic.x + event3.magnetic.x) / 2;
float y = (event2.magnetic.y + event3.magnetic.y) / 2;
float z = (event2.magnetic.z + event3.magnetic.z) / 2;
// Doing calibration first 1 minute
if (millis() - calStart < 60000)
{
// Update min/max values dynamically
x_min = min(x_min, x);
x_max = max(x_max, x);
y_min = min(y_min, y);
y_max = max(y_max, y);
z_min = min(z_min, z);
z_max = max(z_max, z);
}
#ifdef COMPASS_DEBUG
Serial.println("x_min:" + String(x_min) + " x_max: " + String(x_max) +
" y_min: " + String(y_min));
#endif
// Calculate offsets and scales in real-time
float x_offset = (x_max + x_min) / 2;
float y_offset = (y_max + y_min) / 2;
float z_offset = (z_max + z_min) / 2;
float x_scale = (x_max - x_min) / 2;
float y_scale = (y_max - y_min) / 2;
float z_scale = (z_max - z_min) / 2;
// Apply calibration to raw data
float calibrated_x = (x - x_offset) / x_scale;
float calibrated_y = (y - y_offset) / y_scale;
float calibrated_z = (z - z_offset) / z_scale;
// Calculate heading using Z-axis forward, X-axis horizontal
float heading = atan2(calibrated_z, calibrated_x);
// Once you have your heading, you must then add your 'Declination Angle', which
// is the 'Error' of the magnetic field in your location. Find yours here:
// http://www.magnetic-declination.com/ Mine is: -13* 2' W, which is ~13 Degrees,
// or (which we need) 0.22 radians If you cannot find your Declination, comment
// out these two lines, your compass will be slightly off.
float declinationAngle = 0.22;
heading += declinationAngle;
// Correct for when signs are reversed.
if (heading < 0)
heading += 2 * PI;
// Check for wrap due to addition of declination.
if (heading > 2 * PI)
heading -= 2 * PI;
// Convert radians to degrees for readability.
float headingDegrees = heading * 180 / M_PI;
return headingDegrees;
}
#endif
void processHeading();
float historicalCompassRssi[STEPS] = {999};
int compassCounter = 0;
@@ -2596,8 +2444,14 @@ void loop(void)
}
#endif
#endif
if (compass != NULL || droneHeading.lastRead() > -1)
{
processHeading();
}
}
#ifdef COMPASS_ENABLED
void processHeading()
{
#if defined(COMPASS_FREQ)
// delay(1000);
display.clear();
@@ -2614,7 +2468,7 @@ void loop(void)
{
// ToDO: fix go to;
compass:
float headingDegrees = getCompassHeading();
float headingDegrees = headingSensor.heading();
// Serial.println("Heading (degrees): " + String(headingDegrees));
#ifndef COMPASS_FREQ
t = 0;
@@ -2652,7 +2506,7 @@ void loop(void)
#else
rssi = getRssi(false);
#endif
float headingDegreesAfter = getCompassHeading();
float headingDegreesAfter = headingSensor.heading();
float compassDiff = abs(headingDegreesAfter - headingDegrees);
if (compassDiff >= 3)
{
@@ -2865,8 +2719,6 @@ void loop(void)
#if defined(COMPASS_FREQ)
display.clear();
#endif // COMPASS_FREQ
#endif // end COMPASS_ENABLED
}
void doScan()