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; } } }
// ==== 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(); }
int main(void) { WordVal src_addr_init = {SRC_ADDR}; WordVal src_pan_id_init = {SRC_PAN_ID}; WordVal dst_addr_init = {DST_ADDR}; SetupClock(); SwitchClocks(); SetupPorts(); batSetup(); swatchSetup(); radioInit(src_addr_init, src_pan_id_init, RXPQ_MAX_SIZE, TXPQ_MAX_SIZE); radioSetChannel(MY_CHAN); //Set to my channel macSetDestAddr(dst_addr_init); dfmemSetup(); unsigned char memsize; memsize = dfmemGetChipSize(); xlSetup(); gyroSetup(); mcSetup(); cmdSetup(); //senSetup(); adcSetup(); pidSetup(); steeringSetup(); //radioReadTrxId(id); LED_RED = 1; LED_BLUE = 0; LED_YELLOW = 0; //while(1); if(phyGetState() == 0x16) { LED_GREEN = 1; } //print("Ready"); //readDFMemBySample(5); while(1) { cmdHandleRadioRxBuffer(); //Simple idle ; reduces idle current to 70 mA // TODO (abuchan, apullin, fgb) : Idle() causes unexpected behavior //if(radioIsRxQueueEmpty()){ // Idle(); //} } }
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; }
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)); }
/*----------------------------------------------------------------------------- * 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)); }
static void cmdCamParamRequest(MacPacket packet) { Payload pld; CamParamStruct params; MacPacket response; pld = macGetPayload(packet); camGetParams(¶ms); 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*)¶ms); while(!radioEnqueueTxPacket(response)); }
// 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 } }
int main(void) { wakeTime = 0; dcCounter = 0; WordVal src_addr_init = {RADIO_SRC_ADDR}; WordVal src_pan_id_init = {RADIO_SRC_PAN_ID}; WordVal dst_addr_init = {RADIO_DST_ADDR}; SetupClock(); SwitchClocks(); SetupPorts(); tiHSetup(); //swatchSetup(); radioInit(src_addr_init, src_pan_id_init, RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE); radioSetChannel(RADIO_CHANNEL); //Set to my channel macSetDestAddr(dst_addr_init); cmdSetup(); if(phyGetState() == 0x16) { LED_RED = 1; } while(1){ cmdHandleRadioRxBuffer(); } LED_GREEN = 0; LED_RED = 1; LED_YELLOW = 1; _LATG9 = 1; _LATC15 = 1; //testRadio(); /* LED_GREEN = 1; mpuSetup(); LED_GREEN = 0; LED_RED = 1; LED_YELLOW = 1; //batSetup(); //int old_ipl; //mSET_AND_SAVE_CPU_IP(old_ipl, 1) //LED_YELLOW = 1; //dfmemSetup(); //xlSetup(); //gyroSetup(); //tiHSetup(); //mcSetup(); //cmdSetup(); //adcSetup(); //telemSetup(); //Timer 5 //mcSetDutyCycle(1,70.0); //mcSetDutyCycle(2,70.0); //mcSetDutyCycle(3,70.0); //mcSetDutyCycle(4,70.0); #ifdef HALL_SENSORS //hallSetup(); // Timer 1, Timer 2 //hallSteeringSetup(); //doesn't exist yet #else //No hall sensors, standard BEMF control //legCtrlSetup(); // Timer 1 //steeringSetup(); //Timer 5 #endif //tailCtrlSetup(); //ovcamSetup(); /* //radioReadTrxId(id); LED_RED = 1; //Red is use an "alive" indicator LED_GREEN = 0; LED_YELLOW = 0; //tiHSetFloat(1,50.0); //tiHSetFloat(2,75.0); //tiHSetup(); //LED_GREEN = 1; //tiHSetFloat(1,.800); //LED_YELLOW = 1; //Radio startup verification //if(phyGetState() == 0x16) { LED_GREEN = 1; } //Sleeping and low power options //_VREGS = 1; //gyroSleep(); //tiHSetFloat(1,98.0); //tiHSetFloat(2,30.0); //tiHSetFloat(3,99.0); //tiHSetFloat(4,99.0); LED_GREEN = 0; LED_RED = 0; LED_YELLOW = 1; int i = 0; while (1) { delay_ms(2000); int i = i+1; LED_GREEN = i%2; // cmdHandleRadioRxBuffer(); #ifndef __DEBUG //Idle will not work with debug //Simple idle: if (radioIsRxQueueEmpty()) { Idle(); //_T1IE = 0; } #endif //delay_ms(1000); //cmdEcho(0, 1 , (unsigned char*)(&i) ); //i++; //if(radioIsRxQueueEmpty() && (t1_ticks >= wakeTime + 5000) ){ //Idle(); //LED_RED = 0; //gyroSleep(); //Sleep(); //} } /* if(g_radio_duty_cycle){ if(dcCounter == 0){ //LED_GREEN = 1; atSetRXAACKON(); }else{ //LED_GREEN = 0; atSetTRXOFF(); } } else{ //LED_GREEN = 1; } dcCounter = (dcCounter + 1) % 8; if(radioIsRxQueueEmpty() && !inMotion){ //gyroSleep(); LED_RED = 0; _SWDTEN = 1; //restart wdt Sleep(); //Idle(); } //should be asleep here, waiting for WTD wakeup ClrWdt(); //clear wdt _SWDTEN = 0; //software disable wdt LED_RED = 1; //spin up clock if(_COSC != 0b010){ while(OSCCONbits.LOCK!=1); } //gyroWake(); } } void testRadio(void) { //designed to be used with testRadio.py. //test radio.py should produce many echoes that cycle through the ASCII characters //comment out all code in main and use test Radio to test the radio only. //This code does not initialize non-radio-nessisary components. wakeTime = 0; dcCounter = 0; WordVal src_addr_init = {RADIO_SRC_ADDR}; WordVal src_pan_id_init = {RADIO_SRC_PAN_ID}; WordVal dst_addr_init = {RADIO_DST_ADDR}; SetupClock(); SwitchClocks(); SetupPorts(); int old_ipl; mSET_AND_SAVE_CPU_IP(old_ipl, 1) swatchSetup(); radioInit(src_addr_init, src_pan_id_init, RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE); radioSetChannel(RADIO_CHANNEL); //Set to my channel macSetDestAddr(dst_addr_init); cmdSetup(); LED_GREEN = ON; int i = 0; while (1) { i++; cmdHandleRadioRxBuffer(); } LED_RED = OFF; }*/ }
// ====== 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); }