mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 17:49:33 +00:00
Change .hpp to .h
This commit is contained in:
parent
f58bcbf75c
commit
87c75842f9
@ -2,7 +2,7 @@
|
|||||||
// Repository: https://github.com/okalachev/flix
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
#include <MPU9250.h>
|
#include <MPU9250.h>
|
||||||
#include "pid.hpp"
|
#include "pid.h"
|
||||||
|
|
||||||
static String command;
|
static String command;
|
||||||
static String value;
|
static String value;
|
||||||
|
@ -3,9 +3,9 @@
|
|||||||
|
|
||||||
#pragma diag_suppress 144, 513
|
#pragma diag_suppress 144, 513
|
||||||
|
|
||||||
#include "pid.hpp"
|
#include "pid.h"
|
||||||
#include "vector.hpp"
|
#include "vector.h"
|
||||||
#include "quaternion.hpp"
|
#include "quaternion.h"
|
||||||
|
|
||||||
#define PITCHRATE_P 0.05
|
#define PITCHRATE_P 0.05
|
||||||
#define PITCHRATE_I 0.2
|
#define PITCHRATE_I 0.2
|
||||||
|
@ -1,8 +1,8 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||||
// Repository: https://github.com/okalachev/flix
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
#include "quaternion.hpp"
|
#include "quaternion.h"
|
||||||
#include "vector.hpp"
|
#include "vector.h"
|
||||||
|
|
||||||
#define ONE_G 9.807f
|
#define ONE_G 9.807f
|
||||||
#define ACC_MIN 0.9f
|
#define ACC_MIN 0.9f
|
||||||
|
@ -1,8 +1,8 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||||
// Repository: https://github.com/okalachev/flix
|
// Repository: https://github.com/okalachev/flix
|
||||||
|
|
||||||
#include "vector.hpp"
|
#include "vector.h"
|
||||||
#include "quaternion.hpp"
|
#include "quaternion.h"
|
||||||
|
|
||||||
#define SERIAL_BAUDRATE 115200
|
#define SERIAL_BAUDRATE 115200
|
||||||
|
|
||||||
|
49
flix/pid.hpp
49
flix/pid.hpp
@ -1,49 +0,0 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
||||||
// 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;
|
|
||||||
};
|
|
@ -1,211 +0,0 @@
|
|||||||
// Lightweight rotation quaternion library
|
|
||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
};
|
|
@ -1,90 +0,0 @@
|
|||||||
// Lightweight vector library
|
|
||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
||||||
// 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);
|
|
||||||
}
|
|
||||||
};
|
|
@ -35,7 +35,7 @@
|
|||||||
#include <fstream>
|
#include <fstream>
|
||||||
|
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
#include "flix.hpp"
|
#include "flix.h"
|
||||||
#include "util.ino"
|
#include "util.ino"
|
||||||
#include "joystick.h"
|
#include "joystick.h"
|
||||||
#include "time.ino"
|
#include "time.ino"
|
||||||
|
@ -1,48 +0,0 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
||||||
// 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() {};
|
|
Loading…
x
Reference in New Issue
Block a user