test bio change leader success

class_model/include/class_model/convert_degree.h

@@ -5,7 +5,7 @@ float ConvertDeg(float x ,float y){
         degree = 90-(atan(y/x)*180/3.14);
     }
     if(x<0 && y>0){
-        degree = atan(y/x)*180/3.14 + 360;
+        degree = abs(atan(y/x)*180/3.14) + 270;
     }
     if(x<0 && y<0){
         degree = (atan(x/y)*180/3.14) + 180;

class_model/include/class_model/follower.h

@@ -1,7 +1,9 @@
 /*Follow the leader in a fixed formation */
 #ifndef FOLLOWER_H
 #define FOLLOWER_H
-#define swarmNum 5
+#define swarmNum 3
+#define changeLeader
+// #define NoChangeLeader
 #include <ros/ros.h>
 #include "class_model/sensor.h"
 #include "class_model/mode.h"
@@ -38,6 +40,7 @@ public:
     
     void follower(global_location** member_data, size_t index, int refID);
     int find_ref_drone(global_location** data, size_t index, int leaderID);
+    void face2target(global_location* target, global_location* leader);
 
     float lon[3],lat[3];
     global_location vector_LT,vector_SM,pre_LT,vector_LR;
@@ -66,6 +69,8 @@ private:
     float slope,c,y;              // function plane()'s variable
     global_location pre_target;  // function plane()'s variable
     int flag = 0,pre_plane = 0;
+    float error_heading, error_yaw ;  //function face2target()'s variable
+    int heading_status = 0;          //function face2target()'s variable
 
     void calculate_position(float k,float theta,int plane);
     void plane(global_location* target, global_location* leader, global_location* follower);