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

added a new namespace for update functions

This commit is contained in:
vivek-shankar 2018-06-06 22:55:49 -04:00
parent 58c09093a7
commit e953de6191
3 changed files with 772 additions and 762 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/buzz_update.h
View File

@ -3,18 +3,26 @@
#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 CODE_REQUEST_PADDING = 250;
static const uint16_t MIN_UPDATE_PACKET = 251; static const uint16_t MIN_UPDATE_PACKET = 251;
static const uint16_t UPDATE_CODE_HEADER_SIZE = 5; static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
@ -159,5 +167,5 @@ int compile_bzz(std::string bzz_file);
/***************************************************/ /***************************************************/
void updates_set_robots(int robots); void updates_set_robots(int robots);
}
#endif #endif

22
src/buzz_update.cpp
View File

@ -1,14 +1,14 @@
#include <stdio.h> /** @file buzz_utility.cpp
#include <stdlib.h> * @version 1.0
#include <fcntl.h> * @date 27.09.2016
#include <unistd.h> * @brief Buzz Implementation as a node in ROS.
#include <sys/inotify.h> * @author Vivek Shankar Varadharajan and David St-Onge
#include "buzz_update.h" * @copyright 2016 MistLab. All rights reserved.
#include <buzz_utility.h> */
#include <string.h>
#include <buzz/buzzdebug.h>
#include <sys/time.h>
#include "buzz_update.h"
namespace buzz_update{
/*Temp for data collection*/ /*Temp for data collection*/
// static int neigh=-1; // static int neigh=-1;
static struct timeval t1, t2; static struct timeval t1, t2;
@ -36,6 +36,7 @@ static size_t old_byte_code_size = 0;
/*Initialize updater*/ /*Initialize updater*/
void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id) void 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();
Robot_id = robot_id; Robot_id = robot_id;
dbgf_name = dbgfname; dbgf_name = dbgfname;
bcfname = bo_filename; bcfname = bo_filename;
@ -706,3 +707,4 @@ Patch_packets_received_counter,Patch_request_packets_received,Patch_packets_sent
<< old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << "," << old_byte_code_size << "," << *(size_t*)updater->bcode_size << "," << *(size_t*)updater->patch_size << ","
<< (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_; << (int)*(uint8_t*)updater->update_no << "," << (int)packet_id_;
}*/ }*/
}

20
src/roscontroller.cpp
View File

@ -26,7 +26,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());
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);
@ -106,7 +106,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); 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
@ -119,7 +119,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(); buzz_update::update_routine();
ROS_WARN("OUT OF UPDATE ROUTINE"); ROS_WARN("OUT OF UPDATE ROUTINE");
if (time_step == BUZZRATE) if (time_step == BUZZRATE)
{ {
@ -152,7 +152,7 @@ void roscontroller::RosControllerRun()
// 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); 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();
@ -570,10 +570,10 @@ with size......... | /
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 (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;
@ -586,10 +586,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));
@ -1012,8 +1012,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);
} }