8 Hz improved loop rate
This commit is contained in:
parent
dc01484105
commit
7830235c78
|
@ -195,7 +195,9 @@ void roscontroller::RosControllerRun()
|
||||||
robot_id)) {
|
robot_id)) {
|
||||||
ROS_INFO("[%i] Bytecode file found and set", robot_id);
|
ROS_INFO("[%i] Bytecode file found and set", robot_id);
|
||||||
std::string standby_bo = Compile_bzz(stand_by) + ".bo";
|
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
|
// MAIN LOOP
|
||||||
//////////////////////////////////////////////////////
|
//////////////////////////////////////////////////////
|
||||||
|
@ -220,14 +222,14 @@ void roscontroller::RosControllerRun()
|
||||||
}
|
}
|
||||||
const uint8_t shrt_id= 0xFF;
|
const uint8_t shrt_id= 0xFF;
|
||||||
float result;
|
float result;
|
||||||
if ( GetAPIRssi(shrt_id, result) )
|
//if ( GetAPIRssi(shrt_id, result) )
|
||||||
log<<result<<",";
|
// log<<result<<",";
|
||||||
else
|
//else
|
||||||
log<<"0,";
|
// log<<"0,";
|
||||||
if ( GetRawPacketLoss(shrt_id, result) )
|
// if ( GetRawPacketLoss(shrt_id, result) )
|
||||||
log<<result<<",";
|
// log<<result<<",";
|
||||||
else
|
//else
|
||||||
log<<"0,";
|
// log<<"0,";
|
||||||
if ( GetFilteredPacketLoss(shrt_id, result) )
|
if ( GetFilteredPacketLoss(shrt_id, result) )
|
||||||
log<<result<<",";
|
log<<result<<",";
|
||||||
else
|
else
|
||||||
|
@ -236,7 +238,7 @@ void roscontroller::RosControllerRun()
|
||||||
neighbours_pos_publisher();
|
neighbours_pos_publisher();
|
||||||
send_MPpayload();
|
send_MPpayload();
|
||||||
/*Check updater state and step code*/
|
/*Check updater state and step code*/
|
||||||
update_routine(bcfname.c_str(), dbgfname.c_str());
|
// update_routine(bcfname.c_str(), dbgfname.c_str());
|
||||||
/*Log In Msg queue size*/
|
/*Log In Msg queue size*/
|
||||||
log<<(int)buzz_utility::get_inmsg_size()<<",";
|
log<<(int)buzz_utility::get_inmsg_size()<<",";
|
||||||
/*Step buzz script */
|
/*Step buzz script */
|
||||||
|
@ -246,8 +248,8 @@ void roscontroller::RosControllerRun()
|
||||||
/*call flight controler service to set command long*/
|
/*call flight controler service to set command long*/
|
||||||
flight_controller_service_call();
|
flight_controller_service_call();
|
||||||
/*Set multi message available after update*/
|
/*Set multi message available after update*/
|
||||||
if (get_update_status())
|
//if (get_update_status())
|
||||||
{
|
//{
|
||||||
/* set_read_update_status();
|
/* set_read_update_status();
|
||||||
multi_msg = true;
|
multi_msg = true;
|
||||||
log << cur_pos.latitude << "," << cur_pos.longitude << ","
|
log << cur_pos.latitude << "," << cur_pos.longitude << ","
|
||||||
|
@ -262,7 +264,7 @@ void roscontroller::RosControllerRun()
|
||||||
<< "," << (double)it->second.z;
|
<< "," << (double)it->second.z;
|
||||||
}
|
}
|
||||||
log << std::endl;*/
|
log << std::endl;*/
|
||||||
}
|
//}
|
||||||
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
|
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
|
||||||
// no_of_robots=get_number_of_robots();
|
// no_of_robots=get_number_of_robots();
|
||||||
get_number_of_robots();
|
get_number_of_robots();
|
||||||
|
@ -270,11 +272,11 @@ void roscontroller::RosControllerRun()
|
||||||
/*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF
|
/*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF
|
||||||
SCRIPT IS NOT graphform.bzz*/
|
SCRIPT IS NOT graphform.bzz*/
|
||||||
static buzzvm_t VM = buzz_utility::get_vm();
|
static buzzvm_t VM = buzz_utility::get_vm();
|
||||||
std::stringstream state_buff;
|
|
||||||
//buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
|
//buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
|
||||||
// buzzvm_gload(VM);
|
// buzzvm_gload(VM);
|
||||||
// buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
|
// buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
|
||||||
// buzzvm_pop(VM);
|
// buzzvm_pop(VM);
|
||||||
|
std::stringstream state_buff;
|
||||||
// state_buff<< graph_state->s.value.str<<",";
|
// state_buff<< graph_state->s.value.str<<",";
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
|
||||||
buzzvm_gload(VM);
|
buzzvm_gload(VM);
|
||||||
|
@ -287,7 +289,7 @@ void roscontroller::RosControllerRun()
|
||||||
// buzz_utility::set_robot_var(no_of_robots);
|
// buzz_utility::set_robot_var(no_of_robots);
|
||||||
/*Set no of robots for updates TODO only when not updating*/
|
/*Set no of robots for updates TODO only when not updating*/
|
||||||
// if(multi_msg)
|
// if(multi_msg)
|
||||||
updates_set_robots(no_of_robots);
|
//updates_set_robots(no_of_robots);
|
||||||
// ROS_INFO("ROBOTS: %i , acutal :
|
// ROS_INFO("ROBOTS: %i , acutal :
|
||||||
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
|
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
|
||||||
get_xbee_status();
|
get_xbee_status();
|
||||||
|
@ -633,7 +635,7 @@ void roscontroller::prepare_msg_and_publish()
|
||||||
delete[] out;
|
delete[] out;
|
||||||
delete[] payload_out_ptr;
|
delete[] payload_out_ptr;
|
||||||
/*Check for updater message if present send*/
|
/*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)
|
multi_msg)
|
||||||
{
|
{
|
||||||
uint8_t *buff_send = 0;
|
uint8_t *buff_send = 0;
|
||||||
|
@ -675,7 +677,7 @@ void roscontroller::prepare_msg_and_publish()
|
||||||
payload_pub.publish(update_packets);
|
payload_pub.publish(update_packets);
|
||||||
multi_msg = false;
|
multi_msg = false;
|
||||||
delete[] payload_64;
|
delete[] payload_64;
|
||||||
}
|
}*/
|
||||||
}
|
}
|
||||||
/*---------------------------------------------------------------------------------
|
/*---------------------------------------------------------------------------------
|
||||||
/Flight controller service call every step if there is a command set from bzz
|
/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;
|
bool result_bool;
|
||||||
float result_float;
|
float result_float;
|
||||||
const uint8_t all_ids = 0xFF;
|
const uint8_t all_ids = 0xFF;
|
||||||
if(GetDequeFull(result_bool))
|
//if(GetDequeFull(result_bool))
|
||||||
{
|
//{
|
||||||
buzzuav_closures::set_deque_full(result_bool);
|
// buzzuav_closures::set_deque_full(result_bool);
|
||||||
}
|
//}
|
||||||
if(GetRssi(result_float))
|
//if(GetRssi(result_float))
|
||||||
{
|
//{
|
||||||
buzzuav_closures::set_rssi(result_float);
|
// buzzuav_closures::set_rssi(result_float);
|
||||||
}
|
//}
|
||||||
if(GetRawPacketLoss(all_ids, result_float))
|
//if(GetRawPacketLoss(all_ids, result_float))
|
||||||
{
|
//{
|
||||||
buzzuav_closures::set_raw_packet_loss(result_float);
|
// buzzuav_closures::set_raw_packet_loss(result_float);
|
||||||
}
|
//}
|
||||||
if(GetFilteredPacketLoss(all_ids, result_float))
|
//if(GetFilteredPacketLoss(all_ids, result_float))
|
||||||
{
|
//{
|
||||||
buzzuav_closures::set_filtered_packet_loss(result_float);
|
// buzzuav_closures::set_filtered_packet_loss(result_float);
|
||||||
}
|
//}
|
||||||
// This part needs testing since it can overload the xbee module
|
// This part needs testing since it can overload the xbee module
|
||||||
/*
|
/*
|
||||||
* if(GetAPIRssi(all_ids, result_float))
|
* if(GetAPIRssi(all_ids, result_float))
|
||||||
|
|
Loading…
Reference in New Issue