Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Refactor control, remake controlManual to controlTorque

Oleg Kalachev2c21114540b3
diff --git a/flix/control.ino b/flix/control.ino
index 9ac1510..e233dd8 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -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;
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 20cb5cf..3070dbd 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -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);