corrected circle controller, made crazyflie crash safely

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -216,6 +216,7 @@ add_executable(PPSClient src/PPSClient.cpp)
 add_executable(SafeControllerService src/SafeControllerService.cpp)
 add_executable(CustomControllerService src/CustomControllerService.cpp)
 add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
+add_executable(CircleControllerService src/CircleControllerService.cpp)
 
 
 
@@ -248,6 +249,7 @@ add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TA
 add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(CustomControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(CircleControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 
 
 # GUI-- dependencies
@@ -281,6 +283,8 @@ target_link_libraries(CustomControllerService ${catkin_LIBRARIES})
 
 target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
 
+target_link_libraries(CircleControllerService ${catkin_LIBRARIES})
+
 
 # GUI -- link libraries
 target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff

pps_ws/src/d_fall_pps/param/SafeController.yaml

@@ -15,4 +15,4 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
 estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
 
 #setpoint in meters (x, y, z, yaw)
-defaultSetpoint: [-0.5, 0.0, 0.4, 1.2]
+defaultSetpoint: [0.0, 0.0, 0.0, 0.0]

pps_ws/src/d_fall_pps/src/CircleControllerService.cpp

@@ -51,9 +51,9 @@ float saturationThrust;
 CrazyflieData previousLocation;
 
 //circle stuff
-float time;
-const float OMEGA = 0.1*2*PI;
-const float RADIUS = 0.2;
+float currentTime;
+const float OMEGA = 0.5*2*PI;
+const float RADIUS = 0.35;
 
 
 void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
@@ -141,17 +141,17 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
 }
 
 void calculateCircle(Setpoint &circlePoint){
-    circlePoint.x = RADIUS*cos(OMEGA*time);
-    circlePoint.y = RADIUS*sin(OMEGA*time);
+    circlePoint.x = RADIUS*cos(OMEGA*currentTime);
+    circlePoint.y = RADIUS*sin(OMEGA*currentTime);
     circlePoint.z = 0.5;
-    circlePoint.yaw = OMEGA*time;
+    circlePoint.yaw = OMEGA*currentTime;
 
 }
 
 bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
     CrazyflieData vicon = request.ownCrazyflie;
 	
-    time += request.ownCrazyflie.acquiringTime;
+    currentTime += request.ownCrazyflie.acquiringTime;
 
 	Setpoint circlePoint;
     calculateCircle(circlePoint);
@@ -212,7 +212,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 int main(int argc, char* argv[]) {
     ros::init(argc, argv, "CircleControllerService");
 
-    time = 0;
+    currentTime = 0;
 
     ros::NodeHandle nodeHandle("~");
     loadParameters(nodeHandle);

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -125,7 +125,7 @@ void viconCallback(const ViconData& viconData) {
 						ROS_ERROR_STREAM("custom controller name: valid: " << customController.getService());
 						usingSafeController = true;
 					} else {
-						usingSafeController = safetyCheck(global, controllerCall.response.controlOutput);
+						usingSafeController = !safetyCheck(global, controllerCall.response.controlOutput);
 						if(usingSafeController) {
 							ROS_INFO_STREAM("safety check failed, switching to safe controller");
 						}