128 lines
3.5 KiB
C++
128 lines
3.5 KiB
C++
#ifndef INDOOR_SYNTHETICTURNS_H
|
|
#define INDOOR_SYNTHETICTURNS_H
|
|
|
|
#include "SyntheticWalker.h"
|
|
|
|
#include "../sensors/imu/AccelerometerData.h"
|
|
#include "../sensors/imu/GyroscopeData.h"
|
|
#include "../geo/Heading.h"
|
|
|
|
#include "../math/distribution/Normal.h"
|
|
|
|
/**
|
|
* simulates acceleromter and gyroscope data
|
|
* based on synthetic walking data
|
|
*
|
|
* @brief The SyntheticTurns class
|
|
*/
|
|
class SyntheticTurns : public SyntheticWalker::Listener {
|
|
|
|
public:
|
|
|
|
class Listener {
|
|
public:
|
|
virtual void onSyntheticTurnData(const Timestamp ts, const AccelerometerData acc, const GyroscopeData gyro) = 0;
|
|
};
|
|
|
|
private:
|
|
|
|
/** the walker to listen to */
|
|
SyntheticWalker* walker;
|
|
|
|
Distribution::Normal<float> dAccX = Distribution::Normal<float>(0, 2.5);
|
|
Distribution::Normal<float> dAccY = Distribution::Normal<float>(0, 1.5);
|
|
Distribution::Normal<float> dAccZ = Distribution::Normal<float>(0, 1);
|
|
|
|
Distribution::Normal<float> dGyroX = Distribution::Normal<float>(0, 0.02);
|
|
Distribution::Normal<float> dGyroY = Distribution::Normal<float>(0, 0.02);
|
|
Distribution::Normal<float> dGyroZ = Distribution::Normal<float>(0, 0.02);
|
|
|
|
Distribution::Normal<float> dMaxChange = Distribution::Normal<float>(0.011, 0.003);
|
|
Distribution::Normal<float> dChange = Distribution::Normal<float>(1.0, 0.25);
|
|
Distribution::Normal<float> dHeadErr = Distribution::Normal<float>(0.15, 0.10); // heading error, slightly biased
|
|
|
|
|
|
std::vector<Listener*> listeners;
|
|
|
|
public:
|
|
|
|
/** ctor with the walker to follow */
|
|
SyntheticTurns(SyntheticWalker* walker) {
|
|
walker->addListener(this);
|
|
dAccX.setSeed(1);
|
|
dAccY.setSeed(3);
|
|
dAccZ.setSeed(5);
|
|
dGyroX.setSeed(7);
|
|
dGyroY.setSeed(9);
|
|
dGyroZ.setSeed(11);
|
|
}
|
|
|
|
/** attach a listener to this provider */
|
|
void addListener(Listener* l) {
|
|
this->listeners.push_back(l);
|
|
}
|
|
|
|
protected:
|
|
|
|
Timestamp lastTs;
|
|
Heading desiredHead = Heading(0);
|
|
Heading curHead = Heading(0);
|
|
Point3 lastPos = Point3(NAN, NAN, NAN);
|
|
float change;
|
|
|
|
inline float clamp(const float val, const float min, const float max) {
|
|
if (val < min) {return min;}
|
|
if (val > max) {return max;}
|
|
return val;
|
|
}
|
|
|
|
void onWalk(const Timestamp walkedTime, float walkedDistance, const Point3 curPos) override {
|
|
|
|
// time sine last onWalk();
|
|
if (lastTs.isZero()) {lastTs = walkedTime; return;}
|
|
const Timestamp deltaTs = walkedTime - lastTs;
|
|
lastTs = walkedTime;
|
|
|
|
if (lastPos.x != lastPos.x) {
|
|
lastPos = curPos;
|
|
} else {
|
|
desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();;
|
|
lastPos = curPos;
|
|
}
|
|
|
|
// difference between current-heading and desired-heading
|
|
const float diffRad = Heading::getSignedDiff(curHead, desiredHead);
|
|
|
|
// slowly change the current heading to match the desired one
|
|
const float maxChange = dMaxChange.draw();
|
|
const float toChange = clamp(diffRad, -maxChange, +maxChange);
|
|
//if (change < toChange) {change += toChange*0.01;}
|
|
if (change > toChange) {change *= 0.93;}
|
|
if (change < toChange) {change += dChange.draw()/10000;}
|
|
//if (change > toChange) {change -= dChange.draw();}
|
|
|
|
|
|
|
|
curHead += change;
|
|
|
|
// convert to gyro's radians-per-second
|
|
const float radPerSec = change * 1000 / deltaTs.ms();;
|
|
|
|
const float accX = 0.00 + dAccX.draw();
|
|
const float accY = 0.00 + dAccY.draw();
|
|
const float accZ = 9.81 + dAccZ.draw();
|
|
AccelerometerData acc(accX, accY, accZ);
|
|
|
|
const float gyroX = dGyroX.draw();
|
|
const float gyroY = dGyroY.draw();
|
|
const float gyroZ = dGyroZ.draw() + radPerSec;
|
|
GyroscopeData gyro(gyroX, gyroY, gyroZ);
|
|
|
|
for (Listener* l : listeners) {l->onSyntheticTurnData(walkedTime, acc, gyro);}
|
|
|
|
}
|
|
|
|
};
|
|
|
|
#endif // INDOOR_SYNTHETICTURNS_H
|