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

reformat comments

This commit is contained in:
dave 2017-12-13 12:47:42 -05:00
parent d42d0a0cdb
commit 276d46ee83
6 changed files with 1362 additions and 1250 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
include/buzzuav_closures.h
View File

@ -33,46 +33,95 @@ typedef enum {
*/ */
int buzzros_print(buzzvm_t vm); int buzzros_print(buzzvm_t vm);
void setWPlist(std::string path); void setWPlist(std::string path);
/* /*
* buzzuav_goto(latitude,longitude,altitude) function in Buzz * closure to move following a vector
* commands the UAV to go to a position supplied
*/ */
int buzz_floor(buzzvm_t vm);
int buzzuav_moveto(buzzvm_t vm); int buzzuav_moveto(buzzvm_t vm);
/*
* closure to store a new GPS goal
*/
int buzzuav_storegoal(buzzvm_t vm); int buzzuav_storegoal(buzzvm_t vm);
/*
* closure to control the gimbal
*/
int buzzuav_setgimbal(buzzvm_t vm); int buzzuav_setgimbal(buzzvm_t vm);
/*
* parse a csv list of waypoints
*/
void parse_gpslist(); void parse_gpslist();
/*
* closure to take a picture
*/
int buzzuav_takepicture(buzzvm_t vm); int buzzuav_takepicture(buzzvm_t vm);
/* Returns the current command from local variable*/ /*
* Returns the current command from local variable
*/
int getcmd(); int getcmd();
/*Sets goto position from rc client*/ /*
* Sets goto position from rc client
*/
void rc_set_goto(int id, double latitude, double longitude, double altitude); void rc_set_goto(int id, double latitude, double longitude, double altitude);
/*Sets gimbal orientation from rc client*/ /*
*Sets gimbal orientation from rc client
*/
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t); void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t);
/*sets rc requested command */ /*
* sets rc requested command
*/
void rc_call(int rc_cmd); void rc_call(int rc_cmd);
/* sets the battery state */ /*
* sets the battery state
*/
void set_battery(float voltage, float current, float remaining); void set_battery(float voltage, float current, float remaining);
/*
* sets the xbee network status
*/
void set_deque_full(bool state); void set_deque_full(bool state);
void set_rssi(float value); void set_rssi(float value);
void set_raw_packet_loss(float value); void set_raw_packet_loss(float value);
void set_filtered_packet_loss(float value); void set_filtered_packet_loss(float value);
void set_api_rssi(float value); void set_api_rssi(float value);
/* sets current position */ /*
* sets current position
*/
void set_currentpos(double latitude, double longitude, double altitude); void set_currentpos(double latitude, double longitude, double altitude);
/*retuns the current go to position */ /*
* returns the current go to position
*/
double* getgoto(); double* getgoto();
/*
* returns the current Buzz state
*/
std::string getuavstate(); std::string getuavstate();
/*
* returns the gimbal commands
*/
float* getgimbal(); float* getgimbal();
/* updates flight status*/ /*
*updates flight status
*/
void flight_status_update(uint8_t state); void flight_status_update(uint8_t state);
/* Update neighbors table */ /*
*Update neighbors table
*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation); void neighbour_pos_callback(int id, float range, float bearing, float elevation);
/*
* update neighbors from in msgs
*/
void update_neighbors(buzzvm_t vm); void update_neighbors(buzzvm_t vm);
/*
* closure to add a neighbor status
*/
int buzzuav_addNeiStatus(buzzvm_t vm); int buzzuav_addNeiStatus(buzzvm_t vm);
/*
* returns the current array of neighbors status
*/
mavros_msgs::Mavlink get_status(); mavros_msgs::Mavlink get_status();
/*Flight status*/ /*
*Flight status
*/
void set_obstacle_dist(float dist[]); void set_obstacle_dist(float dist[]);
/* /*
@ -91,7 +140,8 @@ int buzzuav_disarm(buzzvm_t vm);
*/ */
int buzzuav_land(buzzvm_t vm); int buzzuav_land(buzzvm_t vm);
/* Command the UAV to go to home location /*
* Command the UAV to go to home location
*/ */
int buzzuav_gohome(buzzvm_t vm); int buzzuav_gohome(buzzvm_t vm);
@ -107,7 +157,9 @@ int buzzuav_update_xbee_status(buzzvm_t vm);
* Updates current position in Buzz * Updates current position in Buzz
*/ */
int buzzuav_update_currentpos(buzzvm_t vm); int buzzuav_update_currentpos(buzzvm_t vm);
int buzzuav_update_targets(buzzvm_t vm); /*
* add new target in the BVM
*/
int buzzuav_addtargetRB(buzzvm_t vm); int buzzuav_addtargetRB(buzzvm_t vm);
/* /*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it * Updates flight status and rc command in Buzz, put it in a tabel to acess it
@ -121,10 +173,10 @@ int buzzuav_update_flight_status(buzzvm_t vm);
* Proximity and ground sensors to do !!!! * Proximity and ground sensors to do !!!!
*/ */
int buzzuav_update_prox(buzzvm_t vm); int buzzuav_update_prox(buzzvm_t vm);
/*
* returns the current FC command
*/
int bzz_cmd(); int bzz_cmd();
int dummy_closure(buzzvm_t vm); int dummy_closure(buzzvm_t vm);
//#endif
} }

2
readme.md
View File

@ -99,4 +99,4 @@ References
------ ------
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843 * ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
* Over-The-Air Updates for Robotic Swarms. Accepted by IEEE Software (September 2017 - pending minor revision). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G. * Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.

189
src/buzz_utility.cpp
View File

@ -24,10 +24,10 @@ std::map<int, Pos_struct> users_map;
/****************************************/ /****************************************/
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
uint16_t* u64_cvt_u16(uint64_t u64) uint16_t* u64_cvt_u16(uint64_t u64)
/*
/ Deserializes uint64_t into 4 uint16_t, freeing out is left to the user
------------------------------------------------------------------------*/
{ {
uint16_t* out = new uint16_t[4]; uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF; uint32_t int32_1 = u64 & 0xFFFFFFFF;
@ -36,61 +36,66 @@ uint16_t* u64_cvt_u16(uint64_t u64)
out[1] = (int32_1 & (0xFFFF0000)) >> 16; out[1] = (int32_1 & (0xFFFF0000)) >> 16;
out[2] = int32_2 & 0xFFFF; out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000)) >> 16; out[3] = (int32_2 & (0xFFFF0000)) >> 16;
// DEBUG
// cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" "; // cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out; return out;
} }
int get_robotid() int get_robotid()
/*
/ return this robot ID
------------------------------------------------------------------------*/
{ {
return Robot_id; return Robot_id;
} }
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
/*******************************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _______________________________________________________________________________________________________________ */
/*| | |*/
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
void in_msg_append(uint64_t* payload) void in_msg_append(uint64_t* payload)
/*
/ Appends obtained messages to buzz in message Queue
---------------------------------------------------------------------
/ Message format of payload (Each slot is uint64_t)
/ _______________________________________________________________________________________________________________
/| | |
/|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|
/|__________________________________________________________________________|____________________________________|
---------------------------------------------------------------------------------------------------------------------*/
{ {
/* Go through messages and append them to the vector */ // Go through messages and append them to the vector
uint16_t* data = u64_cvt_u16((uint64_t)payload[0]); uint16_t* data = u64_cvt_u16((uint64_t)payload[0]);
/*Size is at first 2 bytes*/ // Size is at first 2 bytes
uint16_t size = data[0] * sizeof(uint64_t); uint16_t size = data[0] * sizeof(uint64_t);
delete[] data; delete[] data;
uint8_t* pl = (uint8_t*)malloc(size); uint8_t* pl = (uint8_t*)malloc(size);
/* Copy packet into temporary buffer */ // Copy packet into temporary buffer
memcpy(pl, payload, size); memcpy(pl, payload, size);
IN_MSG.push_back(pl); IN_MSG.push_back(pl);
} }
void in_message_process() void in_message_process()
/*
/ Process msgs in
---------------------------------------------------------------------------------------------------------------------*/
{ {
while (!IN_MSG.empty()) while (!IN_MSG.empty())
{ {
/* Go through messages and append them to the FIFO */ // Go through messages and append them to the FIFO
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front(); uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
size_t tot = 0; size_t tot = 0;
/*Size is at first 2 bytes*/ // Size is at first 2 bytes
uint16_t size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t); uint16_t size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/*Decode neighbor Id*/ // Decode neighbor Id
uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot); uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/* Go through the messages until there's nothing else to read */ // Go through the messages until there's nothing else to read
uint16_t unMsgSize = 0; uint16_t unMsgSize = 0;
/*Obtain Buzz messages push it into queue*/ // Obtain Buzz messages push it into queue
do do
{ {
/* Get payload size */ // Get payload size
unMsgSize = *(uint16_t*)(first_INmsg + tot); unMsgSize = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */ // Append message to the Buzz input message queue
if (unMsgSize > 0 && unMsgSize <= size - tot) if (unMsgSize > 0 && unMsgSize <= size - tot)
{ {
buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize)); buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize));
@ -100,33 +105,34 @@ void in_message_process()
free(first_INmsg); free(first_INmsg);
IN_MSG.erase(IN_MSG.begin()); IN_MSG.erase(IN_MSG.begin());
} }
/* Process messages VM call*/ // Process messages VM call*
buzzvm_process_inmsgs(VM); buzzvm_process_inmsgs(VM);
} }
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
uint64_t* obt_out_msg() uint64_t* obt_out_msg()
/*
/ Obtains messages from buzz out message Queue
-------------------------------------------------*/
{ {
/* Process out messages */ // Process out messages
buzzvm_process_outmsgs(VM); buzzvm_process_outmsgs(VM);
uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE); uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE); memset(buff_send, 0, MAX_MSG_SIZE);
/*Taking into consideration the sizes included at the end*/ // Taking into consideration the sizes included at the end
ssize_t tot = sizeof(uint16_t); ssize_t tot = sizeof(uint16_t);
/* Send robot id */ // Send robot id
*(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot; *(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot;
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/* Send messages from FIFO */ // Send messages from FIFO
do do
{ {
/* Are there more messages? */ // Are there more messages?
if (buzzoutmsg_queue_isempty(VM)) if (buzzoutmsg_queue_isempty(VM))
break; break;
/* Get first message */ // Get first message
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM); buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */ // Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes
// DEBUG
// ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t))); // ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE) if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE)
{ {
@ -134,15 +140,15 @@ uint64_t* obt_out_msg()
break; break;
} }
/* Add message length to data buffer */ // Add message length to data buffer
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m); *(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/* Add payload to data buffer */ // Add payload to data buffer
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m)); memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
tot += buzzmsg_payload_size(m); tot += buzzmsg_payload_size(m);
/* Get rid of message */ // Get rid of message
buzzoutmsg_queue_next(VM); buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m); buzzmsg_payload_destroy(&m);
} while (1); } while (1);
@ -154,18 +160,18 @@ uint64_t* obt_out_msg()
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);
/*for(int i=0;i<total_size;i++){ // DEBUG
cout<<" payload from out msg "<<*(payload_64+i)<<endl; // for(int i=0;i<total_size;i++){
}*/ // cout<<" payload from out msg "<<*(payload_64+i)<<endl;
/* Send message */ //}
// Send message
return payload_64; return payload_64;
} }
/****************************************/
/*Buzz script not able to load*/
/****************************************/
static const char* buzz_error_info() static const char* buzz_error_info()
/*
/ Buzz script not able to load
---------------------------------*/
{ {
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc); buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
char* msg; char* msg;
@ -181,11 +187,10 @@ static const char* buzz_error_info()
return msg; return msg;
} }
/****************************************/
/*Buzz hooks that can be used inside .bzz file*/
/****************************************/
static int buzz_register_hooks() static int buzz_register_hooks()
/*
/ Buzz hooks that can be used inside .bzz file
------------------------------------------------*/
{ {
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
@ -233,11 +238,10 @@ static int buzz_register_hooks()
return VM->state; return VM->state;
} }
/**************************************************/
/*Register dummy Buzz hooks for test during update*/
/**************************************************/
static int testing_buzz_register_hooks() static int testing_buzz_register_hooks()
/*
/ Register dummy Buzz hooks for test during update
---------------------------------------------------*/
{ {
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print)); buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
@ -285,14 +289,14 @@ static int testing_buzz_register_hooks()
return VM->state; return VM->state;
} }
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id) int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id)
/*
/ Sets the .bzz and .bdbg file
---------------------------------*/
{ {
ROS_INFO(" Robot ID: %i", robot_id); ROS_INFO(" Robot ID: %i", robot_id);
Robot_id = robot_id; Robot_id = robot_id;
/* Read bytecode from file and fill the bo buffer*/ // Read bytecode from file and fill the bo buffer
FILE* fd = fopen(bo_filename, "rb"); FILE* fd = fopen(bo_filename, "rb");
if (!fd) if (!fd)
{ {
@ -313,16 +317,16 @@ int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robo
} }
fclose(fd); fclose(fd);
/* Save bytecode file name */ // Save bytecode file name
BO_FNAME = strdup(bo_filename); BO_FNAME = strdup(bo_filename);
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size); return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
} }
/****************************************/
/*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)
/*
/ Sets a new update
-----------------------*/
{ {
// Reset the Buzz VM // Reset the Buzz VM
if (VM) if (VM)
@ -379,10 +383,10 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_
return 1; return 1;
} }
/****************************************/
/*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)
/*
/ Performs a initialization test
-----------------------------------*/
{ {
// Reset the Buzz VM // Reset the Buzz VM
if (VM) if (VM)
@ -439,38 +443,40 @@ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t
return 1; return 1;
} }
/****************************************/
/*Swarm struct*/
/****************************************/
struct buzzswarm_elem_s struct buzzswarm_elem_s
/*
/ Swarm struct
----------------*/
{ {
buzzdarray_t swarms; buzzdarray_t swarms;
uint16_t age; uint16_t age;
}; };
typedef struct buzzswarm_elem_s* buzzswarm_elem_t; typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
/*Step through the buzz script*/
void update_sensors() void update_sensors()
/*
/ Update from all external inputs
-------------------------------*/
{ {
/* Update sensors*/ // Update sensors
buzzuav_closures::buzzuav_update_battery(VM); 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_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM); buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM); buzzuav_closures::update_neighbors(VM);
buzzuav_closures::buzzuav_update_targets(VM);
// update_users();
buzzuav_closures::buzzuav_update_flight_status(VM); buzzuav_closures::buzzuav_update_flight_status(VM);
} }
void buzz_script_step() void buzz_script_step()
/*
/ Step through the buzz script
-------------------------------*/
{ {
/*Process available messages*/ // Process available messages
in_message_process(); in_message_process();
/*Update sensors*/ // Update sensors
update_sensors(); update_sensors();
/* Call Buzz step() function */ // Call Buzz step() function
if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY)
{ {
ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info()); ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info());
@ -478,11 +484,10 @@ void buzz_script_step()
} }
} }
/****************************************/
/*Destroy the bvm and other resorces*/
/****************************************/
void buzz_script_destroy() void buzz_script_destroy()
/*
/ Destroy the bvm and other related ressources
-------------------------------------*/
{ {
if (VM) if (VM)
{ {
@ -499,21 +504,20 @@ void buzz_script_destroy()
ROS_INFO("Script execution stopped."); ROS_INFO("Script execution stopped.");
} }
/****************************************/
/****************************************/
/****************************************/
/*Execution completed*/
/****************************************/
int buzz_script_done() int buzz_script_done()
/*
/ Check if the BVM execution terminated
---------------------------------------*/
{ {
return VM->state != BUZZVM_STATE_READY; return VM->state != BUZZVM_STATE_READY;
} }
int update_step_test() int update_step_test()
/*
/ Step test for the update mechanism
------------------------------------*/
{ {
/*Process available messages*/ // Process available messages
in_message_process(); in_message_process();
buzzuav_closures::buzzuav_update_battery(VM); buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM); buzzuav_closures::buzzuav_update_prox(VM);
@ -533,11 +537,17 @@ int update_step_test()
} }
buzzvm_t get_vm() buzzvm_t get_vm()
/*
/ return the BVM
----------------*/
{ {
return VM; return VM;
} }
void set_robot_var(int ROBOTS) void set_robot_var(int ROBOTS)
/*
/ set swarm size in the BVM
-----------------------------*/
{ {
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, ROBOTS); buzzvm_pushi(VM, ROBOTS);
@ -545,6 +555,9 @@ void set_robot_var(int ROBOTS)
} }
int get_inmsg_size() int get_inmsg_size()
/*
/ get the incoming msgs size
------------------------------*/
{ {
return IN_MSG.size(); return IN_MSG.size();
} }

236
src/buzzuav_closures.cpp
View File

@ -13,8 +13,6 @@ namespace buzzuav_closures
{ {
// TODO: Minimize the required global variables and put them in the header // TODO: Minimize the required global variables and put them in the header
// static const rosbzz_node::roscontroller* roscontroller_ptr; // static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/
// void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
static double goto_pos[3]; static double goto_pos[3];
static double rc_goto_pos[3]; static double rc_goto_pos[3];
static float rc_gimbal[4]; static float rc_gimbal[4];
@ -43,6 +41,9 @@ std::map<int, buzz_utility::neighbors_status> neighbors_status_map;
/****************************************/ /****************************************/
int buzzros_print(buzzvm_t vm) int buzzros_print(buzzvm_t vm)
/*
/ Buzz closure to print out
----------------------------------------------------------- */
{ {
std::ostringstream buffer(std::ostringstream::ate); std::ostringstream buffer(std::ostringstream::ate);
buffer << buzz_utility::get_robotid(); buffer << buzz_utility::get_robotid();
@ -90,11 +91,17 @@ int buzzros_print(buzzvm_t vm)
} }
void setWPlist(string path) void setWPlist(string path)
/*
/ set the absolute path for a csv list of waypoints
----------------------------------------------------------- */
{ {
WPlistname = path + "include/graphs/waypointlist.csv"; WPlistname = path + "include/graphs/waypointlist.csv";
} }
float constrainAngle(float x) float constrainAngle(float x)
/*
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{ {
x = fmod(x, 2 * M_PI); x = fmod(x, 2 * M_PI);
if (x < 0.0) if (x < 0.0)
@ -102,10 +109,10 @@ float constrainAngle(float x)
return x; return x;
} }
/*----------------------------------------/ void rb_from_gps(double nei[], double out[], double cur[])
/*
/ Compute Range and Bearing from 2 GPS set of coordinates / Compute Range and Bearing from 2 GPS set of coordinates
/----------------------------------------*/ /----------------------------------------*/
void rb_from_gps(double nei[], double out[], double cur[])
{ {
double d_lon = nei[1] - cur[1]; double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0]; double d_lat = nei[0] - cur[0];
@ -117,6 +124,9 @@ void rb_from_gps(double nei[], double out[], double cur[])
} }
void parse_gpslist() void parse_gpslist()
/*
/ parse a csv of GPS targets
/----------------------------------------*/
{ {
// Open file: // Open file:
ROS_INFO("WP list file: %s", WPlistname.c_str()); ROS_INFO("WP list file: %s", WPlistname.c_str());
@ -147,6 +157,7 @@ void parse_gpslist()
lat = atof(strtok(NULL, DELIMS)); lat = atof(strtok(NULL, DELIMS));
alt = atoi(strtok(NULL, DELIMS)); alt = atoi(strtok(NULL, DELIMS));
tilt = atoi(strtok(NULL, DELIMS)); tilt = atoi(strtok(NULL, DELIMS));
// DEBUG
// ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid); // ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
RB_arr.latitude = lat; RB_arr.latitude = lat;
RB_arr.longitude = lon; RB_arr.longitude = lon;
@ -164,15 +175,15 @@ void parse_gpslist()
fin.close(); fin.close();
} }
/*----------------------------------------/ int buzzuav_moveto(buzzvm_t vm)
/*
/ Buzz closure to move following a 2D vector / Buzz closure to move following a 2D vector
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm)
{ {
buzzvm_lnum_assert(vm, 3); buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* dx */ buzzvm_lload(vm, 1); // dx
buzzvm_lload(vm, 2); /* dy */ buzzvm_lload(vm, 2); // dy
buzzvm_lload(vm, 3); /* dheight */ buzzvm_lload(vm, 3); //* dheight
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
@ -182,55 +193,17 @@ int buzzuav_moveto(buzzvm_t vm)
goto_pos[0] = dx; goto_pos[0] = dx;
goto_pos[1] = dy; goto_pos[1] = dy;
goto_pos[2] = height + dh; goto_pos[2] = height + dh;
// DEBUG
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], // ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
// goto_pos[1], goto_pos[2]); // goto_pos[1], goto_pos[2]);
buzz_cmd = COMMAND_MOVETO; // TO DO what should we use buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros?
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_update_targets(buzzvm_t vm)
{
if (vm->state != BUZZVM_STATE_READY)
return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1));
buzzvm_pusht(vm);
buzzobj_t targettbl = buzzvm_stack_at(vm, 1);
double rb[3], tmp[3];
map<int, buzz_utility::RB_struct>::iterator it;
for (it = targets_map.begin(); it != targets_map.end(); ++it)
{
tmp[0] = (it->second).latitude;
tmp[1] = (it->second).longitude;
tmp[2] = height;
rb_from_gps(tmp, rb, cur_pos);
// ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl);
// When we get here, the "targets" table is on top of the stack
// ROS_INFO("Buzz_utility will save user %i.", it->first);
// Push user id
buzzvm_pushi(vm, it->first);
// Create entry table
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
// Insert range
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, rb[0]);
buzzvm_tput(vm);
// Insert longitude
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, rb[1]);
buzzvm_tput(vm);
// Save entry into data table
buzzvm_push(vm, entry);
buzzvm_tput(vm);
}
buzzvm_gstore(vm);
return vm->state;
}
int buzzuav_addtargetRB(buzzvm_t vm) int buzzuav_addtargetRB(buzzvm_t vm)
/*
/ Buzz closure to add a target (goal) GPS
/----------------------------------------*/
{ {
buzzvm_lnum_assert(vm, 3); buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // longitude buzzvm_lload(vm, 1); // longitude
@ -249,7 +222,6 @@ int buzzuav_addtargetRB(buzzvm_t vm)
rb_from_gps(tmp, rb, cur_pos); rb_from_gps(tmp, rb, cur_pos);
if (fabs(rb[0]) < 100.0) if (fabs(rb[0]) < 100.0)
{ {
// printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
buzz_utility::RB_struct RB_arr; buzz_utility::RB_struct RB_arr;
RB_arr.latitude = tmp[0]; RB_arr.latitude = tmp[0];
RB_arr.longitude = tmp[1]; RB_arr.longitude = tmp[1];
@ -260,6 +232,7 @@ int buzzuav_addtargetRB(buzzvm_t vm)
if (it != targets_map.end()) if (it != targets_map.end())
targets_map.erase(it); targets_map.erase(it);
targets_map.insert(make_pair(uid, RB_arr)); targets_map.insert(make_pair(uid, RB_arr));
// DEBUG
// ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude); // ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state; return vm->state;
} }
@ -270,6 +243,9 @@ int buzzuav_addtargetRB(buzzvm_t vm)
} }
int buzzuav_addNeiStatus(buzzvm_t vm) int buzzuav_addNeiStatus(buzzvm_t vm)
/*
/ closure to add neighbors status to the BVM
/----------------------------------------*/
{ {
buzzvm_lnum_assert(vm, 5); buzzvm_lnum_assert(vm, 5);
buzzvm_lload(vm, 1); // fc buzzvm_lload(vm, 1); // fc
@ -296,6 +272,9 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
} }
mavros_msgs::Mavlink get_status() mavros_msgs::Mavlink get_status()
/*
/ return neighbors status from BVM
/----------------------------------------*/
{ {
mavros_msgs::Mavlink payload_out; mavros_msgs::Mavlink payload_out;
map<int, buzz_utility::neighbors_status>::iterator it; map<int, buzz_utility::neighbors_status>::iterator it;
@ -307,27 +286,25 @@ mavros_msgs::Mavlink get_status()
payload_out.payload64.push_back(it->second.xbee); payload_out.payload64.push_back(it->second.xbee);
payload_out.payload64.push_back(it->second.flight_status); payload_out.payload64.push_back(it->second.flight_status);
} }
/*Add Robot id and message number to the published message*/ // Add Robot id and message number to the published message
payload_out.sysid = (uint8_t)neighbors_status_map.size(); payload_out.sysid = (uint8_t)neighbors_status_map.size();
/*payload_out.msgid = (uint32_t)message_number;
message_number++;*/
return payload_out; return payload_out;
} }
/*----------------------------------------/
int buzzuav_takepicture(buzzvm_t vm)
/*
/ Buzz closure to take a picture here. / Buzz closure to take a picture here.
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_takepicture(buzzvm_t vm)
{ {
// cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
buzz_cmd = COMMAND_PICTURE; buzz_cmd = COMMAND_PICTURE;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*----------------------------------------/ int buzzuav_setgimbal(buzzvm_t vm)
/*
/ Buzz closure to change locally the gimbal orientation / Buzz closure to change locally the gimbal orientation
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_setgimbal(buzzvm_t vm)
{ {
buzzvm_lnum_assert(vm, 4); buzzvm_lnum_assert(vm, 4);
buzzvm_lload(vm, 1); // time buzzvm_lload(vm, 1); // time
@ -343,15 +320,15 @@ int buzzuav_setgimbal(buzzvm_t vm)
rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value; rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value;
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value; rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]); ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
buzz_cmd = COMMAND_GIMBAL; buzz_cmd = COMMAND_GIMBAL;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*----------------------------------------/ int buzzuav_storegoal(buzzvm_t vm)
/*
/ Buzz closure to store locally a GPS destination from the fleet / Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/ /----------------------------------------*/
int buzzuav_storegoal(buzzvm_t vm)
{ {
buzzvm_lnum_assert(vm, 3); buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // altitude buzzvm_lload(vm, 1); // altitude
@ -380,21 +357,25 @@ int buzzuav_storegoal(buzzvm_t vm)
if (fabs(rb[0]) < 150.0) if (fabs(rb[0]) < 150.0)
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]); ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*----------------------------------------/
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
/----------------------------------------*/
int buzzuav_arm(buzzvm_t vm) int buzzuav_arm(buzzvm_t vm)
/*
/ Buzz closure to arm
/---------------------------------------*/
{ {
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM; cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n"); printf(" Buzz requested Arm \n");
buzz_cmd = COMMAND_ARM; buzz_cmd = COMMAND_ARM;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_disarm(buzzvm_t vm) int buzzuav_disarm(buzzvm_t vm)
/*
/ Buzz closure to disarm
/---------------------------------------*/
{ {
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1; cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n"); printf(" Buzz requested Disarm \n");
@ -402,10 +383,10 @@ int buzzuav_disarm(buzzvm_t vm)
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*---------------------------------------/
/ Buzz closure for basic UAV commands
/---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm) int buzzuav_takeoff(buzzvm_t vm)
/*
/ Buzz closure to takeoff
/---------------------------------------*/
{ {
buzzvm_lnum_assert(vm, 1); buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); /* Altitude */ buzzvm_lload(vm, 1); /* Altitude */
@ -419,6 +400,9 @@ int buzzuav_takeoff(buzzvm_t vm)
} }
int buzzuav_land(buzzvm_t vm) int buzzuav_land(buzzvm_t vm)
/*
/ Buzz closure to land
/-------------------------------------------------------------*/
{ {
cur_cmd = mavros_msgs::CommandCode::NAV_LAND; cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
printf(" Buzz requested Land !!! \n"); printf(" Buzz requested Land !!! \n");
@ -427,6 +411,9 @@ int buzzuav_land(buzzvm_t vm)
} }
int buzzuav_gohome(buzzvm_t vm) int buzzuav_gohome(buzzvm_t vm)
/*
/ Buzz closure to return Home
/-------------------------------------------------------------*/
{ {
cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n"); printf(" Buzz requested gohome !!! \n");
@ -434,20 +421,26 @@ int buzzuav_gohome(buzzvm_t vm)
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/*-------------------------------
/ Get/Set to transfer variable from Roscontroller to buzzuav
/------------------------------------*/
double* getgoto() double* getgoto()
/*
/ return the GPS goal
/-------------------------------------------------------------*/
{ {
return goto_pos; return goto_pos;
} }
float* getgimbal() float* getgimbal()
/*
/ return current gimbal commands
---------------------------------------*/
{ {
return rc_gimbal; return rc_gimbal;
} }
string getuavstate() string getuavstate()
/*
/ return current BVM state
---------------------------------------*/
{ {
static buzzvm_t VM = buzz_utility::get_vm(); static buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1)); buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
@ -458,11 +451,17 @@ string getuavstate()
} }
int getcmd() int getcmd()
/*
/ return current mavros command to the FC
---------------------------------------*/
{ {
return cur_cmd; return cur_cmd;
} }
int bzz_cmd() int bzz_cmd()
/*
/ return and clean the custom command from Buzz to the FC
----------------------------------------------------------*/
{ {
int cmd = buzz_cmd; int cmd = buzz_cmd;
buzz_cmd = 0; buzz_cmd = 0;
@ -470,6 +469,9 @@ int bzz_cmd()
} }
void rc_set_goto(int id, double latitude, double longitude, double altitude) void rc_set_goto(int id, double latitude, double longitude, double altitude)
/*
/ update interface RC GPS goal input
-----------------------------------*/
{ {
rc_id = id; rc_id = id;
rc_goto_pos[0] = latitude; rc_goto_pos[0] = latitude;
@ -478,6 +480,9 @@ void rc_set_goto(int id, double latitude, double longitude, double altitude)
} }
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t) void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
/*
/ update interface RC gimbal control input
-----------------------------------*/
{ {
rc_id = id; rc_id = id;
rc_gimbal[0] = yaw; rc_gimbal[0] = yaw;
@ -487,17 +492,26 @@ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
} }
void rc_call(int rc_cmd_in) void rc_call(int rc_cmd_in)
/*
/ update interface RC command input
-----------------------------------*/
{ {
rc_cmd = rc_cmd_in; rc_cmd = rc_cmd_in;
} }
void set_obstacle_dist(float dist[]) void set_obstacle_dist(float dist[])
/*
/ update interface proximity value array
-----------------------------------*/
{ {
for (int i = 0; i < 5; i++) for (int i = 0; i < 5; i++)
obst[i] = dist[i]; obst[i] = dist[i];
} }
void set_battery(float voltage, float current, float remaining) void set_battery(float voltage, float current, float remaining)
/*
/ update interface battery value array
-----------------------------------*/
{ {
batt[0] = voltage; batt[0] = voltage;
batt[1] = current; batt[1] = current;
@ -505,6 +519,9 @@ void set_battery(float voltage, float current, float remaining)
} }
int buzzuav_update_battery(buzzvm_t vm) int buzzuav_update_battery(buzzvm_t vm)
/*
/ update BVM battery table
-----------------------------------*/
{ {
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
@ -524,6 +541,9 @@ int buzzuav_update_battery(buzzvm_t vm)
return vm->state; return vm->state;
} }
/*
/ Set of function to update interface variable of xbee network status
----------------------------------------------------------------------*/
void set_deque_full(bool state) void set_deque_full(bool state)
{ {
deque_full = state; deque_full = state;
@ -550,6 +570,9 @@ void set_api_rssi(float value)
} }
int buzzuav_update_xbee_status(buzzvm_t vm) int buzzuav_update_xbee_status(buzzvm_t vm)
/*
/ update BVM xbee_status table
-----------------------------------*/
{ {
buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
@ -577,16 +600,16 @@ int buzzuav_update_xbee_status(buzzvm_t vm)
return vm->state; return vm->state;
} }
/***************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude) void set_currentpos(double latitude, double longitude, double altitude)
/*
/ update interface position array
-----------------------------------*/
{ {
cur_pos[0] = latitude; cur_pos[0] = latitude;
cur_pos[1] = longitude; cur_pos[1] = longitude;
cur_pos[2] = altitude; cur_pos[2] = altitude;
} }
/*adds neighbours position*/ // adds neighbours position
void neighbour_pos_callback(int id, float range, float bearing, float elevation) void neighbour_pos_callback(int id, float range, float bearing, float elevation)
{ {
buzz_utility::Pos_struct pos_arr; buzz_utility::Pos_struct pos_arr;
@ -599,12 +622,12 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation)
neighbors_map.insert(make_pair(id, pos_arr)); neighbors_map.insert(make_pair(id, pos_arr));
} }
/* update at each step the VM table */ // update at each step the VM table
void update_neighbors(buzzvm_t vm) void update_neighbors(buzzvm_t vm)
{ {
/* Reset neighbor information */ // Reset neighbor information
buzzneighbors_reset(vm); buzzneighbors_reset(vm);
/* Get robot id and update neighbor information */ // Get robot id and update neighbor information
map<int, buzz_utility::Pos_struct>::iterator it; map<int, buzz_utility::Pos_struct>::iterator it;
for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it) for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it)
{ {
@ -612,8 +635,10 @@ void update_neighbors(buzzvm_t vm)
} }
} }
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm) int buzzuav_update_currentpos(buzzvm_t vm)
/*
/ Update the BVM position table
/------------------------------------------------------*/
{ {
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
@ -634,16 +659,18 @@ int buzzuav_update_currentpos(buzzvm_t vm)
} }
void flight_status_update(uint8_t state) void flight_status_update(uint8_t state)
/*
/ Update the interface status variable
/------------------------------------------------------*/
{ {
status = state; status = state;
} }
/*---------------------------------------------------- int buzzuav_update_flight_status(buzzvm_t vm)
/*
/ Create the generic robot table with status, remote controller current comand and destination / Create the generic robot table with status, remote controller current comand and destination
/ and current position of the robot / and current position of the robot
/ TODO: change global name for robot
/------------------------------------------------------*/ /------------------------------------------------------*/
int buzzuav_update_flight_status(buzzvm_t vm)
{ {
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
@ -657,7 +684,6 @@ int buzzuav_update_flight_status(buzzvm_t vm)
buzzvm_pushi(vm, status); buzzvm_pushi(vm, status);
buzzvm_tput(vm); buzzvm_tput(vm);
buzzvm_gstore(vm); buzzvm_gstore(vm);
// also set rc_controllers goto
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);
@ -680,32 +706,31 @@ int buzzuav_update_flight_status(buzzvm_t vm)
return vm->state; return vm->state;
} }
/******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script
proximity[0].angle and proximity[0].value - front
"" "" "" - right and back
proximity[3].angle and proximity[3].value - left
proximity[4].angle = -1 and proximity[4].value -bottom */
/******************************************************/
int buzzuav_update_prox(buzzvm_t vm) int buzzuav_update_prox(buzzvm_t vm)
/*
/ Create an obstacle Buzz table from proximity sensors
/ Acessing proximity in buzz script
/ proximity[0].angle and proximity[0].value - front
/ "" "" "" - right and back
/ proximity[3].angle and proximity[3].value - left
/ proximity[4].angle = -1 and proximity[4].value -bottom
-------------------------------------------------------------*/
{ {
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1); buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
buzzvm_gstore(vm); buzzvm_gstore(vm);
/* Fill into the proximity table */ // Fill into the proximity table
buzzobj_t tProxRead; buzzobj_t tProxRead;
float angle = 0; float angle = 0;
for (size_t i = 0; i < 4; ++i) for (size_t i = 0; i < 4; ++i)
{ {
/* Create table for i-th read */ // Create table for i-th read
buzzvm_pusht(vm); buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1); tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm); buzzvm_pop(vm);
/* Fill in the read */ // Fill in the read
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[i + 1]); buzzvm_pushf(vm, obst[i + 1]);
@ -714,19 +739,19 @@ int buzzuav_update_prox(buzzvm_t vm)
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle); buzzvm_pushf(vm, angle);
buzzvm_tput(vm); buzzvm_tput(vm);
/* Store read table in the proximity table */ // Store read table in the proximity table
buzzvm_push(vm, tProxTable); buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, i); buzzvm_pushi(vm, i);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_tput(vm); buzzvm_tput(vm);
angle += 1.5708; angle += 1.5708;
} }
/* Create table for bottom read */ // Create table for bottom read
angle = -1; angle = -1;
buzzvm_pusht(vm); buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1); tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm); buzzvm_pop(vm);
/* Fill in the read */ // Fill in the read
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[0]); buzzvm_pushf(vm, obst[0]);
@ -735,7 +760,7 @@ int buzzuav_update_prox(buzzvm_t vm)
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0)); buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle); buzzvm_pushf(vm, angle);
buzzvm_tput(vm); buzzvm_tput(vm);
/*Store read table in the proximity table*/ // Store read table in the proximity table
buzzvm_push(vm, tProxTable); buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, 4); buzzvm_pushi(vm, 4);
buzzvm_push(vm, tProxRead); buzzvm_push(vm, tProxRead);
@ -743,11 +768,10 @@ int buzzuav_update_prox(buzzvm_t vm)
return vm->state; return vm->state;
} }
/**********************************************/
/*Dummy closure for use during update testing */
/**********************************************/
int dummy_closure(buzzvm_t vm) int dummy_closure(buzzvm_t vm)
/*
/ Dummy closure for use during update testing
----------------------------------------------------*/
{ {
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }

12
src/rosbuzz.cpp
View File

@ -8,19 +8,17 @@
#include "roscontroller.h" #include "roscontroller.h"
/**
* This program implements Buzz node in ros using mavros_msgs.
*/
int main(int argc, char** argv) int main(int argc, char** argv)
/*
/ This program implements Buzz in a ROS node using mavros_msgs.
-----------------------------------------------------------------*/
{ {
/*Initialize rosBuzz node*/ // Initialize rosBuzz node
ros::init(argc, argv, "rosBuzz"); ros::init(argc, argv, "rosBuzz");
ros::NodeHandle nh_priv("~"); ros::NodeHandle nh_priv("~");
ros::NodeHandle nh; ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv); rosbzz_node::roscontroller RosController(nh, nh_priv);
/*Register ros controller object inside buzz*/
// buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun(); RosController.RosControllerRun();
return 0; return 0;
} }

2085
src/roscontroller.cpp
View File

File diff suppressed because it is too large Load Diff