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;
}
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();
}
//Recieved UART Xbee packet, send packet out over the radio
void xbeeHandleTx(Payload uart_pld){
MacPacket tx_packet;
WordVal dst_addr;
Payload rx_pld;
//char test;
//Get destination address from uart_pld package
//Frame ID and options packet are currently ignored for this type of packet...
dst_addr.byte.HB = uart_pld->pld_data[DEST_ADDR_HB_POS - RX_DATA_OFFSET];
dst_addr.byte.LB = uart_pld->pld_data[DEST_ADDR_LB_POS - RX_DATA_OFFSET];
//test = dst_addr.byte.LB;
//Create new packet with just the data that needs to be sent by the radio
int payloadLength = payGetPayloadLength(uart_pld)-(RX_FRAME_OFFSET-RX_DATA_OFFSET)-PAYLOAD_HEADER_LENGTH-1;
rx_pld = payCreateEmpty(payloadLength);
//test = payGetPayloadLength(rx_pld);
payAppendData(rx_pld, -PAYLOAD_HEADER_LENGTH,
payGetPayloadLength(rx_pld),
&(uart_pld->pld_data[RX_DATA_OFFSET]));
//Place packet in radio queue for sending
tx_packet = radioRequestPacket(payloadLength);
tx_packet->payload = rx_pld;
tx_packet->payload_length = payGetPayloadLength(rx_pld);//rx_pld_len.byte.LB - (RX_FRAME_OFFSET - RX_DATA_OFFSET);
//tx_packet->dest_pan_id = src_pan_id; //Already set when macCreatePacket is called.
tx_packet->dest_addr = dst_addr;
radioEnqueueTxPacket(tx_packet);
}
static void cmdSetCtrldTurnRate(unsigned char status, unsigned char length, unsigned char *frame) {
int rate;
Payload pld;
rate = frame[0] + (frame[1] << 8);
steeringSetAngRate(rate);
//Send confirmation packet
pld = payCreateEmpty(2);
//pld->pld_data[0] = status;
//pld->pld_data[1] = CMD_SET_CTRLD_TURN_RATE;
memcpy((pld->pld_data) + 2, frame, sizeof (int));
payAppendData(pld, 0, sizeof (rate), (unsigned char*) (&rate));
paySetStatus(pld, status);
paySetType(pld, CMD_SET_CTRLD_TURN_RATE);
radioSendPayload((WordVal) macGetDestAddr(), pld);
}
// ====== 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);
}
