118 lines
2.7 KiB
C++
118 lines
2.7 KiB
C++
#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
|