// ==== 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; } }
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(); }
static void cmdGetMemContents(MacPacket packet) { Payload pld; MacPacket data_packet; unsigned char *frame; DfmemGeometryStruct geo; pld = macGetPayload(packet); frame = payGetData(pld); dfmemGetGeometryParams(&geo); unsigned int start_page = frame[0] + (frame[1] << 8); unsigned int end_page = frame[2] + (frame[3] << 8); unsigned int tx_data_size = frame[4] + (frame[5] << 8); unsigned int page, j; unsigned char count = 0; // Send back memory contents for (page = start_page; page < end_page; ++page) { j = 0; while (j + tx_data_size <= geo.bytes_per_page) { data_packet = NULL; while(data_packet == NULL) { data_packet = radioRequestPacket(tx_data_size); } macSetDestAddr(data_packet, macGetSrcAddr(packet)); macSetDestPan(data_packet, macGetSrcPan(packet)); pld = macGetPayload(data_packet); dfmemRead(page, j, tx_data_size, payGetData(pld)); paySetStatus(pld, count++); paySetType(pld, CMD_RESPONSE_TELEMETRY); while(!radioEnqueueTxPacket(data_packet)); j += tx_data_size; delay_ms(20); } } // Signal end of transfer LED_GREEN = 0; LED_RED = 0; LED_ORANGE = 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 } }
// ====== 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); }