DataListener.h 1.23 KB
Newer Older
1
2
#pragma once

3
#include "SerialUnix.h"
4
5
6
7
8
9
10
#include "SerialCommon.h"

#include <list>
#include <vector>																																				
#include <string>
#include <stdio.h>
#include <cmath>
11
#include <chrono>
12

13
struct anchor_raw_data
14
15
{
	unsigned int from, to, distance;
16
	anchor_raw_data(unsigned int f, unsigned int t, unsigned int d) : from(f), to(t), distance(d) {}
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
};

struct anchor_position_data
{
	unsigned int id;
	double x, y, z;
	anchor_position_data(){}
	anchor_position_data(unsigned int _id, double _x, double _y, double _z) : id(_id), x(_x), y(_y), z(_z) {}
};

class DataListener
{
public:
	DataListener();
	~DataListener();

	void setOffset(double x, double y, double z);
34
	bool startAnchorLocalistaion();
35
	void invertAnchor(unsigned int id);
36

37
private:
38
39
	Serial serial;
	std::list<anchor_raw_data> anchors_raw;
40
41
42
	std::vector<anchor_position_data> anchor_position;
	anchor_position_data offset;

43
44
	std::chrono::high_resolution_clock::time_point locTimer;
	bool finishedLoc;
45

46
47
48
	bool _check_data_validity();
	double _get_distance_average(unsigned int anchor_from, unsigned int anchor_to);
	void _resetLocalisation();
49
	void _LOC_Debugger(std::string value);
50
51
52

	bool _startSerialCommunication();
	bool _calculateAnchorPosition();
53
54

	void _writeToYAML();
55
};