Spiri
/
ROSBuzz_MISTLab
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

merge fix

This commit is contained in:
dave 2017-04-20 23:33:56 -04:00
parent a14b753b7e 3e1653cdaa
commit 0bc4001542
6 changed files with 108 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/buzz_utility.h
View File

@ -49,7 +49,7 @@ int buzz_script_done();
int update_step_test(); int update_step_test();
uint16_t get_robotid(); int get_robotid();
buzzvm_t get_vm(); buzzvm_t get_vm();

3
include/roscontroller.h
View File

@ -70,6 +70,7 @@ private:
//std::map< int, buzz_utility::Pos_struct> pub_neigh_pos; //std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
int timer_step=0; int timer_step=0;
int robot_id=0; int robot_id=0;
std::string robot_name = "";
//int oldcmdID=0; //int oldcmdID=0;
int rc_cmd; int rc_cmd;
float fcu_timeout; float fcu_timeout;
@ -80,7 +81,7 @@ private:
/*tmp to be corrected*/ /*tmp to be corrected*/
uint8_t no_cnt=0; uint8_t no_cnt=0;
uint8_t old_val=0; uint8_t old_val=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name, robot_name; std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name;
std::string stream_client_name; std::string stream_client_name;
std::string relative_altitude_sub_name; std::string relative_altitude_sub_name;
std::string setpoint_nonraw; std::string setpoint_nonraw;

165
src/buzz_utility.cpp
View File

@ -51,6 +51,9 @@ namespace buzz_utility{
return out; return out;
} }
int get_robotid() {
return Robot_id;
}
/***************************************************/ /***************************************************/
/*Appends obtained messages to buzz in message Queue*/ /*Appends obtained messages to buzz in message Queue*/
/***************************************************/ /***************************************************/
@ -252,18 +255,10 @@ namespace buzz_utility{
int buzz_script_set(const char* bo_filename, int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id) { const char* bdbg_filename, int robot_id) {
//cout<<"bo file name"<<bo_filename;
/* Get hostname */
//char hstnm[30];
//char* hstnm ="M1003";
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm+4, NULL, 10);
cout << " Robot ID: " <<robot_id<< endl; cout << " Robot ID: " <<robot_id<< endl;
/* Reset the Buzz VM */ /* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
Robot_id =robot_id; Robot_id = robot_id;
VM = buzzvm_new((int)robot_id); VM = buzzvm_new((int)robot_id);
/* Get rid of debug info */ /* Get rid of debug info */
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
@ -287,69 +282,62 @@ namespace buzz_utility{
} }
fclose(fd); fclose(fd);
/* Read debug information */ /* Read debug information */
//if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
// buzzvm_destroy(&VM);
// buzzdebug_destroy(&DBG_INFO);
// perror(bdbg_filename);
// return 0;
//}
/* Set byte code */
//if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
// buzzvm_destroy(&VM);
// buzzdebug_destroy(&DBG_INFO);
// fprintf(stdout, "%s: Error loading Buzz script\n\n", bo_filename);
// return 0;
//}
/* Register hook functions */
/*if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error registering hooks\n\n", bo_filename); perror(bdbg_filename);
return 0; return 0;
}*/ }
/* Set byte code */
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error loading Buzz script\n\n", bo_filename);
return 0;
}
/* Register hook functions */
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
fprintf(stderr, "%s: Error registering hooks\n\n", bo_filename);
return 0;
}
/* Save bytecode file name */ /* Save bytecode file name */
//BO_FNAME = strdup(bo_filename); BO_FNAME = strdup(bo_filename);
/* Execute the global part of the script */ /* Execute the global part of the script */
//buzzvm_execute_script(VM); buzzvm_execute_script(VM);
/* Call the Init() function */ /* Call the Init() function */
//buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
/* All OK */ /* All OK */
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size); return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
} }
/****************************************/ /****************************************/
/*Sets a new update */ /*Sets a new update */
/****************************************/ /****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */ /* // Reset the Buzz VM
//char hstnm[30];
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm + 4, NULL, 10);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
/* Get rid of debug info */ // Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
/* Read debug information */ // Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename); perror(bdbg_filename);
return 0; return 0;
} }
/* Set byte code */ // Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0; return 0;
} }
/* Register hook functions */ // Register hook functions
if(buzz_register_hooks() != BUZZVM_STATE_READY) { if(buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -357,46 +345,40 @@ namespace buzz_utility{
return 0; return 0;
} }
/* Execute the global part of the script */ // Execute the global part of the script
buzzvm_execute_script(VM); buzzvm_execute_script(VM);
/* Call the Init() function */ // Call the Init() function
buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
/* All OK */ // All OK
return 1; */ return 1;
} }
/****************************************/ /****************************************/
/*Performs a initialization test */ /*Performs a initialization test */
/****************************************/ /****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
/* Get hostname */ /* // Reset the Buzz VM
//char hstnm[30];
//gethostname(hstnm, 30);
/* Make numeric id from hostname */
/* NOTE: here we assume that the hostname is in the format Mnn */
//int id = strtol(hstnm + 4, NULL, 10);
/* Reset the Buzz VM */
if(VM) buzzvm_destroy(&VM); if(VM) buzzvm_destroy(&VM);
VM = buzzvm_new(Robot_id); VM = buzzvm_new(Robot_id);
/* Get rid of debug info */ // Get rid of debug info
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO); if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
DBG_INFO = buzzdebug_new(); DBG_INFO = buzzdebug_new();
/* Read debug information */ // Read debug information
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) { if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
perror(bdbg_filename); perror(bdbg_filename);
return 0; return 0;
} }
/* Set byte code */ // Set byte code
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) { if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME); fprintf(stdout, "%s: Error loading Buzz script\n\n", BO_FNAME);
return 0; return 0;
} }
/* Register hook functions */ // Register hook functions
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) { if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM); buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO); buzzdebug_destroy(&DBG_INFO);
@ -404,12 +386,12 @@ namespace buzz_utility{
return 0; return 0;
} }
/* Execute the global part of the script */ // Execute the global part of the script
buzzvm_execute_script(VM); buzzvm_execute_script(VM);
/* Call the Init() function */ // Call the Init() function
buzzvm_function_call(VM, "init", 0); buzzvm_function_call(VM, "init", 0);
/* All OK */ // All OK
return 1; */ return 1;
} }
/****************************************/ /****************************************/
@ -425,31 +407,35 @@ namespace buzz_utility{
void check_swarm_members(const void* key, void* data, void* params) { void check_swarm_members(const void* key, void* data, void* params) {
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data; buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
int* status = (int*)params; int* status = (int*)params;
fprintf(stderr, "CHECKING SWARM MEMBERS\n");
if(*status == 3) return; if(*status == 3) return;
fprintf(stderr, "CHECKING SWARM MEMBERS:%i\n",buzzdarray_get(e->swarms, 0, uint16_t));
if(buzzdarray_size(e->swarms) != 1) { if(buzzdarray_size(e->swarms) != 1) {
fprintf(stderr, "Swarm list size is not 1\n"); fprintf(stderr, "Swarm list size is not 1\n");
*status = 3; *status = 3;
} }
else { else {
int sid = 1; int sid = 1;
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && if(!buzzdict_isempty(VM->swarms)) {
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
sid, fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
buzzdarray_get(e->swarms, 0, uint16_t)); sid,
*status = 3; buzzdarray_get(e->swarms, 0, uint16_t));
return; *status = 3;
} return;
sid = 2; }
if(*buzzdict_get(VM->swarms, &sid, uint8_t) && }
buzzdarray_get(e->swarms, 0, uint16_t) != sid) { if(buzzdict_size(VM->swarms)>1) {
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n", sid = 2;
sid, if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
buzzdarray_get(e->swarms, 0, uint16_t)); buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
*status = 3; fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
return; sid,
} buzzdarray_get(e->swarms, 0, uint16_t));
*status = 3;
return;
}
}
} }
} }
/*Step through the buzz script*/ /*Step through the buzz script*/
@ -471,21 +457,16 @@ namespace buzz_utility{
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
// usleep(10000); //buzzvm_process_outmsgs(VM); //--> done in out_msg_process() function called each step
//usleep(10000);
/*Print swarm*/ /*Print swarm*/
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); //buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
int SwarmSize = buzzdict_size(VM->swarmmembers)+1; //int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize); //fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
/* Check swarm state -- SOMETHING CRASHING HERE!! */ /* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
int status = 1; // int status = 1;
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status); // buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
/* if(status == 1 &&
buzzdict_size(VM->swarmmembers) < 9)
status = 2;*/
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
buzzvm_pushi(VM, status);
buzzvm_gstore(VM);
} }
/****************************************/ /****************************************/

1
src/buzzuav_closures.cpp
View File

@ -30,6 +30,7 @@ namespace buzzuav_closures{
int buzzros_print(buzzvm_t vm) { int buzzros_print(buzzvm_t vm) {
int i; int i;
char buffer [50] = ""; char buffer [50] = "";
sprintf(buffer,"%s [%i]", buffer, (int)buzz_utility::get_robotid());
for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) { for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
buzzvm_lload(vm, i); buzzvm_lload(vm, i);
buzzobj_t o = buzzvm_stack_at(vm, 1); buzzobj_t o = buzzvm_stack_at(vm, 1);

51
src/roscontroller.cpp
View File

@ -24,12 +24,12 @@ namespace rosbzz_node{
SetStreamRate(0, 10, 1); SetStreamRate(0, 10, 1);
/// Get Robot Id - wait for Xbee to be started /// Get Robot Id - wait for Xbee to be started
if(xbeeplugged) if(xbeeplugged)
GetRobotId(); GetRobotId();
else else
robot_id = strtol(robot_name.c_str()+4, NULL, 10); robot_id=strtol(robot_name.c_str() + 5, NULL, 10);;
setpoint_counter = 0; setpoint_counter = 0;
fcu_timeout = TIMEOUT; fcu_timeout = TIMEOUT;
home[0]=0;home[1]=0;home[2]=0; home[0]=0.0;home[1]=0.0;home[2]=0.0;
} }
/*--------------------- /*---------------------
@ -46,6 +46,7 @@ namespace rosbzz_node{
void roscontroller::GetRobotId() void roscontroller::GetRobotId()
{ {
mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id";
mavros_msgs::ParamGet::Response robot_id_srv_response; mavros_msgs::ParamGet::Response robot_id_srv_response;
while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){
@ -54,6 +55,8 @@ namespace rosbzz_node{
} }
robot_id=robot_id_srv_response.value.integer; robot_id=robot_id_srv_response.value.integer;
//robot_id = 8;
} }
/*------------------------------------------------- /*-------------------------------------------------
@ -64,7 +67,10 @@ namespace rosbzz_node{
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) { if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
fprintf(stdout, "Bytecode file found and set\n"); fprintf(stdout, "Bytecode file found and set\n");
init_update_monitor(bcfname.c_str(),stand_by.c_str()); //init_update_monitor(bcfname.c_str(),stand_by.c_str());
///////////////////////////////////////////////////////
// MAIN LOOP
//////////////////////////////////////////////////////
while (ros::ok() && !buzz_utility::buzz_script_done()) while (ros::ok() && !buzz_utility::buzz_script_done())
{ {
/*Update neighbors position inside Buzz*/ /*Update neighbors position inside Buzz*/
@ -72,7 +78,7 @@ namespace rosbzz_node{
/*Neighbours of the robot published with id in respective topic*/ /*Neighbours of the robot published with id in respective topic*/
neighbours_pos_publisher(); neighbours_pos_publisher();
/*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());
/*Step buzz script */ /*Step buzz script */
buzz_utility::buzz_script_step(); buzz_utility::buzz_script_step();
/*Prepare messages and publish them in respective topic*/ /*Prepare messages and publish them in respective topic*/
@ -80,10 +86,10 @@ namespace rosbzz_node{
/*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;
} }*/
/*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();
@ -143,9 +149,11 @@ namespace rosbzz_node{
n_c.getParam("stand_by", stand_by); n_c.getParam("stand_by", stand_by);
if(n_c.getParam("xbee_plugged", xbeeplugged)); if(n_c.getParam("xbee_plugged", xbeeplugged));
else {ROS_ERROR("Provide the xbee_plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");} else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
n_c.getParam("xbee_status_srv", xbeesrv_name); if(!xbeeplugged){
n_c.getParam("robot_name", robot_name); if(n_c.getParam("robot_name", robot_name));
else {ROS_ERROR("Provide the xbee plugged boolean in Launch file"); system("rosnode kill rosbuzz_node");}
}
GetSubscriptionParameters(n_c); GetSubscriptionParameters(n_c);
// initialize topics to null? // initialize topics to null?
@ -158,7 +166,8 @@ namespace rosbzz_node{
//todo: make it as an array in yaml? //todo: make it as an array in yaml?
m_sMySubscriptions.clear(); m_sMySubscriptions.clear();
std::string gps_topic, gps_type; std::string gps_topic, gps_type;
node_handle.getParam("topics/gps", gps_topic); if(node_handle.getParam("topics/gps", gps_topic));
else {ROS_ERROR("Provide a gps topic in Launch file"); system("rosnode kill rosbuzz_node");}
node_handle.getParam("type/gps", gps_type); node_handle.getParam("type/gps", gps_type);
m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type)); m_smTopic_infos.insert(pair <std::string, std::string>(gps_topic, gps_type));
@ -200,7 +209,7 @@ namespace rosbzz_node{
//current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this); //current_position_sub = n_c.subscribe("/global_position", 1000, &roscontroller::current_pos,this);
//battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this); //battery_sub = n_c.subscribe("/power_status", 1000, &roscontroller::battery,this);
payload_sub = n_c.subscribe(in_payload, 1000, &roscontroller::payload_obt,this); payload_sub = n_c.subscribe("/"+robot_name+in_payload, 1000, &roscontroller::payload_obt,this);
//flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this); //flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_extended_status_update,this);
//Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this); //Robot_id_sub = n_c.subscribe("/device_id_xbee_", 1000, &roscontroller::set_robot_id,this);
obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this); obstacle_sub= n_c.subscribe("/guidance/obstacle_distance",100, &roscontroller::obstacle_dist,this);
@ -346,35 +355,35 @@ namespace rosbzz_node{
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 && multi_msg){ /*if((int)get_update_mode()!=CODE_RUNNING && is_msg_present()==1 && multi_msg){
uint8_t* buff_send = 0; uint8_t* buff_send = 0;
uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());; uint16_t updater_msgSize=*(uint16_t*) (getupdate_out_msg_size());;
int tot=0; int tot=0;
mavros_msgs::Mavlink update_packets; mavros_msgs::Mavlink update_packets;
fprintf(stdout,"Transfering code \n"); fprintf(stdout,"Transfering code \n");
fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize); fprintf(stdout,"Sent Update packet Size: %u \n",updater_msgSize);
/*allocate mem and clear it*/ // allocate mem and clear it
buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize); buff_send =(uint8_t*)malloc(sizeof(uint16_t)+updater_msgSize);
memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize); memset(buff_send, 0,sizeof(uint16_t)+updater_msgSize);
/*Append updater msg size*/ // Append updater msg size
*(uint16_t*)(buff_send + tot)=updater_msgSize; *(uint16_t*)(buff_send + tot)=updater_msgSize;
//fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize); //fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/*Append updater msgs*/ // Append updater msgs
memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize); memcpy(buff_send + tot, (uint8_t*)(getupdater_out_msg()), updater_msgSize);
tot += updater_msgSize; tot += updater_msgSize;
/*Destroy the updater out msg queue*/ // Destroy the updater out msg queue
destroy_out_msg_queue(); destroy_out_msg_queue();
uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t))); uint16_t total_size =(ceil((float)(float)tot/(float)sizeof(uint64_t)));
uint64_t* payload_64 = new uint64_t[total_size]; uint64_t* payload_64 = new uint64_t[total_size];
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t)); memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
free(buff_send); free(buff_send);
/*Send a constant number to differenciate updater msgs*/ // Send a constant number to differenciate updater msgs
update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT); update_packets.payload64.push_back((uint64_t)UPDATER_MESSAGE_CONSTANT);
for(int i=0;i<total_size;i++){ for(int i=0;i<total_size;i++){
update_packets.payload64.push_back(payload_64[i]); update_packets.payload64.push_back(payload_64[i]);
} }
/*Add Robot id and message number to the published message*/ // Add Robot id and message number to the published message
if (message_number < 0) message_number=0; if (message_number < 0) message_number=0;
else message_number++; else message_number++;
update_packets.sysid=(uint8_t)robot_id; update_packets.sysid=(uint8_t)robot_id;
@ -382,7 +391,7 @@ namespace rosbzz_node{
payload_pub.publish(update_packets); payload_pub.publish(update_packets);
multi_msg=false; multi_msg=false;
delete[] payload_64; delete[] payload_64;
} }*/
} }
/*--------------------------------------------------------------------------------- /*---------------------------------------------------------------------------------

2
src/testflockfev.bzz
View File

@ -50,7 +50,7 @@ function hexagon() {
statef=land statef=land
} else { } else {
timeW = timeW+1 timeW = timeW+1
uav_moveto(0.0,0.35) uav_moveto(0.0,0.0)
} }
} }