// 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; }