Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Restore accidentally removed files
Oleg Kalachev34fd303027ec
diff --git a/flix/pid.h b/flix/pid.hnew file mode 100644index 0000000..1b2de4d--- /dev/null+++ b/flix/pid.h@@ -0,0 +1,53 @@+// Copyright (c) 2023 Oleg Kalachev <[email protected]>+// Repository: https://github.com/okalachev/flix++// PID controller implementation++#pragma once++class PID+{+public:+ float p = 0;+ float i = 0;+ float d = 0;+ float windup = 0;++ float derivative = 0;+ float integral = 0;++ PID(float p, float i, float d, float windup = 0) : p(p), i(i), d(d), windup(windup) {};++ float update(float error, float dt)+ {+ if (!isfinite(error) || !isfinite(dt)) {+ // TODO: brutal way to remove glitches+ Serial.println("nan in error or dt");+ return NAN;+ }++ if (dt > 0) {+ // calculate integral if dt is valid+ integral += error * dt;+ if (isfinite(prevError)) {+ // calculate derivative if both dt and prevError are valid+ float _derivative = (error - prevError) / dt;+ derivative = derivative * 0.8 + 0.2 * _derivative; // lpf WARNING:+ }+ }++ prevError = error;++ return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID+ }++ void reset()+ {+ prevError = NAN;+ integral = 0;+ derivative = 0;+ }++private:+ float prevError = NAN;+};diff --git a/flix/quaternion.h b/flix/quaternion.hnew file mode 100644index 0000000..2513bf2--- /dev/null+++ b/flix/quaternion.h@@ -0,0 +1,211 @@+// Lightweight rotation quaternion library+// Copyright (c) 2023 Oleg Kalachev <[email protected]>+// Repository: https://github.com/okalachev/flix++#pragma once++#include "vector.h"++class Quaternion : public Printable {+public:+ float w, x, y, z;++ Quaternion(): w(1), x(0), y(0), z(0) {};++ Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {};++ static Quaternion fromAxisAngle(float a, float b, float c, float angle)+ {+ float halfAngle = angle * 0.5;+ float sin2 = sin(halfAngle);+ float cos2 = cos(halfAngle);+ float sinNorm = sin2 / sqrt(a * a + b * b + c * c);+ return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm);+ }++ static Quaternion fromAngularRates(float x, float y, float z)+ {+ return Quaternion::fromAxisAngle(x, y, z, sqrt(x * x + y * y + z * z));+ }++ static Quaternion fromAngularRates(const Vector& rates)+ {+ if (rates.zero()) {+ return Quaternion();+ }+ return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());+ }++ static Quaternion fromEulerZYX(float x, float y, float z)+ {+ float cx = cos(x / 2);+ float cy = cos(y / 2);+ float cz = cos(z / 2);+ float sx = sin(x / 2);+ float sy = sin(y / 2);+ float sz = sin(z / 2);++ return Quaternion(+ cx * cy * cz + sx * sy * sz,+ sx * cy * cz - cx * sy * sz,+ cx * sy * cz + sx * cy * sz,+ cx * cy * sz - sx * sy * cz);+ }++ static Quaternion fromBetweenVectors(Vector u, Vector v)+ {+ float dot = u.x * v.x + u.y * v.y + u.z * v.z;+ float w1 = u.y * v.z - u.z * v.y;+ float w2 = u.z * v.x - u.x * v.z;+ float w3 = u.x * v.y - u.y * v.x;++ Quaternion ret(+ dot + sqrt(dot * dot + w1 * w1 + w2 * w2 + w3 * w3),+ w1,+ w2,+ w3);+ ret.normalize();+ return ret;+ }++ static Quaternion _fromBetweenVectors(float a, float b, float c, float x, float y, float z)+ {+ float dot = a * x + b * y + c * z;+ float w1 = b * z - c * y;+ float w2 = c * x - a * z;+ float w3 = a * y - b * x;++ Quaternion ret(+ dot + sqrt(dot * dot + w1 * w1 + w2 * w2 + w3 * w3),+ w1,+ w2,+ w3);+ ret.normalize();+ return ret;+ };++ void toAxisAngle(float& a, float& b, float& c, float& angle)+ {+ angle = acos(w) * 2;+ a = x / sin(angle / 2);+ b = y / sin(angle / 2);+ c = z / sin(angle / 2);+ }++ Vector toEulerZYX() const+ {+ return Vector(+ atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)),+ asin(2 * (w * y - z * x)),+ atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)));+ }++ float getYaw() const+ {+ // https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122+ float yaw;+ float sqx = x * x;+ float sqy = y * y;+ float sqz = z * z;+ float sqw = w * w;++ double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);++ if (sarg <= -0.99999) {+ yaw = -2 * atan2(y, x);+ } else if (sarg >= 0.99999) {+ yaw = 2 * atan2(y, x);+ } else {+ yaw = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz);+ }+ return yaw;+ }++ void setYaw(float yaw)+ {+ // TODO: optimize?+ Vector euler = toEulerZYX();+ (*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw);+ }++ void toAngularRates(float& x, float& y, float& z)+ {+ // this->toAxisAngle(); // TODO:+ }++ Quaternion & operator*=(const Quaternion &q)+ {+ Quaternion ret(+ w * q.w - x * q.x - y * q.y - z * q.z,+ w * q.x + x * q.w + y * q.z - z * q.y,+ w * q.y + y * q.w + z * q.x - x * q.z,+ w * q.z + z * q.w + x * q.y - y * q.x);+ return (*this = ret);+ }++ Quaternion operator*(const Quaternion& q)+ {+ return Quaternion(+ w * q.w - x * q.x - y * q.y - z * q.z,+ w * q.x + x * q.w + y * q.z - z * q.y,+ w * q.y + y * q.w + z * q.x - x * q.z,+ w * q.z + z * q.w + x * q.y - y * q.x);+ }++ Quaternion inversed() const+ {+ float normSqInv = 1 / (w * w + x * x + y * y + z * z);+ return Quaternion(+ w * normSqInv,+ -x * normSqInv,+ -y * normSqInv,+ -z * normSqInv);+ }++ float norm() const+ {+ return sqrt(w * w + x * x + y * y + z * z);+ }++ void normalize()+ {+ float n = norm();+ w /= n;+ x /= n;+ y /= n;+ z /= n;+ }++ Vector conjugate(const Vector& v)+ {+ Quaternion qv(0, v.x, v.y, v.z);+ Quaternion res = (*this) * qv * inversed();+ return Vector(res.x, res.y, res.z);+ }++ Vector conjugateInversed(const Vector& v)+ {+ Quaternion qv(0, v.x, v.y, v.z);+ Quaternion res = inversed() * qv * (*this);+ return Vector(res.x, res.y, res.z);+ }++ inline Vector rotate(const Vector& v)+ {+ return conjugateInversed(v);+ }++ inline bool finite() const+ {+ return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);+ }++ size_t printTo(Print& p) const {+ size_t r = 0;+ r += p.print(w, 15) + p.print(" ");+ r += p.print(x, 15) + p.print(" ");+ r += p.print(y, 15) + p.print(" ");+ r += p.print(z, 15);+ return r;+ }+};diff --git a/flix/vector.h b/flix/vector.hnew file mode 100644index 0000000..ead55ee--- /dev/null+++ b/flix/vector.h@@ -0,0 +1,90 @@+// Lightweight vector library+// Copyright (c) 2023 Oleg Kalachev <[email protected]>+// Repository: https://github.com/okalachev/flix++#pragma once++class Vector : public Printable+{+public:+ float x, y, z;++ Vector(): x(0), y(0), z(0) {};++ Vector(float x, float y, float z): x(x), y(y), z(z) {};++ float norm() const+ {+ return sqrt(x * x + y * y + z * z);+ }++ bool zero() const+ {+ return x == 0 && y == 0 && z == 0;+ }++ void normalize()+ {+ float n = norm();+ x /= n;+ y /= n;+ z /= n;+ }++ Vector operator * (float b)+ {+ return Vector(x * b, y * b, z * b);+ }++ Vector operator + (const Vector& b) const+ {+ return Vector(x + b.x, y + b.y, z + b.z);+ }++ Vector operator - (const Vector& b) const+ {+ return Vector(x - b.x, y - b.y, z - b.z);+ }++ inline bool operator == (const Vector& b) const+ {+ return x == b.x && y == b.y && z == b.z;+ }++ inline bool operator != (const Vector& b) const+ {+ return !(*this == b);+ }++ inline bool finite() const+ {+ return isfinite(x) && isfinite(y) && isfinite(z);+ }++ static float dot(const Vector& a, const Vector& b)+ {+ return a.x * b.x + a.y * b.y + a.z * b.z;+ }++ static Vector cross(const Vector& a, const Vector& b)+ {+ return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);+ }++ static float angleBetweenVectors(const Vector& a, const Vector& b)+ {+ return acos(dot(a, b) / (a.norm() * b.norm()));+ }++ static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v)+ {+ return cross(u, v);+ }++ size_t printTo(Print& p) const {+ return+ p.print(x, 15) + p.print(" ") ++ p.print(y, 15) + p.print(" ") ++ p.print(z, 15);+ }+};diff --git a/gazebo/Arduino.h b/gazebo/Arduino.hindex 7826ae6..30f576e 100644--- a/gazebo/Arduino.h+++ b/gazebo/Arduino.h@@ -1,6 +1,5 @@// Copyright (c) 2023 Oleg Kalachev <[email protected]>// Repository: https://github.com/okalachev/flix-// Distributed under the MIT License (https://opensource.org/licenses/MIT)// Partial implementation of Arduino API for simulationdiff --git a/gazebo/flix.h b/gazebo/flix.hnew file mode 100644index 0000000..f68370d--- /dev/null+++ b/gazebo/flix.h@@ -0,0 +1,48 @@+// Copyright (c) 2023 Oleg Kalachev <[email protected]>+// Repository: https://github.com/okalachev/flix++// Declarations of some functions and variables in Arduino code++#include <cmath>+#include "vector.h"+#include "quaternion.h"++#define RC_CHANNELS 6++#define MOTOR_REAR_LEFT 0+#define MOTOR_FRONT_LEFT 3+#define MOTOR_FRONT_RIGHT 2+#define MOTOR_REAR_RIGHT 1++uint32_t startTime;+uint32_t stepTime;+uint32_t steps;+float stepsPerSecond;+float dt;+float motors[4];+int16_t channels[16]; // raw rc channels WARNING: unsigned on hardware+float controls[RC_CHANNELS];+Vector acc;+Vector rates;+Quaternion attitude;++// control+void control();+void interpretRC();+static void controlAttitude();+static void controlAttitudeAlter();+static void controlManual();+static void controlRate();+void desaturate(float& a, float& b, float& c, float& d);+static void indicateSaturation();++// cli+static void showTable();+static void cliTestMotor(uint8_t n);++// mocks+void setLED(bool on) {};+void calibrateGyro() {};+void fullMotorTest(int n) {};+void sendMotors() {};+void printIMUCal() { Serial.print("N/A"); };