Spiri
/
services-ros1-xbee_mav
12
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

some crash fixes

This commit is contained in:
vivek-shankar 2017-02-21 19:27:01 -05:00
parent db37ca34d1
commit 9177700c26
2 changed files with 36 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/CommunicationManager.cpp
View File

@ -430,7 +430,7 @@ inline void CommunicationManager::Check_In_Messages_and_Transfer_To_Server() //
inline void CommunicationManager::Send_Mavlink_Message_Callback( inline void CommunicationManager::Send_Mavlink_Message_Callback(
const mavros_msgs::Mavlink::ConstPtr& mavlink_msg) 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); packets_handler_.Handle_Mavlink_Message(mavlink_msg);
} }

75
src/PacketsHandler.cpp
View File

@ -48,8 +48,6 @@ bool PacketsHandler::Init(SerialDevice* serial_device,
if (!Load_Database_Addresses()) if (!Load_Database_Addresses())
return false; return false;
// TO DO node dicov
Send_SL_and_SH_Commands(); Send_SL_and_SH_Commands();
in_packets_ = in_packets; 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); fragmented_packet->at(i)->append(serialized_packet->c_str() + offset, NBR_of_bytes);
offset += NBR_of_bytes; offset += NBR_of_bytes;
} }
std::cout << "[Debug: ] Multi packets received from buzz with framgents: "<<NBR_of_fragments << std::endl;
out_packets_.Push_Back({mavlink_msg->msgid, fragmented_packet}); out_packets_.Push_Back({mavlink_msg->msgid, fragmented_packet});
} }
else if (serialized_packet->size() < XBEE_NETWORK_MTU) else if (serialized_packet->size() < XBEE_NETWORK_MTU)
@ -126,7 +124,7 @@ void PacketsHandler::Process_Fragment(std::shared_ptr<std::string> fragment)
uint16_t offset = static_cast<uint16_t>( uint16_t offset = static_cast<uint16_t>(
static_cast<uint16_t>(static_cast<unsigned char>(fragment->at(4))) << 8 | static_cast<uint16_t>(static_cast<unsigned char>(fragment->at(4))) << 8 |
static_cast<uint16_t>(static_cast<unsigned char>(fragment->at(5)))); static_cast<uint16_t>(static_cast<unsigned char>(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); assembly_map_it_ = packets_assembly_map_.find(node_8_bits_address);
if (assembly_map_it_ != packets_assembly_map_.end()) if (assembly_map_it_ != packets_assembly_map_.end())
@ -166,7 +164,7 @@ void PacketsHandler::Process_Fragment(std::shared_ptr<std::string> 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()) if (offset >= buffer->size())
buffer->append(fragment + FRAGMENT_HEADER_SIZE, length - FRAGMENT_HEADER_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<std::string> frame) // TO DO change useless std::shared_ptr<std::string> frame args to ->c_str() void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptr<std::string> frame)
{ {
uint8_t packet_ID = frame->at(12); uint8_t packet_ID = frame->at(12);
uint8_t node_8_bits_address = frame->at(13); uint8_t node_8_bits_address = frame->at(13);
if (frame->at(11) == 'A') if (frame->at(11) == 'A')
{ {
/*std::lock_guard<std::mutex> guard(mutex_); mutex_.lock();
connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address); connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address);
if (connected_network_nodes_it_ == connected_network_nodes_.end()) if (connected_network_nodes_it_ == connected_network_nodes_.end())
{ {
mutex_.unlock();
Add_New_Node_To_Network(node_8_bits_address); Add_New_Node_To_Network(node_8_bits_address);
mutex_.lock();
connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address); connected_network_nodes_it_ = connected_network_nodes_.find(node_8_bits_address);
} }
@ -202,7 +202,9 @@ void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptr<std::string
for (uint8_t i = 14; i < frame->size(); i++) for (uint8_t i = 14; i < frame->size(); i++)
fragments_indexes_to_transmit_.insert(frame->at(i)); fragments_indexes_to_transmit_.insert(frame->at(i));
} }
}*/ }
mutex_.unlock();
} }
else if (frame->at(11) == 'P') else if (frame->at(11) == 'P')
{ {
@ -247,7 +249,7 @@ void PacketsHandler::Process_Ping_Or_Acknowledgement(std::shared_ptr<std::string
} }
std::string Ack_frame; std::string Ack_frame;
Generate_Transmit_Request_Frame(Acknowledgement.c_str(), &Ack_frame, Acknowledgement.size()); // TO Do send as unicast Generate_Transmit_Request_Frame(Acknowledgement.c_str(), &Ack_frame, Acknowledgement.size());
serial_device_->Send_Frame(Ack_frame); serial_device_->Send_Frame(Ack_frame);
usleep(delay_interframes_); usleep(delay_interframes_);
@ -280,9 +282,9 @@ void PacketsHandler::Process_Command_Response(const char* command_response)
uint64_t new_node_address = 0; uint64_t new_node_address = 0;
std::lock_guard<std::mutex> guard(mutex_); std::lock_guard<std::mutex> guard(mutex_);
if (command_response[2] == static_cast<unsigned char>(0)) // TO DO check this if (command_response[2] == static_cast<unsigned char>(0))
{ {
new_node_address = static_cast<uint64_t>( // TO DO correct this new_node_address = static_cast<uint64_t>(
static_cast<unsigned char>(command_response[5]) << 56 | static_cast<unsigned char>(command_response[5]) << 56 |
static_cast<unsigned char>(command_response[6]) << 48 | static_cast<unsigned char>(command_response[6]) << 48 |
static_cast<unsigned char>(command_response[7]) << 40 | static_cast<unsigned char>(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') else if (command_response[0] == 'S' && command_response[1] == 'H')
{ {
if (command_response[2] == static_cast<unsigned char>(0)) // TO DO check this if (command_response[2] == static_cast<unsigned char>(0))
{ {
loaded_SH_ = true; 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') else if (command_response[0] == 'S' && command_response[1] == 'L')
{ {
if (command_response[2] == static_cast<unsigned char>(0)) // TO DO check this if (command_response[2] == static_cast<unsigned char>(0))
{ {
loaded_SL_ = true; 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, void PacketsHandler::Insert_Fragment_Header(bool single_fragment,
std::shared_ptr<std::string> fragment, const uint8_t packet_ID, std::shared_ptr<std::string> 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) if (!single_fragment)
{ {
@ -415,7 +417,7 @@ void PacketsHandler::Process_Out_Packets()
{ {
Process_Out_Standard_Messages(); Process_Out_Standard_Messages();
out_packet = out_packets_.Pop_Front(); out_packet = out_packets_.Pop_Front();
std::cout << "Multi packets sending" << std::endl;
Send_Packet(out_packet); Send_Packet(out_packet);
} }
} }
@ -429,23 +431,19 @@ void PacketsHandler::Send_Packet(const Out_Packet_S& packet)
std::vector<std::string> frames; std::vector<std::string> frames;
Init_Network_Nodes_For_New_Transmission(packet.packet_ID_, &frames, packet.packet_buffer_); 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(); std::clock_t start_time = std::clock();
//while (NBR_of_transmission <= optimum_MT_NBR_ && !Check_Packet_Transmitted_To_All_Nodes())
//{
//NBR_of_transmission++;
while (std::clock() - start_time <= MAX_TIME_TO_SEND_PACKET && !Check_Packet_Transmitted_To_All_Nodes())
{
NBR_of_transmission++;
Transmit_Fragments(frames); 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()); 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(std::clock() - start_time, NBR_of_transmission);
Adjust_Optimum_MT_Number(elapsed_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"; const std::string FILE_PATH = "/home/ubuntu/ROS_WS/src/xbeemav/Resources/database.xml";
if (!boost::filesystem::exists(FILE_PATH)) if (!boost::filesystem::exists(FILE_PATH))
{ {
std::cout << "database.xml Not Found." << std::endl; 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<std::mutex> guard(mutex_); std::lock_guard<std::mutex> guard(mutex_);
fragments_indexes_to_transmit_.clear(); fragments_indexes_to_transmit_.clear();
for (auto it : connected_network_nodes_) for (auto& it : connected_network_nodes_)
it.second = false; it.second = false;
current_processed_packet_ID_ = packet_ID; current_processed_packet_ID_ = packet_ID;
@ -598,23 +597,19 @@ void PacketsHandler::Transmit_Fragments(const std::vector<std::string>& frames)
serial_device_->Send_Frame(frames.at(index)); serial_device_->Send_Frame(frames.at(index));
usleep(delay_interframes_); usleep(delay_interframes_);
} }
fragments_indexes_to_transmit_.clear();
} }
//***************************************************************************** //*****************************************************************************
void PacketsHandler::Adjust_Optimum_MT_Number(const std::clock_t elapsed_time, void PacketsHandler::Adjust_Optimum_MT_Number(const std::clock_t elapsed_time,
const std::size_t NBR_of_transmission) const std::size_t NBR_of_transmission)
{ {
if (NBR_of_transmission < optimum_MT_NBR_) if (NBR_of_transmission > 1 && elapsed_time < MAX_TIME_TO_SEND_PACKET)
optimum_MT_NBR_ = NBR_of_transmission; delay_interframes_ += 5000;
else if (NBR_of_transmission == optimum_MT_NBR_) else if (NBR_of_transmission == 1 && delay_interframes_ >= 5000)
{ delay_interframes_ -= 5000;
std::lock_guard<std::mutex> guard(mutex_);
if (elapsed_time <= MAX_TIME_TO_SEND_PACKET && fragments_indexes_to_transmit_.size() > 0)
optimum_MT_NBR_++;
}
} }