Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Cleanups
Oleg Kalachevcb27e0f61f9e
diff --git a/flix/control.ino b/flix/control.inoindex 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 reversedmotors[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.hindex 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 <cmath>