WindRose 1.0
Custom Hardware , Firmware and Software for Robotic Cellular Automata Experiments
 
Loading...
Searching...
No Matches
SSMachine.h
Go to the documentation of this file.
1
8
9#ifndef SSMachine_h
10#define SSMachine_h
11
12#include "Config.h" //Config header
13#include "Sensing.h"
14#include "Being.h"
15#include "WRMux.h"
16
23class SSMachine
24{
25 private:
26 Stream *_stmRef; // Stream reference object
27 WRMux _mux; //Multiplexer controller
28 uint8_t ck_a=0; //Checksum accumulator A
29 uint8_t ck_b=0; //Checksum accumulator B
30 public:
31 boolean trustpack = false; //Flag to indicate if the packet is trusted
32 uint32_t lostpack = 0; //Number of lost packets
33
38 void setSerial(Stream *stmObject)
39 {
40 this->_stmRef=stmObject; //Set the reference to the serial port
41 }
42
46 void printLostPack()
47 {
48 this->_stmRef->print("Lost packets: ");
49 this->_stmRef->println(this->lostpack);
50 }
51
55 void clearBuffer()
56 {
57 while (this->_stmRef->available()) this->_stmRef->read(); //Clearing the Serial buffer
58 }
59
64 void sendData(Being *me)
65 {
66 byte DATA_buffer[MAX_SENSING_BYTES_PER_PACKET + 8]; // All sensors + 4 ASBM + 4 Lifetime
67 uint8_t ck = 0; //Byte checksum accumulator
68 byte CRC_ck_a = 0x06; //Checksum accumulator A (Starting by the seed A)
69 byte CRC_ck_b = 0x02; //Checksum seed B (Starting by the seed B)
70
71 //HERE IS THE PACKET HEADER (6 bytes)
72
73 this->_stmRef->write("WRoS"); //Write the header
74 this->_stmRef->write(CRC_ck_a); //Write the seed A
75 this->_stmRef->write(CRC_ck_b); //Write the seed B
76
77 //HERE STARTS THE PAYLOAD OF THE PACKET
78
79 //Write the Neighborhood Mapping
80 DATA_buffer[ck] = me->NGS; //Write the NGS
81 ck +=1;
82
83 //Write the Being Age (4 bytes)
84 me->age = millis(); //Time since start running
85 DATA_buffer[ck] = byte(me->age >> 24 & 0xFF);
86 DATA_buffer[ck+1] = byte(me->age >> 16 & 0xFF);
87 DATA_buffer[ck+2] = byte(me->age >> 8 & 0xFF);
88 DATA_buffer[ck+3] = byte(me->age & 0xFF);
89 ck += 4;
90
91 //Write the ASBM (4 bytes)
92 DATA_buffer[ck] = byte(me->ASBM >> 24 & 0xFF);
93 DATA_buffer[ck+1] = byte(me->ASBM >> 16 & 0xFF);
94 DATA_buffer[ck+2] = byte(me->ASBM >> 8 & 0xFF);
95 DATA_buffer[ck+3] = byte(me->ASBM & 0xFF);
96 ck += 4;
97
98 //HERE STARTS THE DYNAMIC SENSOR REPORTING
99
100 //Check the ASBM and write the data for Coordinates Sensor
101 if(me->isactive(coord)){ //Coord_t
102 DATA_buffer[ck] = byte(me->sensors.coord.x >> 8 & 0xFF);
103 DATA_buffer[ck+1] = byte(me->sensors.coord.x & 0xFF);
104 DATA_buffer[ck+2] = byte(me->sensors.coord.y >> 8 & 0xFF);
105 DATA_buffer[ck+3] = byte(me->sensors.coord.y & 0xFF);
106 DATA_buffer[ck+4] = byte(me->sensors.coord.z >> 8 & 0xFF);
107 DATA_buffer[ck+5] = byte(me->sensors.coord.z & 0xFF);
108 ck += 6;
109 }
110
111 //Check the ASBM and write the data for Angle Sensor
112 if(me->isactive(angle)){ //Angle_t
113 DATA_buffer[ck] = byte(me->sensors.angle.pitch >> 8 & 0xFF);
114 DATA_buffer[ck+1] = byte(me->sensors.angle.pitch & 0xFF);
115 DATA_buffer[ck+2] = byte(me->sensors.angle.roll >> 8 & 0xFF);
116 DATA_buffer[ck+3] = byte(me->sensors.angle.roll & 0xFF);
117 DATA_buffer[ck+4] = byte(me->sensors.angle.yaw >> 8 & 0xFF);
118 DATA_buffer[ck+5] = byte(me->sensors.angle.yaw & 0xFF);
119 ck += 6;
120 }
121
122 //Check the ASBM and write the data for Accelerometer Sensor
123 if(me->isactive(accel)){ //Accel_t
124 DATA_buffer[ck] = byte(me->sensors.accel.x >> 8 & 0xFF);
125 DATA_buffer[ck+1] = byte(me->sensors.accel.x & 0xFF);
126 DATA_buffer[ck+2] = byte(me->sensors.accel.y >> 8 & 0xFF);
127 DATA_buffer[ck+3] = byte(me->sensors.accel.y & 0xFF);
128 DATA_buffer[ck+4] = byte(me->sensors.accel.z >> 8 & 0xFF);
129 DATA_buffer[ck+5] = byte(me->sensors.accel.z & 0xFF);
130 ck += 6;
131 }
132
133 //Check the ASBM and write the data for Gyroscope Sensor
134 if(me->isactive(ang_rate)){ //Ang_rate_t
135 DATA_buffer[ck] = byte(me->sensors.ang_rate.x >> 8 & 0xFF);
136 DATA_buffer[ck+1] = byte(me->sensors.ang_rate.x & 0xFF);
137 DATA_buffer[ck+2] = byte(me->sensors.ang_rate.y >> 8 & 0xFF);
138 DATA_buffer[ck+3] = byte(me->sensors.ang_rate.y & 0xFF);
139 DATA_buffer[ck+4] = byte(me->sensors.ang_rate.z >> 8 & 0xFF);
140 DATA_buffer[ck+5] = byte(me->sensors.ang_rate.z & 0xFF);
141 ck += 6;
142 }
143
144 //Check the ASBM and write the data for GPS Sensor
145 if(me->isactive(gps_pos)){ //GPS_pos_t
146 DATA_buffer[ck] = byte(me->sensors.gps_pos.lat >> 24 & 0xFF);
147 DATA_buffer[ck+1] = byte(me->sensors.gps_pos.lat >> 16 & 0xFF);
148 DATA_buffer[ck+2] = byte(me->sensors.gps_pos.lat >> 8 & 0xFF);
149 DATA_buffer[ck+3] = byte(me->sensors.gps_pos.lat & 0xFF);
150 DATA_buffer[ck+4] = byte(me->sensors.gps_pos.lon >> 24 & 0xFF);
151 DATA_buffer[ck+5] = byte(me->sensors.gps_pos.lon >> 16 & 0xFF);
152 DATA_buffer[ck+6] = byte(me->sensors.gps_pos.lon >> 8 & 0xFF);
153 DATA_buffer[ck+7] = byte(me->sensors.gps_pos.lon & 0xFF);
154 DATA_buffer[ck+8] = byte(me->sensors.gps_pos.alt >> 24 & 0xFF);
155 DATA_buffer[ck+9] = byte(me->sensors.gps_pos.alt >> 16 & 0xFF);
156 DATA_buffer[ck+10] = byte(me->sensors.gps_pos.alt >> 8 & 0xFF);
157 DATA_buffer[ck+11] = byte(me->sensors.gps_pos.alt & 0xFF);
158 DATA_buffer[ck+12] = byte(me->sensors.gps_pos.gspeed >> 24 & 0xFF);
159 DATA_buffer[ck+13] = byte(me->sensors.gps_pos.gspeed >> 16 & 0xFF);
160 DATA_buffer[ck+14] = byte(me->sensors.gps_pos.gspeed >> 8 & 0xFF);
161 DATA_buffer[ck+15] = byte(me->sensors.gps_pos.gspeed & 0xFF);
162 ck += 16;
163 }
164
165 //Check the ASBM and write the data for Barometer Sensor
166 if(me->isactive(wind)){ //Wind_t
167 DATA_buffer[ck] = byte(me->sensors.wind.speed >> 8 & 0xFF);
168 DATA_buffer[ck+1] = byte(me->sensors.wind.speed & 0xFF);
169 DATA_buffer[ck+2] = byte(me->sensors.wind.dir >> 8 & 0xFF);
170 DATA_buffer[ck+3] = byte(me->sensors.wind.dir & 0xFF);
171 ck += 4;
172 }
173
174 //Check the ASBM and write the data for Lightmeter Sensor
175 if(me->isactive(light)){ //Light_t
176 DATA_buffer[ck] = byte(me->sensors.light.lux >> 24 & 0xFF);
177 DATA_buffer[ck+1] = byte(me->sensors.light.lux >> 16 & 0xFF);
178 DATA_buffer[ck+2] = byte(me->sensors.light.lux >> 8 & 0xFF);
179 DATA_buffer[ck+3] = byte(me->sensors.light.lux & 0xFF);
180 DATA_buffer[ck+4] = byte(me->sensors.light.temp >> 8 & 0xFF);
181 DATA_buffer[ck+5] = byte(me->sensors.light.temp & 0xFF);
182 ck += 6;
183 }
184
185 //HERE ENDS THE DYNAMIC SENSOR REPORTING
186
187 //SENDING THE PAYLOAD OF THE PACKET
188 for (int i=0;i<ck;i++) this->_stmRef->write(DATA_buffer[i]); //Write the data to the output stream
189
190 //FINISHING THE PACKET WITH THE CRC CHECKSUM
191
192 for (int i=0;i<ck;i++) //
193 {
194 CRC_ck_a += DATA_buffer[i]; //Calculate the CRC checksum for the data
195 CRC_ck_b += CRC_ck_a;
196 }
197 this->_stmRef->write(CRC_ck_a); //Write the CRC checksum to the output stream
198 this->_stmRef->write(CRC_ck_b);
199 }
200
205 void checksum(byte dt)
206 {
207 this->ck_a += dt; //Calculate the CRC checksum for the data
208 this->ck_b += this->ck_a;
209 }
210
215 byte readsum(){
216 byte _data = this->_stmRef->read(); //Read the data from the input stream
217 this->checksum(_data); //Calculate the CRC checksum for the data
218 return _data;
219 }
220
225 void getData(Being *nb)
226 {
227 byte data; //Variable to store the read data
228 boolean keepgoing =true; //Variable to keep track of the data read
229 uint8_t chances = 5; //Variable to keep track of the number of chances to read the data
230 SensingData sensdata; //Sensor class object to store the data
231 uint8_t DT_step=0; //Variable to step through the machine state
232 uint8_t DT_ck_a, DT_ck_b = 0; //Variables to store the external CRC checksum
233 this->ck_a = 0; //Reset the internal CRC checksum accumulator A
234 this->ck_b = 0; //Reset the internal CRC checksum accumulator B
235#ifdef WRDEBUG
236 _stmRef->println("Entered getData");
237#endif
238 nb->alive = false; //Reset the neighbour being alive flag
239 this->trustpack = false; //Reset the trust packet flag
240
241 if (this->_stmRef->available() > 0){ //If there is some data
242#ifdef WRDEBUG
243 _stmRef->println("Ther is something to read");
244#endif
245 //for(int i=0;i<numc;i++) //Process bytes received
246 while(this->_stmRef->available() && keepgoing)
247 {
248#ifdef WRDEBUG
249 _stmRef->println("Entered the read loop");
250#endif
251 data = this->_stmRef->read();
252 switch(DT_step) //We start from zero. Here starts the State Machine
253 {
254 case 0:
255 if(data==char('W')) // Data sync char 1 (0x57)
256 { // DATA sync char 1
257 DT_step++; //Hmmm... first data packet is correct, lets jump to the next step
258 }
259 break;
260 case 1:
261 if(data==char('R')) // DATA sync char 2 (0x52)
262 {
263 DT_step++; //Oooh... second data packet is correct, lets jump to the step 2
264 }
265 else
266 DT_step=0; //Nop, not this time... so restart to step zero and try again.
267 break;
268 case 2:
269 if(data==char('o')) // DATA sync char 3 (0x6F)
270 DT_step++; //Yayyy! third data packet is correct, lets jump to the step 3
271 else
272 DT_step=0; //Nop, so close, but not this time... so restart from step zero and try again.
273 break;
274 case 3:
275 if(data==char('S')){ // DATA sync char 4 (0x53)
276 DT_step++; //YEESSSS!!! The fourth data packet is correct, jump to the step 4
277#ifdef WRDEBUG
278 _stmRef->println("Header OK");
279#endif
280 }
281 else
282 DT_step=0; //Nop, is not correct, so restart from step zero and try again.
283 break;
284 case 4:
285 this->ck_a = data; //Getting the first chk sum seed
286 DT_step++;
287 break;
288 case 5:
289 this->ck_b = data;//Getting the second chk sum seed
290 DT_step++;
291 break;
292
293 //HERE IS (LIFETIME AND ASBM) GRABBING (MSB -> Most Significant Byte First Order)
294
295 case 6:
296 this->checksum(data); //First checksum (data taken from the while loop)
297 nb->NGS = data; //Getting the NGS (Neighbourhood State Mapping)
298
299 nb->age = 0; //Reset the neighbour lifetime;
300 data = this->readsum(); //Starting to use the function readsum (read+checksum)
301 nb->age |= uint32_t(data) << 24; //Get the MSB of the lifetime (Byte 4)
302 data = this->readsum();
303 nb->age |= uint32_t(data) << 16; //Get the MSB of the lifetime (Byte 3)
304 data = this->readsum();
305 nb->age |= uint32_t(data) << 8; //Get the MSB of the lifetime (Byte 2)
306 data = this->readsum();
307 nb->age |= uint32_t(data); //Get the MSB of the lifetime (Byte 1)
308
309 nb->ASBM = 0x00;
310 data = this->readsum();
311 nb->ASBM |= uint32_t(data)<<24; //Getting the Active Sensor Map (Byte 4)
312 data = this->readsum();
313 nb->ASBM |= uint32_t(data)<<16; //Getting the Active Sensor Map (Byte 3)
314 data = this->readsum();
315 nb->ASBM |= uint32_t(data)<<8; //Getting the Active Sensor Map (Byte 2)
316 data = this->readsum();
317 nb->ASBM |= data; //Getting the Active Sensor Map (Byte 1)
318
319 //HERE STARTS THE DYNAMIC SENSOR GRABBING
320
321 for(int i=0;i<32;i++){ //Looping through the 32 bits of the Active Sensor Map
322 if(nb->ASBM & (1<<i)){ //Checking if the bit is set (active sensor)
323 switch(i){ //Checking the sensor type
324 case 0: //Coord Sensor (Coord_t)
325 //Clearing the Coord_t struct
326 sensdata.coord.x = 0;
327 sensdata.coord.y = 0;
328 sensdata.coord.z = 0;
329 //Getting the data
330 data = this->readsum();
331 sensdata.coord.x |= int16_t(data) << 8;
332 data = this->readsum();
333 sensdata.coord.x |= data;
334 data = this->readsum();
335 sensdata.coord.y |= int16_t(data) << 8;
336 data = this->readsum();
337 sensdata.coord.y |= data;
338 data = this->readsum();
339 sensdata.coord.z |= int16_t(data) << 8;
340 data = this->readsum();
341 sensdata.coord.z |= data;
342 break;
343 case 1: //Angle Sensor (Angle_t)
344 //Clearing the Angle_t struct
345 sensdata.angle.pitch = 0;
346 sensdata.angle.roll = 0;
347 sensdata.angle.yaw = 0;
348 //Getting the data
349 data = this->readsum();
350 sensdata.angle.pitch |= int16_t(data) << 8;
351 data = this->readsum();
352 sensdata.angle.pitch |= data;
353 data = this->readsum();
354 sensdata.angle.roll |= int16_t(data) << 8;
355 data = this->readsum();
356 sensdata.angle.roll |= data;
357 data = this->readsum();
358 sensdata.angle.yaw |= int16_t(data) << 8;
359 data = this->readsum();
360 sensdata.angle.yaw |= data;
361 break;
362 case 2: //Accelerometer Sensor (Accel_t)
363 //Clearing the Accel_t struct
364 sensdata.accel.x = 0;
365 sensdata.accel.y = 0;
366 sensdata.accel.z = 0;
367 //Getting the data
368 data = this->readsum();
369 sensdata.accel.x |= int16_t(data) << 8;
370 data = this->readsum();
371 sensdata.accel.x |= data;
372 data = this->readsum();
373 sensdata.accel.y |= int16_t(data) << 8;
374 data = this->readsum();
375 sensdata.accel.y |= data;
376 data = this->readsum();
377 sensdata.accel.z |= int16_t(data) << 8;
378 data = this->readsum();
379 sensdata.accel.z |= data;
380 break;
381 case 3: //Ang_rate Sensor (Ang_rate_t)
382 //Clearing the Ang_rate_t struct
383 sensdata.ang_rate.x = 0;
384 sensdata.ang_rate.y = 0;
385 sensdata.ang_rate.z = 0;
386 //Getting the data
387 data = this->readsum();
388 sensdata.ang_rate.x |= int16_t(data) << 8;
389 data = this->readsum();
390 sensdata.ang_rate.x |= data;
391 data = this->readsum();
392 sensdata.ang_rate.y |= int16_t(data) << 8;
393 data = this->readsum();
394 sensdata.ang_rate.y |= data;
395 data = this->readsum();
396 sensdata.ang_rate.z |= int16_t(data) << 8;
397 data = this->readsum();
398 sensdata.ang_rate.z |= data;
399 break;
400 case 4: //Gps_pos Sensor (Gps_pos_t)
401 //Clearing the Gps_pos_t struct
402 sensdata.gps_pos.lat = 0;
403 sensdata.gps_pos.lon = 0;
404 sensdata.gps_pos.alt = 0;
405 sensdata.gps_pos.gspeed = 0;
406 //Getting the data
407 data = this->readsum();
408 sensdata.gps_pos.lat |= int32_t(data) << 24;
409 data = this->readsum();
410 sensdata.gps_pos.lat |= int32_t(data) << 16;
411 data = this->readsum();
412 sensdata.gps_pos.lat |= int32_t(data) << 8;
413 data = this->readsum();
414 sensdata.gps_pos.lat |= data;
415 data = this->readsum();
416 sensdata.gps_pos.lon |= int32_t(data) << 24;
417 data = this->readsum();
418 sensdata.gps_pos.lon |= int32_t(data) << 16;
419 data = this->readsum();
420 sensdata.gps_pos.lon |= int32_t(data) << 8;
421 data = this->readsum();
422 sensdata.gps_pos.lon |= data;
423 data = this->readsum();
424 sensdata.gps_pos.alt |= int32_t(data) << 24;
425 data = this->readsum();
426 sensdata.gps_pos.alt |= int32_t(data) << 16;
427 data = this->readsum();
428 sensdata.gps_pos.alt |= int32_t(data) << 8;
429 data = this->readsum();
430 sensdata.gps_pos.alt |= data;
431 data = this->readsum();
432 sensdata.gps_pos.gspeed |= int32_t(data) << 24;
433 data = this->readsum();
434 sensdata.gps_pos.gspeed |= int32_t(data) << 16;
435 data = this->readsum();
436 sensdata.gps_pos.gspeed |= int32_t(data) << 8;
437 data = this->readsum();
438 sensdata.gps_pos.gspeed |= data;
439 break;
440 case 5: //Wind Sensor (Wind_t)
441 //Clearing the Wind_t struct
442 sensdata.wind.speed = 0;
443 sensdata.wind.dir = 0;
444 //Getting the data
445 data = this->readsum();
446 sensdata.wind.speed |= int16_t(data) << 8;
447 data = this->readsum();
448 sensdata.wind.speed |= data;
449 data = this->readsum();
450 sensdata.wind.dir |= int16_t(data) << 8;
451 data = this->readsum();
452 sensdata.wind.dir |= data;
453 break;
454 case 6: //Light Sensor (Light_t)
455 //Clearing the Light_t struct
456 sensdata.light.lux = 0;
457 sensdata.light.temp = 0;
458 //Getting the data
459 data = this->readsum();
460 sensdata.light.lux |= int32_t(data) << 24;
461 data = this->readsum();
462 sensdata.light.lux |= int32_t(data) << 16;
463 data = this->readsum();
464 sensdata.light.lux |= int32_t(data) << 8;
465 data = this->readsum();
466 sensdata.light.lux |= data;
467 data = this->readsum();
468 sensdata.light.temp |= int16_t(data) << 8;
469 data = this->readsum();
470 sensdata.light.temp |= data;
471 break;
472 default:
473 break;
474 }
475 }
476 }
477 //HERE ENDS THE DYNAMIC SENSOR GRABBING
478
479 DT_step++;
480 break;
481
482 //Checksum Block
483 case 7:
484 DT_ck_a = data; //Getting byte checksum A
485 DT_step++;
486 break;
487 case 8:
488 DT_ck_b = data; //Getting byte checksum B
489 if((this->ck_a==DT_ck_a) && (this->ck_b==DT_ck_b)){ //Checksum is correct
490 nb->sensors = sensdata; //Sending the data to the neigbour repository
491 nb->alive = true; //Sets the neighbour as alive
492 this->trustpack = true; //Trust the package
493 keepgoing = false; //Stops the loop
494 }
495 else{ //Checksum is not correct
496 this->lostpack++; //Increment the lost package counter
497 DT_step=0; //Reset the step counter to start over
498 this->ck_a = 0; //Reset the checksum A
499 this->ck_b = 0; //Reset the checksum B
500 if(chances-- == 0) keepgoing = false; //Stops the loop if the chances are over
501 }
502 break; //End of the checksum block
503 }
504 }
505 this->_mux.disable(); //Inhibits the multiplexing
506 this->clearBuffer(); //Clears the serial buffer
507 }
508 }
509};
510
511#endif
Being.h
This file defines the WindRose's Being class.
Config.h
This file is for keeping any general configuration parameters.
Sensing.h
This file contains the Sensing types, the sensors enumeration, and defines the sensing holder Sensing...
MAX_SENSING_BYTES_PER_PACKET
#define MAX_SENSING_BYTES_PER_PACKET
Definition of all sensor bytes summed up in a single packet.
Definition Sensing.h:15
WRMux.h
This file defines the WindRose's Multiplexer Control class WRMux.
Being
The base class (the soul) for all the beings in the simulation.
Definition Being.h:23
Being::age
uint32_t age
The age of the being, since the board is up.
Definition Being.h:30
Being::isactive
boolean isactive(uint8_t bit_pos)
Checks if a sensor is active in the 32bit Active Sensor Bit Map (ASBM)
Definition Being.h:70
Being::alive
boolean alive
The state of the being (dead = 0, alive = 1)
Definition Being.h:28
Being::sensors
SensingData sensors
Sensor dada holder It has all the sensor types defined in the Sensing.h file. Think of it as a holde...
Definition Being.h:35
Being::NGS
byte NGS
The neighborhood state mapping (NGS)
Definition Being.h:37
Being::ASBM
long ASBM
The active sensor mapping (ASBM)
Definition Being.h:39
SSMachine
The class to control the Serial State Machine.
Definition SSMachine.h:24
SSMachine::readsum
byte readsum()
This function reads the data from the input stream, calculates the CRC checksum and returns the data.
Definition SSMachine.h:215
SSMachine::getData
void getData(Being *nb)
This function grabs the data from the input stream and feeds the local neigbour sensing data holder.
Definition SSMachine.h:225
SSMachine::clearBuffer
void clearBuffer()
Clears the Serial Buffer.
Definition SSMachine.h:55
SSMachine::setSerial
void setSerial(Stream *stmObject)
Initializes the Serial State Machine by setting its internal Stream object to a pointer to the Serial...
Definition SSMachine.h:38
SSMachine::printLostPack
void printLostPack()
Prints the Lost Packets counter to the Serial port.
Definition SSMachine.h:46
SSMachine::checksum
void checksum(byte dt)
This function is used to calculate the CRC checksum for the data.
Definition SSMachine.h:205
SSMachine::sendData
void sendData(Being *me)
Sends the data of the itself Sensing data to the Serial port.
Definition SSMachine.h:64
WRMux
The class for the WindRose Multiplexer Control.
Definition WRMux.h:21
WRMux::disable
void disable()
Sets the inhibit pin and disable the mux.
Definition WRMux.h:52
SensingData
Sensing holder structure with all the sensor types, ordered by the ASBM bits from its least significa...
Definition Sensing.h:130