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

added update changes and neighbour pos clear on dev

This commit is contained in:
vivek-shankar 2018-06-14 13:37:00 -04:00
parent 6d52a57dcd
commit a9038601ae
6 changed files with 799 additions and 780 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

248
include/buzz_update.h
View File

@ -1,161 +1,171 @@
#ifndef BUZZ_UPDATE_H #ifndef BUZZ_UPDATE_H
#define BUZZ_UPDATE_H #define BUZZ_UPDATE_H
/*Simulation or robot check*/
//#define SIMULATION 1 // set in CMAKELIST
#include <stdlib.h> #include <stdlib.h>
#include <stdio.h> #include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <sys/time.h>
#include <sys/inotify.h>
#include <buzz/buzztype.h> #include <buzz/buzztype.h>
#include <buzz/buzzdict.h> #include <buzz/buzzdict.h>
#include <buzz/buzzdarray.h> #include <buzz/buzzdarray.h>
#include <buzz/buzzvstig.h> #include <buzz/buzzvstig.h>
#include <buzz_utility.h>
#include <fstream> #include <fstream>
#define delete_p(p) \ #define delete_p(p) \
do \ do \
{ \ { \
free(p); \ free(p); \
p = NULL; \ p = NULL; \
} while (0) } while (0)
namespace buzz_update
static const uint16_t CODE_REQUEST_PADDING = 250;
static const uint16_t MIN_UPDATE_PACKET = 251;
static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
/*********************/
/* Updater states */
/********************/
typedef enum {
CODE_RUNNING = 0, // Code executing
CODE_STANDBY, // Standing by for others to update
} code_states_e;
/*********************/
/*Message types */
/********************/
typedef enum {
SENT_CODE = 0, // Broadcast code
RESEND_CODE, // ReBroadcast request
} code_message_e;
/*************************/
/*Updater message queue */
/*************************/
struct updater_msgqueue_s
{ {
uint8_t* queue; static const uint16_t CODE_REQUEST_PADDING = 250;
uint8_t* size; static const uint16_t MIN_UPDATE_PACKET = 251;
}; static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
typedef struct updater_msgqueue_s* updater_msgqueue_t; static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
/*********************/
/* Updater states */
/********************/
struct updater_code_s typedef enum {
{ CODE_RUNNING = 0, // Code executing
uint8_t* bcode; CODE_STANDBY, // Standing by for others to update
uint8_t* bcode_size; } code_states_e;
};
typedef struct updater_code_s* updater_code_t;
/**************************/ /*********************/
/*Updater data*/ /*Message types */
/**************************/ /********************/
struct buzz_updater_elem_s typedef enum {
{ SENT_CODE = 0, // Broadcast code
/* robot id */ RESEND_CODE, // ReBroadcast request
// uint16_t robotid; } code_message_e;
/*current Bytecode content */
uint8_t* bcode;
/*old Bytecode name */
const char* old_bcode;
/*current bcode size*/
size_t* bcode_size;
/*Update patch*/
uint8_t* patch;
/* Update patch size*/
size_t* patch_size;
/*current Bytecode content */
uint8_t* standby_bcode;
/*current bcode size*/
size_t* standby_bcode_size;
/*updater out msg queue */
updater_msgqueue_t outmsg_queue;
/*updater in msg queue*/
updater_msgqueue_t inmsg_queue;
/*Current state of the updater one in code_states_e ENUM*/
int* mode;
uint8_t* update_no;
};
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
/**************************************************************************/ /*************************/
/*Updater routine from msg processing to file checks to be called from main*/ /*Updater message queue */
/**************************************************************************/ /*************************/
void update_routine();
/************************************************/ struct updater_msgqueue_s
/*Initalizes the updater */ {
/************************************************/ uint8_t* queue;
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id); uint8_t* size;
};
typedef struct updater_msgqueue_s* updater_msgqueue_t;
/*********************************************************/ struct updater_code_s
/*Appends buffer of given size to in msg queue of updater*/ {
/*********************************************************/ uint8_t* bcode;
uint8_t* bcode_size;
};
typedef struct updater_code_s* updater_code_t;
void code_message_inqueue_append(uint8_t* msg, uint16_t size); /**************************/
/*Updater data*/
/**************************/
/*********************************************************/ struct buzz_updater_elem_s
/*Processes messages inside the queue of the updater*/ {
/*********************************************************/ /* robot id */
// uint16_t robotid;
/*current Bytecode content */
uint8_t* bcode;
/*old Bytecode name */
const char* old_bcode;
/*current bcode size*/
size_t* bcode_size;
/*Update patch*/
uint8_t* patch;
/* Update patch size*/
size_t* patch_size;
/*current Bytecode content */
uint8_t* standby_bcode;
/*current bcode size*/
size_t* standby_bcode_size;
/*updater out msg queue */
updater_msgqueue_t outmsg_queue;
/*updater in msg queue*/
updater_msgqueue_t inmsg_queue;
/*Current state of the updater one in code_states_e ENUM*/
int* mode;
uint8_t* update_no;
};
typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
void code_message_inqueue_process(); /**************************************************************************/
/*Updater routine from msg processing to file checks to be called from main*/
/**************************************************************************/
void update_routine();
/*****************************************************/ /************************************************/
/* obtains messages from out msgs queue of the updater*/ /*Initalizes the updater */
/*******************************************************/ /************************************************/
uint8_t* getupdater_out_msg(); int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
/******************************************************/ /*********************************************************/
/*obtains out msg queue size*/ /*Appends buffer of given size to in msg queue of updater*/
/*****************************************************/ /*********************************************************/
uint8_t* getupdate_out_msg_size();
/**************************************************/ void code_message_inqueue_append(uint8_t* msg, uint16_t size);
/*destroys the out msg queue*/
/*************************************************/
void destroy_out_msg_queue();
/***************************************************/ /*********************************************************/
/*obatins updater state*/ /*Processes messages inside the queue of the updater*/
/***************************************************/ /*********************************************************/
// int get_update_mode();
// buzz_updater_elem_t get_updater(); void code_message_inqueue_process();
/***************************************************/
/*sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file);
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size); /*****************************************************/
/*Obtains messages from out msgs queue of the updater*/
/*******************************************************/
uint8_t* getupdater_out_msg();
/****************************************************/ /******************************************************/
/*Destroys the updater*/ /*Obtains out msg queue size*/
/***************************************************/ /*****************************************************/
uint8_t* getupdate_out_msg_size();
void destroy_updater(); /**************************************************/
/*Destroys the out msg queue*/
/*************************************************/
void destroy_out_msg_queue();
int is_msg_present(); // buzz_updater_elem_t get_updater();
/***************************************************/
/*Sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file, bool dbg);
int get_update_status(); /****************************************************/
/*Tests the code from a buffer*/
/***************************************************/
void set_read_update_status(); int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
int compile_bzz(std::string bzz_file); /****************************************************/
/*Destroys the updater*/
/***************************************************/
void updates_set_robots(int robots); void destroy_updater();
// void set_packet_id(int packet_id); /****************************************************/
/*Checks for updater message*/
/***************************************************/
// void collect_data(std::ofstream& logger); int is_msg_present();
/****************************************************/
/*Compiles a bzz script to bo and bdbg*/
/***************************************************/
int compile_bzz(std::string bzz_file);
/****************************************************/
/*Set number of robots in the updater*/
/***************************************************/
void updates_set_robots(int robots);
}
#endif #endif

4
include/buzzuav_closures.h
View File

@ -118,6 +118,10 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation)
* update neighbors from in msgs * update neighbors from in msgs
*/ */
void update_neighbors(buzzvm_t vm); void update_neighbors(buzzvm_t vm);
/*
*Clear neighbours struct
*/
void clear_neighbours_pos();
/* /*
* closure to add a neighbor status * closure to add a neighbor status
*/ */

1
include/roscontroller.h
View File

@ -97,6 +97,7 @@ private:
float fcu_timeout; float fcu_timeout;
int armstate; int armstate;
int barrier; int barrier;
int update;
int message_number = 0; int message_number = 0;
uint8_t no_of_robots = 0; uint8_t no_of_robots = 0;
bool rcclient; bool rcclient;

1293
src/buzz_update.cpp
View File

File diff suppressed because it is too large Load Diff

5
src/buzzuav_closures.cpp
View File

@ -678,6 +678,11 @@ void update_neighbors(buzzvm_t vm)
} }
} }
// Clear neighbours pos
void clear_neighbours_pos(){
neighbors_map.clear();
}
int buzzuav_update_currentpos(buzzvm_t vm) int buzzuav_update_currentpos(buzzvm_t vm)
/* /*
/ Update the BVM position table / Update the BVM position table

28
src/roscontroller.cpp
View File

@ -27,7 +27,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
std::string fname = Compile_bzz(bzzfile_name); std::string fname = Compile_bzz(bzzfile_name);
bcfname = fname + ".bo"; bcfname = fname + ".bo";
dbgfname = fname + ".bdb"; dbgfname = fname + ".bdb";
set_bzz_file(bzzfile_name.c_str()); buzz_update::set_bzz_file(bzzfile_name.c_str(),debug);
buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/"); buzzuav_closures::setWPlist(bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/");
// Initialize variables // Initialize variables
SetMode("LOITER", 0); SetMode("LOITER", 0);
@ -107,7 +107,7 @@ void roscontroller::RosControllerRun()
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";
// Intialize the update monitor // Intialize the update monitor
init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id); update = buzz_update::init_update_monitor(bcfname.c_str(), standby_bo.c_str(), dbgfname.c_str(), robot_id);
// set ROS loop rate // set ROS loop rate
ros::Rate loop_rate(BUZZRATE); ros::Rate loop_rate(BUZZRATE);
// DEBUG // DEBUG
@ -120,7 +120,7 @@ void roscontroller::RosControllerRun()
grid_publisher(); grid_publisher();
send_MPpayload(); send_MPpayload();
// Check updater state and step code // Check updater state and step code
update_routine(); if(update) buzz_update::update_routine();
if (time_step == BUZZRATE) if (time_step == BUZZRATE)
{ {
time_step = 0; time_step = 0;
@ -144,15 +144,10 @@ void roscontroller::RosControllerRun()
prepare_msg_and_publish(); prepare_msg_and_publish();
// Call the flight controler service // Call the flight controler service
flight_controller_service_call(); flight_controller_service_call();
// Set multi message available after update
if (get_update_status())
{
set_read_update_status();
}
// Set ROBOTS variable (swarm size) // Set ROBOTS variable (swarm size)
get_number_of_robots(); get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots); buzz_utility::set_robot_var(no_of_robots);
updates_set_robots(no_of_robots); if(update) buzz_update::updates_set_robots(no_of_robots);
// get_xbee_status(); // commented out because it may slow down the node too much, to be tested // get_xbee_status(); // commented out because it may slow down the node too much, to be tested
ros::spinOnce(); ros::spinOnce();
@ -561,11 +556,11 @@ with size......... | /
payload_pub.publish(payload_out); payload_pub.publish(payload_out);
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 (is_msg_present()) if (update && buzz_update::is_msg_present())
{ {
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*)(buzz_update::getupdate_out_msg_size());
; ;
int tot = 0; int tot = 0;
mavros_msgs::Mavlink update_packets; mavros_msgs::Mavlink update_packets;
@ -578,10 +573,10 @@ with size......... | /
*(uint16_t*)(buff_send + tot) = updater_msgSize; *(uint16_t*)(buff_send + tot) = 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*)(buzz_update::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(); buzz_update::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));
@ -750,6 +745,7 @@ void roscontroller::maintain_pos(int tim_step)
if (timer_step >= BUZZRATE) if (timer_step >= BUZZRATE)
{ {
neighbours_pos_map.clear(); neighbours_pos_map.clear();
buzzuav_closures::clear_neighbours_pos();
// raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but // raw_neighbours_pos_map.clear(); // TODO: currently not a problem, but
// have to clear ! // have to clear !
timer_step = 0; timer_step = 0;
@ -1031,8 +1027,8 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize); fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize);
if (unMsgSize > 0) if (unMsgSize > 0)
{ {
code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize); buzz_update::code_message_inqueue_append((uint8_t*)(pl + sizeof(uint16_t)), unMsgSize);
code_message_inqueue_process(); buzz_update::code_message_inqueue_process();
} }
free(pl); free(pl);
} }