Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Restore accidentally removed files

Oleg Kalachev34fd303027ec
diff --git a/flix/pid.h b/flix/pid.h
new file mode 100644
index 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.h
new file mode 100644
index 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.h
new file mode 100644
index 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.h
index 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 simulation
diff --git a/gazebo/flix.h b/gazebo/flix.h
new file mode 100644
index 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"); };