void
macData_BC_Request( u8 len, u8 * data)
{
ftData *data_frame = (ftData*)(mac_buffer_tx+1); // Create a struct pointer to the global variable...
// if we have not found a channel to broadcast on do nothing
if (panDescriptor.logicalChannel == 0xff)
return;
Led1_on();
debugMsgStr("\r\nmacData_BC_Request->");
// Build the frame.
data_frame->fcf=FCF_DATA_NOACK; //FCF Data frame no ACK -- to prevent multiple acks
data_frame->seq = macConfig.dsn++;
data_frame->panid = macConfig.panId;
data_frame->srcAddr = macConfig.shortAddress;
data_frame->originAddr = macConfig.shortAddress;
data_frame->finalDestAddr = BROADCASTADDR;
data_frame->destAddr = BROADCASTADDR;
data_frame->type =DATA_FRAME;
u8 i;
for(i=0; i<len; i++)
((u8*)&data_frame->payload)[i] = *data++;
*mac_buffer_tx = len + ftDataHeaderSize; // save length away
macConfig.busy = true; // this locks out debug tasks
radioSendData(*mac_buffer_tx, (u8*)data_frame);
Led1_off();
}
// Target function to timer, sends ping packet after a delay
// note that arguments are via static vars due to the indirect nature of the call
static void
macDoPingRequest(void)
{
ftPing frame;
frame.fcf = FCF_DATA;
frame.seq = macConfig.dsn++;
frame.panid = macConfig.panId;
frame.srcAddr = macConfig.shortAddress;
frame.originAddr = macConfig.shortAddress;
frame.finalDestAddr = pingAddr;
frame.rssi = radioGetSavedRssiValue();
frame.lqi = radioGetSavedLqiValue();
frame.type = pingType;
// Set high bit of type if we're sleeping
if (macConfig.sleeping && NODE_TYPE == ENDDEVICE && NODE_SLEEP)
frame.type |= 0x80;
#if (NODE_TYPE == COORD)
{
#if DEBUG==2
debugMsgStr("\r\nin macDoPingRequest");
#endif
// Find the top parentpin
u8 addr = macGetTopParent(pingAddr);
frame.destAddr = addr;
// See if the child is sleeping (only the coord sends directly to a child node)
if (NODE_SLEEP &&
macIsChild(addr)&&
macIsChildSleeping(addr))
{
// Send it later, after child is awake
#if DEBUG==2
debugMsgStr("\r\ncalling macHoldFrame for ping response");
#endif
macHoldFrame(addr, (u8*)&frame, sizeof(ftPing));
// Don't send frame right now
return;
}
else
{
#if DEBUG==2
debugMsgStr("\r\n child: ");
debugMsgInt(addr);
debugMsgStr(" not sleeping");
#endif
}
}
#else
// End/router nodes
frame.destAddr = macConfig.parentShortAddress;
#endif
#if DEBUG==2
debugMsgStr(" sending Ping Response now\n\r");
#endif
radioSendData(sizeof(ftPing), (u8*)&frame);
}
// Target function for timer, send a frame prepared by macDataRequest()
void
mdr_timer(void)
{
// Create a struct pointer to the global variable...
ftData *data_frame = (ftData*)(mac_buffer_tx+1);
radioSendData(*mac_buffer_tx, (u8*)data_frame);
}
unsigned char cmdZeroPos(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
long motor_count[2];
motor_count[0] = pidObjs[0].p_state;
motor_count[1] = pidObjs[1].p_state;
radioSendData(RADIO_DST_ADDR, status, CMD_GET_AMS_POS, //TODO: Robot should respond to source of query, not hardcoded address
sizeof(motor_count), (unsigned char *)motor_count, 0);
pidZeroPos(0); pidZeroPos(1);
return 1;
}
//transmit skin data over radio, cap data length if over threshhold
void handleSkinData(unsigned int length, unsigned char *data){
//Cannot handle any length over 114
if (length > 114) {
length = 114;
}
radioSendData(tactile_src_addr, 0, CMD_TACTILE, length, data, 0);
//data = data + length/2 - 1;
//data[0] = rx_idx;
//radioSendData(RADIO_DST_ADDR, 0, CMD_TACTILE, length/2 +1, data, 0);
}
unsigned char cmdEraseSectors(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame, unsigned int src_addr){
unsigned int numSamples = frame[0] + (frame[1] << 8);
telemErase(numSamples);
//Send confirmation packet; this only happens when flash erase is completed.
radioSendData(src_addr, 0, CMD_ERASE_SECTORS, length, frame, 0);
LED_RED = ~LED_RED;
return 1;
}
unsigned char cmdZeroPos(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame, unsigned int src_addr) {
long motor_count[2];
motor_count[0] = pidObjs[0].p_state;
motor_count[1] = pidObjs[1].p_state;
radioSendData(src_addr, status, CMD_ZERO_POS,
sizeof(motor_count), (unsigned char *)motor_count, 0);
pidZeroPos(0);
pidZeroPos(1);
return 1;
}
unsigned char cmdEraseSectors(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame){
unsigned int numSamples = frame[0] + (frame[1] << 8);
telemErase(numSamples);
//Send confirmation packet; this only happens when flash erase is completed.
//Note that the destination is the hard-coded RADIO_DST_ADDR
//todo : extract the destination address properly.
radioSendData(RADIO_DST_ADDR, 0, CMD_ERASE_SECTORS, length, frame, 0);
LED_RED = ~LED_RED;
return 1;
}
// send robot info when queried
unsigned char cmdWhoAmI(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
unsigned char i, string_length; unsigned char *version_string;
// maximum string length to avoid packet size limit
version_string = (unsigned char*)versionGetString();
i = 0;
while((i < 127) && version_string[i] != '\0') {
i++;
}
string_length=i;
radioSendData(RADIO_DST_ADDR, status, CMD_WHO_AM_I, //TODO: Robot should respond to source of query, not hardcoded address
string_length, version_string, 0);
return 1; //success
}
unsigned char cmdGetAMSPos(unsigned char type, unsigned char status,
unsigned char length, unsigned char *frame, unsigned int src_addr) {
long motor_count[2];
motor_count[0] = pidObjs[0].p_state;
motor_count[1] = pidObjs[1].p_state;
// motor_count[0] = encPos[0].pos;
// motor_count[1] = encPos[1].pos;
radioSendData(src_addr, status, CMD_GET_AMS_POS, //TODO: Robot should respond to source of query, not hardcoded address
sizeof(motor_count), (unsigned char *)motor_count, 0);
return 1;
}
unsigned char cmdSetPIDGains(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
int Kp, Ki, Kd, Kaw, ff;
int idx = 0;
Kp = frame[idx] + (frame[idx+1] << 8); idx+=2;
Ki = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kd = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2;
ff = frame[idx] + (frame[idx+1] << 8); idx+=2;
pidSetGains(0,Kp,Ki,Kd,Kaw, ff);
Kp = frame[idx] + (frame[idx+1] << 8); idx+=2;
Ki = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kd = frame[idx] + (frame[idx+1] << 8); idx+=2;
Kaw = frame[idx] + (frame[idx+1] << 8); idx+=2;
ff = frame[idx] + (frame[idx+1] << 8); idx+=2;
pidSetGains(1,Kp,Ki,Kd,Kaw, ff);
radioSendData(RADIO_DST_ADDR, status, CMD_SET_PID_GAINS, 20, frame, 0); //TODO: Robot should respond to source of query, not hardcoded address
//Send confirmation packet
// WARNING: Will fail at high data throughput
//radioConfirmationPacket(RADIO_DEST_ADDR, CMD_SET_PID_GAINS, status, 20, frame);
return 1; //success
}
/**
Send a frame that has been stored for a child node. This is meant
to be called immediately upon receiving a packet from a sleeping child node.
*/
void macSendStoredFrame(u16 addr)
{
# if ((NODE_TYPE == ROUTER || NODE_TYPE == COORD) && NODE_SLEEP )
{
u8 i;
// See if a frame is stored for this node
for (i=0;i<STORED_FRAMES;i++)
{
if (storedFrames[i].len &&
storedFrames[i].addr == addr)
{
#if DEBUG
debugMsgStr("\r\nMac sending postponed frame \r\n");
#endif
// Send this frame, remove checksum
radioSendData(storedFrames[i].len, storedFrames[i].buf);
// Clear out this frame
storedFrames[i].len = 0;
}
}
}
# endif
}
//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;*/
}
void telemSendDataDelay(telemStruct_t* sample) {
delay_ms(5);
radioSendData(RADIO_DEST_ADDR, 0, CMD_FLASH_READBACK, telemPacketSize,
(unsigned char*) sample, 0 );
LED_2 = ~LED_2;
}
void telemSendDataDelay(telemStruct_t* sample, int delaytime_ms, unsigned int src_addr) {
radioSendData(src_addr, 0, CMD_FLASH_READBACK, telemPacketSize, (unsigned char *)sample, 0);
delay_ms(delaytime_ms); // allow radio transmission time
}
