Refactor control, remake controlManual to controlTorque

okalachev · Dec 13, 2023 · 5cd7713 · 5cd7713
1 parent 5af8a68
commit 5cd7713
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 18 deletions.
diff --git a/flix/control.ino b/flix/control.ino
@@ -60,10 +60,12 @@ void control()
  if (mode == STAB) {
  controlAttitude();
  controlRate();
+ controlTorque();
  } else if (mode == ACRO) {
  controlRate();
+ controlTorque();
  } else if (mode == MANUAL) {
- controlManual();
+ controlTorque();
  }
 }
 
@@ -95,6 +97,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()) {
@@ -129,7 +136,6 @@ void controlAttitude()
 void controlRate()
 {
  if (!armed) {
- memset(motors, 0, sizeof(motors));
  rollRatePID.reset();
  pitchRatePID.reset();
  yawRatePID.reset();
@@ -141,28 +147,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
@@ -35,8 +35,8 @@ void signalizeHorizontality();
 void control();
 void interpretRC();
 void controlAttitude();
-void controlManual();
 void controlRate();
+void controlTorque();
 void showTable();
 bool motorsActive();
 void cliTestMotor(uint8_t n);