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);
}
