Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Change .hpp to .h

Oleg Kalachev87c75842f91f
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 <MPU9250.h>
-#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 <[email protected]>
// 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 <[email protected]>
// 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 <[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 && 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 <[email protected]>
-// 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 <[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/flix.cpp b/gazebo/flix.cpp
index 8616752..c80bd07 100644
--- a/gazebo/flix.cpp
+++ b/gazebo/flix.cpp
@@ -35,7 +35,7 @@
#include <fstream>
#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 <[email protected]>
-// Repository: https://github.com/okalachev/flix
-
-// Declarations of some functions and variables in Arduino code
-
-#include <cmath>
-#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() {};