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

Cleanups

Browse Source
This commit is contained in:
Oleg Kalachev 2023-12-13 07:59:08 +03:00
parent 46579ce8a4
commit cb27e0f61f
2 changed files with 1 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
flix/control.ino
View File

@ -116,41 +116,15 @@ void controlAttitude()
Vector upTarget = attitudeTarget.rotate(up); Vector upTarget = attitudeTarget.rotate(up);
float angle = Vector::angleBetweenVectors(upTarget, upActual); float angle = Vector::angleBetweenVectors(upTarget, upActual);
if (!isfinite(angle)) {
// not enough precision to calculate
Serial.println("angle is nan, skip");
return;
}
Vector ratesTargetDir = Vector::angularRatesBetweenVectors(upTarget, upActual); Vector ratesTargetDir = Vector::angularRatesBetweenVectors(upTarget, upActual);
ratesTargetDir.normalize(); ratesTargetDir.normalize();
if (!ratesTargetDir.finite()) {
Serial.println("ratesTargetDir is nan, skip");
// std::cout << "angle is nan" << std::endl;
ratesTarget = Vector(0, 0, 0);
return;
}
ratesTarget.x = rollPID.update(ratesTargetDir.x * angle, dt); ratesTarget.x = rollPID.update(ratesTargetDir.x * angle, dt);
ratesTarget.y = pitchPID.update(ratesTargetDir.y * angle, dt); ratesTarget.y = pitchPID.update(ratesTargetDir.y * angle, dt);
if (yawMode == YAW) { if (yawMode == YAW) {
ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt); ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt);
} }
if (!ratesTarget.finite()) {
Serial.print("ratesTarget: "); Serial.println(ratesTarget);
Serial.print("ratesTargetDir: "); Serial.println(ratesTargetDir);
Serial.print("attitudeTarget: "); Serial.println(attitudeTarget);
Serial.print("attitude: "); Serial.println(attitude);
Serial.print("upActual: "); Serial.println(upActual);
Serial.print("upTarget: "); Serial.println(upTarget);
Serial.print("angle: "); Serial.println(angle);
Serial.print("dt: "); Serial.println(dt);
}
// std::cout << "rsp: " << ratesTarget.x << " " << ratesTarget.y << std::endl;
} }
// passthrough mode // passthrough mode
@ -168,10 +142,6 @@ void controlManual()
motors[MOTOR_REAR_LEFT] = 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[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;
if (!isfinite(motors[0]) || !isfinite(motors[1]) || !isfinite(motors[2]) || !isfinite(motors[3])) {
Serial.println("motors are nan");
}
motors[0] = constrain(motors[0], 0, 1); motors[0] = constrain(motors[0], 0, 1);
motors[1] = constrain(motors[1], 0, 1); motors[1] = constrain(motors[1], 0, 1);
motors[2] = constrain(motors[2], 0, 1); motors[2] = constrain(motors[2], 0, 1);
@ -180,7 +150,7 @@ void controlManual()
void controlRate() void controlRate()
{ {
if (!armed) { // TODO: too rough if (!armed) {
memset(motors, 0, sizeof(motors)); memset(motors, 0, sizeof(motors));
rollRatePID.reset(); rollRatePID.reset();
pitchRatePID.reset(); pitchRatePID.reset();
@ -194,19 +164,11 @@ void controlRate()
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);
if (!torqueTarget.finite()) {
Serial.print("torqueTarget: "); Serial.println(torqueTarget);
Serial.print("ratesTarget: "); Serial.println(ratesTarget);
Serial.print("rates: "); Serial.println(rates);
Serial.print("dt: "); Serial.println(dt);
}
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;
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z; motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;
// constrain and reverse, multiple by -1 if reversed
motors[0] = constrain(motors[0], 0, 1); motors[0] = constrain(motors[0], 0, 1);
motors[1] = constrain(motors[1], 0, 1); motors[1] = constrain(motors[1], 0, 1);
motors[2] = constrain(motors[2], 0, 1); motors[2] = constrain(motors[2], 0, 1);

2
gazebo/Arduino.h
View File

@ -3,8 +3,6 @@
// Partial implementation of Arduino API for simulation // Partial implementation of Arduino API for simulation
// https://github.com/UFABCRocketDesign/Simulator
#pragma once #pragma once
#include <cmath> #include <cmath>