worked on synthetic sensors

worked on grid-walker
minor changes/fixes/improvements

synthetic/SyntheticTurns.h

@@ -1,4 +1,127 @@
-#ifndef SYNTHETICTURNS_H
-#define SYNTHETICTURNS_H
+#ifndef INDOOR_SYNTHETICTURNS_H
+#define INDOOR_SYNTHETICTURNS_H
 
-#endif // 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