diff --git a/include/CommunicationManager.h b/include/CommunicationManager.h index 65084cf..9e3cc22 100644 --- a/include/CommunicationManager.h +++ b/include/CommunicationManager.h @@ -85,6 +85,7 @@ private: Request& request, mavros_msgs::CommandInt::Response& response); void Check_In_Messages_and_Transfer_To_Server(); unsigned short Caculate_Checksum(std::string* frame); + void Send_multi_msg(); Mist::Xbee::SerialDevice serial_device_; Thread_Safe_Deque* in_messages_; ros::NodeHandle node_handle_; diff --git a/src/CommunicationManager.cpp b/src/CommunicationManager.cpp index fd45cf1..8cdf063 100644 --- a/src/CommunicationManager.cpp +++ b/src/CommunicationManager.cpp @@ -44,7 +44,7 @@ namespace Xbee //***************************************************************************** CommunicationManager::CommunicationManager(): START_DLIMITER(static_cast(0x7E)), - LOOP_RATE(50) /* 10 fps */ + LOOP_RATE(60) /* 10 fps */ { } @@ -173,6 +173,7 @@ void CommunicationManager::Run_In_Swarm_Mode() while (ros::ok()) { Check_In_Messages_and_Transfer_To_Topics(); + Send_multi_msg(); ros::spinOnce(); loop_rate.sleep(); } @@ -680,6 +681,12 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback( serial_device_.Send_Frame(multi_msgs_send_dict.at(sending_chunk_no)); } else */ + + //} +} + +void CommunicationManager::Send_multi_msg(){ + if( !( multi_msgs_send_dict.empty() ) ){ /*If the sent message chunk not the last message then send else clear the dict*/ if( (uint16_t)(multi_msgs_send_dict.size() ) - 1 == sending_chunk_no && (uint16_t)ack_received_dict.size() == (uint16_t)(no_of_dev)-1){ @@ -713,9 +720,8 @@ inline void CommunicationManager::Send_Mavlink_Message_Callback( } - //} -} +} //***************************************************************************** void CommunicationManager::Display_Drone_Type_and_Running_Mode(