From 8552cf79c887703bdf7c7998a5f4755770f9274b Mon Sep 17 00:00:00 2001 From: pyhs Date: Wed, 5 Jul 2017 09:37:35 -0400 Subject: [PATCH] Added xbee status update functions --- include/buzz_utility.h | 2 + include/buzzuav_closures.h | 10 +++++ include/roscontroller.h | 2 + src/buzz_utility.cpp | 25 ++++++++---- src/buzzuav_closures.cpp | 78 +++++++++++++++++++++++++++++++++++--- src/roscontroller.cpp | 36 ++++++++++++++++++ 6 files changed, 140 insertions(+), 13 deletions(-) diff --git a/include/buzz_utility.h b/include/buzz_utility.h index 40be6f0..b61faf3 100644 --- a/include/buzz_utility.h +++ b/include/buzz_utility.h @@ -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); diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index 1aa8022..0e9ff4a 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -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 */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 3d2ecec..fe54707 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -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(); + }; } diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp index 2d9a117..b644439 100644 --- a/src/buzz_utility.cpp +++ b/src/buzz_utility.cpp @@ -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 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", diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 964170a..74e5ca8 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -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(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 diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index c7b2303..27cdd4e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -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); + */ +} + }