From c56441de6481ade22693e0043a41ba90534c27da Mon Sep 17 00:00:00 2001
From: bucyril <bucyril@ethz.ch>
Date: Tue, 23 May 2017 14:42:08 +0200
Subject: [PATCH] corrected circle controller, made crazyflie crash safely
---
pps_ws/src/d_fall_pps/CMakeLists.txt | 4 ++++
pps_ws/src/d_fall_pps/param/SafeController.yaml | 2 +-
.../d_fall_pps/src/CircleControllerService.cpp | 16 ++++++++--------
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 2 +-
4 files changed, 14 insertions(+), 10 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 0b32f4a8..a6ae7a09 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/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
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index 595348ed..a2f9b963 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/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]
diff --git a/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp b/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp
index 7c88e462..83f9bb46 100755
--- a/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp
+++ b/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);
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 86eb6660..e7f3cb9c 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/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");
}
--
GitLab