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

minor changes for estimation error info-stream

parent 08a770dd
......@@ -399,13 +399,13 @@ 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;
}
*/
// 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);
......@@ -556,15 +556,56 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
outYaw = u(11,0);
}
if (outer_loop_counter % 1 == 0)
if (outer_loop_counter % 50 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] COMMANDS FROM LQG:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] --------- ESTIMATES AGENT 1 --------------");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: roll estimate = " << estimator_state_prev(0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: pitch estimate = " << estimator_state_prev(1) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: yaw estimate = " << estimator_state_prev(2) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi estimate = " << estimator_state_prev(3) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta estimate = " << estimator_state_prev(4) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi_dot estimate = " << estimator_state_prev(5) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta_dot estimate = " << estimator_state_prev(6) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] --------- ESTIMATES AGENT 2 --------------");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent2: roll estimate = " << estimator_state_prev(7) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent2: pitch estimate = " << estimator_state_prev(8) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent2: yaw estimate = " << estimator_state_prev(9) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent2: cable phi estimate = " << estimator_state_prev(10) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent2: cable theta estimate = " << estimator_state_prev(11) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent2: cable phi_dot estimate = " << estimator_state_prev(12) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent2: cable theta_dot estimate = " << estimator_state_prev(13) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] --------- ESTIMATES AGENT 3 --------------");
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent3: roll estimate = " << estimator_state_prev(14) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent3: pitch estimate = " << estimator_state_prev(15) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent3: yaw estimate = " << estimator_state_prev(16) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent3: cable phi estimate = " << estimator_state_prev(17) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent3: cable theta estimate = " << estimator_state_prev(18) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent3: cable phi_dot estimate = " << estimator_state_prev(19) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent3: cable theta_dot estimate = " << estimator_state_prev(20) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] --------- ESTIMATES LOAD --------------");
ROS_INFO_STREAM("[STUDENT CONTROLLER] x estimate = " << estimator_state_prev(21) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y estimate = " << estimator_state_prev(22) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z estimate = " << estimator_state_prev(23) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] vx estimate = " << estimator_state_prev(24) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] vy estimate = " << estimator_state_prev(25) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] vz estimate = " << estimator_state_prev(26) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll estimate = " << estimator_state_prev(27) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch estimate = " << estimator_state_prev(28) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw estimate = " << estimator_state_prev(29) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll rate estimate = " << estimator_state_prev(30) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch rate estimate = " << estimator_state_prev(31) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw rate estimate = " << estimator_state_prev(32) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] ---------- COMMANDS FROM LQG: --------------");
ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired yaw-rate computed = " << outYaw );
float est_error_roll = estimator_state_prev(0)-request.otherCrazyflies[0].roll;
/* float est_error_roll = estimator_state_prev(0)-request.otherCrazyflies[0].roll;
float est_error_pitch = estimator_state_prev(1)-request.otherCrazyflies[0].pitch;
float est_error_yaw = estimator_state_prev(2)-request.otherCrazyflies[0].yaw;
float est_error_load_x = estimator_state_prev(21)-request.otherCrazyflies[3].x;
......@@ -581,7 +622,7 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi estimate = " << estimator_state_prev(3) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable theta estimate = " << estimator_state_prev(4) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable phi_dot estimate = " << estimator_state_prev(5) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << estimator_state_prev(6) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] agent1: cable ttheta_dot estimate = " << estimator_state_prev(6) );*/
}
......@@ -605,7 +646,7 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
void display_control_agent1( Controller::Request &request, Controller::Response &response)
{
/*
// Control output
Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev_only_agent1;
// Estimator update
......@@ -626,8 +667,9 @@ void display_control_agent1( Controller::Request &request, Controller::Response
outYaw = u(3,0);
}
// Debugging
if (outer_loop_counter % 100 == 0)
if (outer_loop_counter % 50 == 0)
{
float est_error_roll = estimator_state_prev_only_agent1(0)-request.otherCrazyflies[0].roll;
float est_error_pitch = estimator_state_prev_only_agent1(1)-request.otherCrazyflies[0].pitch;
......@@ -644,20 +686,14 @@ void display_control_agent1( Controller::Request &request, Controller::Response
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable theta estimate = " << estimator_state_prev_only_agent1(4) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable phi_dot estimate = " << estimator_state_prev_only_agent1(5) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: cable ttheta_dot estimate = " << estimator_state_prev_only_agent1(6) );
}
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] COMMANDS:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: u thrust sum computed = " << thrustSum );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired roll-rate computed = " << outRoll );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Only agent1: desired yaw-rate computed = " << outYaw );
}
// Debugging
if (outer_loop_counter % 100 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] errors:");
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter1 = " << y_only_agent_1(0,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter1 = " << y_only_agent_1(1,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter1 = " << y_only_agent_1(2,0) );
......@@ -698,7 +734,7 @@ void display_control_agent1( Controller::Request &request, Controller::Response
// it can either be Motor, Rate or Angle based
// response.controlOutput.onboardControllerType = MOTOR_MODE;
response.controlOutput.onboardControllerType = RATE_MODE;
// response.controlOutput.onboardControllerType = ANGLE_MODE;
// response.controlOutput.onboardControllerType = ANGLE_MODE; */
}
void get_y_from_CFData( Controller::Request &request )
......@@ -888,12 +924,12 @@ void get_y_from_CFData_withLoad( Controller::Request &request )
y_to_controller = y_meas - y_eq;
// Debugging
if (outer_loop_counter % 1 == 0)
{
ROS_INFO_STREAM("--------------------------" );
ROS_INFO_STREAM("[STUDENT CONTROLLER] Agent : " << my_agentID);
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " << y_eq.transpose() );
//ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );
//ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " << y_eq.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for copter1 = " << y_to_controller(0,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for copter1 = " << y_to_controller(1,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for copter1 = " << y_to_controller(2,0) );
......@@ -918,8 +954,9 @@ void get_y_from_CFData_withLoad( Controller::Request &request )
ROS_INFO_STREAM("[STUDENT CONTROLLER] roll-error for load = " << y_to_controller(21,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] pitch-error for load = " << y_to_controller(22,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] yaw-error for load = " << y_to_controller(23,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_to_controller^T = " << y_to_controller.transpose() );
}
//ROS_INFO_STREAM("[STUDENT CONTROLLER] y_to_controller^T = " << y_to_controller.transpose() );
}
}
......
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