From 476f24f774f101236cc9a83df5f21fe52c9493ac Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 6 Jan 2024 00:08:30 +0300 Subject: [PATCH] Clarify rates control code --- flix/control.ino | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/flix/control.ino b/flix/control.ino index d563f36..bd145a2 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -137,10 +137,12 @@ void controlRate() { } Vector ratesFiltered = ratesFilter.update(rates); + Vector error = ratesTarget - ratesFiltered; - torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque" - torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt); - torqueTarget.z = yawRatePID.update(ratesTarget.z - ratesFiltered.z, dt); + // Calculate desired torque, where 0 - no torque, 1 - maximum possible torque + torqueTarget.x = rollRatePID.update(error.x, dt); + torqueTarget.y = pitchRatePID.update(error.y, dt); + torqueTarget.z = yawRatePID.update(error.z, dt); } void controlTorque() {