diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp index a50f6eb82a91e55f050e086e1079667e5811ba85..0d656944849224d8f7347d488385b2836af77534 100644 --- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp +++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp @@ -160,6 +160,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & response.controlOutput.pitch = outPitch; response.controlOutput.yaw = outYaw; + ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll); + ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch); + ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw); + response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + gravity_force); response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + gravity_force); response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + gravity_force);