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

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);

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
*/

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();
};
}

View File

@ -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",

View File

@ -24,6 +24,11 @@ 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;
@ -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){

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);
*/
}
}