bio drone debug complete

class_model/include/class_model/Leader.h

@@ -35,6 +35,7 @@ private:
     ros::NodeHandle node_handle_;
 
     global_location target_location;
+    global_location pre_target;
     global_location follower_location;
     global_location leader_location;
     global_location current_location;

class_model/include/class_model/command.h

@@ -6,18 +6,20 @@
 #include <geometry_msgs/TwistStamped.h>
 #include <mavros_msgs/GlobalPositionTarget.h>
 #include <class_model/sensor.h>
+#include <geographic_msgs/GeoPoseStamped.h>
 
 class CommandClass {
 public:
     CommandClass();
     virtual ~CommandClass();
-    void set_global_position(float lat,float lon,float alt);
+    void set_global_position(double lon,double lat,float alt,float yaw);
     void set_velocity(float x,float y,float alt,float yaw,float second);
     void fix_velocity(float x,float y,float alt,float yaw,float second);
     void velocity_init();
     void set_target_position(float x,float y);
     ThreadGPSClass gps_object;
 
+
 private:
     // ROS NodeHandle
     ros::NodeHandle node_handle_;

class_model/include/class_model/follower.h

@@ -40,7 +40,7 @@ public:
     int find_ref_drone(global_location** data, size_t index, int leaderID);
 
     float lon[3],lat[3];
-    global_location vector_LT,vector_SM;
+    global_location vector_LT,vector_SM,pre_LT,vector_LR;
     
 
 private:
@@ -61,6 +61,10 @@ private:
     float error_lon,error_lat;
     float follower_pid[3] = {1 , 0.00000000001 ,0};// D downsize
     float ignore_small = 0.50;
+    float m,n,k,cosTheta,pre_m,pre_k,u,LTcos;  // function direct()'s variable
+    global_location pre_tar,ref_point;  // function plane()'s variable
+    float slope,c,y;              // function plane()'s variable
+    global_location pre_target;  // function plane()'s variable
     int flag = 0;
 
     void calculate_position(float k,float theta,int plane);