Added xbee status update functions

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

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

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

src/buzz_utility.cpp

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

src/buzzuav_closures.cpp

@@ -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){

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