a lot!!! of changes
added main menu added debug display many debug widgets for plotting live data worked on android live sensors added offline-data sensor feeding some dummy data sensors worked on the map display added ui debug for grid-points, particles and weights added a cool dude to display the estimation added real filtering based on the Indoor components c++11 fixes for android compilation online and offline filtering support new resampling technique for testing map loading via dialog
This commit is contained in:
kazu
217
ui/debug/SensorDataWidget.cpp
Normal file
217
ui/debug/SensorDataWidget.cpp
Normal file
@@ -0,0 +1,217 @@
|
||||
#include "../misc/fixc11.h"
|
||||
#include "SensorDataWidget.h"
|
||||
|
||||
#include "plot/PlottWidget.h"
|
||||
#include <QGridLayout>
|
||||
#include <QColor>
|
||||
|
||||
#include "../sensors/SensorFactory.h"
|
||||
#include "PlotTurns.h"
|
||||
#include "PlotWiFiScan.h"
|
||||
|
||||
|
||||
template <typename Data> void removeOld(Data& data, const Timestamp limit) {
|
||||
if (data.size() < 2) {return;}
|
||||
while ( (data.back().key - data.front().key) > limit.ms()) {
|
||||
data.remove(0);
|
||||
}
|
||||
}
|
||||
|
||||
template <int num> class PlotXLines : public PlotWidget {
|
||||
|
||||
protected:
|
||||
|
||||
QColor colors[4] = {QColor(255,0,0), QColor(0,192,0), QColor(0,0,255), QColor(0,0,0)};
|
||||
LinePlot line[num];
|
||||
|
||||
public:
|
||||
|
||||
PlotXLines(QWidget* parent) : PlotWidget(parent) {
|
||||
for (int i = 0; i < num; ++i) {
|
||||
pc.addPlot(&line[i]);
|
||||
line[i].setColor(colors[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void addLineNode(const Timestamp ts, const float y, const int idx) {
|
||||
LinePlot& lp = line[idx];
|
||||
lp.getData().add(ts.ms(), y);
|
||||
}
|
||||
|
||||
Timestamp lastRefresh;
|
||||
bool needsRefresh(const Timestamp ts) {
|
||||
const Timestamp diff = ts - lastRefresh;
|
||||
return (diff > Timestamp::fromMS(100));
|
||||
}
|
||||
|
||||
|
||||
void refresh(const Timestamp ts) {
|
||||
|
||||
// ensure event from main-thread using queued-connection
|
||||
QMetaObject::invokeMethod(this, "update", Qt::QueuedConnection);
|
||||
|
||||
lastRefresh = ts;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class PlotAcc : public PlotXLines<3> {
|
||||
|
||||
protected:
|
||||
|
||||
PointPlot steps;
|
||||
|
||||
public:
|
||||
|
||||
PlotAcc(QWidget* parent) : PlotXLines(parent) {
|
||||
steps.setColor(colors[2]);
|
||||
steps.setPointSize(8);
|
||||
pc.addPlot(&steps);
|
||||
const float s = 4.2;
|
||||
const float ref = 9.81;
|
||||
pc.setValRange(Range(ref-s, ref+s));
|
||||
}
|
||||
|
||||
void addStep(const Timestamp ts) {
|
||||
steps.getData().add(ts.ms(), 9.81);
|
||||
}
|
||||
|
||||
void add(const Timestamp ts, const AccelerometerData& data) {
|
||||
addLineNode(ts, data.x, 0);
|
||||
addLineNode(ts, data.y, 1);
|
||||
addLineNode(ts, data.z, 2);
|
||||
if (needsRefresh(ts)) {
|
||||
limit();
|
||||
refresh(ts);
|
||||
}
|
||||
}
|
||||
|
||||
void limit() {
|
||||
const Timestamp limit = Timestamp::fromMS(3000);
|
||||
removeOld(line[0].getData(), limit);
|
||||
removeOld(line[1].getData(), limit);
|
||||
removeOld(line[2].getData(), limit);
|
||||
removeOld(steps.getData(), limit - Timestamp::fromMS(100)); // remove steps a little before. prevents errors
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class PlotGyro : public PlotXLines<3> {
|
||||
|
||||
public:
|
||||
|
||||
PlotGyro(QWidget* parent) : PlotXLines(parent) {
|
||||
const float s = 1;
|
||||
const float ref = 0;
|
||||
pc.setValRange(Range(ref-s, ref+s));
|
||||
}
|
||||
|
||||
void add(const Timestamp ts, const GyroscopeData& data) {
|
||||
addLineNode(ts, data.x, 0);
|
||||
addLineNode(ts, data.y, 1);
|
||||
addLineNode(ts, data.z, 2);
|
||||
if (needsRefresh(ts)) {
|
||||
limit();
|
||||
refresh(ts);
|
||||
}
|
||||
}
|
||||
|
||||
void limit() {
|
||||
const Timestamp limit = Timestamp::fromMS(3000);
|
||||
removeOld(line[0].getData(), limit);
|
||||
removeOld(line[1].getData(), limit);
|
||||
removeOld(line[2].getData(), limit);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class PlotBaro : public PlotXLines<1> {
|
||||
|
||||
public:
|
||||
|
||||
PlotBaro(QWidget* parent) : PlotXLines(parent) {
|
||||
|
||||
}
|
||||
|
||||
void add(const Timestamp ts, const BarometerData& data) {
|
||||
addLineNode(ts, data.hPa, 0);
|
||||
if (needsRefresh(ts)) {
|
||||
limit();
|
||||
refresh(ts);
|
||||
}
|
||||
const float s = 0.5;
|
||||
const float ref = line[0].getData().front().val;
|
||||
pc.setValRange(Range(ref-s, ref+s));
|
||||
}
|
||||
|
||||
void limit() {
|
||||
removeOld(line[0].getData(), Timestamp::fromMS(8000));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class PlotTurn : public QWidget {
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
SensorDataWidget::SensorDataWidget(QWidget* parent) : QWidget(parent) {
|
||||
|
||||
QGridLayout* lay = new QGridLayout(this);
|
||||
|
||||
plotGyro = new PlotGyro(this);
|
||||
plotAcc = new PlotAcc(this);
|
||||
plotBaro = new PlotBaro(this);
|
||||
plotTurn = new PlotTurns(this);
|
||||
plotWiFi = new PlotWiFiScan(this);
|
||||
|
||||
lay->addWidget(plotGyro, 0, 0, 1, 4, Qt::AlignTop);
|
||||
lay->addWidget(plotAcc, 1, 0, 1, 4, Qt::AlignTop);
|
||||
lay->addWidget(plotBaro, 2, 0, 1, 4, Qt::AlignTop);
|
||||
lay->addWidget(plotTurn, 3, 0, 1, 1, Qt::AlignTop);
|
||||
lay->addWidget(plotWiFi, 3, 1, 1, 3, Qt::AlignTop);
|
||||
|
||||
SensorFactory::get().getAccelerometer().addListener(this);
|
||||
SensorFactory::get().getGyroscope().addListener(this);
|
||||
SensorFactory::get().getBarometer().addListener(this);
|
||||
SensorFactory::get().getSteps().addListener(this);
|
||||
SensorFactory::get().getTurns().addListener(this);
|
||||
SensorFactory::get().getWiFi().addListener(this);
|
||||
|
||||
//setAutoFillBackground(false);
|
||||
|
||||
}
|
||||
|
||||
void SensorDataWidget::onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) {
|
||||
(void) sensor;
|
||||
((PlotAcc*)plotAcc)->add(ts, data);
|
||||
}
|
||||
|
||||
void SensorDataWidget::onSensorData(Sensor<StepData>* sensor, const Timestamp ts, const StepData& data) {
|
||||
(void) sensor;
|
||||
(void) data;
|
||||
((PlotAcc*)plotAcc)->addStep(ts);
|
||||
}
|
||||
|
||||
void SensorDataWidget::onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) {
|
||||
(void) sensor;
|
||||
((PlotGyro*)plotGyro)->add(ts, data);
|
||||
}
|
||||
|
||||
void SensorDataWidget::onSensorData(Sensor<BarometerData>* sensor, const Timestamp ts, const BarometerData& data) {
|
||||
(void) sensor;
|
||||
((PlotBaro*)plotBaro)->add(ts, data);
|
||||
}
|
||||
|
||||
void SensorDataWidget::onSensorData(Sensor<TurnData>* sensor, const Timestamp ts, const TurnData& data) {
|
||||
(void) sensor;
|
||||
((PlotTurns*)plotTurn)->add(ts, data);
|
||||
}
|
||||
|
||||
void SensorDataWidget::onSensorData(Sensor<WiFiMeasurements>* sensor, const Timestamp ts, const WiFiMeasurements& data) {
|
||||
(void) sensor;
|
||||
((PlotWiFiScan*)plotWiFi)->add(ts, data);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user