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 }