FHWS/Indoor
Archived
4
0
Fork 0
Code Issues 19 Pull Requests Releases 1 Wiki Activity

fixed some issues

Browse Source 
added new pose/turn detections
new helper classes
define-flags for libEigen
This commit is contained in:
FrankE
2018-09-04 10:49:00 +02:00
parent f990485d44
commit 857d7a1553
51 changed files with 2149 additions and 207 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
sensors/imu/CompassDetection.h
View File

@@ -2,18 +2,21 @@
#define INDOOR_IMU_COMPASSDETECTION_H
#include "MagnetometerData.h"
#include "PoseDetection.h"
#include "PoseProvider.h"
#include "TurnProvider.h"
#include "../../data/Timestamp.h"
#include "../../math/MovingAverageTS.h"
#include "../../math/MovingStdDevTS.h"
#include "../../geo/Point3.h"
#include "../../Assertions.h"
#include "../../geo/Angle.h"
#include "CompassDetectionPlot.h"
#include <cmath>
#include <vector>
#include "../../math/dsp/iir/BiQuad.h"
/**
@@ -33,21 +36,37 @@ private:
// Timestamp lastEstimation;
// } orientation;
MovingAverageTS<MagnetometerData> avgIn = MovingAverageTS<MagnetometerData>(Timestamp::fromMS(150), MagnetometerData(0,0,0));
MovingAverageTS<MagnetometerData> avgIn;
//MovingStdDevTS<MagnetometerData> stdDev = MovingStdDevTS<MagnetometerData>(Timestamp::fromMS(2000), MagnetometerData(0,0,0));
MovingStdDevTS<float> stdDev = MovingStdDevTS<float>(Timestamp::fromMS(1500), 0);
PoseDetection* pose = nullptr;
const PoseProvider* pose = nullptr;
const TurnProvider* turn = nullptr;
int numMagReadings = 0;
bool stable;
MovingStdDevTS<float> stdDevForSigma = MovingStdDevTS<float>(Timestamp::fromMS(500), 0);
float lastHeading = 0;
float curSigma = 0;
public:
/** ctor */
CompassDetection(PoseDetection* pose) : pose(pose) {
CompassDetection(const PoseProvider* pose, const TurnProvider* turn, const Timestamp avgFrame = Timestamp::fromMS(500)) :
pose(pose), turn(turn), avgIn(avgFrame, MagnetometerData(0,0,0)) {
;
}
/** get the current uncertainty estimation */
float getSigma() {
return curSigma + pose->getSigma() + turn->getSigma();
}
/** add magnetometer readings, returns the current heading, or NAN (if unstable/unknown) */
float addMagnetometer(const Timestamp& ts, const MagnetometerData& mag) {
@@ -73,30 +92,38 @@ public:
// calculate angle
// https://aerocontent.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
const float mx = curMag.x;
const float my = curMag.y;
const float mx = (curMag.x);
const float my = (curMag.y);
const float tmp = std::atan2(my, mx);
//const float tmp = (my > 0) ? (M_PI*0.5 - std::atan(mx/my)) : (M_PI*1.5 - atan(mx/my));
// http://www.magnetic-declination.com/
// http://davidegironi.blogspot.de/2013/01/avr-atmega-hmc5883l-magnetometer-lib-01.html
const float declination = 3.0f / 180.0f * M_PI; // GERMANY!
const float curHeading = - tmp + declination;
//const float declination = 0; // GERMANY!
float curHeading = Angle::makeSafe_2PI(- tmp + declination);
float resHeading;
bool stable = true;
stable = true;
// calculate standard-deviation within a certain timeframe
stdDev.add(ts, curHeading);
const float curDiff = Angle::getSignedDiffRAD_2PI(curHeading, lastHeading);
stdDev.add(ts, curDiff);
stdDevForSigma.add(ts, curDiff);
curSigma = (5.0f/180.0f*(float)M_PI) + stdDevForSigma.get()*10;
lastHeading = curHeading;
// if the standard-deviation is too high,
// the compass is considered unstable
if (numMagReadings < 250 || stdDev.get() > 0.30) {
resHeading = NAN;
stable = false;
} else {
// if (numMagReadings < 250 || stdDev.get() > 0.30) {
// resHeading = NAN;
// stable = false;
// } else {
resHeading = curHeading;
stable = true;
}
// }
#ifdef WITH_DEBUG_PLOT
plot.add(ts, curHeading, stable, mag, curMag);
@@ -107,6 +134,7 @@ public:
}
};
#endif // INDOOR_IMU_COMPASSDETECTION_H