FHWS/YASMIN
Archived
3
0
Fork 0
Code Issues 12 Pull Requests Releases Wiki Activity

started to add ble functions

Browse Source 
added ble as sensor to java and c++
added sensorlistener for ble
added ble to observation and onDataSensor in filter
started to work on ble fingerprints for optimization
This commit is contained in:
mail@toni-fetzer.de
2019-06-05 18:04:31 +02:00
parent b9b9d8f9ac
commit 20ae2f5c2a
23 changed files with 1341 additions and 1152 deletions
Download Patch File Download Diff File Expand all files Collapse all files

207
nav/mesh/NavControllerMesh.cpp
View File

@@ -23,7 +23,7 @@
#include <Indoor/floorplan/v2/Floorplan.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingPercent.h>
#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
//#include <Indoor/smc/filtering/resampling/ParticleFilterResamplingKDE.h>
//#ifndef ANDROID
//#include <valgrind/callgrind.h>
@@ -37,7 +37,7 @@ Q_DECLARE_METATYPE(const void*)
MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floorplan::IndoorMap* im, NM::NavMesh<NM::NavMeshTriangle>* navMesh, WiFiModel* wifiModel) :
NavController(mainController, im), navMesh(navMesh), wifiModel(wifiModel) {
// filter init
// filter init
std::unique_ptr<SMC::ParticleFilterInitializer<MeshBased::MyState>> init(new MeshBased::PFInit(navMesh));
// estimation
@@ -47,11 +47,11 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
// resampling
std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingSimple<MyState>> resample(new SMC::ParticleFilterResamplingSimple<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingKDE<MyState, NM::NavMeshTriangle>(navMesh, 0.2, Point2(1,1)));
//std::unique_ptr<SMC::ParticleFilterResamplingKLD<MyState>> resample(new SMC::ParticleFilterResamplingKLD<MyState>());
//std::unique_ptr<SMC::ParticleFilterResamplingPercent<MyState>> resample(new SMC::ParticleFilterResamplingPercent<MyState>(0.95));
//std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
std::unique_ptr<SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>> resample(new SMC::ParticleFilterResamplingSimpleImpoverishment<MeshBased::MyState, NM::NavMeshTriangle>());
// eval and transition
std::unique_ptr<SMC::ParticleFilterEvaluation<MyState, MyObservation>> eval(new MeshBased::PFEval(wifiModel));
@@ -68,35 +68,37 @@ MeshBased::NavControllerMesh::NavControllerMesh(Controller* mainController, Floo
//pf->setNEffThreshold(0.65); // still too low?
//pf->setNEffThreshold(0.25); // too low
// attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this);
SensorFactory::get().getGyroscope().addListener(this);
SensorFactory::get().getBarometer().addListener(this);
SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this);
// attach as listener to all sensors
SensorFactory::get().getAccelerometer().addListener(this);
SensorFactory::get().getGyroscope().addListener(this);
SensorFactory::get().getBarometer().addListener(this);
SensorFactory::get().getWiFi().addListener(this);
SensorFactory::get().getSteps().addListener(this);
SensorFactory::get().getTurns().addListener(this);
SensorFactory::get().getActivity().addListener(this);
SensorFactory::get().getBLE().addListener(this);
// hacky.. but we need to call this one from the main thread!
//mainController->getMapView()->showParticles(pf->getParticles());
qRegisterMetaType<const void*>();
// hacky.. but we need to call this one from the main thread!
//mainController->getMapView()->showParticles(pf->getParticles());
qRegisterMetaType<const void*>();
}
void MeshBased::NavControllerMesh::start() {
Assert::isFalse(running, "already started!");
running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this);
tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this);
Assert::isFalse(running, "already started!");
running = true;
curCtrl.resetAfterTransition(); // ensure we start empty ;)
tFilter = std::thread(&NavControllerMesh::filterUpdateLoop, this);
tDisplay = std::thread(&NavControllerMesh::updateMapViewLoop, this);
// start all sensors
SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start();
// start all sensors
SensorFactory::get().getAccelerometer().start();
SensorFactory::get().getGyroscope().start();
SensorFactory::get().getBarometer().start();
SensorFactory::get().getWiFi().start();
SensorFactory::get().getBLE().start();
//#ifndef ANDROID
// // #include <valgrind/callgrind.h>
@@ -110,60 +112,67 @@ void MeshBased::NavControllerMesh::start() {
}
void MeshBased::NavControllerMesh::stop() {
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
Assert::isTrue(running, "not started!");
running = false;
tFilter.join();
tDisplay.join();
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
(void) sensor;
(void) data;
(void) ts;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
(void) sensor;
(void) ts;
(void) data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
(void) sensor;
(void) ts;
curObs.wifi = data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<BeaconMeasurement>* sensor, const Timestamp ts, const BeaconMeasurement& data) {
(void) sensor;
(void) ts;
curObs.ble.add(data);
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<GPSData>* sensor, const Timestamp ts, const GPSData& data) {
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
(void) sensor;
(void) ts;
curObs.gps = data;
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
(void) sensor;
(void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
(void) sensor;
(void) ts;
curCtrl.numStepsSinceLastTransition += data.stepsSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
(void) sensor;
(void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
(void) sensor;
(void) ts;
curCtrl.turnSinceLastTransition_rad += data.radSinceLastEvent; // set to zero after each transition
gotSensorData(ts);
}
void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, const Timestamp ts, const ActivityData& data) {
@@ -177,8 +186,8 @@ void MeshBased::NavControllerMesh::onSensorData(Sensor<ActivityData>* sensor, co
/** called when any sensor has received new data */
void MeshBased::NavControllerMesh::gotSensorData(const Timestamp ts) {
curObs.currentTime = ts;
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
curObs.currentTime = ts;
if (Settings::Filter::useMainThread) {filterUpdateIfNeeded();}
}
void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity) {
@@ -193,11 +202,11 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
Assert::isTrue(QMetaObject::invokeMethod(mainController->getInfoWidget(), "showActivity", Qt::QueuedConnection, Q_ARG(const QString&, act)), "call failed");
}
/** particle-filter update loop */
void MeshBased::NavControllerMesh::filterUpdateLoop() {
/** particle-filter update loop */
void MeshBased::NavControllerMesh::filterUpdateLoop() {
while(running && !Settings::Filter::useMainThread) {
while(running && !Settings::Filter::useMainThread) {
// // fixed update rate based on the systems time -> LIVE! even for offline data
// const Timestamp ts1 = Timestamp::fromUnixTime();
@@ -207,37 +216,37 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
// const Timestamp sleep = Timestamp::fromMS(500) - needed;
// std::this_thread::sleep_for(std::chrono::milliseconds(sleep.ms()));
const bool wasUpdated = filterUpdateIfNeeded();
if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }
const bool wasUpdated = filterUpdateIfNeeded();
if (!wasUpdated) { std::this_thread::sleep_for(std::chrono::milliseconds(2)); }
}
}
}
}
Timestamp lastTransition;
Timestamp lastTransition;
/** check whether its time for a filter update, and if so, execute the update and return true */
bool MeshBased::NavControllerMesh::filterUpdateIfNeeded() {
/** check whether its time for a filter update, and if so, execute the update and return true */
bool MeshBased::NavControllerMesh::filterUpdateIfNeeded() {
static float avgSum = 0;
static int avgCount = 0;
static float avgSum = 0;
static int avgCount = 0;
const Timestamp diff = curObs.currentTime - lastTransition;
if (curCtrl.numStepsSinceLastTransition > 0){
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
const Timestamp err = diff - Settings::Filter::updateEvery;
// as the difference is slightly above the 500ms, calculate the error and incorporate it into the next one
const Timestamp err = diff - Settings::Filter::updateEvery;
lastTransition = curObs.currentTime;
const Timestamp ts1 = Timestamp::fromUnixTime();
const Timestamp ts1 = Timestamp::fromUnixTime();
filterUpdate();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString filterTime = QString::number(diff.ms());
avgSum += tsDiff.ms(); ++avgCount;
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
avgSum += tsDiff.ms(); ++avgCount;
//Log::add("xxx", "ts:" + std::to_string(curObs.currentTime.ms()) + " avg:" + std::to_string(avgSum/avgCount));
QMetaObject::invokeMethod(mainController->getInfoWidget(), "showFilterTime", Qt::QueuedConnection, Q_ARG(const QString&, filterTime));
return true;
return true;
} else if(diff >= Timestamp::fromMS(1000)) {
@@ -251,19 +260,19 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
} else {
return false;
return false;
}
}
}
}
DijkstraPath<MyGridNode> pathToDest;
DijkstraPath<MyGridNode> pathToDest;
void MeshBased::NavControllerMesh::filterUpdateEstimationOnly() {
//lastEst = curEst;
MyState sCurEst = pf->updateEvaluationOnly(curObs);
curEst.pos_m = sCurEst.loc.pos;
curEst.pos_m = sCurEst.pos.pos;
curEst.head = sCurEst.heading;
// inform listeners about the new estimation
@@ -278,12 +287,12 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
Assert::isTrue(QMetaObject::invokeMethod(mainController->getMapView2D(), "setPathWalked", Qt::QueuedConnection, Q_ARG(const void*, &estPath)), "call failed");
}
/** perform a filter-update (called from a background-loop) */
void MeshBased::NavControllerMesh::filterUpdate() {
/** perform a filter-update (called from a background-loop) */
void MeshBased::NavControllerMesh::filterUpdate() {
//lastEst = curEst;
MyState sCurEst = pf->update(&curCtrl, curObs);
curEst.pos_m = sCurEst.loc.pos;
curEst.pos_m = sCurEst.pos.pos;
curEst.head = sCurEst.heading;
// inform listeners about the new estimation
@@ -309,18 +318,18 @@ void MeshBased::NavControllerMesh::debugActivity(const ActivityData& activity)
// }
// mainController->getMapView()->showGridImportance();
}
}
/** UI update loop */
void MeshBased::NavControllerMesh::updateMapViewLoop() {
/** UI update loop */
void MeshBased::NavControllerMesh::updateMapViewLoop() {
while(running) {
const Timestamp ts1 = Timestamp::fromUnixTime();
updateMapView();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString mapViewTime = QString::number(tsDiff.ms());
//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
}
}
while(running) {
const Timestamp ts1 = Timestamp::fromUnixTime();
updateMapView();
const Timestamp ts2 = Timestamp::fromUnixTime();
const Timestamp tsDiff = ts2-ts1;
const QString mapViewTime = QString::number(tsDiff.ms());
//QMetaObject::invokeMethod(mainController->getInfoWidget(), "showMapViewTime", Qt::QueuedConnection, Q_ARG(const QString&, mapViewTime));
std::this_thread::sleep_for(std::chrono::milliseconds(display_ms));
}
}