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) { if (mode == STAB) {
controlAttitude(); controlAttitude();
controlRate(); controlRate();
controlTorque();
} else if (mode == ACRO) { } else if (mode == ACRO) {
controlRate(); controlRate();
controlTorque();
} else if (mode == MANUAL) { } else if (mode == MANUAL) {
controlManual(); controlTorque();
} }
} }
@ -94,6 +96,11 @@ void interpretRC()
-controls[RC_CHANNEL_PITCH] * MAX_TILT, -controls[RC_CHANNEL_PITCH] * MAX_TILT,
attitudeTarget.getYaw()); attitudeTarget.getYaw());
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX; 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()) { if (yawMode == YAW_RATE || !motorsActive()) {
@ -128,7 +135,6 @@ void controlAttitude()
void controlRate() void controlRate()
{ {
if (!armed) { if (!armed) {
memset(motors, 0, sizeof(motors));
rollRatePID.reset(); rollRatePID.reset();
pitchRatePID.reset(); pitchRatePID.reset();
yawRatePID.reset(); yawRatePID.reset();
@ -140,28 +146,15 @@ void controlRate()
torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque" torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt); torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt);
torqueTarget.z = yawRatePID.update(ratesTarget.z - ratesFiltered.z, 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 controlTorque()
void controlManual()
{ {
if (controls[RC_CHANNEL_THROTTLE] < 0.1) { if (!armed) {
memset(motors, 0, sizeof(motors)); memset(motors, 0, sizeof(motors));
return; return;
} }
torqueTarget = ratesTarget * 0.01;
motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z; motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
motors[MOTOR_FRONT_RIGHT] = 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_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;

2
gazebo/flix.h
View File

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