add calculate data timer test

class_model/include/class_model/command.h

@@ -1,11 +1,16 @@
 /*Drone Control Command*/
 #ifndef COMMAND_H
 #define COMMAND_H
-
+#define DEBUG
 #include <ros/ros.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <mavros_msgs/GlobalPositionTarget.h>
 #include <class_model/sensor.h>
+#include <class_model/Param.h>
+#include <fstream>
+#include <iostream>
+#include <string>        
+
 // #include <geographic_msgs/GeoPoseStamped.h>
 
 class CommandClass {
@@ -13,7 +18,13 @@ public:
     CommandClass();
     virtual ~CommandClass();
     void set_global_position(double lon,double lat,float alt,float yaw);
+    #ifdef DEBUG
+    ParamClass param_object;
+    void set_velocity(float x,float y,float alt,float yaw,float second,uint64_t get_t,uint64_t cal_t);
+    #endif
+    #ifndef DEBUG
     void set_velocity(float x,float y,float alt,float yaw,float second);
+    #endif
     void fix_velocity(float x,float y,float alt,float yaw,float second);
     void velocity_init();
     void set_target_position(float x,float y);

class_model/include/class_model/convert_degree.h

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

class_model/include/class_model/formation.h

@@ -1,7 +1,7 @@
 /*Follow the leader in a fixed formation */
 #ifndef FORMATION_H
 #define FORMATION_H
-
+#define DEBUG
 #include <ros/ros.h>
 #include "class_model/sensor.h"
 #include "class_model/mode.h"