Formula Student Electronics & Software
The code for the embedded software
Loading...
Searching...
No Matches
can.cpp
Go to the documentation of this file.
1#include "can.h"
2#include "debug.h"
3
4FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can1;
5
6CAN_message_t disable;
7CAN_message_t BTBStatus;
8CAN_message_t BTBCyclic;
9CAN_message_t noDisable;
10CAN_message_t clearErrors;
11CAN_message_t BTBResponse;
12CAN_message_t statusRequest;
13CAN_message_t torqueRequest;
14CAN_message_t enableResponse;
15CAN_message_t DCVoltageRequest;
16CAN_message_t actualSpeedRequest;
18
19CAN_message_t rpmRequest;
20CAN_message_t speedRequest;
21CAN_message_t currentMOTOR;
22CAN_message_t tempMOTOR;
23CAN_message_t tempBAMO;
24CAN_message_t VoltageMotor;
25CAN_message_t torque_motor;
26CAN_message_t battery_voltage;
27
28CAN_message_t ASStatus;
29
30int Ibat;
31int Vbat;
32int Mout;
33int Nact;
34int Vout;
42
43extern volatile bool BTBReady;
44extern volatile bool transmissionEnabled;
45extern volatile bool disabled;
46extern volatile bool TSOn;
47extern volatile bool R2DOverride;
48extern volatile bool ASReady;
49
50
51extern int highTemp;
52extern int soc;
53extern int current;
54extern int speedInt;
55extern int packVoltage;
56extern int lowTemp;
57int speed = 0;
58
59extern int current_BMS;
60
61extern int powerStageTemp;
62extern int motorTemp;
63
64extern int rpm;
65extern int ACCurrent;
66
67extern volatile uint16_t brakeValue;
68
69extern elapsedMillis R2DTimer;
70
71elapsedMillis CANTimer;
72const int CANTimeoutMS = 100;
73
74#define DC_THRESHOLD 4328 // Threshold for DC voltage to be considered present for R2D
75
76elapsedMillis ASEmergencyTimer;
77
78bool entered_emergency = false;
79
80
81// Initialize CAN messages
87 // APPS Message
89 torqueRequest.len = 3;
90 torqueRequest.buf[0] = 0x90;
91
93 enableResponse.len = 4;
94 enableResponse.buf[0] = 0xE8;
95 enableResponse.buf[1] = 0x01;
96 enableResponse.buf[2] = 0x00;
97 enableResponse.buf[3] = 0x00;
98
100 clearErrors.len = 3;
101 clearErrors.buf[0] = 0x8E;
102 clearErrors.buf[1] = 0x44;
103 clearErrors.buf[2] = 0x4D;
104
106 noDisable.len = 3;
107 noDisable.buf[0] = 0x51;
108 noDisable.buf[1] = 0x00;
109 noDisable.buf[2] = 0x00;
110
112 BTBStatus.len = 3;
113 BTBStatus.buf[0] = 0x3D;
114 BTBStatus.buf[1] = 0xE2;
115 BTBStatus.buf[2] = 0x00;
116
118 BTBResponse.len = 4;
119 BTBResponse.buf[0] = 0xE2;
120 BTBResponse.buf[1] = 0x01;
121 BTBResponse.buf[2] = 0x00;
122 BTBResponse.buf[3] = 0x00;
123
126 transmissionRequestEnable.buf[0] = 0x3D;
127 transmissionRequestEnable.buf[1] = 0xE8;
128 transmissionRequestEnable.buf[2] = 0x00;
129
131 statusRequest.len = 3;
132 statusRequest.buf[0] = 0x3D;
133 statusRequest.buf[1] = 0x40;
134 statusRequest.buf[2] = 0x00;
135
137 disable.len = 3;
138 disable.buf[0] = 0x51;
139 disable.buf[1] = 0x04;
140 disable.buf[2] = 0x00;
141
143 DCVoltageRequest.len = 3;
144 DCVoltageRequest.buf[0] = 0x3D;
145 DCVoltageRequest.buf[1] = 0xEB;
146 DCVoltageRequest.buf[2] = 0x64;
147
148 //ASStatus msg
149
150}
151
154 rpmRequest.len = 3;
155 rpmRequest.buf[0] = 0x3D;
156 rpmRequest.buf[1] = 0xCE;
157 rpmRequest.buf[2] = 0x0A;
158 can1.write(rpmRequest);
159
161 speedRequest.len = 3;
162 speedRequest.buf[0] = 0x3D;
163 speedRequest.buf[1] = 0x30;
164 speedRequest.buf[2] = 0x0A;
165 can1.write(speedRequest);
166
168 currentMOTOR.len = 3;
169 currentMOTOR.buf[0] = 0x3D;
170 currentMOTOR.buf[1] = 0x5f;
171 currentMOTOR.buf[2] = 0x0A;
172 can1.write(currentMOTOR);
173
175 tempMOTOR.len = 3;
176 tempMOTOR.buf[0] = 0x3D;
177 tempMOTOR.buf[1] = 0x49;
178 tempMOTOR.buf[2] = 0x0A;
179 can1.write(tempMOTOR);
180
182 tempBAMO.len = 3;
183 tempBAMO.buf[0] = 0x3D;
184 tempBAMO.buf[1] = 0x4A;
185 tempBAMO.buf[2] = 0x0A;
186 can1.write(tempBAMO);
187
189 torque_motor.len = 3;
190 torque_motor.buf[0] = 0x3D;
191 torque_motor.buf[1] = 0xA0;
192 torque_motor.buf[2] = 0x0A;
193 can1.write(torque_motor);
194
196 VoltageMotor.len = 3;
197 VoltageMotor.buf[0] = 0x3D;
198 VoltageMotor.buf[1] = 0x8A;
199 VoltageMotor.buf[2] = 0x0A;
200 can1.write(VoltageMotor);
201
203 battery_voltage.len = 3;
204 battery_voltage.buf[0] = 0x3D;
205 battery_voltage.buf[1] = 0xeb;
206 battery_voltage.buf[2] = 0x0A;
207 can1.write(battery_voltage);
208
209 //This is for the Master Teensy
210 //Period = 0x0A = 10 ms
212 BTBCyclic.len = 3;
213 BTBCyclic.buf[0] = 0x3D;
214 BTBCyclic.buf[1] = 0xE2;
215 BTBCyclic.buf[2] = 0x0A;
216}
217
218void sendTorqueVal(int value_bamo) {
219 uint8_t byte1 = (value_bamo >> 8) & 0xFF; // MSB
220 uint8_t byte2 = value_bamo & 0xFF; // LSB
221
222 torqueRequest.buf[1] = byte2;
223 torqueRequest.buf[2] = byte1;
224
225 can1.write(torqueRequest);
226}
227void sendAPPS(int val1, int val2) {
228 uint8_t byte1 = (val1 >> 8) & 0xFF; // MSB
229 uint8_t byte2 = val1 & 0xFF; // LSB
230 uint8_t byte3 = (val2 >> 8) & 0xFF; // MSB
231 uint8_t byte4 = val2 & 0xFF; // LSB
232
233 CAN_message_t msg;
234 msg.id = 0x111;
235 msg.len = 4;
236 msg.buf[0] = byte2;
237 msg.buf[1] = byte1;
238 msg.buf[2] = byte4;
239 msg.buf[3] = byte3;
240
241 can1.write(msg);
242}
243
245 can1.write(clearErrors);
246
247 while (not transmissionEnabled and CANTimer > CANTimeoutMS) {
249 CANTimer = 0;
250 }
251
252 while (not BTBReady and CANTimer > CANTimeoutMS) {
253 can1.write(BTBStatus);
254 CANTimer = 0;
255 }
256
257 can1.write(noDisable);
258}
259
260void REGIDHandler(const CAN_message_t& msg) {
261 switch (msg.buf[0]) {
262 case 0x30:
263 speed = (msg.buf[2] << 8) | msg.buf[1];
264 break;
265
266 case REGID_DC_VOLTAGE: {
267 long dc_voltage = 0;
268 dc_voltage = (msg.buf[2] << 8) | msg.buf[1];
269 TSOn = (dc_voltage >= DC_THRESHOLD);
270 break;
271 }
272
273 default:
274 break;
275 }
276}
277
278void canSniffer(const CAN_message_t& msg) {
279 switch (msg.id) {
280 case 0x666:
281 current_BMS = ((msg.buf[1] << 8) | msg.buf[0]);
282 break;
283
284 case C3_ID:
285 brakeValue = (msg.buf[2] << 8) | msg.buf[1];
286 if (brakeValue > 165)
287 R2DTimer = 0;
288 break;
289
290 case R2D_ID:
291 R2DOverride = true;
292 break;
293
294 case BAMO_RESPONSE_ID:
295 if (msg.len == 4) {
296 BTBReady = (msg.buf[0] == BTBResponse.buf[0] and msg.buf[1] == BTBResponse.buf[1] and msg.buf[2] == BTBResponse.buf[2] and msg.buf[3] == BTBResponse.buf[3]);
297 if (BTBReady)
298 Serial.println("BTB ready");
299 else
301 break;
302 }
303 if (msg.len == 3) {
304 transmissionEnabled = (msg.buf[0] == enableResponse.buf[0] and msg.buf[1] == enableResponse.buf[1] and msg.buf[2] == enableResponse.buf[2]);
306 Serial.println("Transmission enabled");
307 break;
308 }
309 break;
310 case MASTER_ID:
311 if (msg.buf[0] == 0x31) {
312 if (msg.buf[1] == 2) {
313 // ASState = ASReady
314 ASReady=true;
315 entered_emergency = false;
316 }
317 else if (msg.buf[1] == 5) { // ASState = ASEmergency
318 if (!entered_emergency) {
320 entered_emergency = true;
321 }
322 } else {
323 entered_emergency = false;
324 }
325 }
326
327
328 default:
329 break;
330 }
331}
332
333void canSetup() {
334 can1.begin();
335 can1.setBaudRate(500000);
336 can1.enableFIFO();
337 can1.enableFIFOInterrupt();
338 can1.setFIFOFilter(REJECT_ALL);
339 can1.setFIFOFilter(0, C3_ID, STD);
340 can1.setFIFOFilter(1, R2D_ID, STD);
341 can1.setFIFOFilter(2, BMS_ID, STD);
342 can1.setFIFOFilter(3, BAMO_RESPONSE_ID, STD);
343 can1.setFIFOFilter(4, MASTER_ID, STD);
344 can1.onReceive(canSniffer);
345
347}
#define MASTER_ID
Definition can.h:10
#define BAMO_COMMAND_ID
Definition can.h:12
#define C3_ID
Definition can.h:7
#define REGID_DC_VOLTAGE
Definition can.h:24
#define R2D_ID
Definition can.h:8
int Nact
Definition can.cpp:33
bool entered_emergency
Definition can.cpp:78
int Iq_actual
Definition can.cpp:39
int packVoltage
Definition display.cpp:22
int powerStageTemp
Definition main.cpp:71
CAN_message_t transmissionRequestEnable
Definition can.cpp:17
volatile bool ASReady
Definition main.cpp:22
CAN_message_t battery_voltage
Definition can.cpp:26
int speed
Definition can.cpp:57
CAN_message_t torqueRequest
Definition can.cpp:13
CAN_message_t BTBCyclic
Definition can.cpp:8
const int CANTimeoutMS
Definition can.cpp:72
int Mout
Definition can.cpp:32
volatile bool disabled
Definition main.cpp:15
void sendAPPS(int val1, int val2)
Definition can.cpp:227
elapsedMillis ASEmergencyTimer
Definition can.cpp:76
int motorTemp
Definition main.cpp:72
CAN_message_t enableResponse
Definition can.cpp:14
CAN_message_t DCVoltageRequest
Definition can.cpp:15
volatile bool transmissionEnabled
Definition main.cpp:17
int Iq_cmd
Definition can.cpp:36
volatile bool TSOn
Definition main.cpp:19
int speedInt
Definition display.cpp:14
CAN_message_t ASStatus
Definition can.cpp:28
int Vbat
Definition can.cpp:31
void sendTorqueVal(int value_bamo)
Definition can.cpp:218
int rpm
Definition display.cpp:11
int soc
Definition display.cpp:12
int Ibat
Definition can.cpp:30
void initCanMessages()
Initialize CAN messages.
Definition can.cpp:86
void request_dataLOG_messages()
Definition can.cpp:152
int Imax_peak
Definition can.cpp:37
CAN_message_t rpmRequest
Definition can.cpp:19
int highTemp
Definition display.cpp:20
CAN_message_t VoltageMotor
Definition can.cpp:24
CAN_message_t clearErrors
Definition can.cpp:10
void REGIDHandler(const CAN_message_t &msg)
Definition can.cpp:260
CAN_message_t disable
Definition can.cpp:6
elapsedMillis CANTimer
Definition can.cpp:71
#define DC_THRESHOLD
Definition can.cpp:74
volatile uint16_t brakeValue
Definition apps.cpp:11
int current_BMS
Definition main.cpp:12
CAN_message_t actualSpeedRequest
Definition can.cpp:16
CAN_message_t noDisable
Definition can.cpp:9
CAN_message_t BTBStatus
Definition can.cpp:7
int current
Definition main.cpp:59
int Vout
Definition can.cpp:34
void canSetup()
Definition can.cpp:333
int I_actual_filtered
Definition can.cpp:41
CAN_message_t BTBResponse
Definition can.cpp:11
CAN_message_t torque_motor
Definition can.cpp:25
int ACCurrent
Definition display.cpp:16
CAN_message_t statusRequest
Definition can.cpp:12
int I_lim_inuse
Definition can.cpp:40
CAN_message_t currentMOTOR
Definition can.cpp:21
CAN_message_t tempMOTOR
Definition can.cpp:22
CAN_message_t speedRequest
Definition can.cpp:20
void canSniffer(const CAN_message_t &msg)
Definition can.cpp:278
elapsedMillis R2DTimer
Definition main.cpp:44
int lowTemp
Definition display.cpp:18
volatile bool R2DOverride
Definition main.cpp:20
volatile bool BTBReady
Definition main.cpp:16
CAN_message_t tempBAMO
Definition can.cpp:23
FlexCAN_T4< CAN1, RX_SIZE_256, TX_SIZE_16 > can1
Definition can.cpp:4
int I_con_eff
Definition can.cpp:38
void initBamocarD3()
Definition can.cpp:244
int Tpeak
Definition can.cpp:35
#define BMS_ID
Definition logging.h:21
#define BAMO_RESPONSE_ID
Definition logging.h:22
CAN_message_t msg