void telemSetup() {
dfmemGetGeometryParams(&mem_geo); // Read memory chip sizing
//Telemetry packet size is set at startupt time.
telemPacketSize = sizeof (telemStruct_t);
}
void telemSetup() {
dfmemGetGeometryParams(&mem_geo); // Read memory chip sizing
//Telemetry packet size is set at startupt time.
telemDataSize = sizeof (TELEM_TYPE);
telemPacketSize = sizeof (telemStruct_t);
Nop();
Nop();
//Install telemetry service handler
// Lines removed before to use telemetry module in direct mode, does not
// run its own ISR. (pullin, 10/9/14)
//int retval;
//retval = sysServiceInstallT5(telemServiceRoutine);
//SetupTimer5();
}
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;
}
//Callback function when imageproc receives tactile command from radio
void handleSkinRequest(unsigned char length, unsigned char *frame, unsigned int src_addr) {
tactile_src_addr = src_addr;
rx_idx = frame[0];
//unsigned char tempframe[TACTILE_ROWS * TACTILE_COLS * 2 + 1];
//static unsigned char tempframe[100];
//buffer = tempframe;
buffer_length = 0;
expected_length = 2;
int i;
switch (rx_idx) {
case TACTILE_MODE_G: //query number of rows and columns
sendTactileCommand(length, frame);
/*
//TACTILE_ROWS = 0x00;
//TACTILE_COLS = 0x00;
buffer_length = 4;
//expected_length = 3;
buffer[0] = rx_idx;
buffer[1] = 0x02;
buffer[2] = TACTILE_ROWS;
buffer[3] = TACTILE_COLS;
setRXFlag();
//buffer = rowcol;
//buffer = tempframe;*/
break;
case TACTILE_MODE_A: //sample individual pixel
sendTactileCommand(length,frame);
break;
case TACTILE_MODE_B: //sample frame
sendTactileCommand(length,frame);
break;
case TACTILE_MODE_C: //poll pixel
sendTactileCommand(length,frame);
break;
case TACTILE_MODE_E: //start scan
sendTactileCommand(length,frame);
break;
case TACTILE_MODE_F: //stop scan
rx_idx = TACTILE_RX_IDLE;
sendTactileCommand(length,frame);
break;
case TACTILE_MODE_S: //turn streaming on or off
streaming = frame[1];
unsigned char temp[3] = {rx_idx,1,streaming};
radioSendData(tactile_src_addr, 0, CMD_TACTILE, sizeof(temp), temp, 0);
rx_idx = TACTILE_RX_IDLE;
break;
case TACTILE_MODE_T: //test frame
buffer_length = 0;
expected_length = max_buffer_length;
//buffer = tempframe;
sendTactileCommand(length,frame);
break;
case TACTILE_MODE_L: //load force-torque calibration parameters
Nop();
float temp_f[6];
memcpy(temp_f,&frame[2],6*sizeof(float));
int n_ind;
for (n_ind = 0; n_ind < 6; n_ind++) {
N[n_ind][frame[1]] = temp_f[n_ind]; //frame[1] indicates index [0-ROWS*COLS*3)
}
//char* test3 = (char*)&temp_f;
//unsigned char temp1[6] = {rx_idx,sizeof(float),test3[0],test3[1],test3[2],test3[3]};
//radioSendData(tactile_src_addr, 0, CMD_TACTILE, sizeof(temp1), temp1, 0);
rx_idx = TACTILE_RX_IDLE;
break;
case 'Y': //write N to dfmem
dfmemGetGeometryParams(&mem_geo);
int N_pages = (sizeof(N)/mem_geo.bytes_per_page) + 1; //how many pages needed to hold N
//for (i = N_pages; i > 0; i--) {
// dfmemWrite(N + mem_geo.bytes_per_page*(N_pages-i),i > 1 ? mem_geo.bytes_per_page : sizeof(N)-mem_geo.bytes_per_page*(N_pages-1) ,mem_geo.max_pages-i,0,0);
//}
dfmemWrite(N,sizeof(N),mem_geo.max_pages-1,0,0);
rx_idx = TACTILE_RX_IDLE;
break;
case 'Z':
/*//read N from dfmem
dfmemGetGeometryParams(&mem_geo);
int N_pages1 = (sizeof(N)/mem_geo.bytes_per_page) + 1; //how many pages needed to hold N
//for (i = N_pages1; i > 0; i--) {
// dfmemRead(mem_geo.max_pages-i, 0, i > 1 ? mem_geo.bytes_per_page : sizeof(N)-mem_geo.bytes_per_page*(N_pages1-1), N + mem_geo.bytes_per_page*(N_pages1-i));
//}
dfmemRead(mem_geo.max_pages-1, 0, sizeof(N), N);
*/
rx_idx = TACTILE_RX_IDLE;
/*DisableIntT1;
int pwm = frame[1]+(frame[2]<<8);
tiHSetDC(1,pwm);*/
//zero forces
if (zeroForces == 0) {
zeroForces = 1;
}
break;
default:
sendTactileCommand(length,frame);
break;
}
//blocking wait for skinproc to answer
/*while (buffer_length < expected_length) {
Nop();
}
handleSkinData(buffer_length, buffer);
rx_idx = TACTILE_RX_IDLE;*/
}
