C++ (Cpp) DataFrame::setFooter Examples

Example #1

File: sbg_ins_ig500n.cpp Project: kritchie/SBG_IG-500N

// TODO : Multiple return statements, fix this
std::shared_ptr<SBGOutputFrame> SBGInsIg500N::getNextFrame()
{
	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 **************//
	if ( RS232_PollComport(port, sync_byte, 1 ) == 0 ){
		return 0;
	}
	
	if( sync_byte[0] != SYNCX_VALUE){
		 ROS_WARN("Comm line not sync");
		 return 0;
	}
	   
	//************* HEADER *************//
	if( RS232_PollComport(port,header_temp,HEADER_LENGTH-1) == 0 ){ // Read 4 bytes
		return 0;
	}
	
	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 = 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)];
	frame.setCMD(command);

	//************* DATA *************//
	u_int8_t temp_buffer[1];
	u_int16_t data_length = ((msb_value << 8 )|lsb_value);
	
	ROS_INFO("Data Length:");
	ROS_INFO(std::to_string((int)data_length).c_str());
	
	u_int8_t* data_buffer = new u_int8_t(data_length);
	int read = 0, temp;
	while ( read < data_length ){
		temp = RS232_PollComport(port,temp_buffer,1);
		if( temp == 1 ){
			data_buffer[read] = temp_buffer[0];
			read += temp;
		}
	}
	
	frame.setData(data_buffer);

	//************* FOOTER *************//
	u_int8_t footer_buffer[FOOTER_LENGTH];
	if( RS232_PollComport(port,footer_buffer,FOOTER_LENGTH) == 0 ){ // Read 3 bytes
		return 0;
	}
	if ( footer_buffer[FOOTER_LENGTH-1] != ETX_VALUE )
		return 0;
	frame.setFooter(footer_buffer);

	//************* 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){
		parseCommand(frame, resultFrame);
	} else {
		ROS_WARN(" Wrong CRC value ");
		return 0;
	}
	delete data_buffer;
	if ( resultFrame != 0 || resultFrame != nullptr )
		return resultFrame;
	else 
		return 0;
}