From 6b41f9055e327b6bc84ef867fd4ec5d2a7b0129c Mon Sep 17 00:00:00 2001
From: Angel <roangel@student.ethz.ch>
Date: Fri, 8 Sep 2017 18:28:31 +0200
Subject: [PATCH] small detail in custom controller
---
pps_ws/src/d_fall_pps/src/CustomControllerService.cpp | 3 +++
pps_ws/src/d_fall_pps/src/SafeControllerService.cpp | 1 +
2 files changed, 4 insertions(+)
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index 228d8c31..14478394 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -133,6 +133,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
request.ownCrazyflie.y -= setpoint[1];
request.ownCrazyflie.z -= setpoint[2];
float yaw = request.ownCrazyflie.yaw - setpoint[3];
+ while(yaw > PI) {yaw -= 2 * PI;}
+ while(yaw < -PI) {yaw += 2 * PI;}
+ request.ownCrazyflie.yaw = yaw;
float est[9];
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index efe421bf..4b712ef3 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
@@ -174,6 +174,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
request.ownCrazyflie.y -= setpoint[1];
request.ownCrazyflie.z -= setpoint[2];
float yaw = request.ownCrazyflie.yaw - setpoint[3];
+
//bag.write("Offset", ros::Time::now(), request.ownCrazyflie);
--
GitLab