diff --git a/flix/control.ino b/flix/control.ino index 3e6fce2..387a12b 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -154,6 +154,27 @@ void controlRate() motors[3] = constrain(motors[3], 0, 1); } +// passthrough mode +void controlManual() +{ + if (controls[RC_CHANNEL_THROTTLE] < 0.1) { + 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; + 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); +} + bool motorsActive() { return motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;