128 lines
3.0 KiB
C++
128 lines
3.0 KiB
C++
/*
|
||
* © Copyright 2014 – Urheberrechtshinweis
|
||
* Alle Rechte vorbehalten / All Rights Reserved
|
||
*
|
||
* Programmcode ist urheberrechtlich geschuetzt.
|
||
* Das Urheberrecht liegt, soweit nicht ausdruecklich anders gekennzeichnet, bei Frank Ebner.
|
||
* Keine Verwendung ohne explizite Genehmigung.
|
||
* (vgl. § 106 ff UrhG / § 97 UrhG)
|
||
*/
|
||
|
||
#ifndef TURNDETECTION2_H
|
||
#define TURNDETECTION2_H
|
||
|
||
|
||
#include "GyroscopeData.h"
|
||
#include "AccelerometerData.h"
|
||
#include "../../data/Timestamp.h"
|
||
#include "../../math/MovingAverageTS.h"
|
||
#include "../../math/Matrix3.h"
|
||
|
||
#include "../../geo/Point3.h"
|
||
#include "PoseProvider.h"
|
||
|
||
//#include <eigen3/Eigen/Dense>
|
||
|
||
#include <cmath>
|
||
#include <vector>
|
||
|
||
#include "TurnDetectionPlot.h"
|
||
|
||
|
||
#include "../../Assertions.h"
|
||
|
||
/** THIS WILL NOT WORK AS EXPECTED! */
|
||
class TurnDetection2 {
|
||
|
||
private:
|
||
|
||
PoseProvider* pose = nullptr;
|
||
|
||
Vector3 sumSinceStart = Vector3(0,0,0);
|
||
|
||
Timestamp lastGyroReadingTS;
|
||
Vector3 lastGyroReading = Vector3(0,0,0);
|
||
|
||
bool first = true;
|
||
Matrix3 poseMat = Matrix3::identity();
|
||
|
||
float curRes = 0;
|
||
|
||
#ifdef WITH_DEBUG_PLOT
|
||
TurnDetectionPlot plot;
|
||
#endif
|
||
|
||
public:
|
||
|
||
/** ctor */
|
||
TurnDetection2(PoseProvider* pose) : pose(pose) {
|
||
;
|
||
}
|
||
|
||
|
||
float addGyroscope(const Timestamp& ts, const GyroscopeData& gyro) {
|
||
|
||
|
||
|
||
// ignore the first reading completely, just remember its timestamp
|
||
if (lastGyroReadingTS.isZero()) {lastGyroReadingTS = ts; return curRes;}
|
||
|
||
// time-difference between previous and current reading
|
||
const Timestamp curDiff = ts - lastGyroReadingTS;
|
||
lastGyroReadingTS = ts;
|
||
|
||
// fast sensors might lead to delay = 0 ms. filter those values
|
||
if (curDiff.isZero()) {return curRes;}
|
||
|
||
// current area
|
||
//const Eigen::Vector3f area =
|
||
const Vector3 curArea =
|
||
|
||
// Trapezoid rule (should be more accurate but does not always help?!)
|
||
//(prevGyro * curDiff.sec()) + // squared region
|
||
//((curGyro - prevGyro) * 0.5 * curDiff.sec()); // triangle region to the next (enhances the quality)
|
||
|
||
// just the rectangular region
|
||
(lastGyroReading * curDiff.sec()); // BEST?!
|
||
|
||
//}
|
||
|
||
// adjust sum
|
||
sumSinceStart = sumSinceStart + curArea;
|
||
|
||
// update the previous value
|
||
lastGyroReading = Vector3(gyro.x, gyro.y, gyro.z);
|
||
|
||
|
||
// TESTING
|
||
const float sign = (curArea.z < 0) ? (-1) : (+1);
|
||
const float mag = curArea.norm();
|
||
return (mag * sign);
|
||
|
||
|
||
// // ignore readings until the first orientation-estimation is available
|
||
// // otherwise we would use a wrong rotation matrix which yields wrong results!
|
||
// if (!pose->isKnown()) {return curRes;}
|
||
// if (first) {poseMat = pose->getMatrix(); first = false;}
|
||
|
||
// // rotate the sum since start into our desired coordinate system, where the smartphone lies flat on the ground
|
||
// const Vector3 angles = pose->getMatrix() * sumSinceStart;
|
||
// //const Vector3 angles = poseMat * sumSinceStart;
|
||
|
||
// // rotation = z-axis only!
|
||
// //const float delta = area(2);
|
||
// curRes = angles.z;
|
||
|
||
// #ifdef WITH_DEBUG_PLOT
|
||
// plot.addAbsolute(ts, curRes, gyro, sumSinceStart);
|
||
// #endif
|
||
|
||
// // done
|
||
// return curRes;
|
||
|
||
}
|
||
|
||
};
|
||
|
||
#endif // TURNDETECTION2_H
|