beautified

include/buzz_update.h

@@ -10,76 +10,84 @@
 #include <buzz/buzzdarray.h>
 #include <buzz/buzzvstig.h>
 #include <fstream>
-#define delete_p(p) do { free(p); p = NULL; } while(0)
+#define delete_p(p)                                                                                                    \
+  do                                                                                                                   \
+  {                                                                                                                    \
+    free(p);                                                                                                           \
+    p = NULL;                                                                                                          \
+  } while (0)
 
-static const uint16_t CODE_REQUEST_PADDING=250;
+static const uint16_t CODE_REQUEST_PADDING = 250;
-static const uint16_t MIN_UPDATE_PACKET=251;
+static const uint16_t MIN_UPDATE_PACKET = 251;
-static const uint16_t UPDATE_CODE_HEADER_SIZE=5;
+static const uint16_t UPDATE_CODE_HEADER_SIZE = 5;
-static const uint16_t TIMEOUT_FOR_ROLLBACK=50;
+static const uint16_t TIMEOUT_FOR_ROLLBACK = 50;
 /*********************/
 /*   Updater states */
 /********************/
 
 typedef enum {
-      CODE_RUNNING = 0, // Code executing
+  CODE_RUNNING = 0,  // Code executing
-      CODE_STANDBY,     // Standing by for others to update
+  CODE_STANDBY,      // Standing by for others to update
-         } code_states_e;
+} code_states_e;
 
 /*********************/
 /*Message types     */
 /********************/
 
 typedef enum {
-      SENT_CODE = 0,         // Broadcast code 
+  SENT_CODE = 0,  // Broadcast code
-      RESEND_CODE,             // ReBroadcast request
+  RESEND_CODE,    // ReBroadcast request
-         } code_message_e;
+} code_message_e;
 
 /*************************/
 /*Updater message queue */
 /*************************/
 
-struct updater_msgqueue_s {
+struct updater_msgqueue_s
-      uint8_t* queue;
+{
-      uint8_t* size;
+  uint8_t* queue;
-   } ;
+  uint8_t* size;
-   typedef struct updater_msgqueue_s* updater_msgqueue_t;
+};
+typedef struct updater_msgqueue_s* updater_msgqueue_t;
 
-struct updater_code_s {
+struct updater_code_s
-   uint8_t* bcode;
+{
-   uint8_t* bcode_size;
+  uint8_t* bcode;
-} ;
+  uint8_t* bcode_size;
+};
 typedef struct updater_code_s* updater_code_t;
 
 /**************************/
 /*Updater data*/
 /**************************/
 
-struct buzz_updater_elem_s {
+struct buzz_updater_elem_s
-      /* robot id  */
+{
-      //uint16_t robotid;
+  /* robot id  */
-     /*current Bytecode content */
+  // uint16_t robotid;
-      uint8_t* bcode;
+  /*current Bytecode content */
-     /*old Bytecode name */
+  uint8_t* bcode;
-      const char* old_bcode;
+  /*old Bytecode name */
-      /*current bcode size*/
+  const char* old_bcode;
-      size_t* bcode_size;
+  /*current bcode size*/
-      /*Update patch*/
+  size_t* bcode_size;
-      uint8_t* patch;
+  /*Update patch*/
-      /* Update patch size*/
+  uint8_t* patch;
-      size_t* patch_size;
+  /* Update patch size*/
-      /*current Bytecode content */
+  size_t* patch_size;
-      uint8_t* standby_bcode;
+  /*current Bytecode content */
-      /*current bcode size*/
+  uint8_t* standby_bcode;
-      size_t* standby_bcode_size;
+  /*current bcode size*/
-      /*updater out msg queue */
+  size_t* standby_bcode_size;
-      updater_msgqueue_t outmsg_queue;
+  /*updater out msg queue */
-      /*updater in msg queue*/
+  updater_msgqueue_t outmsg_queue;
-      updater_msgqueue_t inmsg_queue;
+  /*updater in msg queue*/
-      /*Current state of the updater one in code_states_e ENUM*/
+  updater_msgqueue_t inmsg_queue;
-      int* mode;
+  /*Current state of the updater one in code_states_e ENUM*/
-      uint8_t* update_no;
+  int* mode;
-   };
+  uint8_t* update_no;
-   typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
+};
+typedef struct buzz_updater_elem_s* buzz_updater_elem_t;
 
 /**************************************************************************/
 /*Updater routine from msg processing to file checks to be called from main*/
@@ -89,15 +97,13 @@ void update_routine();
 /************************************************/
 /*Initalizes the updater */
 /************************************************/
-void init_update_monitor(const char* bo_filename,const char* stand_by_script,
+void init_update_monitor(const char* bo_filename, const char* stand_by_script, const char* dbgfname, int robot_id);
-                           const char* dbgfname, int robot_id);
-
 
 /*********************************************************/
 /*Appends buffer of given size to in msg queue of updater*/
 /*********************************************************/
 
-void code_message_inqueue_append(uint8_t* msg,uint16_t size);
+void code_message_inqueue_append(uint8_t* msg, uint16_t size);
 
 /*********************************************************/
 /*Processes messages inside the queue of the updater*/
@@ -125,14 +131,13 @@ void destroy_out_msg_queue();
 /***************************************************/
 int get_update_mode();
 
-
 buzz_updater_elem_t get_updater();
 /***************************************************/
 /*sets bzz file name*/
 /***************************************************/
 void set_bzz_file(const char* in_bzz_file);
 
-int test_set_code(uint8_t* BO_BUF, const char* dbgfname,size_t bcode_size);
+int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
 
 /****************************************************/
 /*Destroys the updater*/
@@ -152,5 +157,5 @@ void updates_set_robots(int robots);
 
 void set_packet_id(int packet_id);
 
-void collect_data(std::ofstream &logger);
+void collect_data(std::ofstream& logger);
 #endif

include/buzz_utility.h

@@ -12,20 +12,25 @@
 #include <map>
 
 using namespace std;
-namespace buzz_utility{
+namespace buzz_utility
-
+{
 struct pos_struct
 {
-  double x,y,z;
+  double x, y, z;
-  pos_struct(double x,double y,double z):x(x),y(y),z(z){};
+  pos_struct(double x, double y, double z) : x(x), y(y), z(z){};
-  pos_struct(){}
+  pos_struct()
+  {
+  }
 };
 typedef struct pos_struct Pos_struct;
 struct rb_struct
 {
-  double r,b,latitude,longitude,altitude;
+  double r, b, latitude, longitude, altitude;
-  rb_struct(double la, double lo, double al, double r,double b):latitude(la),longitude(lo),altitude(al),r(r),b(b){};
+  rb_struct(double la, double lo, double al, double r, double b)
-  rb_struct(){}
+    : latitude(la), longitude(lo), altitude(al), r(r), b(b){};
+  rb_struct()
+  {
+  }
 };
 typedef struct rb_struct RB_struct;
 
@@ -35,16 +40,15 @@ struct neiStatus
   uint batt_lvl = 0;
   uint xbee = 0;
   uint flight_status = 0;
-}; typedef struct neiStatus neighbors_status ;
+};
-
+typedef struct neiStatus neighbors_status;
 
 uint16_t* u64_cvt_u16(uint64_t u64);
 
-int buzz_listen(const char* type,
+int buzz_listen(const char* type, int msg_size);
-                       int msg_size);
 int make_table(buzzobj_t* t);
 int buzzusers_reset();
-	int create_stig_tables();
+int create_stig_tables();
 
 void in_msg_append(uint64_t* payload);
 
@@ -52,12 +56,11 @@ uint64_t* obt_out_msg();
 
 void update_sensors();
 
-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);
 
-int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
+int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
 
-int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size);
+int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size);
 
 void buzz_script_step();
 

include/buzzuav_closures.h

@@ -8,23 +8,24 @@
 #include "ros/ros.h"
 #include "buzz_utility.h"
 
-#define EARTH_RADIUS (double) 6371000.0
+#define EARTH_RADIUS (double)6371000.0
-#define DEG2RAD(DEG) (double) ((DEG)*((M_PI)/(180.0)))
+#define DEG2RAD(DEG) (double)((DEG) * ((M_PI) / (180.0)))
-#define RAD2DEG(RAD) (double) ((RAD)*((180.0)/(M_PI)))
+#define RAD2DEG(RAD) (double)((RAD) * ((180.0) / (M_PI)))
 
-namespace buzzuav_closures{
+namespace buzzuav_closures
-	typedef enum {
+{
-		COMMAND_NIL = 0,     // Dummy command
+typedef enum {
-		COMMAND_TAKEOFF,     // Take off
+  COMMAND_NIL = 0,  // Dummy command
-		COMMAND_LAND,
+  COMMAND_TAKEOFF,  // Take off
-		COMMAND_GOHOME,
+  COMMAND_LAND,
-		COMMAND_ARM,
+  COMMAND_GOHOME,
-		COMMAND_DISARM,
+  COMMAND_ARM,
-		COMMAND_GOTO,
+  COMMAND_DISARM,
-		COMMAND_MOVETO,
+  COMMAND_GOTO,
-		COMMAND_PICTURE,
+  COMMAND_MOVETO,
-		COMMAND_GIMBAL,
+  COMMAND_PICTURE,
-	 } Custom_CommandCode;
+  COMMAND_GIMBAL,
+} Custom_CommandCode;
 
 /*
  * prextern int() function in Buzz
@@ -54,7 +55,7 @@ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t);
 /*sets rc requested command */
 void rc_call(int rc_cmd);
 /* 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);
@@ -88,7 +89,7 @@ int buzzuav_arm(buzzvm_t vm);
 /*
  * Disarm from buzz
  */
-int buzzuav_disarm(buzzvm_t vm) ;
+int buzzuav_disarm(buzzvm_t vm);
 /* Commands the UAV to land
  */
 int buzzuav_land(buzzvm_t vm);
@@ -126,7 +127,6 @@ int buzzuav_update_prox(buzzvm_t vm);
 
 int bzz_cmd();
 
-
 int dummy_closure(buzzvm_t vm);
 
 //#endif

include/roscontroller.h

@@ -37,7 +37,7 @@
 #define UPDATER_MESSAGE_CONSTANT 987654321
 #define XBEE_MESSAGE_CONSTANT 586782343
 #define XBEE_STOP_TRANSMISSION 4355356352
-#define TIMEOUT	60
+#define TIMEOUT 60
 #define BUZZRATE 10
 #define CMD_REQUEST_UPDATE 666
 
@@ -45,14 +45,12 @@ using namespace std;
 
 namespace rosbzz_node
 {
-
 class roscontroller
 {
-
 public:
   roscontroller(ros::NodeHandle& n_c, ros::NodeHandle& n_c_priv);
   ~roscontroller();
-  //void RosControllerInit();
+  // void RosControllerInit();
   void RosControllerRun();
 
   static const string CAPTURE_SRV_DEFAULT_NAME;
@@ -63,43 +61,47 @@ private:
     uint8_t history[10];
     uint8_t index = 0;
     uint8_t current = 0;
-    num_robot_count(){}
+    num_robot_count()
-  }; typedef struct num_robot_count Num_robot_count ; // not useful in cpp
+    {
+    }
+  };
+  typedef struct num_robot_count Num_robot_count;  // not useful in cpp
 
   struct gps
   {
-    double longitude=0.0;
+    double longitude = 0.0;
-    double latitude=0.0;
+    double latitude = 0.0;
-    float  altitude=0.0;
+    float altitude = 0.0;
-  }; typedef struct gps GPS ;
+  };
+  typedef struct gps GPS;
 
   GPS target, home, cur_pos;
   double cur_rel_altitude;
 
   uint64_t payload;
-  std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
+  std::map<int, buzz_utility::Pos_struct> neighbours_pos_map;
-  std::map< int,  buzz_utility::Pos_struct> raw_neighbours_pos_map;
+  std::map<int, buzz_utility::Pos_struct> raw_neighbours_pos_map;
-  //std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
+  // std::map< int, buzz_utility::Pos_struct> pub_neigh_pos;
-  int timer_step=0;
+  int timer_step = 0;
-  int robot_id=0;
+  int robot_id = 0;
   std::string robot_name = "";
-        //int oldcmdID=0;
+  // int oldcmdID=0;
 
   int rc_cmd;
   float fcu_timeout;
   int armstate;
   int barrier;
-  int message_number=0;
+  int message_number = 0;
-  uint8_t no_of_robots=0;
+  uint8_t no_of_robots = 0;
   /*tmp to be corrected*/
-  uint8_t no_cnt=0;
+  uint8_t no_cnt = 0;
-  uint8_t old_val=0	;
+  uint8_t old_val = 0;
   std::string bzzfile_name;
   std::string fcclient_name;
   std::string armclient;
   std::string modeclient;
   std::string rcservice_name;
-  std::string bcfname,dbgfname;
+  std::string bcfname, dbgfname;
   std::string out_payload;
   std::string in_payload;
   std::string obstacles_topic;
@@ -136,7 +138,6 @@ private:
   ros::Subscriber local_pos_sub;
   double local_pos_new[3];
 
-
   ros::ServiceClient stream_client;
 
   int setpoint_counter;
@@ -145,7 +146,7 @@ private:
   std::ofstream log;
 
   /*Commands for flight controller*/
-  //mavros_msgs::CommandInt cmd_srv;
+  // mavros_msgs::CommandInt cmd_srv;
   mavros_msgs::CommandLong cmd_srv;
   std::vector<std::string> m_sMySubscriptions;
   std::map<std::string, std::string> m_smTopic_infos;
@@ -159,7 +160,7 @@ private:
   /*Initialize publisher and subscriber, done in the constructor*/
   void Initialize_pub_sub(ros::NodeHandle& n_c);
 
-  std::string current_mode; // SOLO SPECIFIC: just so you don't call the switch to same mode
+  std::string current_mode;  // SOLO SPECIFIC: just so you don't call the switch to same mode
 
   /*Obtain data from ros parameter server*/
   void Rosparameters_get(ros::NodeHandle& n_c_priv);
@@ -178,28 +179,24 @@ private:
   /*Prepare messages and publish*/
   void prepare_msg_and_publish();
 
-
   /*Refresh neighbours Position for every ten step*/
   void maintain_pos(int tim_step);
 
   /*Puts neighbours position inside neigbours_pos_map*/
-  void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
+  void neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
 
   /*Puts raw neighbours position in lat.,long.,alt. inside raw_neigbours_pos_map*/
-  void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr );
+  void raw_neighbours_pos_put(int id, buzz_utility::Pos_struct pos_arr);
 
   /*Set the current position of the robot callback*/
-  void set_cur_pos(double latitude,
+  void set_cur_pos(double latitude, double longitude, double altitude);
-       double longitude,
-       double altitude);
   /*convert from spherical to cartesian coordinate system callback */
-	float constrainAngle(float x);
+  float constrainAngle(float x);
   void gps_rb(GPS nei_pos, double out[]);
-  void gps_ned_cur(float &ned_x, float &ned_y, GPS t);
+  void gps_ned_cur(float& ned_x, float& ned_y, GPS t);
-  void gps_ned_home(float &ned_x, float &ned_y);
+  void gps_ned_home(float& ned_x, float& ned_y);
-  void gps_convert_ned(float &ned_x, float &ned_y,
+  void gps_convert_ned(float& ned_x, float& ned_y, double gps_t_lon, double gps_t_lat, double gps_r_lon,
-      double gps_t_lon, double gps_t_lat,
+                       double gps_r_lat);
-      double gps_r_lon, double gps_r_lat);
 
   /*battery status callback */
   void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg);
@@ -213,7 +210,6 @@ private:
   /*current position callback*/
   void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
 
-
   /*current relative altitude callback*/
   void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
 
@@ -221,7 +217,7 @@ private:
   void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
 
   /* RC commands service */
-  bool rc_callback(mavros_msgs::CommandLong::Request  &req, mavros_msgs::CommandLong::Response &res);
+  bool rc_callback(mavros_msgs::CommandLong::Request& req, mavros_msgs::CommandLong::Response& res);
 
   /*robot id sub callback*/
   void set_robot_id(const std_msgs::UInt8::ConstPtr& msg);
@@ -243,7 +239,7 @@ private:
 
   void local_pos_callback(const geometry_msgs::PoseStamped::ConstPtr& pose);
 
-  //void WaypointMissionSetup(float lat, float lng, float alt);
+  // void WaypointMissionSetup(float lat, float lng, float alt);
 
   void fc_command_setup();
 
@@ -256,15 +252,13 @@ private:
   void get_number_of_robots();
 
   void GetRobotId();
-  bool GetDequeFull(bool &result);
+  bool GetDequeFull(bool& result);
-  bool GetRssi(float &result);
+  bool GetRssi(float& result);
   bool TriggerAPIRssi(const uint8_t short_id);
-  bool GetAPIRssi(const uint8_t short_id, float &result);
+  bool GetAPIRssi(const uint8_t short_id, float& result);
-  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();
-
 };
-
 }

include/uav_utility.h

@@ -5,5 +5,4 @@ extern void uav_setup();
 
 extern void uav_done();
 
-
 #endif

src/rosbuzz.cpp

@@ -1,7 +1,7 @@
 /** @file      rosbuzz.cpp
- *  @version   1.0 
+ *  @version   1.0
  *  @date      27.09.2016
- *  @brief     Buzz Implementation as a node in ROS. 
+ *  @brief     Buzz Implementation as a node in ROS.
  *  @author    Vivek Shankar Varadharajan and David St-Onge
  *  @copyright 2016 MistLab. All rights reserved.
  */
@@ -11,18 +11,16 @@
 /**
  * This program implements Buzz node in ros using mavros_msgs.
  */
-	
+
-int main(int argc, char **argv)
+int main(int argc, char** argv)
 {
   /*Initialize rosBuzz node*/
-  ros::init(argc, argv, "rosBuzz"); 
+  ros::init(argc, argv, "rosBuzz");
   ros::NodeHandle nh_priv("~");
   ros::NodeHandle nh;
   rosbzz_node::roscontroller RosController(nh, nh_priv);
   /*Register ros controller object inside buzz*/
-  //buzzuav_closures::set_ros_controller_ptr(&RosController);
+  // buzzuav_closures::set_ros_controller_ptr(&RosController);
-  RosController.RosControllerRun(); 
+  RosController.RosControllerRun();
   return 0;
 }
-
-

src/uav_utility.cpp

@@ -8,17 +8,17 @@
 /*To do !*/
 /****************************************/
 
-void uav_setup() {
+void uav_setup()
-  
+{
 }
 
 /****************************************/
 /*To do !*/
 /****************************************/
 
-void uav_done() {
+void uav_done()
- 
+{
-   fprintf(stdout, "Robot stopped.\n");
+  fprintf(stdout, "Robot stopped.\n");
 }
 
 /****************************************/