Made updates optional: including update.bzz will turnon update and
not including it will disable updates.
This commit is contained in:
parent
e953de6191
commit
8fa58e5e5b
|
@ -4,6 +4,7 @@
|
|||
####################################################################################################
|
||||
updated="update_ack"
|
||||
update_no=0
|
||||
updates_active = 1
|
||||
function updated_no_bct(){
|
||||
neighbors.broadcast(updated, update_no)
|
||||
}
|
|
@ -103,7 +103,7 @@ namespace buzz_update
|
|||
/************************************************/
|
||||
/*Initalizes the updater */
|
||||
/************************************************/
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
|
||||
int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
|
||||
|
||||
/*********************************************************/
|
||||
/*Appends buffer of given size to in msg queue of updater*/
|
||||
|
|
|
@ -96,6 +96,7 @@ private:
|
|||
float fcu_timeout;
|
||||
int armstate;
|
||||
int barrier;
|
||||
int update;
|
||||
int message_number = 0;
|
||||
uint8_t no_of_robots = 0;
|
||||
bool rcclient;
|
||||
|
|
|
@ -34,12 +34,17 @@ namespace buzz_update{
|
|||
static size_t old_byte_code_size = 0;
|
||||
|
||||
/*Initialize updater*/
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
|
||||
int init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id)
|
||||
{
|
||||
buzzvm_t VM = buzz_utility::get_vm();
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "updates_active", 1));
|
||||
buzzvm_gload(VM);
|
||||
buzzobj_t obj = buzzvm_stack_at(VM, 1);
|
||||
if(obj->o.type == BUZZTYPE_INT && obj->i.value == 1){
|
||||
Robot_id = robot_id;
|
||||
dbgf_name = dbgfname;
|
||||
bcfname = bo_filename;
|
||||
ROS_WARN("UPDATES TURNED ON (modifying .bzz script file will update all robots)");
|
||||
ROS_INFO("Initializing file monitor...");
|
||||
fd = inotify_init1(IN_NONBLOCK);
|
||||
if (fd < 0)
|
||||
|
@ -118,6 +123,12 @@ namespace buzz_update{
|
|||
fp = fopen("old_bcode.bo", "wb");
|
||||
fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp);
|
||||
fclose(fp);
|
||||
return 1;
|
||||
}
|
||||
else{
|
||||
ROS_WARN("UPDATES TURNED OFF... (Hint: Include update.bzz to turn on updates)");
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
/*Check for .bzz file chages*/
|
||||
int check_update()
|
||||
|
@ -187,7 +198,7 @@ namespace buzz_update{
|
|||
std::stringstream read_patched;
|
||||
read_patched << path << name1 << ".bo";
|
||||
fprintf(stdout, "read file name %s \n", read_patched.str().c_str());
|
||||
uint8_t* patched_BO_Buf = 0;
|
||||
uint8_t* patched_bo_buf = 0;
|
||||
FILE* fp = fopen(read_patched.str().c_str(), "rb");
|
||||
if (!fp)
|
||||
{
|
||||
|
@ -196,15 +207,15 @@ namespace buzz_update{
|
|||
fseek(fp, 0, SEEK_END);
|
||||
size_t patched_size = ftell(fp);
|
||||
rewind(fp);
|
||||
patched_BO_Buf = (uint8_t*)malloc(patched_size);
|
||||
if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
|
||||
patched_bo_buf = (uint8_t*)malloc(patched_size);
|
||||
if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
|
||||
{
|
||||
perror(read_patched.str().c_str());
|
||||
}
|
||||
fclose(fp);
|
||||
/*Write the patched to a code struct and return*/
|
||||
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
|
||||
update_code->bcode = patched_BO_Buf;
|
||||
update_code->bcode = patched_bo_buf;
|
||||
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
|
||||
*(uint16_t*)(update_code->bcode_size) = patched_size;
|
||||
|
||||
|
@ -217,7 +228,7 @@ namespace buzz_update{
|
|||
std::stringstream read_patched;
|
||||
read_patched << path << name1 << "-patched.bo";
|
||||
fprintf(stdout, "read file name %s \n", read_patched.str().c_str());
|
||||
uint8_t* patched_BO_Buf = 0;
|
||||
uint8_t* patched_bo_buf = 0;
|
||||
FILE* fp = fopen(read_patched.str().c_str(), "rb");
|
||||
if (!fp)
|
||||
{
|
||||
|
@ -226,8 +237,8 @@ namespace buzz_update{
|
|||
fseek(fp, 0, SEEK_END);
|
||||
size_t patched_size = ftell(fp);
|
||||
rewind(fp);
|
||||
patched_BO_Buf = (uint8_t*)malloc(patched_size);
|
||||
if (fread(patched_BO_Buf, 1, patched_size, fp) < patched_size)
|
||||
patched_bo_buf = (uint8_t*)malloc(patched_size);
|
||||
if (fread(patched_bo_buf, 1, patched_size, fp) < patched_size)
|
||||
{
|
||||
perror(read_patched.str().c_str());
|
||||
}
|
||||
|
@ -239,7 +250,7 @@ namespace buzz_update{
|
|||
|
||||
/*Write the patched to a code struct and return*/
|
||||
updater_code_t update_code = (updater_code_t)malloc(sizeof(struct updater_code_s));
|
||||
update_code->bcode = patched_BO_Buf;
|
||||
update_code->bcode = patched_bo_buf;
|
||||
update_code->bcode_size = (uint8_t*)malloc(sizeof(uint16_t));
|
||||
*(uint16_t*)(update_code->bcode_size) = patched_size;
|
||||
|
||||
|
@ -577,9 +588,6 @@ namespace buzz_update{
|
|||
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
|
||||
buzzvm_pushi(VM, no_of_robot);
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "UPDATE", 1));
|
||||
buzzvm_gstore(VM);
|
||||
return 1;
|
||||
}
|
||||
/*Unable to step something wrong*/
|
||||
|
|
|
@ -37,7 +37,7 @@ roscontroller::roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv)
|
|||
cur_pos.longitude = 0;
|
||||
cur_pos.latitude = 0;
|
||||
cur_pos.altitude = 0;
|
||||
|
||||
update=0;
|
||||
// set stream rate - wait for the FC to be started
|
||||
SetStreamRate(0, 10, 1);
|
||||
|
||||
|
@ -106,7 +106,7 @@ void roscontroller::RosControllerRun()
|
|||
ROS_INFO("[%i] Bytecode file found and set", robot_id);
|
||||
std::string standby_bo = Compile_bzz(stand_by) + ".bo";
|
||||
// Intialize the update monitor
|
||||
buzz_update::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
|
||||
ros::Rate loop_rate(BUZZRATE);
|
||||
// DEBUG
|
||||
|
@ -119,8 +119,7 @@ void roscontroller::RosControllerRun()
|
|||
grid_publisher();
|
||||
send_MPpayload();
|
||||
// Check updater state and step code
|
||||
buzz_update::update_routine();
|
||||
ROS_WARN("OUT OF UPDATE ROUTINE");
|
||||
if(update) buzz_update::update_routine();
|
||||
if (time_step == BUZZRATE)
|
||||
{
|
||||
time_step = 0;
|
||||
|
@ -152,7 +151,7 @@ void roscontroller::RosControllerRun()
|
|||
// Set ROBOTS variable (swarm size)
|
||||
get_number_of_robots();
|
||||
buzz_utility::set_robot_var(no_of_robots);
|
||||
buzz_update::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
|
||||
|
||||
ros::spinOnce();
|
||||
|
@ -570,7 +569,7 @@ with size......... | /
|
|||
delete[] out;
|
||||
delete[] payload_out_ptr;
|
||||
// Check for updater message if present send
|
||||
if (buzz_update::is_msg_present())
|
||||
if (update && buzz_update::is_msg_present())
|
||||
{
|
||||
uint8_t* buff_send = 0;
|
||||
uint16_t updater_msgSize = *(uint16_t*)(buzz_update::getupdate_out_msg_size());
|
||||
|
|
Loading…
Reference in New Issue