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

worked on grid walker

Browse Source
This commit is contained in:
k-a-z-u
2017-11-15 16:41:57 +01:00
parent 08d8292976
commit 7af5131ccf
4 changed files with 190 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
synthetic/SyntheticTurns.h
View File

@@ -7,7 +7,7 @@
#include "../sensors/imu/GyroscopeData.h"
#include "../geo/Heading.h"
#include "../math/distribution/Normal.h"
#include "../math/Distributions.h"
/**
* simulates acceleromter and gyroscope data
@@ -41,6 +41,8 @@ private:
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
Distribution::Uniform<float> dRadDiff = Distribution::Uniform<float>(40,100);
std::vector<Listener*> listeners;
@@ -68,7 +70,7 @@ protected:
Heading desiredHead = Heading(0);
Heading curHead = Heading(0);
Point3 lastPos = Point3(NAN, NAN, NAN);
float change;
double change = 0;
inline float clamp(const float val, const float min, const float max) {
if (val < min) {return min;}
@@ -86,27 +88,33 @@ protected:
if (lastPos.x != lastPos.x) {
lastPos = curPos;
} else {
desiredHead = Heading(lastPos.x, lastPos.y, curPos.x, curPos.y) + dHeadErr.draw();;
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);
const double maxChange = dMaxChange.draw();
const double diffRad = Heading::getSignedDiff(curHead, desiredHead);
//change = clamp(diffRad / dRadDiff.draw(), -maxChange, +maxChange);
change = clamp(diffRad / 25, -maxChange, +maxChange);
// 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();}
// // slowly change the current heading to match the desired one
// //const double maxChange = dMaxChange.draw();
// //const double toChange = clamp(diffRad, -maxChange, +maxChange);
// const double toChange = diffRad;
// //if (change < toChange) {change += toChange*0.01;}
// if (change > toChange) {change *= 0.93;}
// //if (change < toChange) {change += dChange.draw()/10000;} // does not work for small changes?!
// if (change < toChange) {change += (toChange-change) * 0.07;}
// //if (change > toChange) {change -= dChange.draw();}
curHead += change;
// convert to gyro's radians-per-second
const float radPerSec = change * 1000 / deltaTs.ms();;
const double radPerSec = change * 1000 / deltaTs.ms();;
const float accX = 0.00 + dAccX.draw();
const float accY = 0.00 + dAccY.draw();