File content as of revision 0:9ffe8ebd1c40:

/* University of York Robot Lab Pi Swarm Library: Swarm Communications Handler
 *
 * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
 * 
 * Version 0.4  January 2014
 *
 * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
 *
 */
 
 #include "communications.h"
 #include "piswarm.h"
 #include "main.h"
 
 DigitalOut actioning (LED1);
 DigitalOut errorled (LED2);
 DigitalOut tx (LED3);
 DigitalOut rx (LED4);
 Timeout tdma_timeout;
 char tdma_busy = 0;
 char waiting_message [64];
 char waiting_length = 0;

 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.???"};  
 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.???"};

//Decode the received message, action it if it is valid and for me
void decodeMessage(char sender, char target, char id, char command, char * data, char length){     
    char broadcast_message = 0, is_response = 0, request_response = 0, is_user = 0, is_command = 0, function = 0;
        
    if(target==0) broadcast_message = 1;        
    is_response = 0 != (command & (1 << 7));
    request_response = 0 != (command & (1 << 6));
    is_user = 0 != (command & (1 << 5));
    is_command = 0 != (command & (1 << 4));
    function = command % 16;
    
    if (RF_DEBUG==1) {
        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);
        else pc.printf("Message: S:%i T:%i ID:%x C:%x{%s} L:%i", sender, target, id, command, requests_array[function],length);
    }
        
    //Action the message only if I am a recipient
    if(target==0 || target==piswarm.get_id()){
        if(RF_USE_LEDS==1) actioning = 1;
          if(is_response == 1) {
              if(is_user == 0)handle_response(sender, broadcast_message, request_response, id, is_command, function, data, length);
              else handleUserRFResponse(sender, broadcast_message, request_response, id, is_command, function, data, length);
          }else {
              if(is_command == 1) {
                  if(RF_ALLOW_COMMANDS == 1){
                        if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
                        else handle_command(sender, broadcast_message, request_response, id, function, data, length);
                  } else if (RF_DEBUG==1) pc.printf(" - Blocked\n");
                }
              else {
                //A information request has no extra parameters
                  if(length == 0){
                        if(is_user == 1)handleUserRFCommand(sender, broadcast_message, request_response, id, is_command, function, data, length);
                        else handle_request(sender, broadcast_message, request_response, id, function);
                  } else if (RF_DEBUG==1) pc.printf(" - Invalid\n");
                }
            } 
        if(RF_USE_LEDS==1) actioning = 0; 
    } else  if (RF_DEBUG==1) pc.printf(" - Ignored\n");
}

//Send a response message
void send_response(char target, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){
    char message [4+length];
    message[0]=piswarm.get_id();
    message[1]=target;
    message[2]=id;
    message[3]=128 + (success << 6) + (is_command << 4) + function;
    for(int i=0;i<length;i++){
       message[4+i]=data[i];   
    } 
    //Delay the response if it is broadcast and TDMA mode is on
    if(RF_USE_TDMA == 1 && is_broadcast == 1){
        if(tdma_busy == 1){
            if (RF_DEBUG==1) pc.printf("Cannot respond - TDMA busy\n");
        }else{
            tdma_busy = 1;
            strcpy(waiting_message,message);
            waiting_length=length;
            tdma_timeout.attach_us(&tdma_response, RF_TDMA_TIME_PERIOD_US * piswarm.get_id());
            if (RF_DEBUG==1) pc.printf("TDMA Response pending\n");
        }
    }else 
    {
        piswarm.send_rf_message(message,4+length);
        if(RF_DEBUG==1)pc.printf("Response issued");
    }
}

// Send a delayed response
void tdma_response(){
    piswarm.send_rf_message(waiting_message,4+waiting_length);
    tdma_busy = 0;
    if (RF_DEBUG==1) pc.printf("TDMA Response issued\n");
}

// Handle a message that is a response
void handle_response(char sender, char is_broadcast, char success, char id, char is_command, char function, char * data, char length){
    
}

// Handle a message that is a (not-user) command
void handle_command(char sender, char is_broadcast, char request_response, char id, char function, char * data, char length){
    char success = 0;
    switch(function){
        case 0: // Stop             [0 data]
            if(length==0) {
                piswarm.stop();
                if(RF_DEBUG==1) pc.printf(" - Stop Command Issued - ");
                success = 1;
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;   
        case 1: // Forward          [2 bytes: 16-bit signed short]
            if(length==2) {
                int  i_speed = (data[0] << 8) + data[1];
                float speed = i_speed / 32768.0;
                speed--;
                piswarm.forward(speed);
                success = 1;
                if(RF_DEBUG==1) pc.printf(" - Forward %1.2f Command Issued - ",speed);
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;
        case 2: // Backward         [2 bytes: 16-bit signed short]
            if(length==2) {
                int  i_speed = (data[0] << 8) + data[1];
                float speed = i_speed / 32768.0;
                speed--;
                piswarm.backward(speed);
                success = 1;
                if(RF_DEBUG==1) pc.printf(" - Backward %1.2f Command Issued - ",speed);
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;   
        case 3: // Left             [2 bytes: 16-bit signed short]
            if(length==2) {
                int  i_speed = (data[0] << 8) + data[1];
                float speed = i_speed / 32768.0;
                speed--;
                piswarm.left(speed);
                success = 1;
                if(RF_DEBUG==1) pc.printf(" - Left %1.2f Command Issued - ",speed);
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;
        case 4: // Right            [2 bytes: 16-bit signed short]
             if(length==2) {
                int  i_speed = (data[0] << 8) + data[1];
                float speed = i_speed / 32768.0;
                speed--;
                piswarm.right(speed);
                success = 1;
                if(RF_DEBUG==1) pc.printf(" - Right %1.2f Command Issued - ",speed);
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;   
        case 5: // Left Motor       [2 bytes: 16-bit signed short]
             if(length==2) {
                int  i_speed = (data[0] << 8) + data[1];
                float speed = i_speed / 32768.0;
                speed--;
                piswarm.left_motor(speed);
                success = 1;
                if(RF_DEBUG==1) pc.printf(" - Left Motor %1.2f Command Issued - ",speed);
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;
        case 6: // Right Motor      [2 bytes: 16-bit signed short]
             if(length==2) {
                int  i_speed = (data[0] << 8) + data[1];
                float speed = i_speed / 32768.0;
                speed--;
                piswarm.right_motor(speed);
                success = 1;
                if(RF_DEBUG==1) pc.printf(" - Right Motor %1.2f Command Issued - ",speed);
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;
        case 7: // Outer LED Colour [3 bytes: R, G, B]
             if(length==3) {
                piswarm.set_oled_colour (data[0],data[1],data[2]); 
                success = 1;
                if(RF_DEBUG==1) pc.printf(" - Set Outer R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;
        case 8: // Center LED Colour[3 bytes: R, G, B]
             if(length==3) {
                piswarm.set_cled_colour (data[0],data[1],data[2]); 
                success = 1;
                if(RF_DEBUG==1) pc.printf(" - Set Center R%i G%i B%i Command Issued - ",data[0],data[1],data[2]);
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;
        case 9: // Outer LED State  [2 bytes: [xxxxxx01][23456789] ]
            if(length==2) {
                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))); 
                success = 1;
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;
        case 10: // Center LED State [1 bytes: [xxxxxxxE] E=enabled ]
            if(length==1) {
                piswarm.enable_cled (data[0] % 2); 
                success = 1;
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;
        case 11: // Set outer LED    [1 byte:  [xxxEvvvv] E=enabled vvvv=LED]
            if(length==1) {
                int led = data[0] % 16;
                if(led < 10){
                    piswarm.set_oled(led, 0!=(data[0] & (1 << 4)));
                    success = 1;
                } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;   
        case 12: // Play sound      [Minimum 1 byte]
            if(length>0) {
                piswarm.play_tune(data,length);
                success = 1;
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;
        case 13: // Sync time
            if(length==4) {
                unsigned int new_time = 0;
                new_time+=((unsigned int)data[0] << 24);
                new_time+=((unsigned int)data[1] << 16);
                new_time+=((unsigned int)data[2] << 8);
                new_time+=(unsigned int)data[3];
                set_time(new_time);
                display_system_time();
            } else if(RF_DEBUG==1) pc.printf(" - Invalid\n");
            break;   
        case 14: //
            break;
        case 15: //
            break;  
    }
    if(request_response == 1){
        send_response(sender, is_broadcast, success, id, 1, function, NULL, 0);
    }
    
}

//Handle a message that is a (not-user) request
void handle_request(char sender, char is_broadcast, char request_response, char id, char function){
    int response_length = 0;
    char * response = NULL;
    char success = 0;
    
    switch(function){
        case 0: // Null request
            success=1;
            break;   
        case 1: // Request left motor speed
            {
            response_length = 2;
            float speed = piswarm.get_left_motor() * 32767;
            int a_speed = 32768 + (int) speed;
            char msb = (char) (a_speed / 256);
            char lsb = (char) (a_speed % 256);
            response = new char[2];
            response[0]=msb;
            response[1]=lsb;
            success=1;
            break;   
            }
        case 2: // Request right motor speed
            {
            response_length = 2;
            float speed = piswarm.get_right_motor() * 32767;
            int a_speed = 32768 + (int) speed;
            char msb = (char) (a_speed / 256);
            char lsb = (char) (a_speed % 256);
            response = new char[2];
            response[0]=msb;
            response[1]=lsb;
            success=1;
            break;   
            }
        case 3: // Request button state
            {
            response_length = 1;
            response = new char[1];
            response[0]=piswarm.get_switches();
            break;   
            }
        case 4: // Request LED colours
            {
            response_length = 6;
            response = new char[6];
            int oled_colour = piswarm.get_oled_colour();
            int cled_colour = piswarm.get_cled_colour();
            response[0] = (char) (oled_colour >> 16);
            response[1] = (char) ((oled_colour >> 8) % 256);
            response[2] = (char) (oled_colour % 256);
            response[3] = (char) (cled_colour >> 16);
            response[4] = (char) ((cled_colour >> 8) % 256);
            response[5] = (char) (cled_colour % 256);
            break;   
            }
        case 5: // Request LED states
            break;   
        case 6: // Request battery power
            {
            response_length = 2;
            response = new char[2];
            float fbattery = piswarm.battery() * 1000.0;
            unsigned short battery = (unsigned short) fbattery;
            response[0] = battery >> 8;
            response[1] = battery % 256;
            break;   
            }
        case 7: // Request light sensor reading
            {
            response_length = 2;
            response = new char[2];
            float flight = piswarm.read_light_sensor() * 655.0;
            unsigned short light = (unsigned short) flight;
            response[0] = light >> 8;
            response[1] = light % 256;
            break;   
            }
        case 8: // Request accelerometer reading
            break;
        case 9: // Request gyroscope reading
            break;   
        case 10: // Request background IR reading
            break;
        case 11: // Request illuminated IR reading
            break;   
        case 12: // Request line-tracking IR reading
            break;
        case 13: // Request uptime
            break;
        case 14: // 
            break;
        case 15: //
            break;         
    }  
    send_response(sender, is_broadcast, success, id, 0, function, response, response_length);
    
}

void processRadioData(char * data, char length){
    if(RF_USE_LEDS==1) {errorled=0;rx=1;}
    // Decompose the received message
    if(length < 4) errormessage(0);
    else {
        // Establish the sender and target of the packet
        char sender = data[0];
        char target = data[1];
        char id = data[2];
        char command = data[3];
        if(sender<32 || sender>63)errormessage(1);
        else {
            if(target<32 || target>63)errormessage(2);
            else {
                sender -= 32;
                target -= 32;
                decodeMessage(sender,target,id,command,data+4,length-4);
            }
        }
    }
    if(RF_USE_LEDS==1) rx=0;   
}

void errormessage(int index){
    if(RF_USE_LEDS==1) errorled=1;
    switch(index){
        case 0: //Message to short
            if (RF_DEBUG==1) pc.printf("Bad Message: Too short\n");
            break;
        case 1: //Sender out of valid range
            if (RF_DEBUG==1) pc.printf("Bad Message: Invalid sender\n");   
            break;
        case 2: //Target out of valid range 
            if (RF_DEBUG==1) pc.printf("Bad Message: Invalid target\n");
            break;
        case 3:
        
            break;
        case 4: break;
    }
}