aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-08-17 17:16:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Oleg Kalachev
2023-03-26 10:23:30 +03:00
commit e039055c8e
46 changed files with 3049 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

170
flix/cli.ino Normal file
View File

@@ -0,0 +1,170 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#include <MPU9250.h>
#include "pid.hpp"
static String command;
static String value;
static bool parsingCommand = true;
extern PID rollRatePID, pitchRatePID, yawRatePID, rollPID, pitchPID;
extern MPU9250 IMU;
const char* motd =
"\nWelcome to\n"
" _______ __ __ ___ ___\n"
"| ____|| | | | \\ \\ / /\n"
"| |__ | | | | \\ V /\n"
"| __| | | | | > <\n"
"| | | `----.| | / . \\\n"
"|__| |_______||__| /__/ \\__\\\n\n"
"Commands:\n\n"
"show - show all parameters\n"
"<name> <value> - set parameter\n"
"ps - show pitch/roll/yaw\n"
"psq - show attitude quaternion\n"
"imu - show IMU data\n"
"rc - show RC data\n"
"mot - show motor data\n"
"log - dump in-RAM log\n"
"cg - calibrate gyro\n"
"fullmot <n> - test motor on all signals\n"
"wifi - start wi-fi access point\n\n";
bool showMotd = true;
static const struct Param {
const char* name;
float* value;
float* value2;
} params[] = {
{"rp", &rollRatePID.p, &pitchRatePID.p},
{"ri", &rollRatePID.i, &pitchRatePID.i},
{"rd", &rollRatePID.d, &pitchRatePID.d},
{"ap", &rollPID.p, &pitchPID.p},
{"ai", &rollPID.i, &pitchPID.i},
{"ad", &rollPID.d, &pitchPID.d},
{"yp", &yawRatePID.p, nullptr},
{"yi", &yawRatePID.i, nullptr},
{"yd", &yawRatePID.d, nullptr},
{"ss", &stepsPerSecond, nullptr},
// {"m", &mode, nullptr},
};
static void doCommand()
{
if (command == "show") {
showTable();
} else if (command == "ps") {
Vector a = attitude.toEulerZYX();
Serial.println("roll: " + String(a.x * RAD_TO_DEG, 2) +
" pitch: " + String(a.y * RAD_TO_DEG, 2) +
" yaw: " + String(a.z * RAD_TO_DEG, 2));
} else if (command == "psq") {
Serial.println("qx: " + String(attitude.x) +
" qy: " + String(attitude.y) +
" qz: " + String(attitude.z) +
" qw: " + String(attitude.w));
} else if (command == "imu") {
Serial.println("gyro bias " + String(IMU.getGyroBiasX_rads()) + " "
+ String(IMU.getGyroBiasY_rads()) + " "
+ String(IMU.getGyroBiasZ_rads()));
} else if (command == "rc") {
Serial.println("RAW throttle " + String(channels[RC_CHANNEL_THROTTLE]) +
" yaw " + String(channels[RC_CHANNEL_YAW]) +
" pitch " + String(channels[RC_CHANNEL_PITCH]) +
" roll " + String(channels[RC_CHANNEL_ROLL]) +
" aux " + String(channels[RC_CHANNEL_AUX]) +
" mode " + String(channels[RC_CHANNEL_MODE]));
Serial.println("CONTROL throttle " + String(controls[RC_CHANNEL_THROTTLE]) +
" yaw " + String(controls[RC_CHANNEL_YAW]) +
" pitch " + String(controls[RC_CHANNEL_PITCH]) +
" roll " + String(controls[RC_CHANNEL_ROLL]) +
" aux " + String(controls[RC_CHANNEL_AUX]) +
" mode " + String(controls[RC_CHANNEL_MODE]));
} else if (command == "mot") {
Serial.println("MOTOR front-right " + String(motors[MOTOR_FRONT_RIGHT]) +
" front-left " + String(motors[MOTOR_FRONT_LEFT]) +
" rear-right " + String(motors[MOTOR_REAR_RIGHT]) +
" rear-left " + String(motors[MOTOR_REAR_LEFT]));
} else if (command == "log") {
dumpLog();
} else if (command == "cg") {
calibrateGyro();
} else if (command == "mfr") {
cliTestMotor(MOTOR_FRONT_RIGHT);
} else if (command == "mfl") {
cliTestMotor(MOTOR_FRONT_LEFT);
} else if (command == "mrr") {
cliTestMotor(MOTOR_REAR_RIGHT);
} else if (command == "mrl") {
cliTestMotor(MOTOR_REAR_LEFT);
} else if (command == "fullmot") {
fullMotorTest(value.toInt());
} else {
float val = value.toFloat();
if (!isfinite(val)) {
Serial.println("Invalid value: " + value);
}
for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
if (command == params[i].name) {
*params[i].value = val;
if (params[i].value2 != nullptr) *params[i].value2 = val;
Serial.print(command);
Serial.print(" = ");
Serial.println(val, 4);
return;
}
}
Serial.println("Invalid command: '" + command + "'");
}
}
static void showTable()
{
for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) {
Serial.print(params[i].name);
Serial.print(" ");
Serial.println(*params[i].value, 5);
}
}
static void cliTestMotor(uint8_t n)
{
Serial.println("Testing motor " + String(n));
motors[n] = 1;
sendMotors();
delay(5000);
motors[n] = 0;
sendMotors();
Serial.println("Done");
}
void parseInput()
{
if (showMotd) {
Serial.println(motd);
showMotd = false;
}
while (Serial.available()) {
char c = Serial.read();
if (c == '\n') {
parsingCommand = true;
if (!command.isEmpty()) {
doCommand();
}
command.clear();
value.clear();
} else if (c == ' ') {
parsingCommand = false;
} else {
(parsingCommand ? command : value) += c;
}
}
}

267
flix/control.ino Normal file
View File

@@ -0,0 +1,267 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#pragma diag_suppress 144, 513
#include "pid.hpp"
#include "vector.hpp"
#include "quaternion.hpp"
#define PITCHRATE_P 0.05
#define PITCHRATE_I 0.2
#define PITCHRATE_D 0.001
#define PITCHRATE_I_LIM 0.3
#define ROLLRATE_P PITCHRATE_P
#define ROLLRATE_I PITCHRATE_I
#define ROLLRATE_D PITCHRATE_D
#define ROLLRATE_I_LIM PITCHRATE_I_LIM
#define YAWRATE_P 0.3
#define YAWRATE_I 0.0
#define YAWRATE_D 0.0
#define YAWRATE_I_LIM 0.3
#define ROLL_P 4.5
#define ROLL_I 0
#define ROLL_D 0
#define PITCH_P ROLL_P // 8
#define PITCH_I ROLL_I
#define PITCH_D ROLL_D
#define YAW_P 3
#define PITCHRATE_MAX 360 * DEG_TO_RAD
#define ROLLRATE_MAX 360 * DEG_TO_RAD
#define YAWRATE_MAX 360 * DEG_TO_RAD
#define MAX_TILT 30 * DEG_TO_RAD
enum { MANUAL, ACRO, STAB } mode = STAB;
bool armed = false;
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM);
PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM);
PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
PID rollPID(ROLL_P, ROLL_I, ROLL_D);
PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
PID yawPID(YAW_P, 0, 0);
Vector ratesFiltered;
Quaternion attitudeTarget;
Vector ratesTarget;
Vector torqueTarget;
float thrustTarget;
// TODO: ugly
float yawTarget = NAN;
bool controlYaw = false;
void control()
{
interpretRC();
if (mode == STAB) {
controlAttitude();
// controlAttitudeAlter();
}
if (mode == MANUAL) {
controlManual();
} else {
controlRate();
}
}
void interpretRC()
{
if (controls[RC_CHANNEL_MODE] < 0.25) {
mode = MANUAL;
} else if (controls[RC_CHANNEL_MODE] < 0.75) {
mode = ACRO;
} else {
mode = STAB;
}
armed = controls[RC_CHANNEL_THROTTLE] >= 0.1 && controls[RC_CHANNEL_AUX] >= 0.5;
controlYaw = armed && mode == STAB && controls[RC_CHANNEL_YAW] == 0;
if (!controlYaw) {
yawTarget = attitude.getYaw();
}
if (mode == ACRO) {
ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX;
ratesTarget.y = -controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX; // up pitch stick means tilt clockwise in frd
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
} else if (mode == STAB) {
attitudeTarget = Quaternion::fromEulerZYX(
controls[RC_CHANNEL_ROLL] * MAX_TILT,
-controls[RC_CHANNEL_PITCH] * MAX_TILT,
yawTarget); // attitude.getYaw());
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
}
}
static void controlAttitude()
{
if (!armed) {
rollPID.reset();
pitchPID.reset();
yawPID.reset();
return;
}
const Vector up(0, 0, -1);
Vector upActual = attitude.rotate(up);
Vector upTarget = attitudeTarget.rotate(up);
float angle = Vector::angleBetweenVectors(upTarget, upActual);
if (!isfinite(angle)) {
// not enough precision to calculate
Serial.println("angle is nan, skip");
return;
}
Vector ratesTargetDir = Vector::angularRatesBetweenVectors(upTarget, upActual);
ratesTargetDir.normalize();
if (!ratesTargetDir.finite()) {
Serial.println("ratesTargetDir is nan, skip");
// std::cout << "angle is nan" << std::endl;
ratesTarget = Vector(0, 0, 0);
return;
}
ratesTarget.x = rollPID.update(ratesTargetDir.x * angle, dt);
ratesTarget.y = pitchPID.update(ratesTargetDir.y * angle, dt);
if (controlYaw) {
ratesTarget.z = yawPID.update(yawTarget - attitude.getYaw(), dt); // WARNING:
}
if (!ratesTarget.finite()) {
Serial.print("ratesTarget: "); Serial.println(ratesTarget);
Serial.print("ratesTargetDir: "); Serial.println(ratesTargetDir);
Serial.print("attitudeTarget: "); Serial.println(attitudeTarget);
Serial.print("attitude: "); Serial.println(attitude);
Serial.print("upActual: "); Serial.println(upActual);
Serial.print("upTarget: "); Serial.println(upTarget);
Serial.print("angle: "); Serial.println(angle);
Serial.print("dt: "); Serial.println(dt);
}
// std::cout << "rsp: " << ratesTarget.x << " " << ratesTarget.y << std::endl;
}
static void controlAttitudeAlter()
{
if (!armed) {
rollPID.reset();
pitchPID.reset();
yawPID.reset();
return;
}
Vector target = attitudeTarget.toEulerZYX();
Vector att = attitude.toEulerZYX();
ratesTarget.x = rollPID.update(target.x - att.x, dt);
ratesTarget.y = pitchPID.update(target.y - att.y, dt);
if (controlYaw) {
ratesTarget.z = yawPID.update(target.z - att.z, dt); // WARNING:
}
}
// passthrough mode
static void controlManual()
{
if (controls[RC_CHANNEL_THROTTLE] < 0.1) {
memset(motors, 0, sizeof(motors));
return;
}
thrustTarget = controls[RC_CHANNEL_THROTTLE]; // collective thrust
torqueTarget = ratesTarget * 0.01;
motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;
motors[MOTOR_REAR_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;
if (!isfinite(motors[0]) || !isfinite(motors[1]) || !isfinite(motors[2]) || !isfinite(motors[3])) {
Serial.println("motors are nan");
}
motors[0] = constrain(motors[0], 0, 1);
motors[1] = constrain(motors[1], 0, 1);
motors[2] = constrain(motors[2], 0, 1);
motors[3] = constrain(motors[3], 0, 1);
}
static void controlRate()
{
if (!armed) { // TODO: too rough
memset(motors, 0, sizeof(motors));
rollRatePID.reset();
pitchRatePID.reset();
yawRatePID.reset();
return;
}
// collective thrust is throttle * 4
thrustTarget = controls[RC_CHANNEL_THROTTLE]; // WARNING:
ratesFiltered = rates * 0.8 + ratesFiltered * 0.2; // cut-off frequency 40 Hz
torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt);
torqueTarget.z = yawRatePID.update(ratesTarget.z - ratesFiltered.z, dt);
if (!torqueTarget.finite()) {
Serial.print("torqueTarget: "); Serial.println(torqueTarget);
Serial.print("ratesTarget: "); Serial.println(ratesTarget);
Serial.print("rates: "); Serial.println(rates);
Serial.print("dt: "); Serial.println(dt);
}
motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;
motors[MOTOR_REAR_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;
//indicateSaturation();
// desaturate(motors[0], motors[1], motors[2], motors[3]);
// constrain and reverse, multiple by -1 if reversed
motors[0] = constrain(motors[0], 0, 1);
motors[1] = constrain(motors[1], 0, 1);
motors[2] = constrain(motors[2], 0, 1);
motors[3] = constrain(motors[3], 0, 1);
}
void desaturate(float& a, float& b, float& c, float& d)
{
float m = max(max(a, b), max(c, d));
if (m > 1) {
float diff = m - 1;
a -= diff;
b -= diff;
c -= diff;
d -= diff;
}
m = min(min(a, b), min(c, d));
if (m < 0) {
a -= m;
b -= m;
c -= m;
d -= m;
}
}
static bool motorsSaturation = false;
static inline void indicateSaturation() {
bool sat = motors[0] > 1 || motors[1] > 1 || motors[2] > 1 || motors[3] > 1 ||
motors[0] < 0 || motors[1] < 0 || motors[2] < 0 || motors[3] < 0;
if (motorsSaturation != sat) {
motorsSaturation = sat;
setLED(motorsSaturation);
}
}

64
flix/estimate.ino Normal file
View File

@@ -0,0 +1,64 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#include "quaternion.hpp"
#include "vector.hpp"
#define ONE_G 9.807f
#define ACC_MIN 0.9f
#define ACC_MAX 1.1f
#define WEIGHT_ACC 0.5f
void estimate()
{
if (dt == 0) {
return;
}
// applying gyro
attitude *= Quaternion::fromAngularRates(rates * dt);
attitude.normalize();
// test should we apply acc
float accNorm = acc.norm();
if (accNorm < ACC_MIN * ONE_G || accNorm > ACC_MAX * ONE_G) {
// use acc only when we're not accelerating
return;
}
Vector up = attitude.rotate(Vector(0, 0, -1));
Vector accCorrDirection = Vector::angularRatesBetweenVectors(acc, up);
accCorrDirection.normalize();
if (!accCorrDirection.finite()) {
return;
}
Vector accCorr = accCorrDirection * Vector::angleBetweenVectors(up, acc) * dt * WEIGHT_ACC;
if (!accCorr.finite()) {
return; // TODO
}
attitude *= Quaternion::fromAngularRates(accCorr);
attitude.normalize();
if (!attitude.finite()) {
Serial.print("dt "); Serial.println(dt, 15);
Serial.print("up "); Serial.println(up);
Serial.print("acc "); Serial.println(acc);
Serial.print("acc norm "); Serial.println(acc.norm());
Serial.print("upp norm "); Serial.println(up.norm());
Serial.print("acc dot up "); Serial.println(Vector::dot(up, acc), 15);
Serial.print("acc cor ang "); Serial.println(Vector::angleBetweenVectors(up, acc), 15);
Serial.print("acc corr dir "); Serial.println(accCorrDirection);
Serial.print("acc cor "); Serial.println(accCorr);
Serial.print("att "); Serial.println(attitude);
}
}
void signalizeHorizontality()
{
float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));
setLED(angle < 15 * DEG_TO_RAD);
}

74
flix/flix.ino Normal file
View File

@@ -0,0 +1,74 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#include "vector.hpp"
#include "quaternion.hpp"
#define SERIAL_BAUDRATE 115200
#define WIFI_ENABLED 0
#define RC_CHANNELS 6
#define RC_CHANNEL_THROTTLE 2
#define RC_CHANNEL_YAW 3
#define RC_CHANNEL_PITCH 1
#define RC_CHANNEL_ROLL 0
#define RC_CHANNEL_AUX 4
#define RC_CHANNEL_MODE 5
#define MOTOR_REAR_LEFT 0
#define MOTOR_FRONT_LEFT 3
#define MOTOR_FRONT_RIGHT 2
#define MOTOR_REAR_RIGHT 1
uint32_t startTime; // system startup time
uint32_t stepTime; // current step time
uint32_t steps; // total steps count
float stepsPerSecond; // steps per last second
float dt; // time delta from previous step
uint16_t channels[16]; // raw rc channels
float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
uint32_t rcFailSafe, rcLostFrame;
float motors[4]; // normalized motors thrust in range [-1..1]
Vector rates; // angular rates, rad/s
Vector acc; // accelerometer data, m/s/s
Quaternion attitude; // estimated attitude
bool calibrating; // flag we're calibrating
void setupDebug();
void lowPowerMode();
void setup()
{
Serial.begin(SERIAL_BAUDRATE);
Serial.println("Initializing flix");
setupTime();
setupLED();
setupMotors();
setLED(true);
#if WIFI_ENABLED == 1
setupWiFi();
#endif
setupIMU();
setupRC();
setLED(false);
Serial.println("Initializing complete");
}
void loop()
{
if (!readIMU()) return;
step();
readRC();
estimate();
control();
sendMotors();
parseInput();
#if WIFI_ENABLED == 1
sendMavlink();
#endif
logData();
signalizeHorizontality();
}

112
flix/imu.ino Normal file
View File

@@ -0,0 +1,112 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#include <MPU9250.h>
#define IMU_CS_PIN 4 // chip-select pin for IMU SPI connection
MPU9250 IMU(SPI, IMU_CS_PIN);
void setupIMU()
{
Serial.println("Setup IMU");
auto status = IMU.begin();
if (status < 0) {
while (true) {
Serial.print("IMU begin error: "); Serial.println(status);
delay(1000);
}
}
calibrating = true;
calibrateGyro();
// loadGyroCal();
// calibrateAccel();
loadAccelCal();
IMU.setSrd(0); // set sample rate to 1000 Hz
// NOTE: very important, without the above the rate would be terrible 50 Hz
calibrating = false;
}
bool readIMU()
{
if (IMU.readSensor() < 0) {
Serial.println("IMU read error"); // TODO:
return false;
}
auto lastRates = rates;
rates.x = IMU.getGyroX_rads();
rates.y = IMU.getGyroY_rads();
rates.z = IMU.getGyroZ_rads();
acc.x = IMU.getAccelX_mss();
acc.y = IMU.getAccelY_mss();
acc.z = IMU.getAccelZ_mss();
return rates != lastRates;
}
static void calibrateGyro()
{
Serial.println("Calibrating gyro, stand still");
delay(500);
int status = IMU.calibrateGyro();
Serial.println("Calibration status: " + String(status));
Serial.print("Gyro bias: ");
Serial.print(IMU.getGyroBiasX_rads(), 10); Serial.print(" ");
Serial.print(IMU.getGyroBiasY_rads(), 10); Serial.print(" ");
Serial.println(IMU.getGyroBiasZ_rads(), 10);
IMU.setSrd(0);
}
static void calibrateAccel()
{
Serial.println("Cal accel: place level"); delay(3000);
IMU.calibrateAccel();
Serial.println("Cal accel: place nose up"); delay(3000);
IMU.calibrateAccel();
Serial.println("Cal accel: place nose down"); delay(3000);
IMU.calibrateAccel();
Serial.println("Cal accel: place on right side"); delay(3000);
IMU.calibrateAccel();
Serial.println("Cal accel: place on left side"); delay(3000);
IMU.calibrateAccel();
Serial.println("Cal accel: upside down"); delay(300);
IMU.calibrateAccel();
Serial.print("Accel bias: ");
Serial.print(IMU.getAccelBiasX_mss(), 10); Serial.print(" ");
Serial.print(IMU.getAccelBiasY_mss(), 10); Serial.print(" ");
Serial.println(IMU.getAccelBiasZ_mss(), 10);
Serial.print("Accel scale: ");
Serial.print(IMU.getAccelScaleFactorX(), 10); Serial.print(" ");
Serial.print(IMU.getAccelScaleFactorY(), 10); Serial.print(" ");
Serial.println(IMU.getAccelScaleFactorZ(), 10);
}
static void loadAccelCal()
{
IMU.setAccelCalX(-0.0048542023, 1.0008112192);
IMU.setAccelCalY(0.0521845818, 0.9985780716);
IMU.setAccelCalZ(0.5754694939, 1.0045746565);
}
static void loadGyroCal()
{
IMU.setGyroBiasX_rads(-0.0175771303);
IMU.setGyroBiasY_rads(-0.0298212003);
IMU.setGyroBiasZ_rads(0.0148300380);
}
// Accel bias: 0.0463809967 0.0463809967 0.1486964226
// Accel scale: 0.9986976385 0.9993721247 1.0561490059
// Accel bias: 0.0145006180 0.0145006180 0.0000000000
// Accel scale: 0.9989741445 0.9993283749 1.0000000000
// Correct:
// Accel bias: -0.0048542023 0.0521845818 0.5754694939
// Accel scale: 1.0008112192 0.9985780716 1.0045746565

43
flix/led.ino Normal file
View File

@@ -0,0 +1,43 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#define LED_PIN 2
#define BLINK_FAST_PERIOD 300000
#define BLINK_SLOW_PERIOD 1000000
static bool state;
static enum {
OFF,
ON,
BLINK_FAST,
BLINK_SLOW
} LEDscheme = OFF;
void setupLED()
{
pinMode(LED_BUILTIN, OUTPUT);
}
void setLED(bool on)
{
digitalWrite(LED_BUILTIN, on ? HIGH : LOW);
}
void proceedLED()
{
// TODO: this won't work
// TODO:: just check is current second even or odd
if (LEDscheme == BLINK_FAST && stepTime % BLINK_FAST_PERIOD == 0) {
state != state;
digitalWrite(LED_BUILTIN, state ? HIGH : LOW);
} else if (LEDscheme == BLINK_SLOW && stepTime % BLINK_SLOW_PERIOD == 0) {
state != state;
digitalWrite(LED_BUILTIN, state ? HIGH : LOW);
}
}
void blinkLED()
{
setLED(micros() / 500000 % 2);
}

48
flix/log.ino Normal file
View File

@@ -0,0 +1,48 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#define LOG_RATE 200
#define LOG_DURATION 10
#define LOG_PERIOD 1000000 / LOG_RATE
#define LOG_SIZE LOG_DURATION * LOG_RATE
#define LOG_COLUMNS 11
static float logBuffer[LOG_SIZE][LOG_COLUMNS]; // * 4 (float)
static int logPointer = 0;
static uint32_t lastLog = 0;
void logData()
{
if (!armed) return;
if (stepTime - lastLog < LOG_PERIOD) return;
lastLog = stepTime;
logBuffer[logPointer][0] = stepTime;
logBuffer[logPointer][1] = rates.x;
logBuffer[logPointer][2] = rates.y;
logBuffer[logPointer][3] = rates.z;
logBuffer[logPointer][4] = ratesTarget.x;
logBuffer[logPointer][5] = ratesTarget.y;
logBuffer[logPointer][6] = ratesTarget.z;
logBuffer[logPointer][7] = torqueTarget.x;
logBuffer[logPointer][8] = torqueTarget.y;
logBuffer[logPointer][9] = torqueTarget.z;
logBuffer[logPointer][10] = thrustTarget;
logPointer++;
if (logPointer >= 2000) {
logPointer = 0;
}
}
void dumpLog()
{
printf("timestamp,rate.x,rate.y,rate.z,target.rate.x,target.rate.y,target.rate.z,torque.x,torque.y,torque.z,thrust\n");
for (int i = 0; i < LOG_SIZE; i++) {
for (int j = 0; j < LOG_COLUMNS - 1; j++) {
printf("%f,", logBuffer[i][j]);
}
printf("%f\n", logBuffer[i][LOG_COLUMNS - 1]);
}
Serial.println();
}

70
flix/mavlink.ino Normal file
View File

@@ -0,0 +1,70 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#if WIFI_ENABLED == 1
#include "mavlink/common/mavlink.h"
#define SYSTEM_ID 1
#define PERIOD_SLOW 1000000 // us
#define PERIOD_FAST 100000 // us
static uint32_t lastSlow;
static uint32_t lastFast;
void sendMavlink()
{
mavlink_message_t msg;
if (stepTime - lastSlow >= PERIOD_SLOW) {
lastSlow = stepTime;
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_SAFETY_ARMED,
0, calibrating ? MAV_STATE_CALIBRATING : MAV_STATE_STANDBY);
sendMessage(&msg);
// params test
// mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, "PITCHRATE_P", PITCHRATE_D, MAV_PARAM_TYPE_REAL32, 3, 0);
// sendMessage(&msg);
// mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, "PITCHRATE_I", PITCHRATE_I, MAV_PARAM_TYPE_REAL32, 3, 1);
// sendMessage(&msg);
// mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, "PITCHRATE_D", PITCHRATE_D, MAV_PARAM_TYPE_REAL32, 3, 2);
// sendMessage(&msg);
}
if (stepTime - lastFast >= PERIOD_FAST) {
lastFast = stepTime;
// mavlink_msg_attitude_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, stepTime / 1000, NAN, NAN, NAN, rollRate, pitchRate, yawRate);
// sendMessage(&msg);
const float zeroQuat[] = {0, 0, 0, 0};
mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
stepTime / 1000, attitude.w, attitude.x, attitude.y, attitude.z, rates.x, rates.y, rates.z, zeroQuat);
// mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
// stepTime / 1000, attitudeTarget.w, attitudeTarget.x, attitudeTarget.y, attitudeTarget.z, rates.x, rates.y, rates.z, zeroQuat);
sendMessage(&msg);
mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, stepTime / 1000, 0,
controls[0] * 10000, controls[1] * 10000, controls[2] * 10000,
controls[3] * 10000, controls[4] * 10000, controls[5] * 10000,
UINT16_MAX, UINT16_MAX, 255);
sendMessage(&msg);
float actuator[32];
memcpy(motors, actuator, 4 * sizeof(float));
mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, stepTime / 1000, 4, actuator);
sendMessage(&msg);
}
}
static inline void sendMessage(const void *msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, (mavlink_message_t *)msg);
sendWiFi(buf, len);
}
#endif

127
flix/motors.ino Normal file
View File

@@ -0,0 +1,127 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#define MOTOR_0_PIN 12
#define MOTOR_1_PIN 13
#define MOTOR_2_PIN 14
#define MOTOR_3_PIN 15
// #define PWM_FREQUENCY 200
// #define PWM_FREQUENCY 50 // TODO: way low frequency considering the IMU is 1kHz
#define PWM_FREQUENCY 200 // WARNING: original 50
#define PWM_RESOLUTION 8
// #define PWM_MIN 1575
// #define PWM_MAX 2300
#define PWM_NEUTRAL 1500
// #define PWM_REVERSE_MAX 700
// #define PWM_REVERSE_MIN 1425
// static const uint16_t pwmMin[] = {1600-50, 1600-50, 1600-50, 1600-50};
// static const uint16_t pwmMax[] = {2100, 2300, 2000, 2000}; // NOTE: ORIGINAL
static const uint16_t pwmMin[] = {1600, 1600, 1600, 1600}; // NOTE: success
// static const uint16_t pwmMax[] = {2000, 2000, 2000, 2000}; // NOTE: success
static const uint16_t pwmMax[] = {2300, 2300, 2300, 2300};
// from esc description
// static const uint16_t pwmMin[] = {1600, 1600, 1600, 1600};
// static const uint16_t pwmMax[] = {2300, 2300, 2300, 2300};
// static const uint16_t pwmReverseMin[] = {1420+50, 1440+50, 1440+50, 1440+50};
// static const uint16_t pwmReverseMin[] = {1420, 1440, 1440, 1440};
// static const uint16_t pwmReverseMax[] = {700, 1100, 1100, 1100}; // NOTE: ???
static const uint16_t pwmReverseMin[] = {1390, 1440, 1440, 1440};
static const uint16_t pwmReverseMax[] = {1100, 1100, 1100, 1100};
bool useBreak; // TODO: redesign
void setupMotors() {
Serial.println("Setup Motors");
// configure PWM channels
ledcSetup(0, PWM_FREQUENCY, PWM_RESOLUTION);
ledcSetup(1, PWM_FREQUENCY, PWM_RESOLUTION);
ledcSetup(2, PWM_FREQUENCY, PWM_RESOLUTION);
ledcSetup(3, PWM_FREQUENCY, PWM_RESOLUTION);
// attach channels to motor pins
ledcAttachPin(MOTOR_0_PIN, 0);
ledcAttachPin(MOTOR_1_PIN, 1);
ledcAttachPin(MOTOR_2_PIN, 2);
ledcAttachPin(MOTOR_3_PIN, 3);
// send initial break to initialize ESCs
// Serial.println("Calibrating ESCs");
// useBreak = true;
// sendMotors();
// delay(2000);
// useBreak = false;
sendMotors();
Serial.println("Motors initialized");
}
static uint16_t getPWM(float val, int n)
{
if (val == 0) {
return PWM_NEUTRAL; // useBreak ? PWM_NEUTRAL : 0;
} else if (val > 0) {
return mapff(val, 0, 1, pwmMin[n], pwmMax[n]);
} else {
return mapff(val, 0, -1, pwmReverseMin[n], pwmReverseMax[n]);
}
}
static uint8_t pwmToDutyCycle(uint16_t pwm) {
return map(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);
}
void sendMotors()
{
ledcWrite(0, pwmToDutyCycle(getPWM(motors[0], 0)));
ledcWrite(1, pwmToDutyCycle(getPWM(motors[1], 1)));
ledcWrite(2, pwmToDutyCycle(getPWM(motors[2], 2)));
ledcWrite(3, pwmToDutyCycle(getPWM(motors[3], 3)));
}
// =====================
const uint32_t REVERSE_PAUSE = 0.1 * 1000000;
static uint8_t lastMotorDirection[4];
static uint32_t lastDirectionChange[4];
static void handleReversePause()
{
for (int i = 0; i < 4; i++) {
if (abs(sign(motors[i]) - lastMotorDirection[i]) > 1) { // 0 => 1 is not direction change, -1 => 1 is
lastDirectionChange[i] = stepTime;
}
if (stepTime - lastDirectionChange[i] < REVERSE_PAUSE) {
// wait before changing direction
motors[i] = 0;
}
lastMotorDirection[i] = sign(motors[i]);
}
}
// =====================
#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
float motorThrust[1][10] = {
{0,0.5513626834,0.5387840671,0.6498951782,0.7023060797,0.7610062893,0.8679245283,1,0.9937106918,0.9916142558}
};
uint16_t minPwm = 1500;
uint16_t pwmStep = 100;
static float thrustToMotorInput(uint8_t n, float thrust)
{
for (int i = 0; i < ARRAY_SIZE(motorThrust[n]); i++) {
if (thrust <= motorThrust[n][i]) {
// TODO: pwm
return mapff(thrust, 0, 1, motorThrust[n][i-1], motorThrust[n][i]);
}
}
}

47
flix/pid.hpp Normal file
View File

@@ -0,0 +1,47 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#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;
};

211
flix/quaternion.hpp Normal file
View File

@@ -0,0 +1,211 @@
// 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;
}
};

34
flix/rc.ino Normal file
View File

@@ -0,0 +1,34 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#include <SBUS.h>
static const uint16_t channelNeutral[] = {995, 883, 200, 972, 512, 512};
static const uint16_t channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
static SBUS RC(Serial2);
void setupRC()
{
Serial.println("Setup RC");
RC.begin();
}
static uint32_t lastReadRC = 0;
void readRC()
{
bool failSafe, lostFrame;
if (RC.read(channels, &failSafe, &lostFrame)) {
if (failSafe) { rcFailSafe++; return; } // TODO: NOT TESTED YET
if (lostFrame) { rcLostFrame++; return; }
normalizeRC();
lastReadRC = stepTime;
}
}
static void normalizeRC() {
for (uint8_t i = 0; i < RC_CHANNELS; i++) {
controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);
}
}

67
flix/test_motors.ino Normal file
View File

@@ -0,0 +1,67 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
static void _pwm(int n, uint16_t pwm)
{
printf("Motor %d: %d\n", n, pwm);
ledcWrite(n, pwmToDutyCycle(pwm));
delay(5000);
}
void fullMotorTest(int n)
{
printf("> Full test for motor %d\n", n);
bool reverse = false;
if (reverse) {
// _pwm(n, 700);
// _pwm(n, 800);
// _pwm(n, 900);
// _pwm(n, 1000);
// _pwm(n, 1100);
// _pwm(n, 1200);
// _pwm(n, 1300);
// _pwm(n, 1400);
// _pwm(n, 1410);
// _pwm(n, 1420);
// _pwm(n, 1430);
// _pwm(n, 1440);
// _pwm(n, 1450);
// _pwm(n, 1460);
// _pwm(n, 1470);
// _pwm(n, 1480);
// _pwm(n, 1490);
}
_pwm(n, 1500);
// _pwm(n, 1510);
// _pwm(n, 1520);
// _pwm(n, 1530);
// _pwm(n, 1540);
// _pwm(n, 1550);
// _pwm(n, 1560);
// _pwm(n, 1570);
// _pwm(n, 1580);
// _pwm(n, 1590);
_pwm(n, 1600);
_pwm(n, 1700);
_pwm(n, 1800);
_pwm(n, 1900);
_pwm(n, 2000);
_pwm(n, 2100);
_pwm(n, 2200);
_pwm(n, 2300);
_pwm(n, 1500);
}
void fullMotorsTest()
{
printf("Perform full motors test\n");
motors[0] = 0;
motors[1] = 0;
motors[2] = 0;
motors[3] = 0;
fullMotorTest(0);
fullMotorTest(1);
fullMotorTest(2);
fullMotorTest(3);
}

51
flix/time.ino Normal file
View File

@@ -0,0 +1,51 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
const uint32_t MS = 1000;
const uint32_t S = 1000000;
static uint32_t stepsPerSecondCurrent;
static uint32_t stepsPerSecondCurrentLast;
void setupTime()
{
startTime = micros();
}
void step() {
steps++;
auto time = micros();
if (stepTime == 0) { // first step
stepTime = time;
}
dt = (time - stepTime) / 1000000.0;
stepTime = time;
// compute steps per seconds
stepsPerSecondCurrent++;
if (time - stepsPerSecondCurrentLast >= 1000000) {
stepsPerSecond = stepsPerSecondCurrent;
stepsPerSecondCurrent = 0;
stepsPerSecondCurrentLast = time;
}
}
void _step() {
steps++;
auto currentStepTime = micros();
if (stepTime == 0) {
stepTime = currentStepTime;
}
dt = (currentStepTime - stepTime) / 1000000.0;
stepTime = currentStepTime;
// compute steps per second, TODO: move to func
stepsPerSecondCurrent++;
if (stepTime - stepsPerSecondCurrentLast >= 1000000) {
stepsPerSecond = stepsPerSecondCurrent;
stepsPerSecondCurrent = 0;
stepsPerSecondCurrentLast = stepTime;
}
}

46
flix/util.ino Normal file
View File

@@ -0,0 +1,46 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#include "math.h"
float mapf(long x, long in_min, long in_max, float out_min, float out_max)
{
return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}
float mapff(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
// float hypot3(float x, float y, float z)
// {
// return sqrt(x * x + y * y + z * z);
// }
int8_t sign(float x)
{
return (x > 0) - (x < 0);
}
float randomFloat(float min, float max)
{
return min + (max - min) * (float)rand() / RAND_MAX;
}
// === printf ===
// https://github.com/jandelgado/log4arduino/blob/master/src/log4arduino.cpp#L51
// https://webhamster.ru/mytetrashare/index/mtb0/16381244680p5beet5d6
#ifdef ARDUINO
#define PRINTF_MAX_STRING_LEN 200
void printf(const __FlashStringHelper *fmt, ...)
{
char buf[PRINTF_MAX_STRING_LEN];
va_list args;
va_start(args, fmt);
vsnprintf(buf, PRINTF_MAX_STRING_LEN, (PGM_P)fmt, args);
va_end(args);
Serial.print(buf);
}
#endif

90
flix/vector.hpp Normal file
View File

@@ -0,0 +1,90 @@
// 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
flix/wifi.ino Normal file
View File

@@ -0,0 +1,35 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#if WIFI_ENABLED == 1
#include <WiFi.h>
#include <WiFiClient.h>
#include <WiFiAP.h>
#include "SBUS.h"
#include "mavlink/common/mavlink.h"
#define WIFI_SSID "flix"
#define WIFI_PASSWORD "flixwifi"
// #define WIFI_UDP_IP "192.168.4.255"
#define WIFI_UDP_IP "255.255.255.255"
// #define WIFI_UDP_IP "192.168.4.2"
#define WIFI_UDP_PORT 14550
WiFiUDP udp;
void setupWiFi()
{
Serial.println("Setup Wi-Fi");
WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
IPAddress myIP = WiFi.softAPIP();
}
inline void sendWiFi(const uint8_t *buf, size_t len)
{
udp.beginPacket(WIFI_UDP_IP, WIFI_UDP_PORT);
udp.write(buf, len);
udp.endPacket();
}
#endif