Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Change .hpp to .h
Oleg Kalachev87c75842f91f
diff --git a/flix/cli.ino b/flix/cli.inoindex 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.inoindex 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.2diff --git a/flix/estimate.ino b/flix/estimate.inoindex 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.9fdiff --git a/flix/flix.ino b/flix/flix.inoindex 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 115200diff --git a/flix/pid.hpp b/flix/pid.hppdeleted file mode 100644index 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.hppdeleted file mode 100644index 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.hppdeleted file mode 100644index 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.cppindex 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.hppdeleted file mode 100644index 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() {};