removed setpoint to be a parameter for controller, added an option for setpoint in the param file

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -89,7 +89,6 @@ add_message_files(
   FILES
   ViconData.msg
   ControlCommand.msg
-  Setpoint.msg
   CrazyflieContext.msg
   AreaBounds.msg
 )

pps_ws/src/d_fall_pps/msg/Setpoint.msg

-float64 x
-float64 y
-float64 z
-float64 yaw

pps_ws/src/d_fall_pps/param/SafeController.yaml

@@ -10,3 +10,6 @@ gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
 gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
 gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]
 
+#setpoint in meters (x, y, z, yaw)
+setpoint: [0.2, 0.2, 0.6, 0]
+

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -75,15 +75,7 @@ void ppsClientToController(ViconData data, bool autocontrolOn){
 			//for the moment use safecontroller for TESTING
 		
 		Controller srvRate;
-		Setpoint goalLocation;
-
-		goalLocation.x = 0; //testvalue
-		goalLocation.y = 0.5; //testvalue
-		goalLocation.z = 0.4; //testvalue
-		goalLocation.yaw = 0;
-
 		srvRate.request.crazyflieLocation = data;
-		srvRate.request.setpoint = goalLocation;
 
 		//TODO:
 		//return control commands
@@ -132,7 +124,7 @@ void viconCallback(const ViconData& data){
 	if(data.crazyflieName == cflie){	
 		ROS_INFO_STREAM(data);
 		//forward data to safety check
-		bool autocontrolOn = safetyCheck(data);
+		bool autocontrolOn = false;//safetyCheck(data);
 		ppsClientToController(data, autocontrolOn);
 	}
 	else {

pps_ws/src/d_fall_pps/src/SafeControllerService.cpp

@@ -25,6 +25,7 @@
 #define PI 3.1415926535
 #define RATE_CONTROLLER 0
 #define RAD2DEG 57.3
+#define VEL_AVERAGE_SIZE 10
 
 using namespace d_fall_pps;
 
@@ -36,8 +37,14 @@ std::vector<float>  gainMatrixRoll(9);
 std::vector<float>  gainMatrixPitch(9);
 std::vector<float>  gainMatrixYaw(9);
 std::vector<float>  gainMatrixThrust(9);
+
+std::vector<float>  setpoint(4);
 float saturationThrust;
 
+//averaging test, will probably be replaced by Kalman filter
+//float prevVelocities[VEL_AVERAGE_SIZE * 3];
+//int velocityIndex = 0;
+
 ViconData previousLocation;
 
 void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
@@ -62,6 +69,7 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
     loadParameterFloatVector(nodeHandle, "gainMatrixPitch", gainMatrixPitch, 9);
     loadParameterFloatVector(nodeHandle, "gainMatrixYaw", gainMatrixYaw, 9);
     loadParameterFloatVector(nodeHandle, "gainMatrixThrust", gainMatrixThrust, 9);
+    loadParameterFloatVector(nodeHandle, "setpoint", setpoint, 4);
 }
 
 float computeMotorPolyBackward(float thrust) {
@@ -69,20 +77,39 @@ float computeMotorPolyBackward(float thrust) {
 }
 
 void estimateState(Controller::Request &request, float (&est)[9]) {
-    est[0] = request.crazyflieLocation.x - request.setpoint.x;
-    est[1] = request.crazyflieLocation.y - request.setpoint.y;
-    est[2] = request.crazyflieLocation.z - request.setpoint.z;
+    est[0] = request.crazyflieLocation.x - setpoint[0];
+    est[1] = request.crazyflieLocation.y - setpoint[1];
+    est[2] = request.crazyflieLocation.z - setpoint[2];
 
     //linear approximation of derivative of position (no Estimator implemented...)
+    /*prevVelocities[3 * velocityIndex + 0] = (request.crazyflieLocation.x - previousLocation.x) / request.crazyflieLocation.acquiringTime;
+    prevVelocities[3 * velocityIndex + 1] = (request.crazyflieLocation.y - previousLocation.y) / request.crazyflieLocation.acquiringTime;
+    prevVelocities[3 * velocityIndex + 2] = (request.crazyflieLocation.z - previousLocation.z) / request.crazyflieLocation.acquiringTime;
+    velocityIndex = (velocityIndex + 1) % VEL_AVERAGE_SIZE;
+
+    est[3] = 0;
+    est[4] = 0;
+    est[5] = 0;
+    for(int i = 0; i < VEL_AVERAGE_SIZE; ++i) {
+        est[3] += prevVelocities[3 * i + 0] / ((float) VEL_AVERAGE_SIZE);
+        est[4] += prevVelocities[3 * i + 1] / ((float) VEL_AVERAGE_SIZE);
+        est[5] += prevVelocities[3 * i + 2] / ((float) VEL_AVERAGE_SIZE);
+    }*/
+
     est[3] = (request.crazyflieLocation.x - previousLocation.x) / request.crazyflieLocation.acquiringTime;
     est[4] = (request.crazyflieLocation.y - previousLocation.y) / request.crazyflieLocation.acquiringTime;
     est[5] = (request.crazyflieLocation.z - previousLocation.z) / request.crazyflieLocation.acquiringTime;
 
+    //ROS_INFO_STREAM("velocityIndex: " << velocityIndex);
+    ROS_INFO_STREAM("vx: " << est[3]);
+    ROS_INFO_STREAM("vy: " << est[4]);
+    ROS_INFO_STREAM("vz: " << est[5]);
+
     est[6] = request.crazyflieLocation.roll;
     est[7] = request.crazyflieLocation.pitch;
     //ROS_INFO_STREAM("crazyflieyaw: " << request.crazyflieLocation.yaw);
     //ROS_INFO_STREAM("setpointyaw: " << request.setpoint.yaw);
-    float yaw = request.crazyflieLocation.yaw - request.setpoint.yaw;
+    float yaw = request.crazyflieLocation.yaw - setpoint[4];
     //ROS_INFO_STREAM("differenceyaw: " << yaw);
 
     while(yaw > PI) yaw -= 2 * PI;
@@ -109,7 +136,6 @@ void convertIntoBodyFrame(Controller::Request &request, float est[9], float (&st
 
 bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
     ViconData vicon = request.crazyflieLocation;
-    Setpoint goal = request.setpoint;
 
     float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
     estimateState(request, est);

pps_ws/src/d_fall_pps/srv/Controller.srv

 ViconData crazyflieLocation
-Setpoint setpoint
 ---
 ControlCommand controlOutput