James Hilder / Pi_Swarm_Library

V0.5 library for the Pi Swarm robot

communications.cpp@0:9ffe8ebd1c40, 2014-01-31 (annotated)

Committer:
jah128
Date:
Fri Jan 31 22:59:25 2014 +0000
Revision:
0:9ffe8ebd1c40
Child:
1:b067a08ff54e
First commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:9ffe8ebd1c40 1 /* University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
jah128 0:9ffe8ebd1c40 2 *
jah128 0:9ffe8ebd1c40 3 * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
jah128 0:9ffe8ebd1c40 4 *
jah128 0:9ffe8ebd1c40 5 * Version 0.4 January 2014
jah128 0:9ffe8ebd1c40 6 *
jah128 0:9ffe8ebd1c40 7 * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
jah128 0:9ffe8ebd1c40 8 *
jah128 0:9ffe8ebd1c40 9 */
jah128 0:9ffe8ebd1c40 10
jah128 0:9ffe8ebd1c40 11 #include "communications.h"
jah128 0:9ffe8ebd1c40 12 #include "piswarm.h"
jah128 0:9ffe8ebd1c40 13 #include "main.h"
jah128 0:9ffe8ebd1c40 14
jah128 0:9ffe8ebd1c40 15 DigitalOut actioning (LED1);
jah128 0:9ffe8ebd1c40 16 DigitalOut errorled (LED2);
jah128 0:9ffe8ebd1c40 17 DigitalOut tx (LED3);
jah128 0:9ffe8ebd1c40 18 DigitalOut rx (LED4);
jah128 0:9ffe8ebd1c40 19 Timeout tdma_timeout;
jah128 0:9ffe8ebd1c40 20 char tdma_busy = 0;
jah128 0:9ffe8ebd1c40 21 char waiting_message [64];
jah128 0:9ffe8ebd1c40 22 char waiting_length = 0;
jah128 0:9ffe8ebd1c40 23
jah128 0:9ffe8ebd1c40 24 const char * const requests_array[] = { "Req.Ack", "Req.LM", "Req.RM", "Req.But","Req.LEDCol","Req.LED","Req.Lgt","Req.Acc","Req.Gyro","Req.BIR","Req.RIR","Req.DIR","Req.LFS","Req.UpT","Req.???","Req.???"};
jah128 0:9ffe8ebd1c40 25 const char * const commands_array[] = { "Cm.Stop","Cm.Fward","Cm.Bward","Cm.Left","Cm.Right","Cm.LMot","Cm.RMot","Cm.OLEDC","Cm.CLEDC","Cm.OLEDS","Cm.CLEDS","Cm.SetLED","Cm.Play","Cm.Sync","Cm.???","Cm.???"};
jah128 0:9ffe8ebd1c40 26
jah128 0:9ffe8ebd1c40 27 //Decode the received message, action it if it is valid and for me
jah128 0:9ffe8ebd1c40 28 void decodeMessage(char sender, char target, char id, char command, char * data, char length){
jah128 0:9ffe8ebd1c40 29 char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0;
jah128 0:9ffe8ebd1c40 30
jah128 0:9ffe8ebd1c40 31 if(target==0) broadcast_message = 1;
jah128 0:9ffe8ebd1c40 32 is_response = 0 != (command & (1 << 7));
jah128 0:9ffe8ebd1c40 33 request_response = 0 != (command & (1 << 6));
jah128 0:9ffe8ebd1c40 34 is_user = 0 != (command & (1 << 5));
jah128 0:9ffe8ebd1c40 35 is_command = 0 != (command & (1 << 4));
jah128 0:9ffe8ebd1c40 36 function = command % 16;
jah128 0:9ffe8ebd1c40 37
jah128 0:9ffe8ebd1c40 38 if (RF_DEBUG==1) {
jah128 0:9ffe8ebd1c40 39 if(is_command == 1) pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, commands_array[function],length);
jah128 0:9ffe8ebd1c40 40 else pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, requests_array[function],length);
jah128 0:9ffe8ebd1c40 41 }
jah128 0:9ffe8ebd1c40 42
jah128 0:9ffe8ebd1c40 43 //Action the message only if I am a recipient
jah128 0:9ffe8ebd1c40 44 if(target==0 || target==piswarm.get_id()){
jah128 0:9ffe8ebd1c40 45 if(RF_USE_LEDS==1) actioning = 1;
jah128 0:9ffe8ebd1c40 46 if(is_response == 1) {
jah128 0:9ffe8ebd1c40 47 if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 0:9ffe8ebd1c40 48 else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 0:9ffe8ebd1c40 49 }else {
jah128 0:9ffe8ebd1c40 50 if(is_command == 1) {
jah128 0:9ffe8ebd1c40 51 if(RF_ALLOW_COMMANDS == 1){
jah128 0:9ffe8ebd1c40 52 if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 0:9ffe8ebd1c40 53 else handle_command(sender, broadcast_message, request_response, id, function, data, length);
jah128 0:9ffe8ebd1c40 54 } else if (RF_DEBUG==1) pc.printf(" - Blocked\n");
jah128 0:9ffe8ebd1c40 55 }
jah128 0:9ffe8ebd1c40 56 else {
jah128 0:9ffe8ebd1c40 57 //A information request has no extra parameters
jah128 0:9ffe8ebd1c40 58 if(length == 0){
jah128 0:9ffe8ebd1c40 59 if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
jah128 0:9ffe8ebd1c40 60 else handle_request(sender, broadcast_message, request_response, id, function);
jah128 0:9ffe8ebd1c40 61 } else if (RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 62 }
jah128 0:9ffe8ebd1c40 63 }
jah128 0:9ffe8ebd1c40 64 if(RF_USE_LEDS==1) actioning = 0;
jah128 0:9ffe8ebd1c40 65 } else if (RF_DEBUG==1) pc.printf(" - Ignored\n");
jah128 0:9ffe8ebd1c40 66 }
jah128 0:9ffe8ebd1c40 67
jah128 0:9ffe8ebd1c40 68 //Send a response message
jah128 0:9ffe8ebd1c40 69 void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){
jah128 0:9ffe8ebd1c40 70 char message [4+length];
jah128 0:9ffe8ebd1c40 71 message[0]=piswarm.get_id();
jah128 0:9ffe8ebd1c40 72 message[1]=target;
jah128 0:9ffe8ebd1c40 73 message[2]=id;
jah128 0:9ffe8ebd1c40 74 message[3]=128 + (success << 6) + (is_command << 4) + function;
jah128 0:9ffe8ebd1c40 75 for(int i=0;i<length;i++){
jah128 0:9ffe8ebd1c40 76 message[4+i]=data[i];
jah128 0:9ffe8ebd1c40 77 }
jah128 0:9ffe8ebd1c40 78 //Delay the response if it is broadcast and TDMA mode is on
jah128 0:9ffe8ebd1c40 79 if(RF_USE_TDMA == 1 && is_broadcast == 1){
jah128 0:9ffe8ebd1c40 80 if(tdma_busy == 1){
jah128 0:9ffe8ebd1c40 81 if (RF_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n");
jah128 0:9ffe8ebd1c40 82 }else{
jah128 0:9ffe8ebd1c40 83 tdma_busy = 1;
jah128 0:9ffe8ebd1c40 84 strcpy(waiting_message,message);
jah128 0:9ffe8ebd1c40 85 waiting_length=length;
jah128 0:9ffe8ebd1c40 86 tdma_timeout.attach_us(&tdma_response, RF_TDMA_TIME_PERIOD_US * piswarm.get_id());
jah128 0:9ffe8ebd1c40 87 if (RF_DEBUG==1) pc.printf("TDMA Response pending\n");
jah128 0:9ffe8ebd1c40 88 }
jah128 0:9ffe8ebd1c40 89 }else
jah128 0:9ffe8ebd1c40 90 {
jah128 0:9ffe8ebd1c40 91 piswarm.send_rf_message(message,4+length);
jah128 0:9ffe8ebd1c40 92 if(RF_DEBUG==1)pc.printf("Response issued");
jah128 0:9ffe8ebd1c40 93 }
jah128 0:9ffe8ebd1c40 94 }
jah128 0:9ffe8ebd1c40 95
jah128 0:9ffe8ebd1c40 96 // Send a delayed response
jah128 0:9ffe8ebd1c40 97 void tdma_response(){
jah128 0:9ffe8ebd1c40 98 piswarm.send_rf_message(waiting_message,4+waiting_length);
jah128 0:9ffe8ebd1c40 99 tdma_busy = 0;
jah128 0:9ffe8ebd1c40 100 if (RF_DEBUG==1) pc.printf("TDMA Response issued\n");
jah128 0:9ffe8ebd1c40 101 }
jah128 0:9ffe8ebd1c40 102
jah128 0:9ffe8ebd1c40 103 // Handle a message that is a response
jah128 0:9ffe8ebd1c40 104 void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){
jah128 0:9ffe8ebd1c40 105
jah128 0:9ffe8ebd1c40 106 }
jah128 0:9ffe8ebd1c40 107
jah128 0:9ffe8ebd1c40 108 // Handle a message that is a (not-user) command
jah128 0:9ffe8ebd1c40 109 void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length){
jah128 0:9ffe8ebd1c40 110 char success = 0;
jah128 0:9ffe8ebd1c40 111 switch(function){
jah128 0:9ffe8ebd1c40 112 case 0: // Stop [0 data]
jah128 0:9ffe8ebd1c40 113 if(length==0) {
jah128 0:9ffe8ebd1c40 114 piswarm.stop();
jah128 0:9ffe8ebd1c40 115 if(RF_DEBUG==1) pc.printf(" - Stop Command Issued - ");
jah128 0:9ffe8ebd1c40 116 success = 1;
jah128 0:9ffe8ebd1c40 117 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 118 break;
jah128 0:9ffe8ebd1c40 119 case 1: // Forward [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 120 if(length==2) {
jah128 0:9ffe8ebd1c40 121 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 122 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 123 speed--;
jah128 0:9ffe8ebd1c40 124 piswarm.forward(speed);
jah128 0:9ffe8ebd1c40 125 success = 1;
jah128 0:9ffe8ebd1c40 126 if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 127 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 128 break;
jah128 0:9ffe8ebd1c40 129 case 2: // Backward [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 130 if(length==2) {
jah128 0:9ffe8ebd1c40 131 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 132 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 133 speed--;
jah128 0:9ffe8ebd1c40 134 piswarm.backward(speed);
jah128 0:9ffe8ebd1c40 135 success = 1;
jah128 0:9ffe8ebd1c40 136 if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 137 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 138 break;
jah128 0:9ffe8ebd1c40 139 case 3: // Left [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 140 if(length==2) {
jah128 0:9ffe8ebd1c40 141 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 142 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 143 speed--;
jah128 0:9ffe8ebd1c40 144 piswarm.left(speed);
jah128 0:9ffe8ebd1c40 145 success = 1;
jah128 0:9ffe8ebd1c40 146 if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 147 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 148 break;
jah128 0:9ffe8ebd1c40 149 case 4: // Right [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 150 if(length==2) {
jah128 0:9ffe8ebd1c40 151 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 152 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 153 speed--;
jah128 0:9ffe8ebd1c40 154 piswarm.right(speed);
jah128 0:9ffe8ebd1c40 155 success = 1;
jah128 0:9ffe8ebd1c40 156 if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 157 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 158 break;
jah128 0:9ffe8ebd1c40 159 case 5: // Left Motor [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 160 if(length==2) {
jah128 0:9ffe8ebd1c40 161 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 162 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 163 speed--;
jah128 0:9ffe8ebd1c40 164 piswarm.left_motor(speed);
jah128 0:9ffe8ebd1c40 165 success = 1;
jah128 0:9ffe8ebd1c40 166 if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 167 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 168 break;
jah128 0:9ffe8ebd1c40 169 case 6: // Right Motor [2 bytes: 16-bit signed short]
jah128 0:9ffe8ebd1c40 170 if(length==2) {
jah128 0:9ffe8ebd1c40 171 int i_speed = (data[0] << 8) + data[1];
jah128 0:9ffe8ebd1c40 172 float speed = i_speed / 32768.0;
jah128 0:9ffe8ebd1c40 173 speed--;
jah128 0:9ffe8ebd1c40 174 piswarm.right_motor(speed);
jah128 0:9ffe8ebd1c40 175 success = 1;
jah128 0:9ffe8ebd1c40 176 if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed);
jah128 0:9ffe8ebd1c40 177 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 178 break;
jah128 0:9ffe8ebd1c40 179 case 7: // Outer LED Colour [3 bytes: R, G, B]
jah128 0:9ffe8ebd1c40 180 if(length==3) {
jah128 0:9ffe8ebd1c40 181 piswarm.set_oled_colour (data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 182 success = 1;
jah128 0:9ffe8ebd1c40 183 if(RF_DEBUG==1) pc.printf(" - Set Outer R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 184 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 185 break;
jah128 0:9ffe8ebd1c40 186 case 8: // Center LED Colour[3 bytes: R, G, B]
jah128 0:9ffe8ebd1c40 187 if(length==3) {
jah128 0:9ffe8ebd1c40 188 piswarm.set_cled_colour (data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 189 success = 1;
jah128 0:9ffe8ebd1c40 190 if(RF_DEBUG==1) pc.printf(" - Set Center R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
jah128 0:9ffe8ebd1c40 191 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 192 break;
jah128 0:9ffe8ebd1c40 193 case 9: // Outer LED State [2 bytes: [xxxxxx01][23456789] ]
jah128 0:9ffe8ebd1c40 194 if(length==2) {
jah128 0:9ffe8ebd1c40 195 piswarm.set_oleds(0 != (data[0] & (1 << 1)),0 != (data[0] & (1 << 0)),0 != (data[1] & (1 << 7)),0 != (data[1] & (1 << 6)),0 != (data[1] & (1 << 5)),0 != (data[1] & (1 << 4)),0 != (data[1] & (1 << 3)),0 != (data[1] & (1 << 2)),0 != (data[1] & (1 << 1)),0 != (data[1] & (1 << 0)));
jah128 0:9ffe8ebd1c40 196 success = 1;
jah128 0:9ffe8ebd1c40 197 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 198 break;
jah128 0:9ffe8ebd1c40 199 case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ]
jah128 0:9ffe8ebd1c40 200 if(length==1) {
jah128 0:9ffe8ebd1c40 201 piswarm.enable_cled (data[0] % 2);
jah128 0:9ffe8ebd1c40 202 success = 1;
jah128 0:9ffe8ebd1c40 203 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 204 break;
jah128 0:9ffe8ebd1c40 205 case 11: // Set outer LED [1 byte: [xxxEvvvv] E=enabled vvvv=LED]
jah128 0:9ffe8ebd1c40 206 if(length==1) {
jah128 0:9ffe8ebd1c40 207 int led = data[0] % 16;
jah128 0:9ffe8ebd1c40 208 if(led < 10){
jah128 0:9ffe8ebd1c40 209 piswarm.set_oled(led, 0!=(data[0] & (1 << 4)));
jah128 0:9ffe8ebd1c40 210 success = 1;
jah128 0:9ffe8ebd1c40 211 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 212 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 213 break;
jah128 0:9ffe8ebd1c40 214 case 12: // Play sound [Minimum 1 byte]
jah128 0:9ffe8ebd1c40 215 if(length>0) {
jah128 0:9ffe8ebd1c40 216 piswarm.play_tune(data,length);
jah128 0:9ffe8ebd1c40 217 success = 1;
jah128 0:9ffe8ebd1c40 218 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 219 break;
jah128 0:9ffe8ebd1c40 220 case 13: // Sync time
jah128 0:9ffe8ebd1c40 221 if(length==4) {
jah128 0:9ffe8ebd1c40 222 unsigned int new_time = 0;
jah128 0:9ffe8ebd1c40 223 new_time+=((unsigned int)data[0] << 24);
jah128 0:9ffe8ebd1c40 224 new_time+=((unsigned int)data[1] << 16);
jah128 0:9ffe8ebd1c40 225 new_time+=((unsigned int)data[2] << 8);
jah128 0:9ffe8ebd1c40 226 new_time+=(unsigned int)data[3];
jah128 0:9ffe8ebd1c40 227 set_time(new_time);
jah128 0:9ffe8ebd1c40 228 display_system_time();
jah128 0:9ffe8ebd1c40 229 } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
jah128 0:9ffe8ebd1c40 230 break;
jah128 0:9ffe8ebd1c40 231 case 14: //
jah128 0:9ffe8ebd1c40 232 break;
jah128 0:9ffe8ebd1c40 233 case 15: //
jah128 0:9ffe8ebd1c40 234 break;
jah128 0:9ffe8ebd1c40 235 }
jah128 0:9ffe8ebd1c40 236 if(request_response == 1){
jah128 0:9ffe8ebd1c40 237 send_response(sender, is_broadcast, success, id, 1, function, NULL, 0);
jah128 0:9ffe8ebd1c40 238 }
jah128 0:9ffe8ebd1c40 239
jah128 0:9ffe8ebd1c40 240 }
jah128 0:9ffe8ebd1c40 241
jah128 0:9ffe8ebd1c40 242 //Handle a message that is a (not-user) request
jah128 0:9ffe8ebd1c40 243 void handle_request(char sender, char is_broadcast, char request_response, char id, char function){
jah128 0:9ffe8ebd1c40 244 int response_length = 0;
jah128 0:9ffe8ebd1c40 245 char * response = NULL;
jah128 0:9ffe8ebd1c40 246 char success = 0;
jah128 0:9ffe8ebd1c40 247
jah128 0:9ffe8ebd1c40 248 switch(function){
jah128 0:9ffe8ebd1c40 249 case 0: // Null request
jah128 0:9ffe8ebd1c40 250 success=1;
jah128 0:9ffe8ebd1c40 251 break;
jah128 0:9ffe8ebd1c40 252 case 1: // Request left motor speed
jah128 0:9ffe8ebd1c40 253 {
jah128 0:9ffe8ebd1c40 254 response_length = 2;
jah128 0:9ffe8ebd1c40 255 float speed = piswarm.get_left_motor() * 32767;
jah128 0:9ffe8ebd1c40 256 int a_speed = 32768 + (int) speed;
jah128 0:9ffe8ebd1c40 257 char msb = (char) (a_speed / 256);
jah128 0:9ffe8ebd1c40 258 char lsb = (char) (a_speed % 256);
jah128 0:9ffe8ebd1c40 259 response = new char[2];
jah128 0:9ffe8ebd1c40 260 response[0]=msb;
jah128 0:9ffe8ebd1c40 261 response[1]=lsb;
jah128 0:9ffe8ebd1c40 262 success=1;
jah128 0:9ffe8ebd1c40 263 break;
jah128 0:9ffe8ebd1c40 264 }
jah128 0:9ffe8ebd1c40 265 case 2: // Request right motor speed
jah128 0:9ffe8ebd1c40 266 {
jah128 0:9ffe8ebd1c40 267 response_length = 2;
jah128 0:9ffe8ebd1c40 268 float speed = piswarm.get_right_motor() * 32767;
jah128 0:9ffe8ebd1c40 269 int a_speed = 32768 + (int) speed;
jah128 0:9ffe8ebd1c40 270 char msb = (char) (a_speed / 256);
jah128 0:9ffe8ebd1c40 271 char lsb = (char) (a_speed % 256);
jah128 0:9ffe8ebd1c40 272 response = new char[2];
jah128 0:9ffe8ebd1c40 273 response[0]=msb;
jah128 0:9ffe8ebd1c40 274 response[1]=lsb;
jah128 0:9ffe8ebd1c40 275 success=1;
jah128 0:9ffe8ebd1c40 276 break;
jah128 0:9ffe8ebd1c40 277 }
jah128 0:9ffe8ebd1c40 278 case 3: // Request button state
jah128 0:9ffe8ebd1c40 279 {
jah128 0:9ffe8ebd1c40 280 response_length = 1;
jah128 0:9ffe8ebd1c40 281 response = new char[1];
jah128 0:9ffe8ebd1c40 282 response[0]=piswarm.get_switches();
jah128 0:9ffe8ebd1c40 283 break;
jah128 0:9ffe8ebd1c40 284 }
jah128 0:9ffe8ebd1c40 285 case 4: // Request LED colours
jah128 0:9ffe8ebd1c40 286 {
jah128 0:9ffe8ebd1c40 287 response_length = 6;
jah128 0:9ffe8ebd1c40 288 response = new char[6];
jah128 0:9ffe8ebd1c40 289 int oled_colour = piswarm.get_oled_colour();
jah128 0:9ffe8ebd1c40 290 int cled_colour = piswarm.get_cled_colour();
jah128 0:9ffe8ebd1c40 291 response[0] = (char) (oled_colour >> 16);
jah128 0:9ffe8ebd1c40 292 response[1] = (char) ((oled_colour >> 8) % 256);
jah128 0:9ffe8ebd1c40 293 response[2] = (char) (oled_colour % 256);
jah128 0:9ffe8ebd1c40 294 response[3] = (char) (cled_colour >> 16);
jah128 0:9ffe8ebd1c40 295 response[4] = (char) ((cled_colour >> 8) % 256);
jah128 0:9ffe8ebd1c40 296 response[5] = (char) (cled_colour % 256);
jah128 0:9ffe8ebd1c40 297 break;
jah128 0:9ffe8ebd1c40 298 }
jah128 0:9ffe8ebd1c40 299 case 5: // Request LED states
jah128 0:9ffe8ebd1c40 300 break;
jah128 0:9ffe8ebd1c40 301 case 6: // Request battery power
jah128 0:9ffe8ebd1c40 302 {
jah128 0:9ffe8ebd1c40 303 response_length = 2;
jah128 0:9ffe8ebd1c40 304 response = new char[2];
jah128 0:9ffe8ebd1c40 305 float fbattery = piswarm.battery() * 1000.0;
jah128 0:9ffe8ebd1c40 306 unsigned short battery = (unsigned short) fbattery;
jah128 0:9ffe8ebd1c40 307 response[0] = battery >> 8;
jah128 0:9ffe8ebd1c40 308 response[1] = battery % 256;
jah128 0:9ffe8ebd1c40 309 break;
jah128 0:9ffe8ebd1c40 310 }
jah128 0:9ffe8ebd1c40 311 case 7: // Request light sensor reading
jah128 0:9ffe8ebd1c40 312 {
jah128 0:9ffe8ebd1c40 313 response_length = 2;
jah128 0:9ffe8ebd1c40 314 response = new char[2];
jah128 0:9ffe8ebd1c40 315 float flight = piswarm.read_light_sensor() * 655.0;
jah128 0:9ffe8ebd1c40 316 unsigned short light = (unsigned short) flight;
jah128 0:9ffe8ebd1c40 317 response[0] = light >> 8;
jah128 0:9ffe8ebd1c40 318 response[1] = light % 256;
jah128 0:9ffe8ebd1c40 319 break;
jah128 0:9ffe8ebd1c40 320 }
jah128 0:9ffe8ebd1c40 321 case 8: // Request accelerometer reading
jah128 0:9ffe8ebd1c40 322 break;
jah128 0:9ffe8ebd1c40 323 case 9: // Request gyroscope reading
jah128 0:9ffe8ebd1c40 324 break;
jah128 0:9ffe8ebd1c40 325 case 10: // Request background IR reading
jah128 0:9ffe8ebd1c40 326 break;
jah128 0:9ffe8ebd1c40 327 case 11: // Request illuminated IR reading
jah128 0:9ffe8ebd1c40 328 break;
jah128 0:9ffe8ebd1c40 329 case 12: // Request line-tracking IR reading
jah128 0:9ffe8ebd1c40 330 break;
jah128 0:9ffe8ebd1c40 331 case 13: // Request uptime
jah128 0:9ffe8ebd1c40 332 break;
jah128 0:9ffe8ebd1c40 333 case 14: //
jah128 0:9ffe8ebd1c40 334 break;
jah128 0:9ffe8ebd1c40 335 case 15: //
jah128 0:9ffe8ebd1c40 336 break;
jah128 0:9ffe8ebd1c40 337 }
jah128 0:9ffe8ebd1c40 338 send_response(sender, is_broadcast, success, id, 0, function, response, response_length);
jah128 0:9ffe8ebd1c40 339
jah128 0:9ffe8ebd1c40 340 }
jah128 0:9ffe8ebd1c40 341
jah128 0:9ffe8ebd1c40 342 void processRadioData(char * data, char length){
jah128 0:9ffe8ebd1c40 343 if(RF_USE_LEDS==1) {errorled=0;rx=1;}
jah128 0:9ffe8ebd1c40 344 // Decompose the received message
jah128 0:9ffe8ebd1c40 345 if(length < 4) errormessage(0);
jah128 0:9ffe8ebd1c40 346 else {
jah128 0:9ffe8ebd1c40 347 // Establish the sender and target of the packet
jah128 0:9ffe8ebd1c40 348 char sender = data[0];
jah128 0:9ffe8ebd1c40 349 char target = data[1];
jah128 0:9ffe8ebd1c40 350 char id = data[2];
jah128 0:9ffe8ebd1c40 351 char command = data[3];
jah128 0:9ffe8ebd1c40 352 if(sender<32 || sender>63)errormessage(1);
jah128 0:9ffe8ebd1c40 353 else {
jah128 0:9ffe8ebd1c40 354 if(target<32 || target>63)errormessage(2);
jah128 0:9ffe8ebd1c40 355 else {
jah128 0:9ffe8ebd1c40 356 sender -= 32;
jah128 0:9ffe8ebd1c40 357 target -= 32;
jah128 0:9ffe8ebd1c40 358 decodeMessage(sender,target,id,command,data+4,length-4);
jah128 0:9ffe8ebd1c40 359 }
jah128 0:9ffe8ebd1c40 360 }
jah128 0:9ffe8ebd1c40 361 }
jah128 0:9ffe8ebd1c40 362 if(RF_USE_LEDS==1) rx=0;
jah128 0:9ffe8ebd1c40 363 }
jah128 0:9ffe8ebd1c40 364
jah128 0:9ffe8ebd1c40 365 void errormessage(int index){
jah128 0:9ffe8ebd1c40 366 if(RF_USE_LEDS==1) errorled=1;
jah128 0:9ffe8ebd1c40 367 switch(index){
jah128 0:9ffe8ebd1c40 368 case 0: //Message to short
jah128 0:9ffe8ebd1c40 369 if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n");
jah128 0:9ffe8ebd1c40 370 break;
jah128 0:9ffe8ebd1c40 371 case 1: //Sender out of valid range
jah128 0:9ffe8ebd1c40 372 if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n");
jah128 0:9ffe8ebd1c40 373 break;
jah128 0:9ffe8ebd1c40 374 case 2: //Target out of valid range
jah128 0:9ffe8ebd1c40 375 if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n");
jah128 0:9ffe8ebd1c40 376 break;
jah128 0:9ffe8ebd1c40 377 case 3:
jah128 0:9ffe8ebd1c40 378
jah128 0:9ffe8ebd1c40 379 break;
jah128 0:9ffe8ebd1c40 380 case 4: break;
jah128 0:9ffe8ebd1c40 381 }
jah128 0:9ffe8ebd1c40 382 }