Exemple #1
0
static void cmdGetImuLoop(unsigned char status, unsigned char length, unsigned char *frame) {

    unsigned int count;
    unsigned long tic;
    unsigned char *tic_char;
    Payload pld;

    LED_RED = 1;

    count = frame[0] + (frame[1] << 8);

    tic_char = (unsigned char*) &tic;
    swatchReset();
    tic = swatchTic();

    while (count) {

        pld = payCreateEmpty(16); // data length = 16
        paySetData(pld, 4, tic_char);
        payAppendData(pld, 4, 6, xlReadXYZ());
        payAppendData(pld, 10, 6, gyroReadXYZ());
        paySetStatus(pld, status);
        paySetType(pld, CMD_GET_IMU_DATA);

        radioSendPayload(macGetDestAddr(), pld);
        count--;
        payDelete(pld);
        delay_ms(4);
        tic = swatchTic();
    }

    LED_RED = 0;

}
Exemple #2
0
void clksyncHandleRequest(MacPacket packet) {

    Payload pld;
    MacPacket response;
    unsigned long* frame;
    unsigned long s0, m1, m2;
    
    pld = macGetPayload(packet);
    frame = (unsigned long*) payGetData(pld);
    
    s0 = frame[0]; // Read requester time of flight
    m1 = packet->timestamp + sclockGetOffsetTicks(); // Read local time of reception

    response = radioRequestPacket(12); // Sending 3 longs
    if(response == NULL) { return; }

    macSetDestAddr(response, macGetSrcAddr(packet));
    macSetDestPan(response, macGetSrcPan(packet));
    pld = macGetPayload(response); // Create response packet
    paySetType(pld, CMD_CLOCK_UPDATE_RESPONSE);
    paySetData(pld, 4, (unsigned char*) &s0);
    payAppendData(pld, 4, 4, (unsigned char*) & m1);
    
    // Empty TX queue to minimize time of flight error
    while(!radioTxQueueEmpty()) { radioProcess(); }
    
    m2 = sclockGetGlobalTicks(); // Get approximate time of flight
    payAppendData(pld, 8, 4, (unsigned char*) & m2);
    
    while(!radioEnqueueTxPacket(response)) { radioProcess(); }
    radioProcess();    

}
Exemple #3
0
// ==== PRIVATE FUNCTIONS ===================================================== 
static void clksyncSendRequest(SyncStatus sync) {

    MacPacket packet;
    Payload pld;
    unsigned long s0;
    
    packet = radioRequestPacket(4);
    if(packet == NULL) { return; }
    macSetDestAddr(packet, sync->master_addr);
    macSetDestPan(packet, sync->master_pan);
    pld = macGetPayload(packet);
    paySetType(pld, CMD_CLOCK_UPDATE_REQUEST);

    while(!radioTxQueueEmpty()) { radioProcess(); }
    
    s0 = sclockGetGlobalTicks();
    pld = macGetPayload(packet);
    paySetData(pld, 4, (unsigned char*) &s0);

    while(!radioEnqueueTxPacket(packet)) { radioProcess(); }
    radioProcess();

    sync->requests++;
    if(sync->requests > MAX_REQUEST_ATTEMPTS) {
        sync->state = STATE_REQUEST_TIMEOUT;
    }

}
Exemple #4
0
static void cmdDirDumpRequest(MacPacket packet) {

    Payload pld;
    MacPacket response;
    unsigned int *frame, req_addr, req_pan, i, size;

    pld = macGetPayload(packet);
    frame = (unsigned int*) payGetData(pld);

    req_addr = frame[0];
    req_pan = frame[1];

    // Send all if both addresses 0
    if(req_addr == 0 && req_pan == 0) {

        size = dirGetSize();
        DirEntry entries[size];
        dirGetEntries(entries); // Assume we get size # of entries

        i = 0;
        while(i < size) {
            
            response = radioRequestPacket(sizeof(DirEntryStruct));
            if(response == NULL) { continue; }
            macSetDestAddr(response, macGetSrcAddr(packet));
            pld = macGetPayload(response);
            paySetType(pld, CMD_DIR_DUMP_RESPONSE);
            paySetData(pld, sizeof(DirEntryStruct), (unsigned char*) entries[i]);
            while(!radioEnqueueTxPacket(response));
            i++;
        }
                
    } else {

        DirEntry entry;

        entry = dirQueryAddress(req_addr, req_pan);
        if(entry == NULL) { return; }
        
        while(1) {
            
            response = radioRequestPacket(sizeof(DirEntryStruct));
            if(response == NULL) { continue; }
            macSetDestAddr(response, macGetSrcAddr(packet));
            pld = macGetPayload(response);
            paySetType(pld, CMD_DIR_DUMP_RESPONSE);
            //paySetData(pld, sizeof(DirEntryStruct), (unsigned char*) &entry);
            memcpy(payGetData(pld), entry, sizeof(DirEntryStruct));
            while(!radioEnqueueTxPacket(response));
            break;
        }
    }
    
}
Exemple #5
0
static void cmdSleep(unsigned char status, unsigned char length, unsigned char *frame) {
    char sleep = frame[0];
    if (sleep) {
        //g_radio_duty_cycle = 1;
    } else {
        //g_radio_duty_cycle = 0;
        Payload pld;
        pld = payCreateEmpty(1);
        paySetData(pld, 1, (unsigned char*) (&sleep)); //echo back a CMD_SLEEP with '0', incdicating a wakeup
        paySetStatus(pld, status);
        paySetType(pld, CMD_SLEEP);
        radioSendPayload((WordVal) macGetDestAddr(), pld);
    }
}
Exemple #6
0
static void cmdEraseSector(unsigned char status, unsigned char length, unsigned char *frame) {
    //unsigned int numSamples = frame[0] + (frame[1] << 8);
    unsigned long numSamples = *((unsigned long*) (frame));

    telemErase(numSamples);

    //Send a confirmation packet
    Payload pld;
    pld = payCreateEmpty(4);
    paySetData(pld, 4, (unsigned char*) (&numSamples));
    paySetStatus(pld, status);
    paySetType(pld, CMD_ERASE_SECTORS);
    radioSendPayload((WordVal) macGetDestAddr(), pld);
}
Exemple #7
0
unsigned char uartSendPayload(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
    MacPacket packet;
    Payload pld;

    packet = ppoolRequestFullPacket(length);
    if(packet == NULL)
        return 0;

    pld = packet->payload;
    paySetType(pld, type);
    paySetStatus(pld, status);
    paySetData(pld, length, frame);
    if(uartSendPacket(packet)) {
        return 1;
    } else {
        ppoolReturnFullPacket(packet);
        return 0;
    }
}
Exemple #8
0
static void cmdGetGyroCalibParam(MacPacket packet) {
        
    //Payload pld = macGetPayload(packet);
    //unsigned char status = payGetStatus(pld);
    //unsigned char* frame = payGetData(pld);
    unsigned int srcAddr = macGetSrcAddr(packet);
    
    Payload pld;
    MacPacket response;
    
    response = radioRequestPacket(12);
    if(response == NULL) { return; }
    macSetDestAddr(response, srcAddr);
    pld = response->payload;
    paySetData(pld, 12, gyroGetCalibParam());
    paySetStatus(pld, 0);
    paySetType(pld, CMD_GET_GYRO_CALIB_PARAM);
    while(!radioEnqueueTxPacket(response));
}
Exemple #9
0
/*-----------------------------------------------------------------------------
 *          AUX functions
-----------------------------------------------------------------------------*/
static void cmdEcho(MacPacket packet) {
        
    Payload pld = macGetPayload(packet);
    unsigned char status = payGetStatus(pld);
    unsigned char* frame = payGetData(pld);
    unsigned int length = payGetDataLength(pld);
    unsigned int srcAddr = macGetSrcAddr(packet);
    
    MacPacket response;
    
    response = radioRequestPacket(length);
    if(response == NULL) { return; }
    macSetDestAddr(response, srcAddr);
    
    pld = response->payload;
    paySetData(pld, length, frame);
    paySetStatus(pld, status);
    paySetType(pld, CMD_ECHO);
    
    while(!radioEnqueueTxPacket(response));
}
Exemple #10
0
/*****************************************************************************
* Function Name : test_radio
* Description   : Send out a packet containing the data in the array pointed to
*                 by the 'data' argument passed in.
* Parameters    : type - The type field of the radio test packet
*                 status - Status field of radio test packet (not yet used)
*                 length - The length of the payload data array
*                 data - Pointer to the character array containing the payload
*                 data to send back
* Return Value  : success indicator - 0 for failed, 1 for succeeded
*****************************************************************************/
unsigned char test_radio(unsigned char type, unsigned char status,\
                         unsigned char length, unsigned char* data)
{
    MacPacket packet;
    Payload pld;

    // Get a new packet from the pool
    packet = radioRequestPacket(length);
    if(packet == NULL) return 0;
    //macSetDestAddr(RADIO_DEST_ADDR);

    // Prepare the payload
    pld = packet->payload;
    paySetType(pld, type);
    paySetStatus(pld, status);
    paySetData(pld, length, data);

    // Enqueue the packet for broadcast
    radioEnqueueTxPacket(packet);

    return 1; //success
}
Exemple #11
0
static void cmdCamParamRequest(MacPacket packet) {

    Payload pld;
    CamParamStruct params;
    MacPacket response;
    
    pld = macGetPayload(packet);
    camGetParams(&params);
    
    response = radioRequestPacket(sizeof(CamParamStruct));
    if(response == NULL) { return; }
    
    macSetDestAddr(response, macGetSrcAddr(packet));
    pld = macGetPayload(response);
    paySetType(pld, CMD_CAM_PARAM_RESPONSE);
    paySetStatus(pld, 0);
    paySetData(pld, sizeof(CamParamStruct), (unsigned char*)&params);

    while(!radioEnqueueTxPacket(response));


}
Exemple #12
0
// TODO: Move mac packet creation from Radio_DMA to Mac_Packet
void telemSendB(unsigned int addr) {

	MacPacket packet;
	Payload pld;
	TelemetryStructB telemetryB;
	
	// Populate the telemetry fields
	telemPopulateB(&telemetryB);
	
	// Create a radio packet
	packet = radioRequestPacket(TELEMETRY_B_SIZE);
	if(packet == NULL) { return; }
        macSetDestAddr(packet, addr);
        macSetDestPan(packet, netGetLocalPanID());

	// Write the telemetry struct into the packet payload
	pld = macGetPayload(packet);
	paySetType(pld, CMD_RESPONSE_TELEMETRY);
	paySetData(pld, TELEMETRY_B_SIZE, (unsigned char *) &telemetryB);
	 if(!radioEnqueueTxPacket(packet)) {
		 radioReturnPacket(packet);	// Delete packet if append fails
	 }
	
}
Exemple #13
0
// ====== Camera and Vision ===================================================
// TODO: Use a struct to simplify the packetization
static void cmdRequestRawFrame(MacPacket packet) {
    
    unsigned int srcAddr, srcPan, height, width, i, temp;
    unsigned int sent, to_send, block_size = 75;
    MacPacket response;
    Payload pld;
    CamFrame frame;
    CamRow *row;
    CvResultStruct info;

    srcAddr = macGetSrcAddr(packet);
    srcPan = macGetSrcPan(packet);    

    frame = NULL;
    while(frame == NULL) {
        frame = camGetFrame();
    }           

    cvProcessFrame(frame, &info);    

    height = DS_IMAGE_ROWS;
    width = DS_IMAGE_COLS;

    for(i = 0; i < height; i++) {        
        row = &(frame->pixels[i]);
        to_send = width;
        while(to_send > 0) {            
            response = radioRequestPacket(block_size + 6);
            if(response == NULL) { continue; }
            pld = macGetPayload(response);
            paySetType(pld, CMD_RAW_FRAME_RESPONSE);
            paySetStatus(pld, 0);
            macSetDestAddr(response, srcAddr);
            macSetDestPan(response, srcPan);
            temp = frame->frame_num;
            paySetData(pld, 2, (unsigned char *)&temp);
            temp = i;
            payAppendData(pld, 2, 2, (unsigned char*)&temp);
            temp = width - to_send;
            payAppendData(pld, 4, 2, (unsigned char*)&temp);
            temp = (block_size < to_send) ? block_size : to_send;
            payAppendData(pld, 6, temp, *row + (width - to_send));

            while(!radioEnqueueTxPacket(response));

            to_send = to_send - temp;

        }

    }
    sent = 0;
    while(!sent) {
        response = radioRequestPacket(10);
        if(response == NULL) { continue; }
        pld = macGetPayload(response);
        paySetType(pld, CMD_CENTROID_REPORT);
        paySetStatus(pld, 1);
        macSetDestAddr(response, srcAddr);
        macSetDestPan(response, srcPan);
        temp = info.centroid[0];
        paySetData(pld, 2, (unsigned char*)&temp);
        temp = info.centroid[1];
        payAppendData(pld, 2, 2, (unsigned char*)&temp);
        temp = info.max[0];
        payAppendData(pld, 4, 2, (unsigned char*)&temp);
        temp = info.max[1];
        payAppendData(pld, 6, 2, (unsigned char*)&temp);
        temp = info.max_lum;
        payAppendData(pld, 8, 1, (unsigned char*)&temp);
        temp = info.avg_lum;
        payAppendData(pld, 9, 1, (unsigned char*)&temp);
        while(!radioEnqueueTxPacket(response));
        sent = 1;
    }
    camReturnFrame(frame);

}