From 34fd303027ec54afb94ff47b1089e7694ba6c5db Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 24 May 2023 10:58:34 +0300 Subject: [PATCH] Restore accidentally removed files --- flix/pid.h | 53 ++++++++++++ flix/quaternion.h | 211 ++++++++++++++++++++++++++++++++++++++++++++++ flix/vector.h | 90 ++++++++++++++++++++ gazebo/Arduino.h | 1 - gazebo/flix.h | 48 +++++++++++ 5 files changed, 402 insertions(+), 1 deletion(-) create mode 100644 flix/pid.h create mode 100644 flix/quaternion.h create mode 100644 flix/vector.h create mode 100644 gazebo/flix.h 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 +// 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 +// 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 +// 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 // 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 +// Repository: https://github.com/okalachev/flix + +// Declarations of some functions and variables in Arduino code + +#include +#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"); };