EngineControlSTM/bus_connector.cpp at master · Hrombel/EngineControlSTM · 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
#include "settings.h"
#include "bus_connector.h"

typedef struct {
	HBusCmd id;
	BusCommand cmd;
	BusEventHandler callback;

} Subscriber;

static Subscriber subs[MAX_CONNECTIONS] = { 0 };
static unsigned int freeSubIndex = 0;
static HBusCmd subId = 1;

static BusMessage engineRes;
static BusError engineErr;

typedef struct {
	HBusSub id;
	BusSensor sensor;
	BusValueHandler callback;

} ValueSubscriber;
static ValueSubscriber valueSubs[MAX_CONNECTIONS] = { 0 };
static unsigned int freeValueSubIndex = 0;
HBusSub valueSubId = 1;

HBusSub BusConnectorSubscribe(BusSensor sensor, const BusValueHandler callback, HBusSub id) {
	static HBusSub currentId;

	if(!BusSubscribe(sensor))
		return 0;

	if(id) {
		currentId = 0;
		for(int i = 0; i < freeValueSubIndex; i++) {
			if(valueSubs[i].id == id) {
				currentId = id;
				valueSubs[i] = { id, sensor, callback };
				break;
			}
		}
		if(!currentId)
			return 0;
	}
	else {
		if (freeValueSubIndex == MAX_CONNECTIONS)
			return 0;

		currentId = valueSubId++;
		valueSubs[freeValueSubIndex++] = { currentId, sensor, callback };
	}

	return currentId;
}

bool BusSensorValue(BusSensor sensor, int value) {
	for (int i = 0; i < freeValueSubIndex;) {
		if (valueSubs[i].callback(valueSubs[i].id, valueSubs[i].sensor, value)) {
			for (int j = i + 1; j < freeValueSubIndex; j++)
				valueSubs[j - 1] = valueSubs[j];
			freeValueSubIndex--;
		}
		else i++;
	}

	for(int i = 0; i < freeValueSubIndex; i++) {
		if(sensor == valueSubs[i].sensor) return false;
	}
	return true;
}

HBusCmd BusCmd(BusCommand cmd, const BusEventHandler callback, HBusCmd id) {
	static BusEvent e;
	static HBusCmd currentId;

	if (cmd >= BUS_CMD_MAX_ITEM) {
		callback(0, cmd, BUS_INVALID_CMD, e);
		return 0;
	}

	if(id) {
		currentId = 0;
		for(int i = 0; i < freeSubIndex; i++) {
			if(subs[i].id == id) {
				currentId = id;
				subs[i] = { id, cmd, callback };
				break;
			}
		}
		if(!currentId) {
			callback(0, cmd, BUS_INCORRECT_ID, e);
			return 0;
		}
	}
	else {
		if (freeSubIndex == MAX_CONNECTIONS) {
			callback(0, cmd, BUS_MAX_SUBS_REACHED, e);
			return 0;
		}

		currentId = subId++;
		subs[freeSubIndex++] = { currentId, cmd, callback };
	}

	switch (cmd) {
	case BUS_CMD_INIT:
		bus_tick(true, false);
		break;
	case BUS_CMD_STOP:
		bus_tick(false, true);
		break;
	}

	return currentId;
}

void BusControlEvent(bool isError, BusEvent event) {
	BusConnectorResult res = isError ? BUS_ERROR : BUS_MESSAGE;

	for (int i = 0; i < freeSubIndex;) {
		if (subs[i].callback(subs[i].id, subs[i].cmd, res, event)) {
			for (int j = i + 1; j < freeSubIndex; j++)
				subs[j - 1] = subs[j];
			freeSubIndex--;
		}
		else i++;
	}
}