Megahub/lib/lpfuart/src/mode.cpp at main · mirkosertic/Megahub · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
#include "mode.h"

#include "logging.h"

Mode::Mode() : pctMin_(0.0f), pctMax_(100.0f), siMin_(0.0f), siMax_(1023.0f) {
	name_ = "";
	units_ = "";
	for (int i = 0; i < 5; i++) {
		inputTypes_[i] = false;
		outputTypes_[i] = false;
	}
}

Mode::~Mode() {}

void Mode::reset() {
	format_.reset();
	datasets_.clear();

	pctMin_ = 0.0f;
	pctMax_ = 100.0;
	siMin_ = 0.0f;
	siMax_ = 1023.0f;
	name_ = "";
	units_ = "";
	for (int i = 0; i < 5; i++) {
		inputTypes_[i] = false;
		outputTypes_[i] = false;
	}
}

void Mode::registerInputType(InputOutputType inputType) {
	inputTypes_[static_cast<int>(inputType)] = true;
}

void Mode::registerOutputType(InputOutputType outputType) {
	outputTypes_[static_cast<int>(outputType)] = true;
}

void Mode::setName(const std::string& name) {
	this->name_ = name;
}

void Mode::setUnits(const std::string& units) {
	this->units_ = units;
}

void Mode::setPctMinMax(float min, float max) {
	pctMin_ = min;
	pctMax_ = max;
}

void Mode::setSiMinMax(float min, float max) {
	siMin_ = min;
	siMax_ = max;
}

void Mode::setFormat(std::unique_ptr<Format> format) {
	format_ = std::move(format);
	datasets_.assign(format_->getDatasets(), Dataset{});
}

std::string Mode::getName() {
	return name_;
}

std::string Mode::getUnits() {
	return units_;
}

float Mode::getPctMin() {
	return pctMin_;
}

float Mode::getPctMax() {
	return pctMax_;
}

float Mode::getSiMin() {
	return siMin_;
}

float Mode::getSiMax() {
	return siMax_;
}

void Mode::processDataPacket(const uint8_t* payload, int payloadSize) {

	if (format_ == nullptr) {
		WARN("Not fully initialized yet!");
		return;
	}

	const char hexChars[] = "0123456789ABCDEF";
	std::string payloadHex;
	payloadHex.reserve(payloadSize * 3); // 2 chars + space per byte

	for (int i = 0; i < payloadSize; ++i) {
		int v = payload[i] & 0xFF;
		payloadHex.push_back(hexChars[(v >> 4) & 0xF]);
		payloadHex.push_back(hexChars[v & 0xF]);
		if (i + 1 < payloadSize) {
			payloadHex.push_back(' ');
		}
	}

	int datasetSize = 0;
	switch (format_->getFormatType()) {
		case Format::FormatType::DATA8: {
			datasetSize = 1;
			break;
		}
		case Format::FormatType::DATA16: {
			datasetSize = 2;
			break;
		}
		case Format::FormatType::DATA32: {
			datasetSize = 4;
			break;
		}
		case Format::FormatType::DATAFLOAT: {
			datasetSize = 4;
			break;
		}
		default:
			WARN("Unknown format type in mode : %d, ignoring data message", format_->getFormatType());
			return;
	}

	DEBUG("Got data %s, expecting %d datasets of type %d", payloadHex.c_str(), format_->getDatasets(),
	      format_->getFormatType());

	int expectedSize = format_->getDatasets() * datasetSize;
	if (expectedSize != payloadSize) {
		WARN("Got data %s, expecting %d datasets of type %d, but wrong size. Expected %d, got %d", payloadHex.c_str(),
		     format_->getDatasets(), format_->getFormatType(), expectedSize, payloadSize);
		return;
	}

	const uint8_t* startPtr = payload;
	for (int i = 0; i < format_->getDatasets(); i++) {
		DEBUG("Got data %s, expecting %d datasets of type %d, parsing dataset %d", payloadHex.c_str(),
		      format_->getDatasets(), format_->getFormatType(), i);
		datasets_[i].readData(format_->getFormatType(), startPtr);
		startPtr += datasetSize;
	}
}

Dataset* Mode::getDataset(int index) {
	return &datasets_[index];
}

Format* Mode::getFormat() {
	return format_.get();
}