Ejemplo n.º 1
0
/**
@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;
	}
}
Ejemplo n.º 2
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;
}