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 aa932e89 authored by beuchatp's avatar beuchatp
Browse files

LQG 1e-3*Qu , 100Hz , printing estimates for copter 1

parent 6c5f1471
......@@ -398,20 +398,21 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// update measurement vector using vicon Data
//get_y_from_CFData( request );
get_y_from_CFData_withLoad( request );
/*
y_only_agent_1 = y_to_controller;
for(int i = 6 ; i < 24 ; ++i )
{
y_only_agent_1(i,0) = 0;
}
*/
}
// compute centralized controller commands
//centralized_control_command( request, response );
display_control_agent1( request, response);
centralized_control_command( request, response );
//display_control_agent1( request, response);
//float delta_z_load_current = request.otherCrazyflies[3].z - load_setpoint[2];
// gravity_force_copterloadsystem -= 0.001*delta_z_load_current;
......@@ -633,6 +634,18 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired yaw-rate computed = " << outYaw );
}
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll estimate = " << estimator_state_prev_only_agent1[0] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch estimate = " << estimator_state_prev_only_agent1[1] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw estimate = " << estimator_state_prev_only_agent1[2] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi estimate = " << estimator_state_prev_only_agent1[3] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta estimate = " << estimator_state_prev_only_agent1[4] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi_dot estimate = " << estimator_state_prev_only_agent1[5] );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << estimator_state_prev_only_agent1[6] );
}
// PREPARE AND RETURN THE VARIABLE "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