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

Added xbee status update functions

This commit is contained in:
pyhs 2017-07-05 09:37:35 -04:00
parent 82adb66f7b
commit 8552cf79c8
6 changed files with 140 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/buzz_utility.h
View File

@ -49,6 +49,8 @@ uint64_t* obt_out_msg();
void update_sensors();
void update_xbee_status();
int buzz_script_set(const char* bo_filename,
const char* bdbg_filename, int robot_id);

10
include/buzzuav_closures.h
View File

@ -47,6 +47,11 @@ void rc_set_goto(double pos[]);
void rc_call(int rc_cmd);
/* sets the battery state */
void set_battery(float voltage,float current,float remaining);
void set_deque_full(bool state);
void set_rssi(float value);
void set_raw_packet_loss(float value);
void set_filtered_packet_loss(float value);
void set_api_rssi(float value);
/* sets current position */
void set_currentpos(double latitude, double longitude, double altitude);
/*retuns the current go to position */
@ -83,6 +88,11 @@ int buzzuav_gohome(buzzvm_t vm);
* Updates battery information in Buzz
*/
int buzzuav_update_battery(buzzvm_t vm);
int buzzuav_update_deque_full(buzzvm_t vm);
int buzzuav_update_rssi(buzzvm_t vm);
int buzzuav_update_raw_packet_loss(buzzvm_t vm);
int buzzuav_update_filtered_packet_loss(buzzvm_t vm);
int buzzuav_update_api_rssi(buzzvm_t vm);
/*
* Updates current position in Buzz
*/

2
include/roscontroller.h
View File

@ -244,6 +244,8 @@ private:
bool GetRawPacketLoss(const uint8_t short_id, float &result);
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
void get_xbee_status();
};
}

25
src/buzz_utility.cpp
View File

@ -18,7 +18,7 @@ namespace buzz_utility{
static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0;
static uint32_t MSG_SIZE = 600;//250; // Only 100 bytes of Buzz messages every step
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
static uint8_t Robot_id = 0;
static std::vector<uint8_t*> IN_MSG;
std::map< int, Pos_struct> users_map;
@ -78,7 +78,7 @@ namespace buzz_utility{
int buzzusers_add(int id, double latitude, double longitude, double altitude) {
if(VM->state != BUZZVM_STATE_READY) return VM->state;
// Get users "p" table
// Get users "p" table
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
buzzvm_gload(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
@ -94,8 +94,8 @@ namespace buzz_utility{
buzzvm_tput(VM);
buzzvm_push(VM, data);
}
// When we get here, the "data" table is on top of the stack
// Push user id
// When we get here, the "data" table is on top of the stack
// Push user id
buzzvm_pushi(VM, id);
// Create entry table
buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
@ -489,7 +489,7 @@ int create_stig_tables() {
ROS_ERROR("[%i] Error registering hooks", Robot_id);
return 0;
}
/* Create vstig tables
if(create_stig_tables() != BUZZVM_STATE_READY) {
buzzvm_destroy(&VM);
@ -499,7 +499,7 @@ int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
/* Save bytecode file name */
BO_FNAME = strdup(bo_filename);
@ -561,7 +561,7 @@ int create_stig_tables() {
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
return 0;
}*/
// 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);
@ -689,11 +689,22 @@ int create_stig_tables() {
buzzuav_closures::buzzuav_update_flight_status(VM);
}
void update_xbee_status(){
/* Update sensors*/
buzzuav_closures::buzzuav_update_deque_full(VM);
buzzuav_closures::buzzuav_update_rssi(VM);
buzzuav_closures::buzzuav_update_raw_packet_loss(VM);
buzzuav_closures::buzzuav_update_filtered_packet_loss(VM);
buzzuav_closures::buzzuav_update_api_rssi(VM);
}
void buzz_script_step() {
/*Process available messages*/
in_message_process();
/*Update sensors*/
update_sensors();
update_xbee_status();
/* Call Buzz step() function */
if(buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY) {
ROS_ERROR("%s: execution terminated abnormally: %s",

78
src/buzzuav_closures.cpp
View File

@ -24,7 +24,12 @@ namespace buzzuav_closures{
static int rc_cmd=0;
static int buzz_cmd=0;
static float height=0;
static bool deque_full = false;
static float rssi = 0.0;
static float raw_packet_loss = 0.0;
static float filtered_packet_loss = 0.0;
static float api_rssi = 0.0;
std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
@ -95,7 +100,7 @@ namespace buzzuav_closures{
return x;
}
void rb_from_gps(double nei[], double out[], double cur[]){
void rb_from_gps(double nei[], double out[], double cur[]){
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
@ -148,7 +153,7 @@ namespace buzzuav_closures{
//buzzvm_dup(vm);
double rb[3], tmp[3];
map< int, buzz_utility::RB_struct >::iterator it;
for (it=targets_map.begin(); it!=targets_map.end(); ++it){
for (it=targets_map.begin(); it!=targets_map.end(); ++it){
tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height;
rb_from_gps(tmp, rb, cur_pos);
ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
@ -334,7 +339,68 @@ namespace buzzuav_closures{
buzzvm_gstore(vm);
return vm->state;
}
/****************************************/
void set_deque_full(bool state)
{
deque_full = state;
}
int buzzuav_update_deque_full(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "deque_full", 1));
buzzvm_pushi(vm, static_cast<uint8_t>(deque_full));
buzzvm_gstore(vm);
return vm->state;
}
void set_rssi(float value)
{
rssi = value;
}
int buzzuav_update_rssi(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "rssi", 1));
buzzvm_pushf(vm, rssi);
buzzvm_gstore(vm);
return vm->state;
}
void set_raw_packet_loss(float value)
{
raw_packet_loss = value;
}
int buzzuav_update_raw_packet_loss(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "raw_packet_loss", 1));
buzzvm_pushf(vm, raw_packet_loss);
buzzvm_gstore(vm);
return vm->state;
}
void set_filtered_packet_loss(float value)
{
filtered_packet_loss = value;
}
int buzzuav_update_filtered_packet_loss(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "filtered_packet_loss", 1));
buzzvm_pushf(vm, filtered_packet_loss);
buzzvm_gstore(vm);
return vm->state;
}
void set_api_rssi(float value)
{
api_rssi = value;
}
int buzzuav_update_api_rssi(buzzvm_t vm) {
buzzvm_pushs(vm, buzzvm_string_register(vm, "api_rssi", 1));
buzzvm_pushf(vm, api_rssi);
buzzvm_gstore(vm);
return vm->state;
}
/***************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude){
@ -409,7 +475,7 @@ namespace buzzuav_closures{
rc_cmd=0;
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
buzzvm_pushi(vm, status);
buzzvm_tput(vm);
buzzvm_tput(vm);
buzzvm_gstore(vm);
//also set rc_controllers goto
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
@ -434,7 +500,7 @@ namespace buzzuav_closures{
/******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script
/* Acessing proximity in buzz script
proximity[0].angle and proximity[0].value - front
"" "" "" - right and back
proximity[3].angle and proximity[3].value - left

36
src/roscontroller.cpp
View File

@ -231,6 +231,7 @@ void roscontroller::RosControllerRun()
updates_set_robots(no_of_robots);
// ROS_INFO("ROBOTS: %i , acutal :
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
get_xbee_status();
/*run once*/
ros::spinOnce();
/*loop rate of ros*/
@ -1202,4 +1203,39 @@ void roscontroller::get_number_of_robots() {
}
*/
}
void roscontroller::get_xbee_status()
/* Description:
* Call all the xbee node services and update the xbee status
------------------------------------------------------------------ */
{
bool result_bool;
float result_float;
const uint8_t all_ids = 0xFF;
if(GetDequeFull(result_bool))
{
buzzuav_closures::set_deque_full(result_bool);
}
if(GetRssi(result_float))
{
buzzuav_closures::set_rssi(result_float);
}
if(GetRawPacketLoss(all_ids, result_float))
{
buzzuav_closures::set_raw_packet_loss(result_float);
}
if(GetFilteredPacketLoss(all_ids, result_float))
{
buzzuav_closures::set_filtered_packet_loss(result_float);
}
// This part needs testing since it can overload the xbee module
/*
* if(GetAPIRssi(all_ids, result_float))
* {
* buzzuav_closures::set_api_rssi(result_float);
* }
* TriggerAPIRssi(all_ids);
*/
}
}