From 8418723ccc08640691efe494296e83f8c066f047 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 26 Aug 2025 01:00:56 +0300 Subject: [PATCH] Refactor control subsystem Add interpretControls function to convert pilot commands and mode into control targets and make control functions independent from the mode. Add ratesExtra target for rates feed-forward; remove yawMode. Rename controlRate to controlRates to reflect rates variable name. Remove USER mode. --- flix/control.ino | 95 ++++++++++++++++++++--------------------------- flix/flix.ino | 3 +- flix/mavlink.ino | 4 +- flix/quaternion.h | 16 ++++++++ flix/rc.ino | 2 +- flix/util.h | 8 ++++ flix/vector.h | 15 ++++++++ gazebo/flix.h | 7 ++-- 8 files changed, 88 insertions(+), 62 deletions(-) diff --git a/flix/control.ino b/flix/control.ino index 5957a21..2d5db74 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -34,7 +34,7 @@ #define TILT_MAX radians(30) #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz -enum { MANUAL, ACRO, STAB, USER } mode = STAB; +enum { MANUAL, ACRO, STAB } mode = STAB; enum { YAW, YAW_RATE } yawMode = YAW; bool armed = false; @@ -49,6 +49,7 @@ float tiltMax = TILT_MAX; Quaternion attitudeTarget; Vector ratesTarget; +Vector ratesExtra; // feedforward rates Vector torqueTarget; float thrustTarget; @@ -56,63 +57,47 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode; void control() { - interpretRC(); + interpretControls(); failsafe(); - if (mode == STAB) { - controlAttitude(); - controlRate(); - controlTorque(); - } else if (mode == ACRO) { - controlRate(); - controlTorque(); - } else if (mode == MANUAL) { - controlTorque(); - } + controlAttitude(); + controlRates(); + controlTorque(); } -void interpretRC() { - armed = controlThrottle >= 0.05 && controlArmed >= 0.5; +void interpretControls() { + armed = controlThrottle >= 0.05; + if (controlArmed < 0.5) armed = false; // NOTE: put ACRO or MANUAL modes there if you want to use them - if (controlMode < 0.25) { - mode = STAB; - } else if (controlMode < 0.75) { - mode = STAB; - } else { - mode = STAB; - } + if (controlMode < 0.25) mode = STAB; + if (controlMode < 0.75) mode = STAB; + if (controlMode > 0.75) mode = STAB; thrustTarget = controlThrottle; - if (mode == ACRO) { - yawMode = YAW_RATE; - ratesTarget.x = controlRoll * maxRate.x; - ratesTarget.y = controlPitch* maxRate.y; - ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU - - } else if (mode == STAB) { - yawMode = controlYaw == 0 ? YAW : YAW_RATE; - - attitudeTarget = Quaternion::fromEuler(Vector( - controlRoll * tiltMax, - controlPitch * tiltMax, - attitudeTarget.getYaw())); - ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU - - } else if (mode == MANUAL) { - // passthrough mode - yawMode = YAW_RATE; - torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01; + if (mode == STAB) { + float yawTarget = attitudeTarget.getYaw(); + if (invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target if NAN or pilot commands yaw rate + attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget)); + ratesExtra = Vector(0, 0, -controlYaw * maxRate.z); // positive yaw stick means clockwise rotation in FLU } - if (yawMode == YAW_RATE || !motorsActive()) { - // update yaw target as we don't have control over the yaw - attitudeTarget.setYaw(attitude.getYaw()); + if (mode == ACRO) { + attitudeTarget.invalidate(); // skip attitude control + ratesTarget.x = controlRoll * maxRate.x; + ratesTarget.y = controlPitch * maxRate.y; + ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU + } + + if (mode == MANUAL) { // passthrough mode + attitudeTarget.invalidate(); // skip attitude control + ratesTarget.invalidate(); // skip rate control + torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01; } } void controlAttitude() { - if (!armed) { + if (!armed || attitudeTarget.invalid()) { // skip attitude control rollPID.reset(); pitchPID.reset(); yawPID.reset(); @@ -125,17 +110,16 @@ void controlAttitude() { Vector error = Vector::rotationVectorBetween(upTarget, upActual); - ratesTarget.x = rollPID.update(error.x, dt); - ratesTarget.y = pitchPID.update(error.y, dt); + ratesTarget.x = rollPID.update(error.x, dt) + ratesExtra.x; + ratesTarget.y = pitchPID.update(error.y, dt) + ratesExtra.y; - if (yawMode == YAW) { - float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()); - ratesTarget.z = yawPID.update(yawError, dt); - } + float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()); + ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z; } -void controlRate() { - if (!armed) { + +void controlRates() { + if (!armed || ratesTarget.invalid()) { // skip rates control rollRatePID.reset(); pitchRatePID.reset(); yawRatePID.reset(); @@ -151,8 +135,10 @@ void controlRate() { } void controlTorque() { - if (!armed) { - memset(motors, 0, sizeof(motors)); + if (!torqueTarget.valid()) return; // skip torque control + + if (!armed || thrustTarget < 0.05) { + memset(motors, 0, sizeof(motors)); // stop motors if no thrust return; } @@ -172,7 +158,6 @@ const char* getModeName() { case MANUAL: return "MANUAL"; case ACRO: return "ACRO"; case STAB: return "STAB"; - case USER: return "USER"; default: return "UNKNOWN"; } } diff --git a/flix/flix.ino b/flix/flix.ino index 531ad18..7d494c9 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -12,7 +12,8 @@ double t = NAN; // current step time, s float dt; // time delta from previous step, s -float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; // pilot's inputs, range [-1, 1] +float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1] +float controlArmed = NAN, controlMode = NAN; Vector gyro; // gyroscope data Vector acc; // accelerometer data, m/s/s Vector rates; // filtered angular rates, rad/s diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 88653ed..8f48ef0 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -102,8 +102,8 @@ void handleMavlink(const void *_msg) { controlPitch = m.x / 1000.0f * mavlinkControlScale; controlRoll = m.y / 1000.0f * mavlinkControlScale; controlYaw = m.r / 1000.0f * mavlinkControlScale; - controlMode = 1; // STAB mode - controlArmed = 1; // armed + controlMode = NAN; + controlArmed = NAN; controlTime = t; if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0; diff --git a/flix/quaternion.h b/flix/quaternion.h index 24bd1e9..29a3d04 100644 --- a/flix/quaternion.h +++ b/flix/quaternion.h @@ -64,6 +64,22 @@ public: return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z); } + bool valid() const { + return finite(); + } + + bool invalid() const { + return !valid(); + } + + void invalidate() { + w = NAN; + x = NAN; + y = NAN; + z = NAN; + } + + float norm() const { return sqrt(w * w + x * x + y * y + z * z); } diff --git a/flix/rc.ino b/flix/rc.ino index b0c8261..09ba957 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -42,7 +42,7 @@ void normalizeRC() { controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN; controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN; controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN; - controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : 1; // assume armed by default + controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : NAN; controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; } diff --git a/flix/util.h b/flix/util.h index 9e63532..90925d5 100644 --- a/flix/util.h +++ b/flix/util.h @@ -19,6 +19,14 @@ float mapff(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } +bool invalid(float x) { + return !isfinite(x); +} + +bool valid(float x) { + return isfinite(x); +} + // Wrap angle to [-PI, PI) float wrapAngle(float angle) { angle = fmodf(angle, 2 * PI); diff --git a/flix/vector.h b/flix/vector.h index b2d9a89..272e1db 100644 --- a/flix/vector.h +++ b/flix/vector.h @@ -21,6 +21,21 @@ public: return isfinite(x) && isfinite(y) && isfinite(z); } + bool valid() const { + return finite(); + } + + bool invalid() const { + return !valid(); + } + + void invalidate() { + x = NAN; + y = NAN; + z = NAN; + } + + float norm() const { return sqrt(x * x + y * y + z * z); } diff --git a/gazebo/flix.h b/gazebo/flix.h index 53da2b3..f843597 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -15,7 +15,8 @@ double t = NAN; float dt; float motors[4]; -float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; +float controlRoll, controlPitch, controlYaw, controlThrottle = NAN; +float controlArmed = NAN, controlMode = NAN; Vector acc; Vector gyro; Vector rates; @@ -28,9 +29,9 @@ void computeLoopRate(); void applyGyro(); void applyAcc(); void control(); -void interpretRC(); +void interpretControls(); void controlAttitude(); -void controlRate(); +void controlRates(); void controlTorque(); const char* getModeName(); void sendMotors();