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
2016-09-16 19:30:04 +02:00
parent d910e88220
commit 075d8bb633
90 changed files with 4735 additions and 624 deletions

View File

@@ -2,18 +2,7 @@
#define ACCELEROMETERSENSOR_H
#include "Sensor.h"
struct AccelerometerData {
float x;
float y;
float z;
AccelerometerData(const float x, const float y, const float z) : x(x), y(y), z(z) {;}
std::string asString() const {
std::stringstream ss;
ss << "(" << x << "," << y << "," << z << ")";
return ss.str();
}
};
#include <Indoor/sensors/imu/AccelerometerData.h>
class AccelerometerSensor : public Sensor<AccelerometerData> {

11
sensors/BarometerSensor.h Normal file
View File

@@ -0,0 +1,11 @@
#ifndef BAROMETERSENSOR_H
#define BAROMETERSENSOR_H
#include "Sensor.h"
#include <Indoor/sensors/pressure/BarometerData.h>
class BarometerSensor : public Sensor<BarometerData> {
};
#endif // BAROMETERSENSOR_H

11
sensors/GyroscopeSensor.h Normal file
View File

@@ -0,0 +1,11 @@
#ifndef GYROSCOPESENSOR_H
#define GYROSCOPESENSOR_H
#include "Sensor.h"
#include <Indoor/sensors/imu/GyroscopeData.h>
class GyroscopeSensor : public Sensor<GyroscopeData> {
};
#endif // GYROSCOPESENSOR_H

View File

@@ -3,6 +3,9 @@
#include <vector>
#include <QObject>
#include <Indoor/data/Timestamp.h>
template <typename T> class Sensor;
/** listen for sensor events */
template <typename T> class SensorListener {
@@ -10,7 +13,7 @@ template <typename T> class SensorListener {
public:
/** incoming sensor data */
virtual void onSensorData(const T& data) = 0;
virtual void onSensorData(Sensor<T>* sensor, const Timestamp ts, const T& data) = 0;
};
@@ -38,9 +41,17 @@ public:
protected:
/** inform all attached listeners */
void informListeners(const T& sensorData) const {
void informListeners(const T& sensorData) {
const Timestamp now = Timestamp::fromRunningTime();
for (SensorListener<T>* l : listeners) {
l->onSensorData(sensorData);
l->onSensorData(this, now, sensorData);
}
}
/** inform all attached listeners. call this if you know the timestamp */
void informListeners(const Timestamp ts, const T& sensorData) {
for (SensorListener<T>* l : listeners) {
l->onSensorData(this, ts, sensorData);
}
}

View File

@@ -12,36 +12,69 @@
#include "dummy/AccelerometerSensorDummy.h"
#include "android/AccelerometerSensorAndroid.h"
#include "GyroscopeSensor.h"
#include "android/GyroscopeSensorAndroid.h"
#include "dummy/GyroscopeSensorDummy.h"
#include "BarometerSensor.h"
#include "android/BarometerSensorAndroid.h"
#include "dummy/BarometerSensorDummy.h"
#include "StepSensor.h"
#include "TurnSensor.h"
class SensorFactory {
private:
/** this one is a dirty hack, as static class member variables do not work header-only */
static SensorFactory** getPtr() {
static SensorFactory* ptr = nullptr;
return &ptr;
}
public:
/** set the to-be-used sensor-fatory */
static void set(SensorFactory* fac) {
Assert::isNull(*getPtr(), "set() was already called. currentely this is not intended");
*getPtr() = fac;
}
/** get the currently configured sensory factory */
static SensorFactory& get() {
Assert::isNotNull(*getPtr(), "call set() first to set an actual factory instance!");
return **getPtr();
}
public:
/** get the WiFi sensor */
static WiFiSensor& getWiFi() {
#ifdef ANDROID
return WiFiSensorAndroid::get();
#else
return WiFiSensorDummy::get();
#endif
}
virtual WiFiSensor& getWiFi() = 0;
/** get the Accelerometer sensor */
static AccelerometerSensor& getAccelerometer() {
#ifdef ANDROID
return AccelerometerSensorAndroid::get();
#else
return AccelerometerSensorDummy::get();
#endif
}
virtual AccelerometerSensor& getAccelerometer() = 0;
/** get the Gyroscope sensor */
virtual GyroscopeSensor& getGyroscope() = 0;
/** get the Barometer sensor */
virtual BarometerSensor& getBarometer() = 0;
/** get the Step sensor */
static StepSensor& getSteps() {
StepSensor& getSteps() {
static StepSensor steps(getAccelerometer());
return steps;
}
/** get the Turn sensor */
TurnSensor& getTurns() {
static TurnSensor turns(getAccelerometer(), getGyroscope());
return turns;
}
};
#endif // SENSORFACTORY_H

View File

@@ -1,66 +1,46 @@
#ifndef STEPSENSOR_H
#define STEPSENSOR_H
#include "../misc/fixc11.h"
#include <Indoor/sensors/imu/StepDetection.h>
#include "AccelerometerSensor.h"
#include "Sensor.h"
struct StepData {
;
const int stepsSinceLastEvent = 0;
StepData(const int stepsSinceLastEvent) : stepsSinceLastEvent(stepsSinceLastEvent) {;}
};
class StepSensor : public Sensor<StepData>, public SensorListener<AccelerometerData> {
/**
* step-sensor detects steps from the accelerometer
*/
class StepSensor : public SensorListener<AccelerometerData>, public Sensor<StepData> {
private:
AccelerometerSensor& acc;
StepDetection sd;
public:
/** hidden ctor. use singleton */
StepSensor(AccelerometerSensor& acc) : acc(acc) {
;
}
void start() override {
StepSensor(AccelerometerSensor& acc) {
acc.addListener(this);
acc.start();
}
void stop() override {
throw "todo";
virtual void start() override {
//
}
virtual void onSensorData(const AccelerometerData& data) override {
parse(data);
virtual void stop() override {
//
}
protected:
const float threshold = 11.0;
const int blockTime = 25;
int block = 0;
void parse(const AccelerometerData& data) {
const float x = data.x;
const float y = data.y;
const float z = data.z;
const float mag = std::sqrt( (x*x) + (y*y) + (z*z) );
if (block > 0) {
--block;
} else if (mag > threshold) {
informListeners(StepData());
block = blockTime;
virtual void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
(void) sensor;
const bool step = sd.add(ts, data);
if (step) {
informListeners(ts, StepData(1));
}
}
};
#endif // STEPSENSOR_H

53
sensors/TurnSensor.h Normal file
View File

@@ -0,0 +1,53 @@
#ifndef TURNSENSOR_H
#define TURNSENSOR_H
#include <Indoor/sensors/imu/TurnDetection.h>
#include "AccelerometerSensor.h"
#include "GyroscopeSensor.h"
struct TurnData {
float radSinceLastEvent;
float radSinceStart;
TurnData() : radSinceLastEvent(0), radSinceStart(0) {;}
};
class TurnSensor : public SensorListener<AccelerometerData>, public SensorListener<GyroscopeData>, public Sensor<TurnData> {
private:
TurnDetection turn;
TurnData data;
public:
/** ctor */
TurnSensor(AccelerometerSensor& acc, GyroscopeSensor& gyro) {
acc.addListener(this);
gyro.addListener(this);
}
void start() override {
//
}
void stop() override {
//
}
virtual void onSensorData(Sensor<AccelerometerData>* sensor, const Timestamp ts, const AccelerometerData& data) override {
(void) sensor;
turn.addAccelerometer(ts, data);
}
virtual void onSensorData(Sensor<GyroscopeData>* sensor, const Timestamp ts, const GyroscopeData& data) override {
(void) sensor;
const float rad = turn.addGyroscope(ts, data);
this->data.radSinceLastEvent = rad;
this->data.radSinceStart += rad;
informListeners(ts, this->data);
}
};
#endif // TURNSENSOR_H

View File

@@ -1,35 +1,36 @@
#ifndef WIFISENSOR_H
#define WIFISENSOR_H
#include "../misc/fixc11.h"
#include <string>
#include <sstream>
#include "Sensor.h"
#include <Indoor/sensors/radio/WiFiMeasurements.h>
//struct WiFiSensorDataEntry {
// std::string bssid;
// float rssi;
// WiFiSensorDataEntry(const std::string& bssid, const float rssi) : bssid(bssid), rssi(rssi) {;}
// std::string asString() const {
// std::stringstream ss;
// ss << bssid << '\t' << (int)rssi;
// return ss.str();
// }
//};
struct WiFiSensorDataEntry {
std::string bssid;
float rssi;
WiFiSensorDataEntry(const std::string& bssid, const float rssi) : bssid(bssid), rssi(rssi) {;}
std::string asString() const {
std::stringstream ss;
ss << bssid << '\t' << (int)rssi;
return ss.str();
}
};
struct WiFiSensorData {
std::vector<WiFiSensorDataEntry> entries;
std::string asString() const {
std::stringstream ss;
for(const WiFiSensorDataEntry& e : entries) {ss << e.asString() << '\n';}
return ss.str();
}
};
//struct WiFiSensorData {
// std::vector<WiFiSensorDataEntry> entries;
// std::string asString() const {
// std::stringstream ss;
// for(const WiFiSensorDataEntry& e : entries) {ss << e.asString() << '\n';}
// return ss.str();
// }
//};
/** interface for all wifi sensors */
class WiFiSensor : public Sensor<WiFiSensorData> {
class WiFiSensor : public Sensor<WiFiMeasurements> {
protected:

View File

@@ -51,6 +51,6 @@ public:
};
#endif ANDROID
#endif // ANDROID
#endif // ACCELEROMETERSENSORANDROID_H

View File

@@ -0,0 +1,55 @@
#ifndef BAROMETERSENSORANDROID_H
#define BAROMETERSENSORANDROID_H
#ifdef ANDROID
#include <sstream>
#include "../BarometerSensor.h"
#include <QtSensors/QPressureSensor>
#include "../AccelerometerSensor.h"
class BarometerSensorAndroid : public BarometerSensor {
private:
QPressureSensor baro;
/** hidden ctor. use singleton */
BarometerSensorAndroid() {
;
}
public:
/** singleton access */
static BarometerSensorAndroid& get() {
static BarometerSensorAndroid baro;
return baro;
}
void start() override {
auto onSensorData = [&] () {
BarometerData data(baro.reading()->pressure());
informListeners(data);
};
baro.connect(&baro, &QPressureSensor::readingChanged, onSensorData);
baro.start();
}
void stop() override {
throw "TODO";
}
};
#endif
#endif // BAROMETERSENSORANDROID_H

View File

@@ -0,0 +1,59 @@
#ifndef GYROSCOPESENSORANDROID_H
#define GYROSCOPESENSORANDROID_H
#ifdef ANDROID
#include <sstream>
#include "../GyroscopeSensor.h"
#include <QtSensors/QGyroscope>
#include "../AccelerometerSensor.h"
class GyroscopeSensorAndroid : public GyroscopeSensor {
private:
QGyroscope gyro;
/** hidden ctor. use singleton */
GyroscopeSensorAndroid() {
;
}
public:
/** singleton access */
static GyroscopeSensorAndroid& get() {
static GyroscopeSensorAndroid gyro;
return gyro;
}
float degToRad(const float deg) {
return deg / 180.0f * M_PI;
}
void start() override {
auto onSensorData = [&] () {
GyroscopeData data(degToRad(gyro.reading()->x()), degToRad(gyro.reading()->y()), degToRad(gyro.reading()->z()));
informListeners(data);
};
gyro.connect(&gyro, &QGyroscope::readingChanged, onSensorData);
gyro.start();
}
void stop() override {
throw "TODO";
}
};
#endif // ANDROID
#endif // GYROSCOPESENSORANDROID_H

View File

@@ -0,0 +1,41 @@
#ifndef SENSORFACTORYANDROID_H
#define SENSORFACTORYANDROID_H
#ifdef ANDROID
#include "../SensorFactory.h"
#include "WiFiSensorAndroid.h"
#include "AccelerometerSensorAndroid.h"
#include "GyroscopeSensorAndroid.h"
#include "BarometerSensorAndroid.h"
/**
* sensor factory that provides real hardware sensors from
* an android smartphone that fire real data values
*/
class SensorFactoryAndroid : public SensorFactory {
public:
WiFiSensor& getWiFi() override {
return WiFiSensorAndroid::get();
}
AccelerometerSensor& getAccelerometer() override {
return AccelerometerSensorAndroid::get();
}
GyroscopeSensor& getGyroscope() override {
return GyroscopeSensorAndroid::get();
}
BarometerSensor& getBarometer() override {
return BarometerSensorAndroid::get();
}
};
#endif
#endif // SENSORFACTORYANDROID_H

View File

@@ -0,0 +1,20 @@
#ifdef ANDROID
#include "WiFiSensorAndroid.h"
extern "C" {
/** called after each successful WiFi scan */
JNIEXPORT void JNICALL Java_indoor_java_WiFi_onScanComplete(JNIEnv* env, jobject jobj, jbyteArray arrayID) {
(void) env; (void) jobj;
jsize length = env->GetArrayLength(arrayID);
jboolean isCopy;
jbyte* data = env->GetByteArrayElements(arrayID, &isCopy);
std::string str((char*)data, length);
env->ReleaseByteArrayElements(arrayID, data, JNI_ABORT);
WiFiSensorAndroid::get().handle(str);
}
}
#endif

View File

@@ -40,7 +40,7 @@ public:
void handle(const std::string& data) {
// to-be-constructed sensor data
WiFiSensorData sensorData;
WiFiMeasurements sensorData;
// parse each mac->rssi entry
for (int i = 0; i < (int)data.length(); i += 17+1+2) {
@@ -49,7 +49,7 @@ public:
const int8_t pad1 = data[i+18];
const int8_t pad2 = data[i+19];
if (pad1 != 0 || pad2 != 0) {Debug::error("padding error within WiFi scan result");}
sensorData.entries.push_back(WiFiSensorDataEntry(bssid, rssi));
sensorData.entries.push_back(WiFiMeasurement(AccessPoint(bssid), rssi));
}
// call listeners
@@ -60,20 +60,7 @@ public:
};
extern "C" {
/** called after each successful WiFi scan */
JNIEXPORT void JNICALL Java_indoor_java_WiFi_onScanComplete(JNIEnv* env, jobject jobj, jbyteArray arrayID) {
(void) env; (void) jobj;
jsize length = env->GetArrayLength(arrayID);
jboolean isCopy;
jbyte* data = env->GetByteArrayElements(arrayID, &isCopy);
std::string str((char*)data, length);
env->ReleaseByteArrayElements(arrayID, data, JNI_ABORT);
WiFiSensorAndroid::get().handle(str);
}
}
#endif

View File

@@ -2,9 +2,17 @@
#define ACCELEROMETERSENSORDUMMY_H
#include "../AccelerometerSensor.h"
#include "RandomSensor.h"
#include <random>
class AccelerometerSensorDummy : public AccelerometerSensor {
class AccelerometerSensorDummy : public RandomSensor<AccelerometerData, AccelerometerSensor> {
private:
/** hidden ctor */
AccelerometerSensorDummy() : RandomSensor(Timestamp::fromMS(10)) {
;
}
public:
@@ -14,13 +22,25 @@ public:
return acc;
}
void start() override {
//throw "todo";
protected:
std::minstd_rand gen;
std::uniform_real_distribution<float> distNoise = std::uniform_real_distribution<float>(-0.5, +0.5);
AccelerometerData getRandomEntry() override {
const Timestamp ts = Timestamp::fromRunningTime();
const float Hz = 1.6;
const float intensity = 2.0;
const float x = distNoise(gen);
const float y = distNoise(gen);
const float z = 9.81 + std::sin(ts.sec()*2*M_PI*Hz) * intensity + distNoise(gen);
return AccelerometerData(x,y,z);
}
void stop() override {
throw "todo";
}
};

View File

@@ -0,0 +1,43 @@
#ifndef BAROMETERSENSORDUMMY_H
#define BAROMETERSENSORDUMMY_H
#include "../BarometerSensor.h"
#include "RandomSensor.h"
#include <random>
class BarometerSensorDummy : public RandomSensor<BarometerData, BarometerSensor> {
private:
std::thread thread;
/** hidden ctor */
BarometerSensorDummy() : RandomSensor(Timestamp::fromMS(100)) {
;
}
public:
/** singleton access */
static BarometerSensorDummy& get() {
static BarometerSensorDummy baro;
return baro;
}
protected:
std::minstd_rand gen;
std::uniform_real_distribution<float> distNoise = std::uniform_real_distribution<float>(-0.09, +0.09);
BarometerData getRandomEntry() override {
const Timestamp ts = Timestamp::fromRunningTime();
const float hPa = 930 + std::sin(ts.sec()) * 0.5 + distNoise(gen);
return BarometerData(hPa);
}
};
#endif // BAROMETERSENSORDUMMY_H

View File

@@ -0,0 +1,48 @@
#ifndef GYROSCOPESENSORDUMMY_H
#define GYROSCOPESENSORDUMMY_H
#include "../GyroscopeSensor.h"
#include "RandomSensor.h"
#include <random>
class GyroscopeSensorDummy : public RandomSensor<GyroscopeData, GyroscopeSensor> {
private:
/** hidden ctor */
GyroscopeSensorDummy() : RandomSensor(Timestamp::fromMS(10)) {
;
}
public:
/** singleton access */
static GyroscopeSensorDummy& get() {
static GyroscopeSensorDummy gyro;
return gyro;
}
protected:
std::minstd_rand gen;
std::uniform_real_distribution<float> distNoise = std::uniform_real_distribution<float>(-0.1, +0.1);
GyroscopeData getRandomEntry() override {
const Timestamp ts = Timestamp::fromRunningTime();
const float Hz = 0.1;
const float intensity = 0.35;
const float x = distNoise(gen);
const float y = distNoise(gen);
const float z = std::sin(ts.sec()*2*M_PI*Hz) * intensity + distNoise(gen);
return GyroscopeData(x,y,z);
}
};
#endif // GYROSCOPESENSORDUMMY_H

View File

@@ -0,0 +1,56 @@
#ifndef RANDOMSENSOR_H
#define RANDOMSENSOR_H
#include <thread>
#include <Indoor/data/Timestamp.h>
#include <Indoor/Assertions.h>
template <typename Element, typename BaseClass> class RandomSensor : public BaseClass {
private:
std::thread thread;
bool running = false;
Timestamp interval;
public:
RandomSensor(const Timestamp interval) : interval(interval) {
;
}
void start() override {
Assert::isFalse(running, "sensor allready running!");
running = true;
thread = std::thread(&RandomSensor::run, this);
}
void stop() override {
Assert::isTrue(running, "sensor not yet running!");
running = false;
thread.join();
}
protected:
/** subclasses must provide a random entry here */
virtual Element getRandomEntry() = 0;
private:
void run() {
while(running) {
const Element rnd = getRandomEntry();
Sensor<Element>::informListeners(rnd);
std::this_thread::sleep_for(std::chrono::milliseconds(interval.ms()));
}
}
};
#endif // RANDOMSENSOR_H

View File

@@ -0,0 +1,36 @@
#ifndef SENSORFACTORYDUMMY_H
#define SENSORFACTORYDUMMY_H
#include "../SensorFactory.h"
#include "WiFiSensorDummy.h"
#include "AccelerometerSensorDummy.h"
#include "GyroscopeSensorDummy.h"
#include "BarometerSensorDummy.h"
/**
* sensor factory that provides sensors that fire dummy data
*/
class SensorFactoryDummy : public SensorFactory {
public:
WiFiSensor& getWiFi() override {
return WiFiSensorDummy::get();
}
AccelerometerSensor& getAccelerometer() override {
return AccelerometerSensorDummy::get();
}
GyroscopeSensor& getGyroscope() override {
return GyroscopeSensorDummy::get();
}
BarometerSensor& getBarometer() override {
return BarometerSensorDummy::get();
}
};
#endif // SENSORFACTORYDUMMY_H

View File

@@ -54,6 +54,12 @@ private:
aps.push_back(DummyAP("00:00:00:00:00:01", Point2(0, 0)));
aps.push_back(DummyAP("00:00:00:00:00:02", Point2(20, 0)));
aps.push_back(DummyAP("00:00:00:00:00:03", Point2(10, 20)));
aps.push_back(DummyAP("00:00:00:00:00:04", Point2(10, 30)));
aps.push_back(DummyAP("00:00:00:00:00:05", Point2(10, 40)));
aps.push_back(DummyAP("00:00:00:00:00:06", Point2(10, 50)));
aps.push_back(DummyAP("00:00:00:00:00:07", Point2(10, 60)));
aps.push_back(DummyAP("00:00:00:00:00:08", Point2(10, 70)));
aps.push_back(DummyAP("00:00:00:00:00:09", Point2(10, 80)));
float deg = 0;
@@ -73,11 +79,11 @@ private:
const float y = cy + std::cos(deg) * rad;
// construct scan data
WiFiSensorData scan;
WiFiMeasurements scan;
for (DummyAP& ap : aps) {
const float dist = ap.pos.getDistance(Point2(x, y));
const float rssi = LogDistanceModel::distanceToRssi(-40, 1.5, dist);
scan.entries.push_back(WiFiSensorDataEntry(ap.mac, rssi));
scan.entries.push_back(WiFiMeasurement(AccessPoint(ap.mac), rssi));
}
// call

View File

@@ -1,15 +1,38 @@
#ifndef WIFISENSORLINUX_H
#define WIFISENSORLINUX_H
#ifdef LINUX_DESKTOP
#include "../WiFiSensor.h"
#include <thread>
extern "C" {
#include "WiFiSensorLinuxC.h"
}
class WiFiSensorLinux : public WiFiSensor {
private:
std::vector<uint32_t> freqs = {2412, 2417, 2422, 2427, 2432, 2437, 2442, 2447, 2452, 2457, 2462, 2467, 2472};
bool running;
std::thread thread;
int ifIdx;
wifiState state;
private:
WiFiSensorLinux() {
ifIdx = wifiGetInterfaceIndex("wlp3s0");
if (ifIdx == 0) {throw Exception("wifi interface not found!");}
int ret = wifiGetDriver(&state);
if (ret != 0) {throw Exception("wifi driver not found!");}
std::cout << "if: " << ifIdx << std::endl;
}
public:
@@ -21,13 +44,59 @@ public:
}
void start() override {
running = true;
thread = std::thread(&WiFiSensorLinux::run, this);
}
void stop() override {
running = false;
thread.join();
}
private:
void run() {
wifiScanResult result;
wifiChannels channels;
channels.frequencies = freqs.data();
channels.numUsed = freqs.size();
while(running) {
int ret;
// trigger a scan
ret = wifiTriggerScan(&state, ifIdx, &channels);
if (ret != 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
continue;
}
// fetch scan result (blocks)
ret = wifiGetScanResult(&state, ifIdx, &result);
// convert to our format
const Timestamp ts = Timestamp::fromUnixTime();
WiFiMeasurements data;
for (int i = 0; i < result.numUsed; ++i) {
const std::string mac = result.entries[i].mac;
const int rssi = result.entries[i].rssi;
data.entries.push_back(WiFiMeasurement(AccessPoint(mac), rssi));
}
// and call the listeners
informListeners(ts, data);
}
}
};
#endif
#endif // WIFISENSORLINUX_H

View File

@@ -0,0 +1,376 @@
#ifdef LINUX_DESKTOP
#include "WiFiSensorLinuxC.h"
struct trigger_results {
int done;
int aborted;
};
struct handler_args { // For family_handler() and nl_get_multicast_id().
const char *group;
int id;
};
static int error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg) {
// Callback for errors.
printf("error_handler() called.\n");
int *ret = (int*)arg;
*ret = err->error;
return NL_STOP;
}
static int finish_handler(struct nl_msg *msg, void *arg) {
// Callback for NL_CB_FINISH.
int *ret = (int*)arg;
*ret = 0;
return NL_SKIP;
}
static int ack_handler(struct nl_msg *msg, void *arg) {
// Callback for NL_CB_ACK.
int *ret = (int*)arg;
*ret = 0;
return NL_STOP;
}
static int no_seq_check(struct nl_msg *msg, void *arg) {
// Callback for NL_CB_SEQ_CHECK.
return NL_OK;
}
static int family_handler(struct nl_msg *msg, void *arg) {
// Callback for NL_CB_VALID within nl_get_multicast_id(). From http://sourcecodebrowser.com/iw/0.9.14/genl_8c.html.
struct handler_args *grp = arg;
struct nlattr *tb[CTRL_ATTR_MAX + 1];
struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
struct nlattr *mcgrp;
int rem_mcgrp;
nla_parse(tb, CTRL_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
if (!tb[CTRL_ATTR_MCAST_GROUPS]) return NL_SKIP;
nla_for_each_nested(mcgrp, tb[CTRL_ATTR_MCAST_GROUPS], rem_mcgrp) { // This is a loop.
struct nlattr *tb_mcgrp[CTRL_ATTR_MCAST_GRP_MAX + 1];
nla_parse(tb_mcgrp, CTRL_ATTR_MCAST_GRP_MAX, nla_data(mcgrp), nla_len(mcgrp), NULL);
if (!tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME] || !tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]) continue;
if (strncmp((const char*)nla_data(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]), grp->group,
nla_len(tb_mcgrp[CTRL_ATTR_MCAST_GRP_NAME]))) {
continue;
}
grp->id = nla_get_u32(tb_mcgrp[CTRL_ATTR_MCAST_GRP_ID]);
break;
}
return NL_SKIP;
}
int nl_get_multicast_id(struct nl_sock *sock, const char *family, const char *group) {
// From http://sourcecodebrowser.com/iw/0.9.14/genl_8c.html.
struct nl_msg *msg;
struct nl_cb *cb;
int ret, ctrlid;
struct handler_args grp = { .group = group, .id = -ENOENT, };
msg = nlmsg_alloc();
if (!msg) return -ENOMEM;
cb = nl_cb_alloc(NL_CB_DEFAULT);
if (!cb) {
ret = -ENOMEM;
goto out_fail_cb;
}
ctrlid = genl_ctrl_resolve(sock, "nlctrl");
genlmsg_put(msg, 0, 0, ctrlid, 0, 0, CTRL_CMD_GETFAMILY, 0);
ret = -ENOBUFS;
NLA_PUT_STRING(msg, CTRL_ATTR_FAMILY_NAME, family);
ret = nl_send_auto_complete(sock, msg);
if (ret < 0) goto out;
ret = 1;
nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &ret);
nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &ret);
nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, family_handler, &grp);
while (ret > 0) nl_recvmsgs(sock, cb);
if (ret == 0) ret = grp.id;
nla_put_failure:
out:
nl_cb_put(cb);
out_fail_cb:
nlmsg_free(msg);
return ret;
}
void mac_addr_n2a(char *mac_addr, unsigned char *arg) {
// From http://git.kernel.org/cgit/linux/kernel/git/jberg/iw.git/tree/util.c.
int i, l;
l = 0;
for (i = 0; i < 6; i++) {
if (i == 0) {
sprintf(mac_addr+l, "%02x", arg[i]);
l += 2;
} else {
sprintf(mac_addr+l, ":%02x", arg[i]);
l += 3;
}
}
}
void print_ssid(unsigned char *ie, int ielen) {
uint8_t len;
uint8_t *data;
int i;
while (ielen >= 2 && ielen >= ie[1]) {
if (ie[0] == 0 && ie[1] >= 0 && ie[1] <= 32) {
len = ie[1];
data = ie + 2;
for (i = 0; i < len; i++) {
if (isprint(data[i]) && data[i] != ' ' && data[i] != '\\') printf("%c", data[i]);
else if (data[i] == ' ' && (i != 0 && i != len -1)) printf(" ");
else printf("\\x%.2x", data[i]);
}
break;
}
ielen -= ie[1] + 2;
ie += ie[1] + 2;
}
}
static int callback_trigger(struct nl_msg *msg, void *arg) {
// Called by the kernel when the scan is done or has been aborted.
struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
struct trigger_results *results = arg;
//printf("Got something.\n");
//printf("%d\n", arg);
//nl_msg_dump(msg, stdout);
if (gnlh->cmd == NL80211_CMD_SCAN_ABORTED) {
printf("Got NL80211_CMD_SCAN_ABORTED.\n");
results->done = 1;
results->aborted = 1;
} else if (gnlh->cmd == NL80211_CMD_NEW_SCAN_RESULTS) {
printf("Got NL80211_CMD_NEW_SCAN_RESULTS.\n");
results->done = 1;
results->aborted = 0;
} // else probably an uninteresting multicast message.
return NL_SKIP;
}
// called by the kernel for each detected AP
static int callback_dump(struct nl_msg *msg, void *arg) {
// user data
struct wifiScanResult* res = arg;
struct wifiScanResultEntry* entry = &(res->entries[res->numUsed]);
++res->numUsed;
struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
char mac_addr[20];
struct nlattr *tb[NL80211_ATTR_MAX + 1];
struct nlattr *bss[NL80211_BSS_MAX + 1];
static struct nla_policy bss_policy[NL80211_BSS_MAX + 1] = {
[NL80211_BSS_TSF] = { .type = NLA_U64 },
[NL80211_BSS_FREQUENCY] = { .type = NLA_U32 },
[NL80211_BSS_BSSID] = { },
[NL80211_BSS_BEACON_INTERVAL] = { .type = NLA_U16 },
[NL80211_BSS_CAPABILITY] = { .type = NLA_U16 },
[NL80211_BSS_INFORMATION_ELEMENTS] = { },
[NL80211_BSS_SIGNAL_MBM] = { .type = NLA_U32 },
[NL80211_BSS_SIGNAL_UNSPEC] = { .type = NLA_U8 },
[NL80211_BSS_STATUS] = { .type = NLA_U32 },
[NL80211_BSS_SEEN_MS_AGO] = { .type = NLA_U32 },
[NL80211_BSS_BEACON_IES] = { },
};
// Parse and error check.
nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), genlmsg_attrlen(gnlh, 0), NULL);
if (!tb[NL80211_ATTR_BSS]) {
printf("bss info missing!\n");
return NL_SKIP;
}
if (nla_parse_nested(bss, NL80211_BSS_MAX, tb[NL80211_ATTR_BSS], bss_policy)) {
printf("failed to parse nested attributes!\n");
return NL_SKIP;
}
if (!bss[NL80211_BSS_BSSID]) return NL_SKIP;
if (!bss[NL80211_BSS_INFORMATION_ELEMENTS]) return NL_SKIP;
// signal-strength
int mBm = nla_get_s32(bss[NL80211_BSS_SIGNAL_MBM]);
entry->rssi = mBm / 100;
// mac-address
mac_addr_n2a(entry->mac, (unsigned char*)nla_data(bss[NL80211_BSS_BSSID]));
// Start printing.
// mac_addr_n2a(mac_addr, (unsigned char*)nla_data(bss[NL80211_BSS_BSSID]));
// printf("%s, ", mac_addr);
// printf("%d MHz, ", nla_get_u32(bss[NL80211_BSS_FREQUENCY]));
// print_ssid((unsigned char*)nla_data(bss[NL80211_BSS_INFORMATION_ELEMENTS]), nla_len(bss[NL80211_BSS_INFORMATION_ELEMENTS]));
// printf("%f dB", mBm/100.0f);
// printf("\n");
return NL_SKIP;
}
int wifiGetDriver(struct wifiState* state) {
// open nl80211 socket to kernel.
state->socket = nl_socket_alloc();
genl_connect(state->socket);
state->mcid = nl_get_multicast_id(state->socket, "nl80211", "scan");
nl_socket_add_membership(state->socket, state->mcid); // Without this, callback_trigger() won't be called.
state->driverID = genl_ctrl_resolve(state->socket, "nl80211");
return 0;
}
int wifiCleanup(struct wifiState* state) {
nl_socket_drop_membership(state->socket, state->mcid); // No longer need this.
nl_socket_free(state->socket);
}
int wifiGetScanResult(struct wifiState* state, int interfaceIndex, struct wifiScanResult* res) {
// reset the result. very important!
res->numUsed = 0;
// new message
struct nl_msg *msg = nlmsg_alloc();
// configure message
genlmsg_put(msg, 0, 0, state->driverID, 0, NLM_F_DUMP, NL80211_CMD_GET_SCAN, 0);
nla_put_u32(msg, NL80211_ATTR_IFINDEX, interfaceIndex);
// add the to-be-called function for each detected AP
nl_socket_modify_cb(state->socket, NL_CB_VALID, NL_CB_CUSTOM, callback_dump, res);
// send
int ret = nl_send_auto(state->socket, msg);
printf("NL80211_CMD_GET_SCAN sent %d bytes to the kernel.\n", ret);
// get answer (potential error-code)
ret = nl_recvmsgs_default(state->socket);
nlmsg_free(msg);
if (ret < 0) { printf("ERROR: nl_recvmsgs_default() returned %d (%s).\n", ret, nl_geterror(-ret)); return ret; }
}
int wifiTriggerScan(struct wifiState* state, int interfaceIndex, struct wifiChannels* channels) {
// Starts the scan and waits for it to finish. Does not return until the scan is done or has been aborted.
struct trigger_results results = { .done = 0, .aborted = 0 };
struct nl_msg *msg;
struct nl_cb *cb;
struct nl_msg *freqs_to_scan;
int err;
int ret;
// Allocate the messages and callback handler.
msg = nlmsg_alloc();
if (!msg) {
printf("ERROR: Failed to allocate netlink message for msg.\n");
return -ENOMEM;
}
freqs_to_scan = nlmsg_alloc();
if (!freqs_to_scan) {
printf("ERROR: Failed to allocate netlink message for ssids_to_scan.\n");
nlmsg_free(msg);
return -ENOMEM;
}
cb = nl_cb_alloc(NL_CB_DEFAULT);
if (!cb) {
printf("ERROR: Failed to allocate netlink callbacks.\n");
nlmsg_free(msg);
nlmsg_free(freqs_to_scan);
return -ENOMEM;
}
// Setup the messages and callback handler.
genlmsg_put(msg, 0, 0, state->driverID, 0, 0, NL80211_CMD_TRIGGER_SCAN, 0); // Setup which command to run.
nla_put_u32(msg, NL80211_ATTR_IFINDEX, interfaceIndex); // Add message attribute, which interface to use.
// limit to-be-scanned channels?
if (channels->numUsed > 0) {
for (int i = 0; i < channels->numUsed; ++i) { nla_put_u32(freqs_to_scan, 0, channels->frequencies[i]); }
nla_put_nested(msg, NL80211_ATTR_SCAN_FREQUENCIES, freqs_to_scan);
}
nlmsg_free(freqs_to_scan);
nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback_trigger, &results); // Add the callback.
nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
nl_cb_set(cb, NL_CB_SEQ_CHECK, NL_CB_CUSTOM, no_seq_check, NULL); // No sequence checking for multicast messages.
// Send NL80211_CMD_TRIGGER_SCAN to start the scan. The kernel may reply with NL80211_CMD_NEW_SCAN_RESULTS on
// success or NL80211_CMD_SCAN_ABORTED if another scan was started by another process.
err = 1;
ret = nl_send_auto(state->socket, msg); // Send the message.
printf("NL80211_CMD_TRIGGER_SCAN sent %d bytes to the kernel.\n", ret);
printf("Waiting for scan to complete...\n");
while (err > 0) ret = nl_recvmsgs(state->socket, cb); // First wait for ack_handler(). This helps with basic errors.
if (err < 0) {
printf("WARNING: err has a value of %d.\n", err);
}
if (ret < 0) {
printf("ERROR: nl_recvmsgs() returned %d (%s).\n", ret, nl_geterror(-ret));
return ret;
}
while (!results.done) nl_recvmsgs(state->socket, cb); // Now wait until the scan is done or aborted.
if (results.aborted) {
printf("ERROR: Kernel aborted scan.\n");
return 1;
}
printf("Scan is done.\n");
// Cleanup.
nlmsg_free(msg);
nl_cb_put(cb);
return 0;
}
int wifiGetInterfaceIndex(const char *name) {
return if_nametoindex(name);
}
#endif

View File

@@ -0,0 +1,49 @@
#ifndef WIFISENSORLINUXC_H
#define WIFISENSORLINUXC_H
#ifdef LINUX_DESKTOP
#include <errno.h>
#include <netlink/errno.h>
#include <netlink/netlink.h>
#include <netlink/genl/genl.h>
#include <netlink/genl/ctrl.h>
#include <linux/nl80211.h>
#include <net/if.h>
struct wifiChannels {
uint32_t* frequencies; // array of frequencies
uint32_t numUsed; // number of array elements
};
struct wifiScanResultEntry {
char mac[17];
int rssi;
};
struct wifiScanResult {
struct wifiScanResultEntry entries[128];
int numUsed;
};
struct wifiState {
struct nl_sock* socket;
int driverID;
int mcid;
};
/** get the driver used for scanning */
int wifiGetDriver(struct wifiState* state);
/** convert interface name to index number. 0 if interface is not present */
int wifiGetInterfaceIndex(const char* name);
/** trigger a scan on the given channels / the provided interface */
int wifiTriggerScan(struct wifiState* state, int interfaceIndex, struct wifiChannels* channels);
/** blocking get the result of a triggered scan */
int wifiGetScanResult(struct wifiState* state, int interfaceIndex, struct wifiScanResult* res);
#ifdef LINUX_DESKTOP
#endif // WIFISENSORLINUXC_H

View File

@@ -0,0 +1,110 @@
#ifndef ALLINONESENSOR_H
#define ALLINONESENSOR_H
#include "Settings.h"
#include <string>
#include "../WiFiSensor.h"
#include "../AccelerometerSensor.h"
#include "../GyroscopeSensor.h"
#include "../BarometerSensor.h"
#include <Indoor/sensors/offline/OfflineAndroid.h>
class AllInOneSensor :
public WiFiSensor, public AccelerometerSensor, public GyroscopeSensor, public BarometerSensor,
public OfflineAndroidListener {
private:
std::string file;
bool running = false;
std::thread thread;
public:
AllInOneSensor(const std::string& file) : file(file) {
;
}
void start() {
if (running) {return;}
running = true;
thread = std::thread(&AllInOneSensor::run, this);
}
void stop() {
if (!running) {return;}
running = false;
thread.join();
}
protected:
virtual void onGyroscope(const Timestamp _ts, const GyroscopeData data) override {
const Timestamp ts = relativeTS(_ts);
handbrake(ts);
GyroscopeSensor::informListeners(ts, data);
}
virtual void onAccelerometer(const Timestamp _ts, const AccelerometerData data) override {
const Timestamp ts = relativeTS(_ts);
handbrake(ts);
AccelerometerSensor::informListeners(ts, data);
}
virtual void onGravity(const Timestamp ts, const AccelerometerData data) override {
(void) ts;
(void) data;
}
virtual void onWiFi(const Timestamp _ts, const WiFiMeasurements data) override {
const Timestamp ts = relativeTS(_ts);
handbrake(ts);
WiFiMeasurements copy = data;
for (WiFiMeasurement& m : copy.entries) {m.ts = ts;} // make each timestmap also relative
WiFiSensor::informListeners(ts, copy);
}
virtual void onBarometer(const Timestamp _ts, const BarometerData data) override {
const Timestamp ts = relativeTS(_ts);
handbrake(ts);
BarometerSensor::informListeners(ts, data);
}
private:
Timestamp baseTS;
Timestamp relativeTS(const Timestamp ts) {
if (baseTS.isZero()) {baseTS = ts;}
return ts - baseTS;
}
/** handbrake for the offline-parser to ensure realtime events */
Timestamp startSensorTS;
Timestamp startSystemTS;
void handbrake(const Timestamp ts) {
if (startSensorTS.isZero()) {startSensorTS = ts;}
if (startSystemTS.isZero()) {startSystemTS = Timestamp::fromUnixTime();}
const Timestamp runtimeOfflineData = ts - startSensorTS;;
const Timestamp runtimeSystemTime = (Timestamp::fromUnixTime() - startSystemTS) * Settings::offlineSensorSpeedup;
const Timestamp diff = (runtimeOfflineData - runtimeSystemTime);
if (diff > Timestamp::fromMS(0)) {
std::this_thread::sleep_for(std::chrono::milliseconds(diff.ms()));
}
}
private:
/**
* file-parsing runs in a background thread
* the parsing is manually slowed down via handbrake()
* which blocks during the event callbacks
*/
void run() {
OfflineAndroid parser;
parser.parse(file, this);
}
};
#endif // ALLINONESENSOR_H

View File

@@ -0,0 +1,43 @@
#ifndef SENSORFACTORYOFFLINE_H
#define SENSORFACTORYOFFLINE_H
#include <thread>
#include "../SensorFactory.h"
#include "AllInOneSensor.h"
#include <Indoor/sensors/offline/OfflineAndroid.h>
/**
* factory class that provides sensors that fire events from offline data
*/
class SensorFactoryOffline : public SensorFactory {
private:
AllInOneSensor allInOne;
public:
SensorFactoryOffline(const std::string& file) : allInOne(file) {
;
}
WiFiSensor& getWiFi() override {
return allInOne;
}
AccelerometerSensor& getAccelerometer() override {
return allInOne;
}
GyroscopeSensor& getGyroscope() override {
return allInOne;
}
BarometerSensor& getBarometer() override {
return allInOne;
}
};
#endif // SENSORFACTORYOFFLINE_H