diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService.cpp index b7a1bf3fddd94abf1bab21ee8bc74e723f239a9c..75254898379d3d081e31324071f2caad80e8d676 100644 --- a/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService.cpp +++ b/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService.cpp @@ -774,30 +774,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & pitchRate_command = m_mpc_input_vector_to_apply(2); } - // else, use backup controller + // else, issue 0 command else { - // BODY Z CONTROL - // backup LQR controller totalThrust_command = 0.0; - for (int i = 0; i < 9; i++) - totalThrust_command -= m_gainMatrixThrust[i] * stateErrorBody[i]; - - // add feed-forward term - totalThrust_command += m_cf_weight_in_newtons; - - - // BODY X CONTROL - // backup LQR controller - pitchRate_command = 0.0; - for(int i = 0; i < 9; i++) - pitchRate_command -= m_gainMatrixPitchRate[i] * stateErrorBody[i]; - - // BODY Y CONTROL - // backup LQR controller rollRate_command = 0.0; - for(int i = 0; i < 9; i++) - rollRate_command -= m_gainMatrixRollRate[i] * stateErrorBody[i]; + pitchRate_command = 0.0; + yawRate_command = 0.0; } diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService_solution.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService_solution.cpp index 318f3e345093f2afafc7d127f1dcc17ffec801ac..d1c4b062f746cb3867a3e7c820023ee017b033db 100644 --- a/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService_solution.cpp +++ b/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService_solution.cpp @@ -750,30 +750,13 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & pitchRate_command = m_mpc_input_vector_to_apply(2); } - // else, use backup controller + // else, issue 0 command else { - // BODY Z CONTROL - // backup LQR controller totalThrust_command = 0.0; - for (int i = 0; i < 9; i++) - totalThrust_command -= m_gainMatrixThrust[i] * stateErrorBody[i]; - - // add feed-forward term - totalThrust_command += m_cf_weight_in_newtons; - - - // BODY X CONTROL - // backup LQR controller - pitchRate_command = 0.0; - for(int i = 0; i < 9; i++) - pitchRate_command -= m_gainMatrixPitchRate[i] * stateErrorBody[i]; - - // BODY Y CONTROL - // backup LQR controller rollRate_command = 0.0; - for(int i = 0; i < 9; i++) - rollRate_command -= m_gainMatrixRollRate[i] * stateErrorBody[i]; + pitchRate_command = 0.0; + yawRate_command = 0.0; }