To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit dff0150c authored by beuchatp's avatar beuchatp
Browse files

finally corrected body rate commands for current yaw angle

parent 93c42ea7
......@@ -334,4 +334,6 @@ std::string getcwd_string( void );
void distributed_control_command( Controller::Request &request, Controller::Response &response );
void centralized_control_command( Controller::Request &request, Controller::Response &response);
void display_control_agent1( Controller::Request &request, Controller::Response &response) ;
std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw );
\ No newline at end of file
std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw );
void desired_eulerrates_to_bodyrates( Controller::Request &request , float roll_rate , float pitch_rate, float (&bodyrates)[2]);
\ No newline at end of file
......@@ -625,11 +625,14 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << estimator_state_prev(6) );*/
}
// CONVERT
float bodyrates[2];
desired_eulerrates_to_bodyrates( request , outRoll , outPitch, bodyrates);
// PREPARE AND RETURN THE VARIABLE "response"
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.roll = bodyrates[0];
response.controlOutput.pitch = bodyrates[0];
response.controlOutput.yaw = outYaw;
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
......@@ -644,6 +647,22 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
}
void desired_eulerrates_to_bodyrates( Controller::Request &request , float roll_rate , float pitch_rate, float (&bodyrates)[2])
{
float yaw = request.ownCrazyflie.yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
float sinYaw = sin(yaw);
float cosYaw = cos(yaw);
bodyrates[0] = roll_rate * cosYaw + pitch_rate * sinYaw;
bodyrates[1] = -roll_rate * sinYaw + pitch_rate * cosYaw;
return;
}
void display_control_agent1( Controller::Request &request, Controller::Response &response)
{
/*
......
Markdown is supported
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