aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Refactor control, remake controlManual to controlTorque

Browse Source
This commit is contained in:
Oleg Kalachev 2023-12-13 08:33:35 +03:00
parent 4fcf2109ce
commit 2c21114540
2 changed files with 11 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
flix/control.ino
View File

@ -59,10 +59,12 @@ void control()
if (mode == STAB) {
controlAttitude();
controlRate();
controlTorque();
} else if (mode == ACRO) {
controlRate();
controlTorque();
} else if (mode == MANUAL) {
controlManual();
controlTorque();
}
}
@ -94,6 +96,11 @@ void interpretRC()
-controls[RC_CHANNEL_PITCH] * MAX_TILT,
attitudeTarget.getYaw());
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
} else if (mode == MANUAL) {
// passthrough mode
yawMode = YAW_RATE;
torqueTarget = Vector(controls[RC_CHANNEL_ROLL], -controls[RC_CHANNEL_PITCH], controls[RC_CHANNEL_YAW]) * 0.01;
}
if (yawMode == YAW_RATE || !motorsActive()) {
@ -128,7 +135,6 @@ void controlAttitude()
void controlRate()
{
if (!armed) {
memset(motors, 0, sizeof(motors));
rollRatePID.reset();
pitchRatePID.reset();
yawRatePID.reset();
@ -140,28 +146,15 @@ void controlRate()
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);
motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;
motors[MOTOR_REAR_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;
motors[0] = constrain(motors[0], 0, 1);
motors[1] = constrain(motors[1], 0, 1);
motors[2] = constrain(motors[2], 0, 1);
motors[3] = constrain(motors[3], 0, 1);
}
// passthrough mode
void controlManual()
void controlTorque()
{
if (controls[RC_CHANNEL_THROTTLE] < 0.1) {
if (!armed) {
memset(motors, 0, sizeof(motors));
return;
}
torqueTarget = ratesTarget * 0.01;
motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;
motors[MOTOR_REAR_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;

2
gazebo/flix.h
View File

@ -34,8 +34,8 @@ void signalizeHorizontality();
void control();
void interpretRC();
void controlAttitude();
void controlManual();
void controlRate();
void controlTorque();
void showTable();
bool motorsActive();
void cliTestMotor(uint8_t n);