From 142cb9af010ce62b98857276cc291c0d9774117e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 1 Aug 2026 20:13:48 +0300 Subject: [PATCH] Position control draft --- flix/cli.ino | 4 +- flix/control.ino | 16 +++++++- flix/estimate.ino | 1 + flix/log.ino | 5 +++ flix/mavlink.ino | 7 ++++ flix/parameters.ino | 8 ++++ flix/position.ino | 95 +++++++++++++++++++++++++++++++++++++++++++++ 7 files changed, 133 insertions(+), 3 deletions(-) create mode 100644 flix/position.ino diff --git a/flix/cli.ino b/flix/cli.ino index cbbd66e..93fe789 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -9,7 +9,7 @@ #include "filter.h" extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; -extern const int RAW, ACRO, STAB, AUTO; +extern const int RAW, ACRO, STAB, AUTO, POS; extern const int W_AP, W_STA, W_ESPNOW; extern float t, dt, loopRate; extern uint16_t channels[16]; @@ -134,6 +134,8 @@ void doCommand(String str, bool echo = false) { mode = ACRO; } else if (command == "auto") { mode = AUTO; + } else if (command == "pos") { + mode = POS; } else if (command == "rc") { print("channels: "); for (int i = 0; i < 16; i++) { diff --git a/flix/control.ino b/flix/control.ino index 793f064..5d0ae18 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 -const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes +const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3, POS = 4; int mode = STAB; bool armed = false; @@ -56,6 +56,7 @@ int flightModes[] = {STAB, STAB, STAB}; // map for rc mode switch extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode; +extern Vector position, positionTarget; void control() { interpretControls(); @@ -79,7 +80,18 @@ void interpretControls() { thrustTarget = controlThrottle; - if (mode == STAB) { + if (mode == POS) { + if (controlRoll != 0 || controlPitch != 0) { + positionTarget.x = NAN; + positionTarget.y = NAN; + } else if (invalid(positionTarget.x) || invalid(positionTarget.y)) { + // reset position target + positionTarget.x = position.x; + positionTarget.y = position.y; + } + } + + if (mode == STAB || (mode == POS && invalid(positionTarget.x))) { float yawTarget = attitudeTarget.getYaw(); if (!armed || invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget)); diff --git a/flix/estimate.ino b/flix/estimate.ino index 95c1d38..afa6e94 100644 --- a/flix/estimate.ino +++ b/flix/estimate.ino @@ -26,6 +26,7 @@ void estimate() { applyGyro(); applyAcc(); applyLevel(); + estimatePosition(); } void applyGyro() { diff --git a/flix/log.ino b/flix/log.ino index 77a56cb..5edbef5 100644 --- a/flix/log.ino +++ b/flix/log.ino @@ -68,6 +68,11 @@ LogTopic logTopics[] = { {"attitude.pitch", []() { return attitude.getPitch(); }}, {"attitude.yaw", []() { return attitude.getYaw(); }}), + LogTopic(50, + {"position.x", &position.x}, + {"position.y", &position.y}, + {"position.z", &position.z}), + // rc LogTopic(10, {"controlTime", &controlTime, false}, // trigger value diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 26e8026..462a5c9 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -196,6 +196,13 @@ void handleMavlink(const void *_msg) { sendMessage(&msg); } + if (msg.msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) { + mavlink_vision_position_estimate_t m; + mavlink_msg_vision_position_estimate_decode(&msg, &m); + Vector pos(m.x, m.y, m.z); + correctPosition(pos); + } + if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy mavlink_mission_request_list_t m; mavlink_msg_mission_request_list_decode(&msg, &m); diff --git a/flix/parameters.ino b/flix/parameters.ino index 1b78803..979dbf7 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -5,6 +5,7 @@ #include #include "util.h" +#include "pid.h" extern int channelZero[16], channelMax[16]; extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel; @@ -13,6 +14,7 @@ extern int wifiMode, wifiLongRange, udpLocalPort, udpRemotePort, espnowChannel; extern float rcLossTimeout, descendTime, disarmTilt; extern float voltageScale; extern LowPassFilter voltageFilter; +extern PID posX_PID, posY_PID; Preferences storage; @@ -59,6 +61,12 @@ Parameter parameters[] = { {"CTL_FLT_MODE_0", &flightModes[0]}, {"CTL_FLT_MODE_1", &flightModes[1]}, {"CTL_FLT_MODE_2", &flightModes[2]}, + // position + {"POS_XY_P", &posX_PID.p, setupPosition}, + {"POS_XY_I", &posX_PID.i, setupPosition}, + {"POS_XY_D", &posX_PID.d, setupPosition}, + {"POS_XY_WU", &posX_PID.windup, setupPosition}, + {"POS_XY_D_A", &posX_PID.lpf.alpha, setupPosition}, // imu {"IMU_ROT_ROLL", &imuRotation.x}, {"IMU_ROT_PITCH", &imuRotation.y}, diff --git a/flix/position.ino b/flix/position.ino new file mode 100644 index 0000000..dcfb59d --- /dev/null +++ b/flix/position.ino @@ -0,0 +1,95 @@ +#include "vector.h" +#include "quaternion.h" +#include "pid.h" +#include "util.h" + +float mass = 0.065; +float motorThrust = 0.031 * ONE_G; +float positionWeight = 0.9; +float velocityWeight = 0.9; + +PID velX_PID(1.0, 0.0, 0.02); +PID velY_PID(1.0, 0.0, 0.02); +PID velZ_PID(1.0, 0.0, 0.02); + +// PID posX_PID(1.0, 0.0, 0.0); +// PID posY_PID(1.0, 0.0, 0.0); +// PID posZ_PID(1.0, 0.0, 0.0); + +PID posX_PID(0.1, 0.0, 0.0); +PID posY_PID(0.1, 0.0, 0.0); +PID posZ_PID(0.1, 0.0, 0.0); + +Vector acceleration; +Vector velocity; +Vector position; +Vector velocityTarget(NAN, NAN, NAN); +Vector positionTarget(NAN, NAN, NAN); + +extern Quaternion attitudeTarget; +extern float thrustTarget; + +void setupPosition() { + // TODO: posY_PID = posX_PID; + // posX_PID.copy(posY_PID); + posY_PID.p = posX_PID.p; + posY_PID.i = posX_PID.i; + posY_PID.d = posX_PID.d; +} + +void estimatePosition() { + acceleration = Quaternion::rotateVector(acceleration, attitude) + Vector(0, 0, -ONE_G); + velocity += acceleration * dt; + position += velocity * dt; +} + +void correctPosition(Vector& pos) { + // position += (pos - position) * positionWeight; + position = pos; + // positionUpdated = true; + + if (invalid(positionTarget.x) || invalid(positionTarget.y)) return; + + attitudeTarget.setPitch(posX_PID.update(positionTarget.x - position.x)); + attitudeTarget.setRoll(posY_PID.update(positionTarget.y - position.y)); +} + +void correctVelocity(Vector& vel) { + velocity += (vel - velocity) * velocityWeight; +} + +void controlVelocity() { + Vector accelerationTarget( + velX_PID.update(velocityTarget.x - velocity.x), + velY_PID.update(velocityTarget.y - velocity.y), + velZ_PID.update(velocityTarget.z - velocity.z) + ); + + Vector thrustVector = (accelerationTarget + Vector(0, 0, ONE_G)) * mass; + + const Vector up(0, 0, 1); + + attitudeTarget = Quaternion::fromBetweenVectors(up, thrustVector); + float maxThrust = motorThrust * 4; // 4 motors + thrustTarget = thrustVector.norm() / maxThrust; +} + +void controlPosition() { + if (positionTarget.invalid()) return; + + velocityTarget = Vector( + posX_PID.update(positionTarget.x - position.x), + posY_PID.update(positionTarget.y - position.y), + posZ_PID.update(positionTarget.z - position.z) + ); + + controlVelocity(); +} + +void controlPositionSimple() { + if (positionTarget.invalid()) return; + + // straight attitude target control + attitudeTarget.setPitch(posX_PID.update(positionTarget.x - position.x)); + attitudeTarget.setRoll(posY_PID.update(positionTarget.y - position.y)); +}