From 2eada83c49d58ce4710585437a7bd28380f5c628 Mon Sep 17 00:00:00 2001 From: Daniel Koch Date: Tue, 3 Apr 2018 16:59:20 -0600 Subject: [PATCH] When calibrating RC trims, add to existing trim values --- src/controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/controller.cpp b/src/controller.cpp index 779a6d61..462988c1 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -155,9 +155,9 @@ void Controller::calculate_equilbrium_torque_from_rc() turbomath::Vector pid_output = run_pid_loops(0, fake_state, RF_.command_manager_.rc_control(), false); // the output from the controller is going to be the static offsets - RF_.params_.set_param_float(PARAM_X_EQ_TORQUE, pid_output.x); - RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE, pid_output.y); - RF_.params_.set_param_float(PARAM_Z_EQ_TORQUE, pid_output.z); + RF_.params_.set_param_float(PARAM_X_EQ_TORQUE, pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE)); + RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE, pid_output.y + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE)); + RF_.params_.set_param_float(PARAM_Z_EQ_TORQUE, pid_output.z + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE)); RF_.comm_manager_.log(CommLink::LogSeverity::LOG_WARNING, "Equilibrium torques found and applied."); RF_.comm_manager_.log(CommLink::LogSeverity::LOG_WARNING, "Please zero out trims on your transmitter");