From 7830235c78ead1d6e79df67ccaf6c2708935ca48 Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 7 Sep 2017 18:17:03 -0400 Subject: [PATCH 1/2] 8 Hz improved loop rate --- src/roscontroller.cpp | 74 ++++++++++++++++++++++--------------------- 1 file changed, 38 insertions(+), 36 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 1041ec7..6ecd042 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -195,7 +195,9 @@ void roscontroller::RosControllerRun() robot_id)) { ROS_INFO("[%i] Bytecode file found and set", robot_id); std::string standby_bo = Compile_bzz(stand_by) + ".bo"; - init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); + /*loop rate of ros*/ + ros::Rate loop_rate(15); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -220,15 +222,15 @@ void roscontroller::RosControllerRun() } const uint8_t shrt_id= 0xFF; float result; - if ( GetAPIRssi(shrt_id, result) ) - log<::iterator it = neighbours_pos_map.begin(); log << "," << neighbours_pos_map.size(); @@ -262,7 +264,7 @@ void roscontroller::RosControllerRun() << "," << (double)it->second.z; } log << std::endl;*/ - } + //} /*Set ROBOTS variable for barrier in .bzz from neighbours count*/ // no_of_robots=get_number_of_robots(); get_number_of_robots(); @@ -270,12 +272,12 @@ void roscontroller::RosControllerRun() /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF SCRIPT IS NOT graphform.bzz*/ static buzzvm_t VM = buzz_utility::get_vm(); - std::stringstream state_buff; //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1)); // buzzvm_gload(VM); // buzzobj_t graph_state = buzzvm_stack_at(VM, 1); // buzzvm_pop(VM); - // state_buff<< graph_state->s.value.str<<","; + std::stringstream state_buff; + // state_buff<< graph_state->s.value.str<<","; buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1)); buzzvm_gload(VM); buzzobj_t uav_state = buzzvm_stack_at(VM, 1); @@ -287,7 +289,7 @@ void roscontroller::RosControllerRun() // buzz_utility::set_robot_var(no_of_robots); /*Set no of robots for updates TODO only when not updating*/ // if(multi_msg) - updates_set_robots(no_of_robots); + //updates_set_robots(no_of_robots); // ROS_INFO("ROBOTS: %i , acutal : // %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1); get_xbee_status(); @@ -633,7 +635,7 @@ void roscontroller::prepare_msg_and_publish() delete[] out; delete[] payload_out_ptr; /*Check for updater message if present send*/ - if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && + /* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 && multi_msg) { uint8_t *buff_send = 0; @@ -675,7 +677,7 @@ void roscontroller::prepare_msg_and_publish() payload_pub.publish(update_packets); multi_msg = false; delete[] payload_64; - } + }*/ } /*--------------------------------------------------------------------------------- /Flight controller service call every step if there is a command set from bzz @@ -1260,22 +1262,22 @@ void roscontroller::get_xbee_status() bool result_bool; float result_float; const uint8_t all_ids = 0xFF; - if(GetDequeFull(result_bool)) - { - buzzuav_closures::set_deque_full(result_bool); - } - if(GetRssi(result_float)) - { - buzzuav_closures::set_rssi(result_float); - } - if(GetRawPacketLoss(all_ids, result_float)) - { - buzzuav_closures::set_raw_packet_loss(result_float); - } - if(GetFilteredPacketLoss(all_ids, result_float)) - { - buzzuav_closures::set_filtered_packet_loss(result_float); - } + //if(GetDequeFull(result_bool)) + //{ + // buzzuav_closures::set_deque_full(result_bool); + //} + //if(GetRssi(result_float)) + //{ + // buzzuav_closures::set_rssi(result_float); + //} + //if(GetRawPacketLoss(all_ids, result_float)) + //{ + // buzzuav_closures::set_raw_packet_loss(result_float); + //} + //if(GetFilteredPacketLoss(all_ids, result_float)) + //{ + // buzzuav_closures::set_filtered_packet_loss(result_float); + //} // This part needs testing since it can overload the xbee module /* * if(GetAPIRssi(all_ids, result_float)) From b41a086ede24c213a8264588f36a4403a87b3efa Mon Sep 17 00:00:00 2001 From: vivek-shankar Date: Thu, 7 Sep 2017 18:42:26 -0400 Subject: [PATCH 2/2] Looprate setting bug fix. --- src/roscontroller.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 6ecd042..342b0ed 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -197,7 +197,7 @@ void roscontroller::RosControllerRun() std::string standby_bo = Compile_bzz(stand_by) + ".bo"; //init_update_monitor(bcfname.c_str(), standby_bo.c_str()); /*loop rate of ros*/ - ros::Rate loop_rate(15); + ros::Rate loop_rate(BUZZRATE); /////////////////////////////////////////////////////// // MAIN LOOP ////////////////////////////////////////////////////// @@ -295,8 +295,6 @@ void roscontroller::RosControllerRun() get_xbee_status(); /*run once*/ ros::spinOnce(); - /*loop rate of ros*/ - ros::Rate loop_rate(BUZZRATE); loop_rate.sleep(); if (fcu_timeout <= 0) buzzuav_closures::rc_call(mavros_msgs::CommandCode::NAV_LAND);