Added xbee status update functions
This commit is contained in:
parent
82adb66f7b
commit
8552cf79c8
|
@ -49,6 +49,8 @@ uint64_t* obt_out_msg();
|
||||||
|
|
||||||
void update_sensors();
|
void update_sensors();
|
||||||
|
|
||||||
|
void update_xbee_status();
|
||||||
|
|
||||||
int buzz_script_set(const char* bo_filename,
|
int buzz_script_set(const char* bo_filename,
|
||||||
const char* bdbg_filename, int robot_id);
|
const char* bdbg_filename, int robot_id);
|
||||||
|
|
||||||
|
|
|
@ -47,6 +47,11 @@ void rc_set_goto(double pos[]);
|
||||||
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);
|
||||||
|
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 */
|
/* 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 */
|
/*retuns the current go to position */
|
||||||
|
@ -83,6 +88,11 @@ int buzzuav_gohome(buzzvm_t vm);
|
||||||
* Updates battery information in Buzz
|
* Updates battery information in Buzz
|
||||||
*/
|
*/
|
||||||
int buzzuav_update_battery(buzzvm_t vm);
|
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
|
* Updates current position in Buzz
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -244,6 +244,8 @@ private:
|
||||||
bool GetRawPacketLoss(const uint8_t short_id, float &result);
|
bool GetRawPacketLoss(const uint8_t short_id, float &result);
|
||||||
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
|
bool GetFilteredPacketLoss(const uint8_t short_id, float &result);
|
||||||
|
|
||||||
|
void get_xbee_status();
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -18,7 +18,7 @@ namespace buzz_utility{
|
||||||
static uint8_t* BO_BUF = 0;
|
static uint8_t* BO_BUF = 0;
|
||||||
static buzzdebug_t DBG_INFO = 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 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 uint8_t Robot_id = 0;
|
||||||
static std::vector<uint8_t*> IN_MSG;
|
static std::vector<uint8_t*> IN_MSG;
|
||||||
std::map< int, Pos_struct> users_map;
|
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) {
|
int buzzusers_add(int id, double latitude, double longitude, double altitude) {
|
||||||
if(VM->state != BUZZVM_STATE_READY) return VM->state;
|
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_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
||||||
buzzvm_gload(VM);
|
buzzvm_gload(VM);
|
||||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
|
||||||
|
@ -94,8 +94,8 @@ namespace buzz_utility{
|
||||||
buzzvm_tput(VM);
|
buzzvm_tput(VM);
|
||||||
buzzvm_push(VM, data);
|
buzzvm_push(VM, data);
|
||||||
}
|
}
|
||||||
// When we get here, the "data" table is on top of the stack
|
// When we get here, the "data" table is on top of the stack
|
||||||
// Push user id
|
// Push user id
|
||||||
buzzvm_pushi(VM, id);
|
buzzvm_pushi(VM, id);
|
||||||
// Create entry table
|
// Create entry table
|
||||||
buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_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);
|
ROS_ERROR("[%i] Error registering hooks", Robot_id);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Create vstig tables
|
/* Create vstig tables
|
||||||
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
||||||
buzzvm_destroy(&VM);
|
buzzvm_destroy(&VM);
|
||||||
|
@ -499,7 +499,7 @@ int create_stig_tables() {
|
||||||
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
||||||
return 0;
|
return 0;
|
||||||
}*/
|
}*/
|
||||||
|
|
||||||
/* Save bytecode file name */
|
/* Save bytecode file name */
|
||||||
BO_FNAME = strdup(bo_filename);
|
BO_FNAME = strdup(bo_filename);
|
||||||
|
|
||||||
|
@ -561,7 +561,7 @@ int create_stig_tables() {
|
||||||
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
||||||
return 0;
|
return 0;
|
||||||
}*/
|
}*/
|
||||||
|
|
||||||
// Execute the global part of the script
|
// Execute the global part of the script
|
||||||
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
|
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
|
||||||
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
|
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);
|
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() {
|
void buzz_script_step() {
|
||||||
/*Process available messages*/
|
/*Process available messages*/
|
||||||
in_message_process();
|
in_message_process();
|
||||||
/*Update sensors*/
|
/*Update sensors*/
|
||||||
update_sensors();
|
update_sensors();
|
||||||
|
|
||||||
|
update_xbee_status();
|
||||||
/* 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",
|
ROS_ERROR("%s: execution terminated abnormally: %s",
|
||||||
|
|
|
@ -24,7 +24,12 @@ namespace buzzuav_closures{
|
||||||
static int rc_cmd=0;
|
static int rc_cmd=0;
|
||||||
static int buzz_cmd=0;
|
static int buzz_cmd=0;
|
||||||
static float height=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::RB_struct> targets_map;
|
||||||
std::map< int, buzz_utility::Pos_struct> neighbors_map;
|
std::map< int, buzz_utility::Pos_struct> neighbors_map;
|
||||||
|
|
||||||
|
@ -95,7 +100,7 @@ namespace buzzuav_closures{
|
||||||
return x;
|
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_lon = nei[1] - cur[1];
|
||||||
double d_lat = nei[0] - cur[0];
|
double d_lat = nei[0] - cur[0];
|
||||||
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
|
double ned_x = DEG2RAD(d_lat) * EARTH_RADIUS;
|
||||||
|
@ -148,7 +153,7 @@ namespace buzzuav_closures{
|
||||||
//buzzvm_dup(vm);
|
//buzzvm_dup(vm);
|
||||||
double rb[3], tmp[3];
|
double rb[3], tmp[3];
|
||||||
map< int, buzz_utility::RB_struct >::iterator it;
|
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;
|
tmp[0]=(it->second).la;tmp[1]=(it->second).lo;tmp[2]=height;
|
||||||
rb_from_gps(tmp, rb, cur_pos);
|
rb_from_gps(tmp, rb, cur_pos);
|
||||||
ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
|
ROS_WARN("----------Pushing target id %i (%f,%f)", rb[0], rb[1]);
|
||||||
|
@ -334,7 +339,68 @@ namespace buzzuav_closures{
|
||||||
buzzvm_gstore(vm);
|
buzzvm_gstore(vm);
|
||||||
return vm->state;
|
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*/
|
/*current pos update*/
|
||||||
/***************************************/
|
/***************************************/
|
||||||
void set_currentpos(double latitude, double longitude, double altitude){
|
void set_currentpos(double latitude, double longitude, double altitude){
|
||||||
|
@ -409,7 +475,7 @@ namespace buzzuav_closures{
|
||||||
rc_cmd=0;
|
rc_cmd=0;
|
||||||
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
|
buzzvm_pushs(vm, buzzvm_string_register(vm, "status", 1));
|
||||||
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
|
//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));
|
||||||
|
@ -434,7 +500,7 @@ namespace buzzuav_closures{
|
||||||
|
|
||||||
/******************************************************/
|
/******************************************************/
|
||||||
/*Create an obstacle Buzz table from proximity sensors*/
|
/*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
|
proximity[0].angle and proximity[0].value - front
|
||||||
"" "" "" - right and back
|
"" "" "" - right and back
|
||||||
proximity[3].angle and proximity[3].value - left
|
proximity[3].angle and proximity[3].value - left
|
||||||
|
|
|
@ -231,6 +231,7 @@ void roscontroller::RosControllerRun()
|
||||||
updates_set_robots(no_of_robots);
|
updates_set_robots(no_of_robots);
|
||||||
// ROS_INFO("ROBOTS: %i , acutal :
|
// ROS_INFO("ROBOTS: %i , acutal :
|
||||||
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
|
// %i",(int)no_of_robots,(int)buzzdict_size(buzz_utility::get_vm()->swarmmembers)+1);
|
||||||
|
get_xbee_status();
|
||||||
/*run once*/
|
/*run once*/
|
||||||
ros::spinOnce();
|
ros::spinOnce();
|
||||||
/*loop rate of ros*/
|
/*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);
|
||||||
|
*/
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue