Commit 0d2733ad authored by bucyril's avatar bucyril
Browse files

fixed mistake in SafeController with array index on setpoint

parent f75442d6
TeamName: 'Two'
CrazyFlieName: "cfFour"
CrazyFlieAddress: "radio://0/99/2M"
CrazyFlieName: "cfTest"
CrazyFlieAddress: "radio://0/80/2M"
SafeController: ""
SafeControllerType: ""
CustomController: ""
......
......@@ -11,5 +11,5 @@ 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]
setpoint: [0.2, 0.2, 0.6, 0.7]
......@@ -109,7 +109,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
est[7] = request.crazyflieLocation.pitch;
//ROS_INFO_STREAM("crazyflieyaw: " << request.crazyflieLocation.yaw);
//ROS_INFO_STREAM("setpointyaw: " << request.setpoint.yaw);
float yaw = request.crazyflieLocation.yaw - setpoint[4];
float yaw = request.crazyflieLocation.yaw - setpoint[3];
//ROS_INFO_STREAM("differenceyaw: " << yaw);
while(yaw > PI) yaw -= 2 * PI;
......
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