From 9177700c2662b58fb39c867e39c9ec8fdbb2aae9 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Tue, 21 Feb 2017 19:27:01 -0500 Subject: [PATCH] some crash fixes --- src/CommunicationManager.cpp | 2 +- src/PacketsHandler.cpp | 75 +++++++++++++++++------------------- 2 files changed, 36 insertions(+), 41 deletions(-) diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp index 1959db8..1271f89 100644 --- a/src/CommunicationManager.cpp +++ b/src/CommunicationManager.cpp @@ -430,7 +430,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() // inline void CommunicationManager::Send_Mavlink_Message_Callback( const mavros_msgs::Mavlink::ConstPtr& mavlink_msg) { - //std::cout << "Received Message From Buzz" << std::endl; // TO DO delete + std::cout << "Received Message From Buzz" << std::endl; // TO DO delete packets_handler_.Handle_Mavlink_Message(mavlink_msg); } diff --git a/src/PacketsHandler.cpp b/src/PacketsHandler.cpp index bbbd91f..4b0cb45 100644 --- a/src/PacketsHandler.cpp +++ b/src/PacketsHandler.cpp @@ -48,8 +48,6 @@ bool PacketsHandler::Init(SerialDevice* serial_device, if (!Load_Database_Addresses()) return false; - // TO DO node dicov - Send_SL_and_SH_Commands(); in_packets_ = in_packets; @@ -106,7 +104,7 @@ void PacketsHandler::Handle_Mavlink_Message(const mavros_msgs::Mavlink::ConstPtr fragmented_packet->at(i)->append(serialized_packet->c_str() + offset, NBR_of_bytes); offset += NBR_of_bytes; } - std::cout << "[Debug: ] Multi packets received from buzz with framgents: "<msgid, fragmented_packet}); } else if (serialized_packet->size() < XBEE_NETWORK_MTU) @@ -126,7 +124,7 @@ void PacketsHandler::Process_Fragment(std::shared_ptr fragment) uint16_t offset = static_cast( static_cast(static_cast(fragment->at(4))) << 8 | static_cast(static_cast(fragment->at(5)))); - std::cout << "[Debug: ] Fragment received with id"<< (int)fragment_ID<< std::endl; + assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address); if (assembly_map_it_ != packets_assembly_map_.end()) @@ -166,7 +164,7 @@ void PacketsHandler::Process_Fragment(std::shared_ptr fragment) //***************************************************************************** -void PacketsHandler::Insert_Fragment_In_Packet_Buffer(std::string* buffer, const char* fragment, const uint16_t offset, const std::size_t length) // TO DO delete length +void PacketsHandler::Insert_Fragment_In_Packet_Buffer(std::string* buffer, const char* fragment, const uint16_t offset, const std::size_t length) { if (offset >= buffer->size()) buffer->append(fragment + FRAGMENT_HEADER_SIZE, length - FRAGMENT_HEADER_SIZE); @@ -176,20 +174,22 @@ void PacketsHandler::Insert_Fragment_In_Packet_Buffer(std::string* buffer, const //***************************************************************************** -void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptr frame) // TO DO change useless std::shared_ptr frame args to ->c_str() +void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptr frame) { uint8_t packet_ID = frame->at(12); uint8_t node_8_bits_address = frame->at(13); if (frame->at(11) == 'A') { - /*std::lock_guard guard(mutex_); + mutex_.lock(); connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address); if (connected_network_nodes_it_ == connected_network_nodes_.end()) { + mutex_.unlock(); Add_New_Node_To_Network(node_8_bits_address); + mutex_.lock(); connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address); } @@ -202,7 +202,9 @@ void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptrsize(); i++) fragments_indexes_to_transmit_.insert(frame->at(i)); } - }*/ + } + + mutex_.unlock(); } else if (frame->at(11) == 'P') { @@ -247,7 +249,7 @@ void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptrSend_Frame(Ack_frame); usleep(delay_interframes_); @@ -280,9 +282,9 @@ void PacketsHandler::Process_Command_Response(const char* command_response) uint64_t new_node_address = 0; std::lock_guard guard(mutex_); - if (command_response[2] == static_cast(0)) // TO DO check this + if (command_response[2] == static_cast(0)) { - new_node_address = static_cast( // TO DO correct this + new_node_address = static_cast( static_cast(command_response[5]) << 56 | static_cast(command_response[6]) << 48 | static_cast(command_response[7]) << 40 | @@ -302,7 +304,7 @@ void PacketsHandler::Process_Command_Response(const char* command_response) } else if (command_response[0] == 'S' && command_response[1] == 'H') { - if (command_response[2] == static_cast(0)) // TO DO check this + if (command_response[2] == static_cast(0)) { loaded_SH_ = true; @@ -312,7 +314,7 @@ void PacketsHandler::Process_Command_Response(const char* command_response) } else if (command_response[0] == 'S' && command_response[1] == 'L') { - if (command_response[2] == static_cast(0)) // TO DO check this + if (command_response[2] == static_cast(0)) { loaded_SL_ = true; @@ -352,7 +354,7 @@ void PacketsHandler::Serialize_Mavlink_Message(const mavros_msgs::Mavlink::Const //***************************************************************************** void PacketsHandler::Insert_Fragment_Header(bool single_fragment, std::shared_ptr fragment, const uint8_t packet_ID, - const uint8_t fragment_ID, const uint16_t offset) + const uint8_t fragment_ID, const uint16_t offset) { if (!single_fragment) { @@ -415,7 +417,7 @@ void PacketsHandler::Process_Out_Packets() { Process_Out_Standard_Messages(); out_packet = out_packets_.Pop_Front(); - std::cout << "Multi packets sending" << std::endl; + Send_Packet(out_packet); } } @@ -429,23 +431,19 @@ void PacketsHandler::Send_Packet(const Out_Packet_S& packet) std::vector frames; Init_Network_Nodes_For_New_Transmission(packet.packet_ID_, &frames, packet.packet_buffer_); + current_processed_packet_ID_ = packet.packet_ID_; - std::clock_t elapsed_time = std::clock(); - - //while (NBR_of_transmission <= optimum_MT_NBR_ && !Check_Packet_Transmitted_To_All_Nodes()) - //{ - //NBR_of_transmission++; + std::clock_t start_time = std::clock(); + while (std::clock() - start_time <= MAX_TIME_TO_SEND_PACKET && !Check_Packet_Transmitted_To_All_Nodes()) + { + NBR_of_transmission++; Transmit_Fragments(frames); - - // TO DO usleep(flow_control_time) - // TO DO clear fragments_IDs_to_transmit_set Send_End_Of_Packet_Ping(packet.packet_ID_, packet.packet_buffer_->size()); - // TO DO sleep after ping - //} + usleep(500 * 1000); + } - elapsed_time = std::clock() - elapsed_time; - Adjust_Optimum_MT_Number(elapsed_time, NBR_of_transmission); + Adjust_Optimum_MT_Number(std::clock() - start_time, NBR_of_transmission); } @@ -470,6 +468,7 @@ bool PacketsHandler::Load_Database_Addresses() { const std::string FILE_PATH = "/home/ubuntu/ROS_WS/src/xbeemav/Resources/database.xml"; + if (!boost::filesystem::exists(FILE_PATH)) { std::cout << "database.xml Not Found." << std::endl; @@ -574,7 +573,7 @@ void PacketsHandler::Init_Network_Nodes_For_New_Transmission(const uint8_t packe std::lock_guard guard(mutex_); fragments_indexes_to_transmit_.clear(); - for (auto it : connected_network_nodes_) + for (auto& it : connected_network_nodes_) it.second = false; current_processed_packet_ID_ = packet_ID; @@ -598,23 +597,19 @@ void PacketsHandler::Transmit_Fragments(const std::vector& frames) serial_device_->Send_Frame(frames.at(index)); usleep(delay_interframes_); } - + + fragments_indexes_to_transmit_.clear(); } //***************************************************************************** void PacketsHandler::Adjust_Optimum_MT_Number(const std::clock_t elapsed_time, - const std::size_t NBR_of_transmission) -{ - if (NBR_of_transmission < optimum_MT_NBR_) - optimum_MT_NBR_ = NBR_of_transmission; - else if (NBR_of_transmission == optimum_MT_NBR_) - { - std::lock_guard guard(mutex_); - - if (elapsed_time <= MAX_TIME_TO_SEND_PACKET && fragments_indexes_to_transmit_.size() > 0) - optimum_MT_NBR_++; - } + const std::size_t NBR_of_transmission) +{ + if (NBR_of_transmission > 1 && elapsed_time < MAX_TIME_TO_SEND_PACKET) + delay_interframes_ += 5000; + else if (NBR_of_transmission == 1 && delay_interframes_ >= 5000) + delay_interframes_ -= 5000; }