/** @author Reid Kersey <*****@*****.**> 678-736-0899 @brief This function waits a set interval to receive an ACK packet from the Telemetry computer. @param device This is a sturcture of settings for the wireless device */ int waitForResponse(struct WirelessPins *device){ char ack[50]; struct timeval timeout = { 0, WIRELESS_TIMEOUT}; fd_set set; FD_ZERO(&set); FD_SET(devicefd, &set); if(!select(1, &set, NULL, NULL, &timeout) == 1) { logMessage("Wireless Read Timeout", 2); return 0; } int result=fscanf(wirelessFile,"%s",ack); if(result==EOF){ logMessage("Wireless read error", 2); return 0; } if(parseACK(ack, device) != 0){ return 1; }else{ return 0; } }
int SBGInsIg500N::getNextAck() { DataFrame frame; // Used to parse the data read from the RS-232 port std::shared_ptr<SBGOutputFrame> resultFrame; u_int8_t header_buffer[HEADER_LENGTH]; u_int8_t header_temp[HEADER_LENGTH-1]; u_int8_t sync_byte[1]; //************SYNC BYTE***********// while( sync_byte[0] != SYNCX_VALUE ) while( RS232_PollComport(port, sync_byte, 1 )== 0) continue; //********** HEADER *************// u_int8_t tmp_buf[HEADER_LENGTH-1]; int read = 0, temp; while ( read < (HEADER_LENGTH-1) ){ temp = RS232_PollComport(port,tmp_buf,1); if( temp == 1 ){ header_temp[read] = tmp_buf[0]; read += temp; } } header_buffer[0] = sync_byte[0]; for(int i = 0; i < HEADER_LENGTH-1; i++){ header_buffer[i+1] = header_temp[i]; } frame.setHeader(header_buffer); //************* PARSE HEADER *************// u_int8_t command_ack = header_buffer[(STX_LENGTH+SYNCX_LENGTH)]; u_int8_t msb_value = header_buffer[(STX_LENGTH+SYNCX_LENGTH+CMD_LENGTH)]; u_int8_t lsb_value = header_buffer[(STX_LENGTH+SYNCX_LENGTH+CMD_LENGTH+LEN_MSB_LENGTH)]; if(command_ack != DEFAULT_ACK){ ROS_INFO("Not an acknowledge frame"); return 1; } //*************** DATA******************// u_int8_t data_temp[1]; u_int16_t data_length = ((msb_value << 8 )|lsb_value); if(data_length != ACK_LENGTH){ ROS_INFO("The data length isn't equal to 0x01"); return 1; } u_int8_t* data_buffer= new u_int8_t(ACK_LENGTH); while ( RS232_PollComport(port, data_buffer, data_length) == 0 ) continue; //**************** FOOTER ************// u_int8_t footer_buffer[FOOTER_LENGTH]; if(RS232_PollComport(port, footer_buffer, FOOTER_LENGTH) == 0){ return 1; } if(footer_buffer[FOOTER_LENGTH-1] != ETX_VALUE){ return 1; } //***************** CRC *************// u_int16_t crc = ((footer_buffer[0]<<8)|footer_buffer[1]); u_int8_t crc_buffer [HEADER_LENGTH+data_length+FOOTER_LENGTH]; for(int i = 0; i < HEADER_LENGTH; i++){ crc_buffer[i] = header_buffer[i]; } for(int i = 0; i < data_length; i++){ crc_buffer[HEADER_LENGTH + i] = data_buffer[i]; } for(int i = 0; i < FOOTER_LENGTH; i++){ crc_buffer[HEADER_LENGTH + data_length + i] = footer_buffer[i]; } u_int16_t calculated_crc = SBGFrame::calcCRC(crc_buffer,data_length); //************* PARSE COMMAND *************// if(crc == calculated_crc){ parseACK(frame); } else{ ROS_INFO("Error with the crc"); return 1; } delete data_buffer; return 0; }