aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-08-15 16:29:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

Position control draft

Browse Source
This commit is contained in:
Oleg Kalachev
2026-08-01 20:13:48 +03:00
parent 74b082957e
commit 142cb9af01
7 changed files with 133 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
flix/cli.ino
+3 -1
View File
@@ -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++) {
flix/control.ino
+14 -2
View File
@@ -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));
flix/estimate.ino
+1
View File
@@ -26,6 +26,7 @@ void estimate() {
applyGyro();
applyAcc();
applyLevel();
estimatePosition();
}
void applyGyro() {
flix/log.ino
+5
View File
@@ -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
flix/mavlink.ino
+7
View File
@@ -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);
flix/parameters.ino
+8
View File
@@ -5,6 +5,7 @@
#include <Preferences.h>
#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<float> 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},
flix/position.ino
+95
View File
@@ -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));
}