update back and working with diff version upto IEEE Software
This commit is contained in:
vivek-shankar
parent
6018b681d7
commit
7e99692178
|
|
@ -1,5 +1,8 @@
|
|||
#ifndef BUZZ_UPDATE_H
|
||||
#define BUZZ_UPDATE_H
|
||||
/*Simulation or robot check*/
|
||||
#define SIMULATION 1
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <buzz/buzztype.h>
|
||||
|
|
@ -9,8 +12,10 @@
|
|||
#include <fstream>
|
||||
#define delete_p(p) do { free(p); p = NULL; } while(0)
|
||||
|
||||
|
||||
|
||||
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 */
|
||||
/********************/
|
||||
|
|
@ -25,8 +30,8 @@ typedef enum {
|
|||
/********************/
|
||||
|
||||
typedef enum {
|
||||
SEND_CODE = 0, // Broadcast code with state
|
||||
STATE_MSG, // Broadcast state
|
||||
SENT_CODE = 0, // Broadcast code
|
||||
RESEND_CODE, // ReBroadcast request
|
||||
} code_message_e;
|
||||
|
||||
/*************************/
|
||||
|
|
@ -39,6 +44,12 @@ struct updater_msgqueue_s {
|
|||
} ;
|
||||
typedef struct updater_msgqueue_s* updater_msgqueue_t;
|
||||
|
||||
struct updater_code_s {
|
||||
uint8_t* bcode;
|
||||
uint8_t* bcode_size;
|
||||
} ;
|
||||
typedef struct updater_code_s* updater_code_t;
|
||||
|
||||
/**************************/
|
||||
/*Updater data*/
|
||||
/**************************/
|
||||
|
|
@ -48,8 +59,14 @@ struct buzz_updater_elem_s {
|
|||
//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*/
|
||||
|
|
@ -61,19 +78,19 @@ struct buzz_updater_elem_s {
|
|||
/*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*/
|
||||
/**************************************************************************/
|
||||
void update_routine(const char* bcfname,
|
||||
const char* dbgfname);
|
||||
void update_routine();
|
||||
|
||||
/************************************************/
|
||||
/*Initalizes the updater */
|
||||
/************************************************/
|
||||
void init_update_monitor(const char* bo_filename,const char* stand_by_script);
|
||||
void init_update_monitor(const char* bo_filename,const char* stand_by_script,
|
||||
const char* dbgfname, int robot_id);
|
||||
|
||||
|
||||
/*********************************************************/
|
||||
|
|
@ -129,9 +146,11 @@ int get_update_status();
|
|||
|
||||
void set_read_update_status();
|
||||
|
||||
int compile_bzz();
|
||||
int compile_bzz(std::string bzz_file);
|
||||
|
||||
void updates_set_robots(int robots);
|
||||
|
||||
void set_packet_id(int packet_id);
|
||||
|
||||
void collect_data(std::ofstream &logger);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -21,21 +21,40 @@ static int fd,wd =0;
|
|||
static int old_update =0;
|
||||
static buzz_updater_elem_t updater;
|
||||
static int no_of_robot;
|
||||
static char* dbgf_name;
|
||||
static const char* dbgf_name;
|
||||
static const char* bcfname;
|
||||
static const char* old_bcfname="old_bcode.bo";
|
||||
static const char* bzz_file;
|
||||
static int Robot_id=0;
|
||||
static int neigh=-1;
|
||||
static int updater_msg_ready ;
|
||||
static uint16_t update_try_counter=0;
|
||||
static int updated=0;
|
||||
static const uint16_t MAX_UPDATE_TRY=5;
|
||||
static int packet_id_=0;
|
||||
static size_t old_byte_code_size=0;
|
||||
|
||||
|
||||
/*Initialize updater*/
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script){
|
||||
void init_update_monitor(const char* bo_filename, const char* stand_by_script,
|
||||
const char* dbgfname, int robot_id){
|
||||
Robot_id=robot_id;
|
||||
dbgf_name=dbgfname;
|
||||
bcfname=bo_filename;
|
||||
ROS_INFO("Initializing file monitor...");
|
||||
fd=inotify_init1(IN_NONBLOCK);
|
||||
if ( fd < 0 ) {
|
||||
perror( "inotify_init error" );
|
||||
}
|
||||
/* watch /.bzz file for any activity and report it back to update */
|
||||
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
|
||||
/*If simulation set the file monitor only for robot 1*/
|
||||
if(SIMULATION==1 && robot_id==0){
|
||||
/* watch /.bzz file for any activity and report it back to update */
|
||||
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
|
||||
}
|
||||
else if (SIMULATION==0){
|
||||
/* watch /.bzz file for any activity and report it back to update */
|
||||
wd=inotify_add_watch(fd, bzz_file,IN_ALL_EVENTS );
|
||||
}
|
||||
/*load the .bo under execution into the updater*/
|
||||
uint8_t* BO_BUF = 0;
|
||||
FILE* fp = fopen(bo_filename, "rb");
|
||||
|
|
@ -73,8 +92,11 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
|
|||
updater = (buzz_updater_elem_t)malloc(sizeof(struct buzz_updater_elem_s));
|
||||
/*Intialize the updater with the required data*/
|
||||
updater->bcode = BO_BUF;
|
||||
/*Store a copy of the Bcode for rollback*/
|
||||
updater->outmsg_queue = NULL;
|
||||
updater->inmsg_queue = NULL;
|
||||
updater->patch = NULL;
|
||||
updater->patch_size = (size_t*) malloc(sizeof(size_t));
|
||||
updater->bcode_size = (size_t*) malloc(sizeof(size_t));
|
||||
updater->update_no = (uint8_t*) malloc(sizeof(uint16_t));
|
||||
*(uint16_t*)(updater->update_no) =0;
|
||||
|
|
@ -86,10 +108,11 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script){
|
|||
*(int*)(updater->mode)=CODE_RUNNING;
|
||||
//no_of_robot=barrier;
|
||||
updater_msg_ready=0;
|
||||
//neigh = 0;
|
||||
//updater->outmsg_queue=
|
||||
// update_table->barrier=nvs;
|
||||
// open logger
|
||||
|
||||
/*Write the bcode to a file for rollback*/
|
||||
fp=fopen("old_bcode.bo", "wb");
|
||||
fwrite((updater->bcode), *(size_t*)updater->bcode_size, 1, fp);
|
||||
fclose(fp);
|
||||
|
||||
}
|
||||
/*Check for .bzz file chages*/
|
||||
|
|
@ -123,27 +146,131 @@ int check_update(){
|
|||
return check;
|
||||
}
|
||||
|
||||
int test_patch(std::string path, std::string name1,size_t update_patch_size, uint8_t* patch){
|
||||
if(SIMULATION==1){
|
||||
return 1;
|
||||
}
|
||||
else {
|
||||
/*Patch the old bo with new patch*/
|
||||
std::stringstream patch_writefile;
|
||||
patch_writefile<< path<<name1<<"tmp_patch.bo";
|
||||
/*Write the patch to a file*/
|
||||
FILE *fp=fopen(patch_writefile.str().c_str(), "wb");
|
||||
fwrite(patch, update_patch_size, 1, fp);
|
||||
fclose(fp);
|
||||
std::stringstream patch_exsisting;
|
||||
patch_exsisting<< "bspatch "<< path << name1<<".bo "<< path<<name1 << "-patched.bo "<< path<<name1<<"tmp_patch.bo";
|
||||
fprintf(stdout,"Launching bspatch command: %s \n", patch_exsisting.str().c_str());
|
||||
if(system(patch_exsisting.str().c_str()) ) return 0;
|
||||
else return 1;
|
||||
}
|
||||
}
|
||||
|
||||
updater_code_t obtain_patched_bo(std::string path, std::string name1){
|
||||
if(SIMULATION==1){
|
||||
/*Read the exsisting file to simulate the patching*/
|
||||
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;
|
||||
FILE *fp = fopen(read_patched.str().c_str(), "rb");
|
||||
if(!fp) {
|
||||
perror(read_patched.str().c_str());
|
||||
}
|
||||
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) {
|
||||
perror(read_patched.str().c_str());
|
||||
fclose(fp);
|
||||
}
|
||||
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_size = (uint8_t*) malloc(sizeof(uint16_t));
|
||||
*(uint16_t*) (update_code->bcode_size) = patched_size;
|
||||
|
||||
return update_code;
|
||||
}
|
||||
|
||||
else{
|
||||
|
||||
/*Read the new patched file*/
|
||||
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;
|
||||
FILE *fp = fopen(read_patched.str().c_str(), "rb");
|
||||
if(!fp) {
|
||||
perror(read_patched.str().c_str());
|
||||
}
|
||||
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) {
|
||||
perror(read_patched.str().c_str());
|
||||
fclose(fp);
|
||||
}
|
||||
fclose(fp);
|
||||
|
||||
/* delete old bo file & rename new bo file */
|
||||
remove((path + name1 + ".bo").c_str());
|
||||
rename((path + name1 + "-patched.bo").c_str(), (path + name1 + ".bo").c_str());
|
||||
|
||||
/*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_size = (uint8_t*) malloc(sizeof(uint16_t));
|
||||
*(uint16_t*) (update_code->bcode_size) = patched_size;
|
||||
|
||||
return update_code;
|
||||
}
|
||||
}
|
||||
|
||||
void code_message_outqueue_append(){
|
||||
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
|
||||
uint16_t size =0;
|
||||
updater->outmsg_queue->queue = (uint8_t*)malloc(2*sizeof(uint16_t)+ *(size_t*)(updater->bcode_size));
|
||||
/* if size less than 250 Pad with zeors to assure transmission*/
|
||||
uint16_t size = UPDATE_CODE_HEADER_SIZE + *(size_t*)(updater->patch_size);
|
||||
uint16_t padding_size= ( size > MIN_UPDATE_PACKET) ? 0 : MIN_UPDATE_PACKET-size;
|
||||
size +=padding_size;
|
||||
updater->outmsg_queue->queue = (uint8_t*)malloc(size);
|
||||
memset(updater->outmsg_queue->queue, 0, size);
|
||||
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
|
||||
/*append the update no, code size and code to out msg*/
|
||||
*(uint16_t*)(updater->outmsg_queue->size)=size;
|
||||
size=0;
|
||||
/*Append message type*/
|
||||
*(uint8_t*)(updater->outmsg_queue->queue+size) = SENT_CODE;
|
||||
size+=sizeof(uint8_t);
|
||||
/*Append the update no, code size and code to out msg*/
|
||||
*(uint16_t*)(updater->outmsg_queue->queue+size) = *(uint16_t*) (updater->update_no);
|
||||
size+=sizeof(uint16_t);
|
||||
*(uint16_t*)(updater->outmsg_queue->queue+size) = *(size_t*) (updater->bcode_size);
|
||||
*(uint16_t*)(updater->outmsg_queue->queue+size) =(uint16_t) *(size_t*) (updater->patch_size);
|
||||
size+=sizeof(uint16_t);
|
||||
memcpy(updater->outmsg_queue->queue+size, updater->bcode, *(size_t*)(updater->bcode_size));
|
||||
size+=(uint16_t)*(size_t*)(updater->bcode_size);
|
||||
/*FILE *fp;
|
||||
fp=fopen("update.bo", "wb");
|
||||
fwrite((updater->bcode), updater->bcode_size, 1, fp);
|
||||
fclose(fp);*/
|
||||
memcpy(updater->outmsg_queue->queue+size, updater->patch, *(size_t*)(updater->patch_size));
|
||||
size+=(uint16_t)*(size_t*)(updater->patch_size);
|
||||
updater_msg_ready=1;
|
||||
*(uint16_t*)(updater->outmsg_queue->size)=size;
|
||||
}
|
||||
|
||||
//fprintf(stdout,"out msg append transfer code size %d\n", (int)*(size_t*) updater->bcode_size);
|
||||
void outqueue_append_code_request(uint16_t update_no){
|
||||
updater->outmsg_queue=(updater_msgqueue_t)malloc(sizeof(struct updater_msgqueue_s));
|
||||
uint16_t size =0;
|
||||
updater->outmsg_queue->queue = (uint8_t*)malloc(2*sizeof(uint16_t) + sizeof(uint8_t) +CODE_REQUEST_PADDING);
|
||||
updater->outmsg_queue->size = (uint8_t*)malloc(sizeof(uint16_t));
|
||||
memset(updater->outmsg_queue->queue, 0, sizeof(uint16_t) + sizeof(uint8_t) +CODE_REQUEST_PADDING);
|
||||
/*Append message type*/
|
||||
*(uint8_t*)(updater->outmsg_queue->queue+size) = RESEND_CODE;
|
||||
size+=sizeof(uint8_t);
|
||||
/*Append the update no, code size and code to out msg*/
|
||||
*(uint16_t*)(updater->outmsg_queue->queue+size) = update_no;
|
||||
size+=sizeof(uint16_t);
|
||||
*(uint16_t*)(updater->outmsg_queue->queue+size) = update_try_counter;
|
||||
size+=sizeof(uint16_t);
|
||||
*(uint16_t*)(updater->outmsg_queue->size)=size+CODE_REQUEST_PADDING;
|
||||
updater_msg_ready=1;
|
||||
ROS_INFO("[Debug] Requested update no. %u with try: %u \n",update_no,update_try_counter);
|
||||
}
|
||||
|
||||
void code_message_inqueue_append(uint8_t* msg,uint16_t size){
|
||||
|
|
@ -155,43 +282,158 @@ void code_message_inqueue_append(uint8_t* msg,uint16_t size){
|
|||
*(uint16_t*)(updater->inmsg_queue->size) = size;
|
||||
}
|
||||
|
||||
void set_packet_id(int packet_id){
|
||||
/*Used for data logging*/
|
||||
packet_id_=packet_id;
|
||||
}
|
||||
void code_message_inqueue_process(){
|
||||
int size=0;
|
||||
updater_code_t out_code=NULL;
|
||||
|
||||
ROS_INFO("[Debug] Updater processing in msg with mode %d \n", *(int*)(updater->mode) );
|
||||
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue)) );
|
||||
ROS_INFO("[Debug] Updater received code of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)) ) );
|
||||
ROS_INFO("[Debug] %u : Current update number, %u : Received update no \n",( *(uint16_t*) (updater->update_no) ), (*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint8_t))) );
|
||||
ROS_INFO("[Debug] Updater received patch of size %u \n",(*(uint16_t*)(updater->inmsg_queue->queue+sizeof(uint16_t)+sizeof(uint8_t)) ) );
|
||||
|
||||
if( *(int*) (updater->mode) == CODE_RUNNING){
|
||||
//fprintf(stdout,"[debug]Inside inmsg code running");
|
||||
if( *(uint16_t*)(updater->inmsg_queue->queue) > *(uint16_t*) (updater->update_no) ){
|
||||
//fprintf(stdout,"[debug]Inside update number comparision");
|
||||
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue);
|
||||
size +=sizeof(uint16_t);
|
||||
uint16_t update_bcode_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
|
||||
size +=sizeof(uint16_t);
|
||||
//fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
|
||||
//fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
|
||||
//FILE *fp;
|
||||
//fp=fopen("update.bo", "wb");
|
||||
//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
|
||||
//fclose(fp);
|
||||
if( test_set_code((uint8_t*)(updater->inmsg_queue->queue+size),
|
||||
(char*) dbgf_name,(size_t)update_bcode_size) ) {
|
||||
*(uint16_t*)(updater->update_no)=update_no;
|
||||
neigh=1;
|
||||
//gettimeofday(&t1, NULL);
|
||||
if(*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE){
|
||||
size+=sizeof(uint8_t);
|
||||
if( *(uint16_t*)(updater->inmsg_queue->queue+size) > *(uint16_t*) (updater->update_no) ){
|
||||
//fprintf(stdout,"[debug]Inside update number comparision");
|
||||
uint16_t update_no=*(uint16_t*)(updater->inmsg_queue->queue+size);
|
||||
size +=sizeof(uint16_t);
|
||||
uint16_t update_patch_size =*(uint16_t*)(updater->inmsg_queue->queue+size);
|
||||
size +=sizeof(uint16_t);
|
||||
/*Generate patch*/
|
||||
std::string bzzfile_name(bzz_file);
|
||||
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
||||
name1 = name1.substr(0,name1.find_last_of("."));
|
||||
if(test_patch(path, name1,update_patch_size,(updater->inmsg_queue->queue+size)) ){
|
||||
out_code = obtain_patched_bo(path, name1);
|
||||
|
||||
//fprintf(stdout,"in queue process Update no %d\n", (int) update_no);
|
||||
//fprintf(stdout,"in queue process bcode size %d\n", (int) update_bcode_size);
|
||||
//FILE *fp;
|
||||
//fp=fopen("update.bo", "wb");
|
||||
//fwrite((updater->inmsg_queue->queue+size), update_bcode_size, 1, fp);
|
||||
//fclose(fp);
|
||||
|
||||
|
||||
if(test_set_code( (out_code->bcode),
|
||||
(char*) dbgf_name, (size_t) (*(uint16_t*)out_code->bcode_size) ) ){
|
||||
|
||||
printf("TEST PASSED!\n");
|
||||
*(uint16_t*)updater->update_no=update_no;
|
||||
/*Clear exisiting patch if any*/
|
||||
delete_p(updater->patch);
|
||||
/*copy the patch into the updater*/
|
||||
updater->patch = (uint8_t*)malloc(update_patch_size);
|
||||
memcpy(updater->patch, (updater->inmsg_queue->queue+size), update_patch_size);
|
||||
*(size_t*) (updater->patch_size) = update_patch_size;
|
||||
//code_message_outqueue_append();
|
||||
neigh=1;
|
||||
}
|
||||
/*clear the temp code buff*/
|
||||
delete_p(out_code->bcode);
|
||||
delete_p(out_code->bcode_size);
|
||||
delete_p(out_code);
|
||||
}
|
||||
else{
|
||||
ROS_ERROR("Patching the .bo file failed, could be corrupt patch\n");
|
||||
update_try_counter++;
|
||||
outqueue_append_code_request(update_no);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
size=0;
|
||||
if(*(uint8_t*)(updater->inmsg_queue->queue) == RESEND_CODE){
|
||||
size+=sizeof(uint8_t);
|
||||
if( *(uint16_t*)(updater->inmsg_queue->queue+size) == *(uint16_t*) (updater->update_no) ){
|
||||
size+=sizeof(uint16_t);
|
||||
if(*(uint16_t*)(updater->inmsg_queue->queue+size) > update_try_counter){
|
||||
update_try_counter=*(uint16_t*)(updater->inmsg_queue->queue+size);
|
||||
ROS_ERROR("Code appended! update try : %u \n",update_try_counter);
|
||||
code_message_outqueue_append();
|
||||
|
||||
}
|
||||
if(update_try_counter > MAX_UPDATE_TRY) ROS_ERROR("TODO: ROLL BACK !!");
|
||||
}
|
||||
}
|
||||
//fprintf(stdout,"in queue freed\n");
|
||||
delete_p(updater->inmsg_queue->queue);
|
||||
delete_p(updater->inmsg_queue->size);
|
||||
delete_p(updater->inmsg_queue);
|
||||
}
|
||||
void update_routine(const char* bcfname,
|
||||
const char* dbgfname){
|
||||
dbgf_name=(char*)dbgfname;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void create_update_patch(){
|
||||
std::stringstream genpatch;
|
||||
std::stringstream usepatch;
|
||||
|
||||
std::string bzzfile_name(bzz_file);
|
||||
|
||||
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
|
||||
std::string name1 = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
||||
name1 = name1.substr(0,name1.find_last_of("."));
|
||||
|
||||
std::string name2 = name1 + "-update";
|
||||
|
||||
// CALL BSDIFF CMD
|
||||
genpatch<< "bsdiff "<< path << name1 <<".bo "<< path << name2 <<".bo "<< path<<"diff.bo";
|
||||
fprintf(stdout,"Launching bsdiff command: %s \n", genpatch.str().c_str());
|
||||
system(genpatch.str().c_str());
|
||||
|
||||
// BETTER: CALL THE FUNCTION IN BSDIFF.CPP
|
||||
//bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo");
|
||||
|
||||
|
||||
|
||||
/* delete old files & rename new files */
|
||||
|
||||
remove((path + name1 + ".bo").c_str());
|
||||
remove((path + name1 + ".bdb").c_str());
|
||||
|
||||
rename((path + name2 + ".bo").c_str(), (path + name1 + ".bo").c_str());
|
||||
rename((path + name2 + ".bdb").c_str(), (path + name1 + ".bdb").c_str());
|
||||
|
||||
|
||||
/*Read the diff file */
|
||||
std::stringstream patchfileName;
|
||||
patchfileName<<path<<"diff.bo";
|
||||
|
||||
uint8_t* patch_buff = 0;
|
||||
FILE* fp = fopen(patchfileName.str().c_str(), "rb");
|
||||
if(!fp) {
|
||||
perror(patchfileName.str().c_str());
|
||||
}
|
||||
fseek(fp, 0, SEEK_END);
|
||||
size_t patch_size = ftell(fp);
|
||||
rewind(fp);
|
||||
patch_buff = (uint8_t*)malloc(patch_size);
|
||||
if(fread(patch_buff, 1, patch_size, fp) < patch_size) {
|
||||
perror(patchfileName.str().c_str());
|
||||
fclose(fp);
|
||||
}
|
||||
fclose(fp);
|
||||
delete_p(updater->patch);
|
||||
updater->patch = patch_buff;
|
||||
*(size_t*) (updater->patch_size) = patch_size;
|
||||
|
||||
/* Delete the diff file */
|
||||
remove(patchfileName.str().c_str());
|
||||
}
|
||||
|
||||
|
||||
void update_routine(){
|
||||
buzzvm_t VM = buzz_utility::get_vm();
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
|
||||
buzzvm_pushi(VM, *(uint16_t*)(updater->update_no));
|
||||
|
|
@ -199,19 +441,26 @@ void update_routine(const char* bcfname,
|
|||
//fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
|
||||
if(*(int*)updater->mode==CODE_RUNNING){
|
||||
buzzvm_function_call(VM, "updated_neigh", 0);
|
||||
// Check for update
|
||||
if(check_update()){
|
||||
|
||||
|
||||
ROS_INFO("Update found \nUpdating script ...\n");
|
||||
|
||||
if(compile_bzz()){
|
||||
if(compile_bzz(bzz_file)){
|
||||
ROS_WARN("Errors in comipilg script so staying on old script\n");
|
||||
}
|
||||
else {
|
||||
std::string bzzfile_name(bzz_file);
|
||||
stringstream bzzfile_in_compile;
|
||||
std::string path = bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
||||
name = name.substr(0,name.find_last_of("."));
|
||||
bzzfile_in_compile<<path<<name<<"-update.bo";
|
||||
uint8_t* BO_BUF = 0;
|
||||
FILE* fp = fopen(bcfname, "rb"); // to change file edit
|
||||
FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit
|
||||
if(!fp) {
|
||||
perror(bcfname);
|
||||
perror(bzzfile_in_compile.str().c_str());
|
||||
}
|
||||
fseek(fp, 0, SEEK_END);
|
||||
size_t bcode_size = ftell(fp);
|
||||
|
|
@ -222,10 +471,11 @@ void update_routine(const char* bcfname,
|
|||
fclose(fp);
|
||||
}
|
||||
fclose(fp);
|
||||
if(test_set_code(BO_BUF, dbgfname,bcode_size)){
|
||||
if(test_set_code(BO_BUF, dbgf_name,bcode_size)){
|
||||
uint16_t update_no =*(uint16_t*)(updater->update_no);
|
||||
*(uint16_t*)(updater->update_no) =update_no +1;
|
||||
code_message_outqueue_append();
|
||||
|
||||
create_update_patch();
|
||||
VM = buzz_utility::get_vm();
|
||||
ROS_INFO("Current Update no %d\n", *(uint16_t*)(updater->update_no));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "update_no", 1));
|
||||
|
|
@ -244,11 +494,7 @@ void update_routine(const char* bcfname,
|
|||
|
||||
else{
|
||||
|
||||
if(neigh==0 && (!is_msg_present())){
|
||||
ROS_INFO("Sending code... \n");
|
||||
code_message_outqueue_append();
|
||||
|
||||
}
|
||||
timer_steps++;
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "barrier_val",1));
|
||||
buzzvm_gload(VM);
|
||||
|
|
@ -259,10 +505,22 @@ void update_routine(const char* bcfname,
|
|||
*(int*)(updater->mode) = CODE_RUNNING;
|
||||
gettimeofday(&t2, NULL);
|
||||
//collect_data();
|
||||
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgfname, *(size_t*)(updater->bcode_size));
|
||||
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size));
|
||||
//buzzvm_function_call(m_tBuzzVM, "updated", 0);
|
||||
updated=1;
|
||||
update_try_counter=0;
|
||||
timer_steps=0;
|
||||
}
|
||||
else if (timer_steps>TIMEOUT_FOR_ROLLBACK){
|
||||
ROS_ERROR("TIME OUT Reached, decided to roll back");
|
||||
/*Time out hit deceided to roll back*/
|
||||
*(int*)(updater->mode) = CODE_RUNNING;
|
||||
buzz_utility::buzz_script_set(old_bcfname, dbgf_name,
|
||||
(int)VM->robot);
|
||||
updated=1;
|
||||
update_try_counter=0;
|
||||
timer_steps=0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -285,7 +543,7 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size ){
|
|||
ROS_WARN("Initializtion of script test passed\n");
|
||||
if(buzz_utility::update_step_test()){
|
||||
/*data logging*/
|
||||
//start =1;
|
||||
old_byte_code_size=*(size_t*)updater->bcode_size;
|
||||
/*data logging*/
|
||||
ROS_WARN("Step test passed\n");
|
||||
*(int*) (updater->mode) = CODE_STANDBY;
|
||||
|
|
@ -383,7 +641,6 @@ void destroy_updater(){
|
|||
delete_p(updater->inmsg_queue->size);
|
||||
delete_p(updater->inmsg_queue);
|
||||
}
|
||||
//
|
||||
inotify_rm_watch(fd,wd);
|
||||
close(fd);
|
||||
}
|
||||
|
|
@ -399,7 +656,7 @@ void updates_set_robots(int robots){
|
|||
/*--------------------------------------------------------
|
||||
/ Create Buzz bytecode from the bzz script inputed
|
||||
/-------------------------------------------------------*/
|
||||
int compile_bzz(){
|
||||
int compile_bzz(std::string bzz_file){
|
||||
/*Compile the buzz code .bzz to .bo*/
|
||||
std::string bzzfile_name(bzz_file);
|
||||
stringstream bzzfile_in_compile;
|
||||
|
|
@ -407,24 +664,21 @@ int compile_bzz(){
|
|||
std::string name = bzzfile_name.substr(bzzfile_name.find_last_of("/\\") + 1);
|
||||
name = name.substr(0,name.find_last_of("."));
|
||||
bzzfile_in_compile << "bzzc -I " << path << "include/"; //<<" "<<path<< name<<".basm";
|
||||
bzzfile_in_compile << " -b " << path << name << ".bo";
|
||||
bzzfile_in_compile << " -d " << path << name << ".bdb ";
|
||||
bzzfile_in_compile << " -b " << path << name << "-update.bo";
|
||||
bzzfile_in_compile << " -d " << path << name << "-update.bdb ";
|
||||
bzzfile_in_compile << bzzfile_name;
|
||||
ROS_WARN("Launching buzz compilation for update: %s", bzzfile_in_compile.str().c_str());
|
||||
return system(bzzfile_in_compile.str().c_str());
|
||||
}
|
||||
|
||||
|
||||
|
||||
void collect_data(std::ofstream &logger){
|
||||
//fprintf(stdout,"start and end time in data collection Info : %f,%f",(double)begin,(double)end);
|
||||
double time_spent = (t2.tv_sec - t1.tv_sec) * 1000.0; //(double)(end - begin) / CLOCKS_PER_SEC;
|
||||
time_spent += (t2.tv_usec - t1.tv_usec) / 1000.0;
|
||||
//int bytecodesize=(int);
|
||||
logger<<(int)no_of_robot<<","<<neigh<<","<<(double)time_spent<<","<<(int)timer_steps<<","<<*(size_t*)updater->bcode_size<<","<<(int)*(uint8_t*)updater->update_no;
|
||||
timer_steps=0;
|
||||
neigh=0;
|
||||
//fprintf(stdout,"Data logger Info : %i , %i , %f , %ld , %i , %d \n",(int)no_of_robot,neigh,time_spent,*(size_t*)updater->bcode_size,(int)no_of_robot,*(uint8_t*)updater->update_no);
|
||||
//FILE *Fileptr;
|
||||
//Fileptr=fopen("/home/ubuntu/ROS_WS/update_logger.csv", "a");
|
||||
//fprintf(Fileptr,"%i,%i,%i,%i,%i,%u\n",(int)buzz_utility::get_robotid(),neigh,timer_steps,(int)*(size_t*)updater->bcode_size,(int)no_of_robot, *(uint8_t*)updater->update_no);
|
||||
//fclose(Fileptr);
|
||||
}
|
||||
//RID,update trigger,time steps taken,old byte code size, new bytecode size, patch size,update number,
|
||||
//Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent_counter,Patch_request_packets_sent_counter
|
||||
logger<<(int)no_of_robot<<","<<neigh<<","<<(double)time_spent<<","<<(int)timer_steps<<","<<old_byte_code_size<<","<<*(size_t*)updater->bcode_size<<","
|
||||
<<*(size_t*)updater->patch_size<<","<<(int)*(uint8_t*)updater->update_no<<","<<(int)packet_id_;
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -462,14 +462,8 @@ int create_stig_tables() {
|
|||
int buzz_script_set(const char* bo_filename,
|
||||
const char* bdbg_filename, int robot_id) {
|
||||
ROS_INFO(" Robot ID: %i" , robot_id);
|
||||
/* Reset the Buzz VM */
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
Robot_id = robot_id;
|
||||
VM = buzzvm_new((int)robot_id);
|
||||
/* Get rid of debug info */
|
||||
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
||||
DBG_INFO = buzzdebug_new();
|
||||
/* Read bytecode and fill in data structure */
|
||||
Robot_id=robot_id;
|
||||
/* Read bytecode from file and fill the bo buffer*/
|
||||
FILE* fd = fopen(bo_filename, "rb");
|
||||
if(!fd) {
|
||||
perror(bo_filename);
|
||||
|
|
@ -487,61 +481,11 @@ int create_stig_tables() {
|
|||
return 0;
|
||||
}
|
||||
fclose(fd);
|
||||
/* Read debug information */
|
||||
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);
|
||||
ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
|
||||
return 0;
|
||||
}
|
||||
/* Register hook functions */
|
||||
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
ROS_ERROR("[%i] Error registering hooks", Robot_id);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Initialize UAVSTATE variable
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "TURNEDOFF", 1));
|
||||
buzzvm_gstore(VM);
|
||||
|
||||
/* Create vstig tables
|
||||
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
||||
buzzvm_destroy(&VM);
|
||||
buzzdebug_destroy(&DBG_INFO);
|
||||
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
|
||||
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
|
||||
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
||||
return 0;
|
||||
}*/
|
||||
|
||||
/* Save bytecode file name */
|
||||
BO_FNAME = strdup(bo_filename);
|
||||
|
||||
// Execute the global part of the script
|
||||
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
|
||||
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
|
||||
return 0;
|
||||
}
|
||||
// Call the Init() function
|
||||
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
|
||||
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
|
||||
return 0;
|
||||
}
|
||||
/* All OK */
|
||||
|
||||
ROS_INFO("[%i] INIT DONE!!!", Robot_id);
|
||||
|
||||
return 1;//buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
|
||||
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
|
|
@ -717,7 +661,7 @@ int create_stig_tables() {
|
|||
void update_sensors(){
|
||||
/* Update sensors*/
|
||||
buzzuav_closures::buzzuav_update_battery(VM);
|
||||
buzzuav_closures::buzzuav_update_xbee_status(VM);
|
||||
buzzuav_closures::buzzuav_update_xbee_status(VM);
|
||||
buzzuav_closures::buzzuav_update_prox(VM);
|
||||
buzzuav_closures::buzzuav_update_currentpos(VM);
|
||||
buzzuav_closures::update_neighbors(VM);
|
||||
|
|
|
|||
|
|
@ -48,17 +48,6 @@ roscontroller::roscontroller(ros::NodeHandle &n_c, ros::NodeHandle &n_c_priv)
|
|||
} else {
|
||||
robot_id = strtol(robot_name.c_str() + 5, NULL, 10);
|
||||
}
|
||||
std::string path =
|
||||
bzzfile_name.substr(0, bzzfile_name.find_last_of("\\/")) + "/";
|
||||
path = path.substr(0, bzzfile_name.find_last_of("\\/"))+"/log/";
|
||||
std::string folder_check="mkdir -p "+path;
|
||||
system(folder_check.c_str());
|
||||
for(int i=5;i>0;i--){
|
||||
rename((path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i-1)+".log").c_str(),
|
||||
(path +"logger_"+ std::to_string((uint8_t)robot_id)+"_"+std::to_string(i)+".log").c_str());
|
||||
}
|
||||
path += "logger_"+std::to_string(robot_id)+"_0.log";
|
||||
log.open(path.c_str(), std::ios_base::trunc | std::ios_base::out);
|
||||
}
|
||||
|
||||
/*---------------------
|
||||
|
|
@ -196,11 +185,13 @@ void roscontroller::RosControllerRun()
|
|||
/* Set the Buzz bytecode */
|
||||
if (buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),
|
||||
robot_id)) {
|
||||
ROS_INFO("[%i] INIT DONE!!!", robot_id);
|
||||
int packet_loss_tmp,time_step=0;
|
||||
double cur_packet_loss=0;
|
||||
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());
|
||||
/*Intialize the update monitor*/
|
||||
init_update_monitor(bcfname.c_str(), standby_bo.c_str(),dbgfname.c_str(),robot_id);
|
||||
/*loop rate of ros*/
|
||||
ros::Rate loop_rate(BUZZRATE);
|
||||
///////////////////////////////////////////////////////
|
||||
|
|
@ -208,31 +199,11 @@ void roscontroller::RosControllerRun()
|
|||
//////////////////////////////////////////////////////
|
||||
//ROS_WARN("[%i] -----------------------STARTING MAIN LOOP!", robot_id);
|
||||
while (ros::ok() && !buzz_utility::buzz_script_done()) {
|
||||
/*Update neighbors position inside Buzz*/
|
||||
// buzz_closure::neighbour_pos_callback(neighbours_pos_map);
|
||||
|
||||
/*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh,
|
||||
neigh pos, RSSI val, Packet loss, filtered packet loss*/
|
||||
log<<ros::Time::now()<<",";
|
||||
log<<cur_pos.latitude << "," << cur_pos.longitude << ","
|
||||
<< cur_pos.altitude << ",";
|
||||
log << (int)no_of_robots<<",";
|
||||
map<int, buzz_utility::Pos_struct>::iterator it =
|
||||
neighbours_pos_map.begin();
|
||||
log << neighbours_pos_map.size()<< ",";
|
||||
for (; it != neighbours_pos_map.end(); ++it)
|
||||
{
|
||||
log << (double)it->second.x << "," << (double)it->second.y
|
||||
<< "," << (double)it->second.z << ",";
|
||||
}
|
||||
const uint8_t shrt_id= 0xFF;
|
||||
float result;
|
||||
|
||||
/*Neighbours of the robot published with id in respective topic*/
|
||||
neighbours_pos_publisher();
|
||||
send_MPpayload();
|
||||
/*Check updater state and step code*/
|
||||
// update_routine(bcfname.c_str(), dbgfname.c_str());
|
||||
update_routine();
|
||||
if(time_step==BUZZRATE){
|
||||
time_step=0;
|
||||
cur_packet_loss= 1 -( (double)packet_loss_tmp/(BUZZRATE*( (int)no_of_robots-1 )) );
|
||||
|
|
@ -245,8 +216,6 @@ void roscontroller::RosControllerRun()
|
|||
time_step++;
|
||||
}
|
||||
if(debug) ROS_WARN("CURRENT PACKET DROP : %f ",cur_packet_loss);
|
||||
/*Log In Msg queue size*/
|
||||
log<<(int)buzz_utility::get_inmsg_size()<<",";
|
||||
/*Step buzz script */
|
||||
buzz_utility::buzz_script_step();
|
||||
/*Prepare messages and publish them in respective topic*/
|
||||
|
|
@ -254,37 +223,14 @@ void roscontroller::RosControllerRun()
|
|||
/*call flight controler service to set command long*/
|
||||
flight_controller_service_call();
|
||||
/*Set multi message available after update*/
|
||||
//if (get_update_status())
|
||||
//{
|
||||
/* set_read_update_status();
|
||||
multi_msg = true;
|
||||
log << cur_pos.latitude << "," << cur_pos.longitude << ","
|
||||
<< cur_pos.altitude << ",";
|
||||
collect_data(log);
|
||||
map<int, buzz_utility::Pos_struct>::iterator it =
|
||||
neighbours_pos_map.begin();
|
||||
log << "," << neighbours_pos_map.size();
|
||||
for (; it != neighbours_pos_map.end(); ++it)
|
||||
{
|
||||
log << "," << (double)it->second.x << "," << (double)it->second.y
|
||||
<< "," << (double)it->second.z;
|
||||
}
|
||||
log << std::endl;*/
|
||||
//}
|
||||
if (get_update_status())
|
||||
{
|
||||
set_read_update_status();
|
||||
}
|
||||
/*Set ROBOTS variable for barrier in .bzz from neighbours count*/
|
||||
// no_of_robots=get_number_of_robots();
|
||||
get_number_of_robots();
|
||||
buzz_utility::set_robot_var(no_of_robots);
|
||||
/*Retrive the state of the graph and uav and log*/
|
||||
std::stringstream state_buff;
|
||||
state_buff << buzzuav_closures::getuavstate();
|
||||
log<<state_buff.str()<<std::endl;
|
||||
// if(neighbours_pos_map.size() >0) no_of_robots
|
||||
// =neighbours_pos_map.size()+1;
|
||||
// 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);
|
||||
//get_xbee_status(); // commented out because it may slow down the node too much, to be tested
|
||||
/*run once*/
|
||||
ros::spinOnce();
|
||||
|
|
@ -299,11 +245,8 @@ void roscontroller::RosControllerRun()
|
|||
/*sleep for the mentioned loop rate*/
|
||||
timer_step += 1;
|
||||
maintain_pos(timer_step);
|
||||
|
||||
// std::cout<< "HOME: " << home.latitude << ", " << home.longitude;
|
||||
}
|
||||
/* Destroy updater and Cleanup */
|
||||
// update_routine(bcfname.c_str(), dbgfname.c_str(),1);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -605,6 +548,7 @@ void roscontroller::prepare_msg_and_publish()
|
|||
tmp[2] = cur_pos.altitude;
|
||||
memcpy(position, tmp, 3 * sizeof(uint64_t));
|
||||
mavros_msgs::Mavlink payload_out;
|
||||
payload_out.payload64.push_back(XBEE_MESSAGE_CONSTANT);
|
||||
payload_out.payload64.push_back(position[0]);
|
||||
payload_out.payload64.push_back(position[1]);
|
||||
payload_out.payload64.push_back(position[2]);
|
||||
|
|
@ -621,30 +565,25 @@ void roscontroller::prepare_msg_and_publish()
|
|||
payload_out.sysid = (uint8_t)robot_id;
|
||||
payload_out.msgid = (uint32_t)message_number;
|
||||
|
||||
/*Log out message id and message size*/
|
||||
log<<(int)message_number<<",";
|
||||
log<<(int)out[0]<<",";
|
||||
/*publish prepared messages in respective topic*/
|
||||
payload_pub.publish(payload_out);
|
||||
delete[] out;
|
||||
delete[] payload_out_ptr;
|
||||
/*Check for updater message if present send*/
|
||||
/* if ((int)get_update_mode() != CODE_RUNNING && is_msg_present() == 1 &&
|
||||
multi_msg)
|
||||
if (is_msg_present())
|
||||
{
|
||||
uint8_t *buff_send = 0;
|
||||
uint16_t updater_msgSize = *(uint16_t *)(getupdate_out_msg_size());
|
||||
;
|
||||
int tot = 0;
|
||||
mavros_msgs::Mavlink update_packets;
|
||||
fprintf(stdout, "Transfering code \n");
|
||||
fprintf(stdout, "Appending code into message ...\n");
|
||||
fprintf(stdout, "Sent Update packet Size: %u \n", updater_msgSize);
|
||||
// allocate mem and clear it
|
||||
buff_send = (uint8_t *)malloc(sizeof(uint16_t) + updater_msgSize);
|
||||
memset(buff_send, 0, sizeof(uint16_t) + updater_msgSize);
|
||||
// Append updater msg size
|
||||
*(uint16_t *)(buff_send + tot) = updater_msgSize;
|
||||
// fprintf(stdout,"Updater sent msg size : %i \n", (int)updater_msgSize);
|
||||
tot += sizeof(uint16_t);
|
||||
// Append updater msgs
|
||||
memcpy(buff_send + tot, (uint8_t *)(getupdater_out_msg()), updater_msgSize);
|
||||
|
|
@ -669,9 +608,9 @@ void roscontroller::prepare_msg_and_publish()
|
|||
update_packets.sysid = (uint8_t)robot_id;
|
||||
update_packets.msgid = (uint32_t)message_number;
|
||||
payload_pub.publish(update_packets);
|
||||
multi_msg = false;
|
||||
//multi_msg = false;
|
||||
delete[] payload_64;
|
||||
}*/
|
||||
}
|
||||
}
|
||||
/*---------------------------------------------------------------------------------
|
||||
/Flight controller service call every step if there is a command set from bzz
|
||||
|
|
@ -1041,8 +980,7 @@ void roscontroller::SetStreamRate(int id, int rate, int on_off) {
|
|||
/*******************************************************************************************************/
|
||||
void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
|
||||
/*Check for Updater message, if updater message push it into updater FIFO*/
|
||||
if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT &&
|
||||
msg->payload64.size() > 5) {
|
||||
if ((uint64_t)msg->payload64[0] == (uint64_t)UPDATER_MESSAGE_CONSTANT) {
|
||||
uint16_t obt_msg_size = sizeof(uint64_t) * (msg->payload64.size());
|
||||
uint64_t message_obt[obt_msg_size];
|
||||
/* Go throught the obtained payload*/
|
||||
|
|
@ -1055,26 +993,20 @@ void roscontroller::payload_obt(const mavros_msgs::Mavlink::ConstPtr &msg) {
|
|||
/* Copy packet into temporary buffer neglecting update constant */
|
||||
memcpy((void *)pl, (void *)(message_obt + 1), obt_msg_size);
|
||||
uint16_t unMsgSize = *(uint16_t *)(pl);
|
||||
// uint16_t tot;
|
||||
// tot+=sizeof(uint16_t);
|
||||
fprintf(stdout, "Update packet, read msg size : %u \n", unMsgSize);
|
||||
fprintf(stdout, "Update packet received, read msg size : %u \n", unMsgSize);
|
||||
if (unMsgSize > 0) {
|
||||
code_message_inqueue_append((uint8_t *)(pl + sizeof(uint16_t)),
|
||||
unMsgSize);
|
||||
// fprintf(stdout,"before in queue process : utils\n");
|
||||
code_message_inqueue_process();
|
||||
// fprintf(stdout,"after in queue process : utils\n");
|
||||
}
|
||||
free(pl);
|
||||
}
|
||||
/*BVM FIFO message*/
|
||||
else if (msg->payload64.size() > 3) {
|
||||
uint64_t message_obt[msg->payload64.size()];
|
||||
else if (msg->payload64[0]==(uint64_t)XBEE_MESSAGE_CONSTANT) {
|
||||
uint64_t message_obt[msg->payload64.size()-1];
|
||||
/* Go throught the obtained payload*/
|
||||
for (int i = 0; i < (int)msg->payload64.size(); i++) {
|
||||
message_obt[i] = (uint64_t)msg->payload64[i];
|
||||
// cout<<"[Debug:] obtaind message "<<message_obt[i]<<endl;
|
||||
// i++;
|
||||
for (int i = 1; i < (int)msg->payload64.size(); i++) {
|
||||
message_obt[i-1] = (uint64_t)msg->payload64[i];
|
||||
}
|
||||
/* Extract neighbours position from payload*/
|
||||
double neighbours_pos_payload[3];
|
||||
|
|
|
|||
Loading…
Reference in New Issue