diff --git a/lib/heading/Compass.cpp b/lib/heading/Compass.cpp index 69237f4..d8d0d70 100644 --- a/lib/heading/Compass.cpp +++ b/lib/heading/Compass.cpp @@ -1,6 +1,109 @@ #include "heading.h" #include +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 +} diff --git a/lib/heading/heading.h b/lib/heading/heading.h index 193faca..6ce058f 100644 --- a/lib/heading/heading.h +++ b/lib/heading/heading.h @@ -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 +#include +#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); diff --git a/src/main.cpp b/src/main.cpp index 5e825b6..d57ab82 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -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 -#include -/* 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()