Commit 6b41f905 authored by roangel's avatar roangel
Browse files

small detail in custom controller

parent c47eb698
......@@ -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];
......
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment