From 87c75842f91fb78fbf85afddfa52ccad8f44cafe Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 24 May 2023 10:40:12 +0300 Subject: [PATCH] Change .hpp to .h --- flix/cli.ino | 2 +- flix/control.ino | 6 +- flix/estimate.ino | 4 +- flix/flix.ino | 4 +- flix/pid.hpp | 49 ---------- flix/quaternion.hpp | 211 -------------------------------------------- flix/vector.hpp | 90 ------------------- gazebo/flix.cpp | 2 +- gazebo/flix.hpp | 48 ---------- 9 files changed, 9 insertions(+), 407 deletions(-) delete mode 100644 flix/pid.hpp delete mode 100644 flix/quaternion.hpp delete mode 100644 flix/vector.hpp delete mode 100644 gazebo/flix.hpp diff --git a/flix/cli.ino b/flix/cli.ino index 543ff95..4c306b7 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -2,7 +2,7 @@ // Repository: https://github.com/okalachev/flix #include -#include "pid.hpp" +#include "pid.h" static String command; static String value; diff --git a/flix/control.ino b/flix/control.ino index 325f442..cf4626d 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -3,9 +3,9 @@ #pragma diag_suppress 144, 513 -#include "pid.hpp" -#include "vector.hpp" -#include "quaternion.hpp" +#include "pid.h" +#include "vector.h" +#include "quaternion.h" #define PITCHRATE_P 0.05 #define PITCHRATE_I 0.2 diff --git a/flix/estimate.ino b/flix/estimate.ino index 7ac64b2..d86e81b 100644 --- a/flix/estimate.ino +++ b/flix/estimate.ino @@ -1,8 +1,8 @@ // Copyright (c) 2023 Oleg Kalachev // Repository: https://github.com/okalachev/flix -#include "quaternion.hpp" -#include "vector.hpp" +#include "quaternion.h" +#include "vector.h" #define ONE_G 9.807f #define ACC_MIN 0.9f diff --git a/flix/flix.ino b/flix/flix.ino index cbf9671..94a9e6d 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -1,8 +1,8 @@ // Copyright (c) 2023 Oleg Kalachev // Repository: https://github.com/okalachev/flix -#include "vector.hpp" -#include "quaternion.hpp" +#include "vector.h" +#include "quaternion.h" #define SERIAL_BAUDRATE 115200 diff --git a/flix/pid.hpp b/flix/pid.hpp deleted file mode 100644 index 5a16bc7..0000000 --- a/flix/pid.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// 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 && isfinite(prevError)) { - integral += error * dt; - 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.hpp b/flix/quaternion.hpp deleted file mode 100644 index 4b79b05..0000000 --- a/flix/quaternion.hpp +++ /dev/null @@ -1,211 +0,0 @@ -// Lightweight rotation quaternion library -// Copyright (c) 2023 Oleg Kalachev -// Repository: https://github.com/okalachev/flix - -#pragma once - -#include "vector.hpp" - -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.hpp b/flix/vector.hpp deleted file mode 100644 index ead55ee..0000000 --- a/flix/vector.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// 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/flix.cpp b/gazebo/flix.cpp index 8616752..c80bd07 100644 --- a/gazebo/flix.cpp +++ b/gazebo/flix.cpp @@ -35,7 +35,7 @@ #include #include "Arduino.h" -#include "flix.hpp" +#include "flix.h" #include "util.ino" #include "joystick.h" #include "time.ino" diff --git a/gazebo/flix.hpp b/gazebo/flix.hpp deleted file mode 100644 index 120a9f2..0000000 --- a/gazebo/flix.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright (c) 2023 Oleg Kalachev -// Repository: https://github.com/okalachev/flix - -// Declarations of some functions and variables in Arduino code - -#include -#include "vector.hpp" -#include "quaternion.hpp" - -#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; -bool calibrating; - -// 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() {};