diff --git a/flix/control.ino b/flix/control.ino index 6de302c..969281e 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -116,41 +116,15 @@ void controlAttitude() Vector upTarget = attitudeTarget.rotate(up); 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); 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.y = pitchPID.update(ratesTargetDir.y * angle, dt); if (yawMode == YAW) { 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 @@ -168,10 +142,6 @@ void controlManual() motors[MOTOR_REAR_LEFT] = 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[1] = constrain(motors[1], 0, 1); motors[2] = constrain(motors[2], 0, 1); @@ -180,7 +150,7 @@ void controlManual() void controlRate() { - if (!armed) { // TODO: too rough + if (!armed) { memset(motors, 0, sizeof(motors)); rollRatePID.reset(); pitchRatePID.reset(); @@ -194,19 +164,11 @@ void controlRate() torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, 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_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; - // constrain and reverse, multiple by -1 if reversed motors[0] = constrain(motors[0], 0, 1); motors[1] = constrain(motors[1], 0, 1); motors[2] = constrain(motors[2], 0, 1); diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index 3ef4409..bf85eb0 100644 --- a/gazebo/Arduino.h +++ b/gazebo/Arduino.h @@ -3,8 +3,6 @@ // Partial implementation of Arduino API for simulation -// https://github.com/UFABCRocketDesign/Simulator - #pragma once #include