34 Commits

Author SHA1 Message Date
Oleg Kalachev
3104410bb9 Group control parameters
Also add IMU group to accelerometer calibration parameters.
2025-11-19 01:30:02 +03:00
Oleg Kalachev
1551d096fc Merge changes from master 2025-11-14 20:27:02 +03:00
Oleg Kalachev
80f23ab016 Update log analysis documentation 2025-11-14 20:17:34 +03:00
Oleg Kalachev
e6fb264499 Remove unneeded SERIAL_BAUDRATE define 2025-11-14 13:46:02 +03:00
Oleg Kalachev
4d0871b00b Updates in documentation
Fixes, updates, new illustrations.
2025-11-10 20:13:39 +03:00
Oleg Kalachev
f1b993d719 Many updates to documentation
Updates to main readme.
Add much more info to usage article.
Move simulator building to simulation's readme.
Improve assembly article.
Many fixes.
Updates in diagrams.
2025-11-06 13:46:25 +03:00
Oleg Kalachev
2e7330d2f5 Refactor Wi-Fi log download
Use MAVLink LOG_REQUEST_DATA and LOG_DATA for download log instead of console.
Make Wi-Fi download default way of downloading the log.
Make `log` command only print the header and `log dump` dump the log.
2025-11-02 00:24:38 +03:00
Oleg Kalachev
e22df3ab01 Simplify rate limiter code 2025-11-02 00:03:37 +03:00
Oleg Kalachev
8484854576 Keep only one floating point version of map function
Two variants are redundant
2025-11-01 23:55:55 +03:00
Oleg Kalachev
b9d5624f30 Add some excludes to sloc 2025-10-29 03:35:31 +03:00
Oleg Kalachev
205270b8ec Add Rate class for running the code at fixed rate 2025-10-29 03:25:05 +03:00
Oleg Kalachev
f1bedb2b10 Count sloc in Actions 2025-10-29 02:20:50 +03:00
Oleg Kalachev
46d1749a8c Minor code fixes 2025-10-21 19:33:57 +03:00
Oleg Kalachev
66fb7a13c3 Simplify command for command handling 2025-10-21 19:33:57 +03:00
Oleg Kalachev
cfef3b9c36 Fixes to troubleshooting 2025-10-21 19:33:57 +03:00
KiraFlux
1338a9ea79 Quaternion::fromBetweenVectors: pass u and v as const references (#21) 2025-10-19 10:17:38 +03:00
Oleg Kalachev
b60757ec1d Minor code style change 2025-10-18 12:36:20 +03:00
Oleg Kalachev
491e054534 Revert t variable type to float instead of double
For the sake of simplicity and consistency.
2025-10-18 12:28:01 +03:00
Oleg Kalachev
3eaf24f89d Minor change 2025-10-17 19:22:48 +03:00
Oleg Kalachev
dc09459613 Add generic Delay filter 2025-10-17 19:19:27 +03:00
Oleg Kalachev
59c9ea8cb3 Lowercase imu and rc variables
To make it more obvious these are variables, not classes.
2025-10-17 19:02:46 +03:00
Oleg Kalachev
5bdd46c6ad Increase thrust to ARMED_THRUST if it's less 2025-10-17 18:54:01 +03:00
Oleg Kalachev
5b37c87166 Refactor PID controllers
Use t variable instead of passing dt argument.
Reset PID automatically on large dts.
2025-10-17 18:53:15 +03:00
Oleg Kalachev
48ba55aa7e Rename failsafe.ino to safety.ino
To aggregate all the safety related functionality.
2025-10-17 01:09:23 +03:00
Oleg Kalachev
2708c1eafd Add ESP32-S3 build to Actions 2025-10-14 16:56:48 +03:00
Oleg Kalachev
b2c9dfe6ef Fix Gazebo installation
Installation script is deprecated, install using package on Ubuntu 20.04
2025-10-14 11:44:27 +03:00
Oleg Kalachev
0579118dd5 Update VSCode settings
Disable error squiggles as they often work incorrectly.
Decrease number of include libraries to index.
2025-10-14 11:31:47 +03:00
Oleg Kalachev
05818349d8 Improve rc failsafe logic
Don't trigger failsafe if there's no RC at all
Use AUTO mode for descending, instead of STAB
Increase RC loss timeout and descend time
2025-10-12 21:20:46 +03:00
Oleg Kalachev
6c1d651caa Disarm the drone on simulator plugin reset
In order to reset yaw target.
2025-10-07 15:45:30 +03:00
Oleg Kalachev
49a0aa7036 Reset yaw target when drone disarmed
Prevent unexpected behavior when the drone tries to restore its old yaw on takeoff.
2025-10-07 15:42:52 +03:00
Oleg Kalachev
bf9eeb90a4 Include FlixPeriph header instead of MPU9250
This simplifies choosing IMU model
2025-10-07 08:41:56 +03:00
Oleg Kalachev
96836b2e3e Ensure showing correct raw data in imu command
Some IMUs will reset acc and gyro buffer on whoAmI() call
2025-10-07 08:41:56 +03:00
Oleg Kalachev
82d9d3570d Send only mavlink heartbeats until connected 2025-10-03 07:08:17 +03:00
Oleg Kalachev
d7f8c8d934 Add Wi-Fi client mode
WIFI_AP_MODE define
2025-10-03 06:56:03 +03:00
51 changed files with 238 additions and 423 deletions

View File

@@ -25,6 +25,8 @@ jobs:
path: flix/build path: flix/build
- name: Build firmware for ESP32-S3 - name: Build firmware for ESP32-S3
run: make BOARD=esp32:esp32:esp32s3 run: make BOARD=esp32:esp32:esp32s3
- name: Build firmware with WiFi disabled
run: sed -i 's/^#define WIFI_ENABLED 1$/#define WIFI_ENABLED 0/' flix/flix.ino && make
- name: Check c_cpp_properties.json - name: Check c_cpp_properties.json
run: tools/check_c_cpp_properties.py run: tools/check_c_cpp_properties.py

View File

@@ -46,3 +46,14 @@ jobs:
echo -e "t,x,y,z\n0,1,2,3\n1,4,5,6" > log.csv echo -e "t,x,y,z\n0,1,2,3\n1,4,5,6" > log.csv
./csv_to_mcap.py log.csv ./csv_to_mcap.py log.csv
test $(stat -c %s log.mcap) -eq 883 test $(stat -c %s log.mcap) -eq 883
sloc:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v4
- name: Install cloc
run: sudo apt-get install -y cloc
- name: Firmware source lines count
run: cloc --by-file-by-lang flix
- name: Overall source lines count
run: cloc --by-file-by-lang --exclude-ext=svg,dae,css,hbs,md,json,yaml,yml,toml .

View File

@@ -7,8 +7,6 @@
"MD024": false, "MD024": false,
"MD033": false, "MD033": false,
"MD034": false, "MD034": false,
"MD040": false,
"MD059": false,
"MD044": { "MD044": {
"html_elements": false, "html_elements": false,
"code_blocks": false, "code_blocks": false,
@@ -66,6 +64,5 @@
"PX4" "PX4"
] ]
}, },
"MD045": false, "MD045": false
"MD060": false
} }

View File

@@ -1,9 +1,6 @@
<!-- markdownlint-disable MD041 --> # Flix
<p align="center"> **Flix** (*flight + X*) — open source ESP32-based quadcopter made from scratch.
<img src="docs/img/flix.svg" width=180 alt="Flix logo"><br>
<b>Flix</b> (<i>flight + X</i>) — open source ESP32-based quadcopter made from scratch.
</p>
<table> <table>
<tr> <tr>
@@ -21,13 +18,15 @@
* Dedicated for education and research. * Dedicated for education and research.
* Made from general-purpose components. * Made from general-purpose components.
* Simple and clean source code in Arduino (<2k lines firmware). * Simple and clean source code in Arduino (<2k lines firmware).
* Connectivity using Wi-Fi and MAVLink protocol.
* Control using USB gamepad, remote control or smartphone. * Control using USB gamepad, remote control or smartphone.
* Wi-Fi and MAVLink support.
* Wireless command line interface and analyzing. * Wireless command line interface and analyzing.
* Precise simulation with Gazebo. * Precise simulation with Gazebo.
* Python library for scripting and automatic flights. * Python library.
* Textbook on flight control theory and practice ([in development](https://quadcopter.dev)). * Textbook on flight control theory and practice ([in development](https://quadcopter.dev)).
* *Position control (planned)*. * *Position control (using external camera) and autonomous flights¹*.
*¹ — planned.*
## It actually flies ## It actually flies

View File

@@ -35,7 +35,7 @@
### Подсистема управления ### Подсистема управления
Состояние органов управления обрабатывается в функции `interpretControls()` и преобразуется в **команду управления**, которая включает следующее: Состояние органов управления обрабатывается в функции `interpretControls()` и преобразуется в *команду управления*, которая включает следующее:
* `attitudeTarget` *(Quaternion)* — целевая ориентация дрона. * `attitudeTarget` *(Quaternion)* — целевая ориентация дрона.
* `ratesTarget` *(Vector)* — целевые угловые скорости, *рад/с*. * `ratesTarget` *(Vector)* — целевые угловые скорости, *рад/с*.

View File

@@ -87,13 +87,13 @@ Flix поддерживает следующие модели IMU:
#include <FlixPeriph.h> #include <FlixPeriph.h>
#include <SPI.h> #include <SPI.h>
MPU9250 imu(SPI); MPU9250 IMU(SPI);
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
bool success = imu.begin(); bool success = IMU.begin();
if (!success) { if (!success) {
Serial.println("Failed to initialize the IMU"); Serial.println("Failed to initialize IMU");
} }
} }
``` ```
@@ -108,21 +108,21 @@ void setup() {
#include <FlixPeriph.h> #include <FlixPeriph.h>
#include <SPI.h> #include <SPI.h>
MPU9250 imu(SPI); MPU9250 IMU(SPI);
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
bool success = imu.begin(); bool success = IMU.begin();
if (!success) { if (!success) {
Serial.println("Failed to initialize the IMU"); Serial.println("Failed to initialize IMU");
} }
} }
void loop() { void loop() {
imu.waitForData(); IMU.waitForData();
float gx, gy, gz; float gx, gy, gz;
imu.getGyro(gx, gy, gz); IMU.getGyro(gx, gy, gz);
Serial.printf("gx:%f gy:%f gz:%f\n", gx, gy, gz); Serial.printf("gx:%f gy:%f gz:%f\n", gx, gy, gz);
delay(50); // замедление вывода delay(50); // замедление вывода
@@ -135,36 +135,36 @@ void loop() {
## Конфигурация гироскопа ## Конфигурация гироскопа
В коде Flix настройка IMU происходит в функции `configureIMU`. В этой функции настраиваются три основных параметра гироскопа: диапазон измерений, частота сэмплирования и частота LPF-фильтра. В коде Flix настройка IMU происходит в функции `configureIMU`. В этой функции настраиваются три основных параметра гироскопа: диапазон измерений, частота сэмплов и частота LPF-фильтра.
### Частота сэмплирования ### Частота сэмплов
Большинство IMU могут обновлять данные с разной частотой. В полетных контроллерах обычно используется частота обновления от 500 Гц до 8 кГц. Чем выше частота, тем выше точность управления полетом, но и тем больше нагрузка на микроконтроллер. Большинство IMU могут обновлять данные с разной частотой. В полетных контроллерах обычно используется частота обновления от 500 Гц до 8 кГц. Чем выше частота сэмплов, тем выше точность управления полетом, но и больше нагрузка на микроконтроллер.
Частота сэмплирования устанавливается методом `setSampleRate()`. В Flix используется частота 1 кГц: Частота сэмплов устанавливается методом `setSampleRate()`. В Flix используется частота 1 кГц:
```cpp ```cpp
IMU.setRate(IMU.RATE_1KHZ_APPROX); IMU.setRate(IMU.RATE_1KHZ_APPROX);
``` ```
Поскольку не все поддерживаемые IMU могут работать строго на частоте 1 кГц, в библиотеке FlixPeriph существует возможность приближенной настройки частоты сэмплирования. Например, у IMU ICM-20948 при такой настройке реальная частота сэмплирования будет равна 1125 Гц. Поскольку не все поддерживаемые IMU могут работать строго на частоте 1 кГц, в библиотеке FlixPeriph существует возможность приближенной настройки частоты сэмплов. Например, у IMU ICM-20948 при такой настройке реальная частота сэмплирования будет равна 1125 Гц.
Другие доступные для установки в библиотеке FlixPeriph частоты сэмплирования: Другие доступные для установки в библиотеке FlixPeriph частоты сэмплирования:
* `RATE_MIN` — минимальная частота для конкретного IMU. * `RATE_MIN` — минимальная частота сэмплов для конкретного IMU.
* `RATE_50HZ_APPROX` — значение, близкое к 50 Гц. * `RATE_50HZ_APPROX` — значение, близкое к 50 Гц.
* `RATE_1KHZ_APPROX` — значение, близкое к 1 кГц. * `RATE_1KHZ_APPROX` — значение, близкое к 1 кГц.
* `RATE_8KHZ_APPROX` — значение, близкое к 8 кГц. * `RATE_8KHZ_APPROX` — значение, близкое к 8 кГц.
* `RATE_MAX` — максимальная частота для конкретного IMU. * `RATE_MAX` — максимальная частота сэмплов для конкретного IMU.
#### Диапазон измерений #### Диапазон измерений
Большинство MEMS-гироскопов поддерживают несколько диапазонов измерений угловой скорости. Главное преимущество выбора меньшего диапазона — бо́льшая чувствительность. В полетных контроллерах обычно выбирается максимальный диапазон измерений от 2000 до 2000 градусов в секунду, чтобы обеспечить возможность быстрых маневров. Большинство MEMS-гироскопов поддерживают несколько диапазонов измерений угловой скорости. Главное преимущество выбора меньшего диапазона — бо́льшая чувствительность. В полетных контроллерах обычно выбирается максимальный диапазон измерений от 2000 до 2000 градусов в секунду, чтобы обеспечить возможность динамичных маневров.
В библиотеке FlixPeriph диапазон измерений гироскопа устанавливается методом `setGyroRange()`: В библиотеке FlixPeriph диапазон измерений гироскопа устанавливается методом `setGyroRange()`:
```cpp ```cpp
imu.setGyroRange(imu.GYRO_RANGE_2000DPS); IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS);
``` ```
### LPF-фильтр ### LPF-фильтр
@@ -172,7 +172,7 @@ imu.setGyroRange(imu.GYRO_RANGE_2000DPS);
IMU InvenSense могут фильтровать измерения на аппаратном уровне при помощи фильтра нижних частот (LPF). Flix реализует собственный фильтр для гироскопа, чтобы иметь больше гибкости при поддержке разных IMU. Поэтому для встроенного LPF устанавливается максимальная частота среза: IMU InvenSense могут фильтровать измерения на аппаратном уровне при помощи фильтра нижних частот (LPF). Flix реализует собственный фильтр для гироскопа, чтобы иметь больше гибкости при поддержке разных IMU. Поэтому для встроенного LPF устанавливается максимальная частота среза:
```cpp ```cpp
imu.setDLPF(imu.DLPF_MAX); IMU.setDLPF(IMU.DLPF_MAX);
``` ```
## Калибровка гироскопа ## Калибровка гироскопа
@@ -181,7 +181,7 @@ imu.setDLPF(imu.DLPF_MAX);
\\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\] \\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\]
Для точной работы подсистемы оценки ориентации и управления дроном необходимо оценить *bias* гироскопа и учесть его в вычислениях. Для этого при запуске программы производится калибровка гироскопа, которая реализована в функции `calibrateGyro()`. Эта функция считывает данные с гироскопа в состоянии покоя 1000 раз и усредняет их. Полученные значения считаются *bias* гироскопа и в дальнейшем вычитаются из измерений. Для качественной работы подсистемы оценки ориентации и управления дроном необходимо оценить *bias* гироскопа и учесть его в вычислениях. Для этого при запуске программы производится калибровка гироскопа, которая реализована в функции `calibrateGyro()`. Эта функция считывает данные с гироскопа в состоянии покоя 1000 раз и усредняет их. Полученные значения считаются *bias* гироскопа и в дальнейшем вычитаются из измерений.
Программа для вывода данных с гироскопа с калибровкой: Программа для вывода данных с гироскопа с калибровкой:
@@ -189,23 +189,23 @@ imu.setDLPF(imu.DLPF_MAX);
#include <FlixPeriph.h> #include <FlixPeriph.h>
#include <SPI.h> #include <SPI.h>
MPU9250 imu(SPI); MPU9250 IMU(SPI);
float gyroBiasX, gyroBiasY, gyroBiasZ; // bias гироскопа float gyroBiasX, gyroBiasY, gyroBiasZ; // bias гироскопа
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
bool success = imu.begin(); bool success = IMU.begin();
if (!success) { if (!success) {
Serial.println("Failed to initialize the IMU"); Serial.println("Failed to initialize IMU");
} }
calibrateGyro(); calibrateGyro();
} }
void loop() { void loop() {
float gx, gy, gz; float gx, gy, gz;
imu.waitForData(); IMU.waitForData();
imu.getGyro(gx, gy, gz); IMU.getGyro(gx, gy, gz);
// Устранение bias гироскопа // Устранение bias гироскопа
gx -= gyroBiasX; gx -= gyroBiasX;
@@ -226,9 +226,9 @@ void calibrateGyro() {
// Получение 1000 измерений гироскопа // Получение 1000 измерений гироскопа
for (int i = 0; i < samples; i++) { for (int i = 0; i < samples; i++) {
imu.waitForData(); IMU.waitForData();
float gx, gy, gz; float gx, gy, gz;
imu.getGyro(gx, gy, gz); IMU.getGyro(gx, gy, gz);
gyroBiasX += gx; gyroBiasX += gx;
gyroBiasY += gy; gyroBiasY += gy;
gyroBiasZ += gz; gyroBiasZ += gz;

View File

@@ -38,13 +38,13 @@ Utility files:
### Control subsystem ### Control subsystem
Pilot inputs are interpreted in `interpretControls()`, and then converted to the **control command**, which consists of the following: Pilot inputs are interpreted in `interpretControls()`, and then converted to the *control command*, which consists of the following:
* `attitudeTarget` *(Quaternion)* — target attitude of the drone. * `attitudeTarget` *(Quaternion)* — target attitude of the drone.
* `ratesTarget` *(Vector)* — target angular rates, *rad/s*. * `ratesTarget` *(Vector)* — target angular rates, *rad/s*.
* `ratesExtra` *(Vector)* — additional (feed-forward) angular rates, used for yaw rate control in STAB mode, *rad/s*. * `ratesExtra` *(Vector)* — additional (feed-forward) angular rates , used for yaw rate control in STAB mode, *rad/s*.
* `torqueTarget` *(Vector)* — target torque, range [-1, 1]. * `torqueTarget` *(Vector)* — target torque, range [-1, 1].
* `thrustTarget` *(float)* — collective motor thrust target, range [0, 1]. * `thrustTarget` *(float)* — collective thrust target, range [0, 1].
Control command is handled in `controlAttitude()`, `controlRates()`, `controlTorque()` functions. Each function may be skipped if the corresponding control target is set to `NAN`. Control command is handled in `controlAttitude()`, `controlRates()`, `controlTorque()` functions. Each function may be skipped if the corresponding control target is set to `NAN`.
@@ -62,11 +62,6 @@ print("Test value: %.2f\n", testValue);
In order to add a console command, modify the `doCommand()` function in `cli.ino` file. In order to add a console command, modify the `doCommand()` function in `cli.ino` file.
> [!IMPORTANT]
> Avoid using delays in in-flight commands, it will **crash** the drone! (The design is one-threaded.)
>
> For on-the-ground commands, use `pause()` function, instead of `delay()`. This function allows to pause in a way that MAVLink connection will continue working.
## Building the firmware ## Building the firmware
See build instructions in [usage.md](usage.md). See build instructions in [usage.md](usage.md).

View File

@@ -1,38 +0,0 @@
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 734.86 378.46">
<defs>
<style>
.a {
fill: none;
stroke: #d5d5d5;
stroke-miterlimit: 10;
stroke-width: 31px;
}
.b {
fill: #c1c1c1;
}
.c {
fill: #ff9400;
}
.d {
fill: #d5d5d5;
}
</style>
</defs>
<g>
<g>
<line class="a" x1="448.78" y1="294.23" x2="648.77" y2="93.24"/>
<line class="a" x1="449.78" y1="94.24" x2="649.77" y2="295.23"/>
<circle class="b" cx="549.27" cy="193.73" r="41.5"/>
<circle class="c" cx="449.35" cy="93.74" r="77.95"/>
<circle class="c" cx="648.89" cy="93.53" r="77.95"/>
<circle class="c" cx="647.89" cy="294.51" r="77.95"/>
<circle class="c" cx="448.9" cy="294.51" r="77.95"/>
</g>
<path class="d" d="M8,161.93q0-17.85,22.36-17.85h4.81V100.57Q35.17,8.49,131.23,8h1q36.87,0,47.31,7.48L181,17.41l1,2.41V50q0,12.32-5.82,12.31-2.43,0-7.4-4.64t-14.19-9a48.63,48.63,0,0,0-21.22-4.41q-12,0-19.17,3.5a18.85,18.85,0,0,0-9.82,10.62,67.35,67.35,0,0,0-3.52,12.06,82.52,82.52,0,0,0-.85,13.39v60.32h27.28q10.86,0,16.05,5.43a17.52,17.52,0,0,1,5.19,12.42,22.5,22.5,0,0,1-1.21,7.36q-1.22,3.51-6.64,7.24t-14.36,3.74H101.64V344.82a56,56,0,0,1-.61,9.65,37.8,37.8,0,0,1-2.56,7.6,11.83,11.83,0,0,1-6.94,6.4q-5,1.93-13.51,1.93H57.08q-10.47,0-15.7-4.71t-5.73-8.44a77.31,77.31,0,0,1-.48-9.53V180.27H29.4Q8,180.27,8,161.93Z"/>
<path class="d" d="M201.21,348.2V37q0-23.16,20.86-23.4h22.81q22.8,0,22.8,22.44V346.27a68.92,68.92,0,0,1-.49,9.41,22.59,22.59,0,0,1-2.42,7.12,11.48,11.48,0,0,1-6.67,5.43,47.78,47.78,0,0,1-12.74,2.17H225q-11.4,0-17.58-4.47T201.21,348.2Z"/>
<path class="d" d="M284.9,61.08V36.47a40.39,40.39,0,0,1,1.7-12.91,11.36,11.36,0,0,1,6.18-7,25.27,25.27,0,0,1,6.68-2.3c1.45-.15,4-.4,7.76-.72h25.23q10.43,0,16.73,4.7t6.31,18.22V62.05a27.94,27.94,0,0,1-.85,7.11,23,23,0,0,1-2.06,5.43,20,20,0,0,1-2.91,4,10,10,0,0,1-3.52,2.54c-1.21.48-2.54,1-4,1.44a11.53,11.53,0,0,1-3.4.73,13.71,13.71,0,0,0-3.15.48H307.7q-10.43,0-16.61-4.7T284.9,61.08Zm1.94,284.71V166.28q0-22.2,21.83-22.2h20.38q10.92,0,16.62,4t6.67,8.45a60.47,60.47,0,0,1,1,12.18V348.2q0,22.2-21.83,22.2H308.67q-10.43,0-15.64-4.95t-5.7-8.81A90.93,90.93,0,0,1,286.84,345.79Z"/>
</g>
</svg>

Before

Width:  |  Height:  |  Size: 2.2 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 23 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 18 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 18 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 17 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 17 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 10 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 9.6 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 10 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 10 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 68 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 35 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 32 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 108 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 93 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 65 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 28 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 31 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 42 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 43 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 76 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 28 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 44 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 32 KiB

View File

@@ -4,7 +4,7 @@
Do the following: Do the following:
* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#building-the-firmware). * **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#firmware).
* **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU9250 or other peripherals libraries that may conflict with the ones used in the tutorial. * **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU9250 or other peripherals libraries that may conflict with the ones used in the tutorial.
* **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*. * **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*.
@@ -25,7 +25,7 @@ Do the following:
* The `accel` and `gyro` fields should change as you move the drone. * The `accel` and `gyro` fields should change as you move the drone.
* **Calibrate the accelerometer.** if is wasn't done before. Type `ca` command in Serial Monitor and follow the instructions. * **Calibrate the accelerometer.** if is wasn't done before. Type `ca` command in Serial Monitor and follow the instructions.
* **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. * **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct.
* **Check the IMU orientation is set correctly**. If the attitude estimation is rotated, set the correct IMU orientation as described in the [tutorial](usage.md#define-imu-orientation). * **Check the IMU orientation is set correctly**. If the attitude estimation is rotated, make sure `rotateIMU` function is defined correctly in `imu.ino` file.
* **Check the motors type**. Motors with exact 3.7V voltage are needed, not ranged working voltage (3.7V — 6V). * **Check the motors type**. Motors with exact 3.7V voltage are needed, not ranged working voltage (3.7V — 6V).
* **Check the motors**. Perform the following commands using Serial Monitor: * **Check the motors**. Perform the following commands using Serial Monitor:
* `mfr` — should rotate front right motor (counter-clockwise). * `mfr` — should rotate front right motor (counter-clockwise).

View File

@@ -12,7 +12,7 @@ Beginners can [download the source code as a ZIP archive](https://github.com/oka
## Building the firmware ## Building the firmware
You can build and upload the firmware using either **Arduino IDE** (easier for beginners) or **command line**. You can build and upload the firmware using either **Arduino IDE** (easier for beginners) or a **command line**.
### Arduino IDE (Windows, Linux, macOS) ### Arduino IDE (Windows, Linux, macOS)
@@ -73,6 +73,14 @@ ICM20948 imu(SPI); // For ICM-20948
MPU6050 imu(Wire); // For MPU-6050 MPU6050 imu(Wire); // For MPU-6050
``` ```
### Setup the IMU orientation
The IMU orientation is defined in `rotateIMU` function in the `imu.ino` file. Change it so it converts the IMU axes to the drone's axes correctly. **Drone axes are X forward, Y left, Z up**:
<img src="img/drone-axes.svg" width="200">
See various [IMU boards axes orientations table](https://github.com/okalachev/flixperiph/?tab=readme-ov-file#imu-axes-orientation) to help you set up the correct orientation.
### Connect using QGroundControl ### Connect using QGroundControl
QGroundControl is a ground control station software that can be used to monitor and control the drone. QGroundControl is a ground control station software that can be used to monitor and control the drone.
@@ -80,7 +88,7 @@ QGroundControl is a ground control station software that can be used to monitor
1. Install mobile or desktop version of [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html). 1. Install mobile or desktop version of [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html).
2. Power up the drone. 2. Power up the drone.
3. Connect your computer or smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`). 3. Connect your computer or smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
4. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically. 4. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically
### Access console ### Access console
@@ -96,37 +104,11 @@ To access the console using QGroundControl:
1. Connect to the drone using QGroundControl app. 1. Connect to the drone using QGroundControl app.
2. Go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*. 2. Go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
<img src="img/cli.png" width="400">
<img src="img/cli.png" width="400">
> [!TIP] > [!TIP]
> Use `help` command to see the list of available commands. > Use `help` command to see the list of available commands.
### Access parameters
The drone is configured using parameters. To access and modify them, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*:
<img src="img/parameters.png" width="400">
You can also work with parameters using `p` command in the console.
### Define IMU orientation
Use parameters, to define the IMU board axes orientation relative to the drone's axes: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
The drone has *X* axis pointing forward, *Y* axis pointing left, and *Z* axis pointing up, and the supported IMU boards have *X* axis pointing to the pins side and *Z* axis pointing up from the component side:
<img src="img/imu-axes.png" width="200">
Use the following table to set the parameters for common IMU orientations:
|Orientation|Parameters|Orientation|Parameters|
|:-:|-|-|-|
|<img src="img/imu-rot-1.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0 |<img src="img/imu-rot-5.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0|
|<img src="img/imu-rot-2.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|<img src="img/imu-rot-6.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|
|<img src="img/imu-rot-3.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|<img src="img/imu-rot-7.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|
|<img src="img/imu-rot-4.png" width="180"><br>☑️ **Default**|<br>`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|<img src="img/imu-rot-8.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|
### Calibrate accelerometer ### Calibrate accelerometer
Before flight you need to calibrate the accelerometer: Before flight you need to calibrate the accelerometer:
@@ -143,9 +125,9 @@ Before flight you need to calibrate the accelerometer:
* The `accel` and `gyro` fields should change as you move the drone. * The `accel` and `gyro` fields should change as you move the drone.
* The `landed` field should be `1` when the drone is still on the ground and `0` when you lift it up. * The `landed` field should be `1` when the drone is still on the ground and `0` when you lift it up.
2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Compare your attitude indicator (in the *large vertical* mode) to the video: 2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Attitude indicator in QGroundControl is shown below:
<a href="https://youtu.be/yVRN23-GISU"><img width=300 src="https://i3.ytimg.com/vi/yVRN23-GISU/maxresdefault.jpg"></a> <img src="img/qgc-attitude.png" height="200">
3. Perform motor tests in the console. Use the following commands **— remove the propellers before running the tests!** 3. Perform motor tests in the console. Use the following commands **— remove the propellers before running the tests!**
@@ -154,10 +136,6 @@ Before flight you need to calibrate the accelerometer:
* `mrl` — should rotate rear left motor (counter-clockwise). * `mrl` — should rotate rear left motor (counter-clockwise).
* `mrr` — should rotate rear right motor (clockwise). * `mrr` — should rotate rear right motor (clockwise).
Rotation diagram:
<img src="img/motors.svg" width=200>
> [!WARNING] > [!WARNING]
> Never run the motors when powering the drone from USB, always use the battery for that. > Never run the motors when powering the drone from USB, always use the battery for that.
@@ -165,7 +143,7 @@ Before flight you need to calibrate the accelerometer:
There are several ways to control the drone's flight: using **smartphone** (Wi-Fi), using **SBUS remote control**, or using **USB remote control** (Wi-Fi). There are several ways to control the drone's flight: using **smartphone** (Wi-Fi), using **SBUS remote control**, or using **USB remote control** (Wi-Fi).
### Control with a smartphone ### Control with smartphone
1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone. 1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone.
2. Power the drone using the battery. 2. Power the drone using the battery.
@@ -175,9 +153,9 @@ There are several ways to control the drone's flight: using **smartphone** (Wi-F
6. Use the virtual joystick to fly the drone! 6. Use the virtual joystick to fly the drone!
> [!TIP] > [!TIP]
> Decrease `CTL_TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive. > Decrease `CNT_TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive.
### Control with a remote control ### Control with remote control
Before using remote SBUS-connected remote control, you need to calibrate it: Before using remote SBUS-connected remote control, you need to calibrate it:
@@ -185,7 +163,7 @@ Before using remote SBUS-connected remote control, you need to calibrate it:
2. Type `cr` command and follow the instructions. 2. Type `cr` command and follow the instructions.
3. Use the remote control to fly the drone! 3. Use the remote control to fly the drone!
### Control with a USB remote control ### Control with USB remote control
If your drone doesn't have RC receiver installed, you can use USB remote control and QGroundControl app to fly it. If your drone doesn't have RC receiver installed, you can use USB remote control and QGroundControl app to fly it.
@@ -233,9 +211,9 @@ The default mode is *STAB*. In this mode, the drone stabilizes its attitude (ori
In this mode, the pilot controls the angular rates. This control method is difficult to fly and mostly used in FPV racing. In this mode, the pilot controls the angular rates. This control method is difficult to fly and mostly used in FPV racing.
#### RAW #### MANUAL
*RAW* mode disables all the stabilization, and the pilot inputs are mixed directly to the motors. The IMU sensor is not involved. This mode is intended for testing and demonstration purposes only, and basically the drone **cannot fly in this mode**. Manual mode disables all the stabilization, and the pilot inputs are passed directly to the motors. This mode is intended for testing and demonstration purposes only, and basically the drone **cannot fly in this mode**.
#### AUTO #### AUTO
@@ -243,37 +221,15 @@ In this mode, the pilot inputs are ignored (except the mode switch, if configure
If the pilot moves the control sticks, the drone will switch back to *STAB* mode. If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
## Wi-Fi configuration ## Adjusting parameters
You can configure the Wi-Fi using parameters and console commands. You can adjust some of the drone's parameters (include PID coefficients) in QGroundControl. In order to do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*.
The Wi-Fi mode is chosen using `WIFI_MODE` parameter in QGroundControl or in the console: <img src="img/parameters.png" width="400">
* `0` — Wi-Fi is disabled.
* `1` — Access Point mode (*AP*) — the drone creates a Wi-Fi network.
* `2` — Client mode (*STA*) — the drone connects to an existing Wi-Fi network.
* `3` — *ESP-NOW (not implemented yet)*.
> [!WARNING]
> Tests showed that Client mode may cause **additional delays** in remote control (due to retranslations), so it's generally not recommended.
The SSID and password are configured using the `ap` and `sta` console commands:
```
ap <ssid> <password>
sta <ssid> <password>
```
Example of configuring the Access Point mode:
```
ap my-flix-ssid mypassword123
p WIFI_MODE 1
```
## Flight log ## Flight log
After the flight, you can download the flight log for analysis wirelessly. Use the following command on your computer for that: After the flight, you can download the flight log for analysis wirelessly. Use the following for that:
```bash ```bash
make log make log

View File

@@ -4,67 +4,12 @@ This page contains user-built drones based on the Flix project. Publish your pro
--- ---
Author: [goldarte](https://t.me/goldarte).<br>
<img src="img/user/goldarte/1.jpg" height=150> <img src="img/user/goldarte/2.jpg" height=150>
**Flight video:**
<a href="https://drive.google.com/file/d/1nQtFjEcGGLx-l4xkL5ko9ZpOTVU-WDjL/view?usp=sharing"><img height=200 src="img/user/goldarte/video.jpg"></a>
---
## School 548 course
Special course on quadcopter design and engineering took place in october-november 2025 in School 548, Moscow. The course included UAV control theory, electronics, drone assembly and setup practice, using the Flix project.
<img height=200 src="img/user/school548/1.jpg"> <img height=200 src="img/user/school548/2.jpg"> <img height=200 src="img/user/school548/3.jpg">
STL files and other materials: see [here](https://drive.google.com/drive/folders/1wTUzj087LjKQQl3Lz5CjHCuobxoykhyp?usp=share_link).
### Selected works
Author: [KiraFlux](https://t.me/@kiraflux_0XC0000005).<br>
Description: **custom ESPNOW remote control** was implemented, modified firmware to support ESPNOW protocol.<br>
Telegram posts: [1](https://t.me/opensourcequadcopter/106), [2](https://t.me/opensourcequadcopter/114).<br>
Modified Flix firmware: https://github.com/KiraFlux/flix/tree/klyax.<br>
Remote control project: https://github.com/KiraFlux/ESP32-DJC.<br>
Drone design: https://github.com/KiraFlux/Klyax.<br>
<img src="img/user/school548/kiraflux1.jpg" height=150> <img src="img/user/school548/kiraflux2.jpg" height=150>
**ESPNOW remote control demonstration**:
<img height=200 src="img/user/school548/kiraflux-video.jpg"><a href="https://drive.google.com/file/d/1soHDAeHQWnm97Y4dg4nWevJuMiTdJJXW/view?usp=sharing"></a>
Author: [tolyan4krut](https://t.me/tolyan4krut).<br>
Description: the first drone based on ESP32-S3-CAM board **with a camera**, implementing Wi-Fi video streaming. Runs HTTP server and HTTP video stream.<br>
Modified Flix firmware: https://github.com/CatRey/Flix-Camera-Streaming.<br>
[Telegram post](https://t.me/opensourcequadcopter/117).
<img src="img/user/school548/tolyan4krut.jpg" height=150>
**Video streaming and flight demonstration**:
<a href="https://drive.google.com/file/d/1KuOBsujLsk7q8FoqKD8u7uoq4ptS5onp/view?usp=sharing"><img height=200 src="img/user/school548/tolyan4krut-video.jpg"></a>
Author: [Vlad Tolshinov](https://t.me/Vlad_Tolshinov).<br>
Description: custom frame with enlarged arm length, which provides very high flight stability, 65 mm props.
<img src="img/user/school548/vlad_tolshinov1.jpg" height=150> <img src="img/user/school548/vlad_tolshinov2.jpg" height=150>
**Flight video**:
<a href="https://drive.google.com/file/d/1zu00DZxhC7DJ9Z2mYjtxdNQqOOLAyYbp/view?usp=sharing"><img height=200 src="img/user/school548/vlad_tolshinov-video.jpg"></a>
---
## RoboCamp ## RoboCamp
Author: RoboCamp participants.<br> Author: RoboCamp participants.<br>
Description: 3D-printed and wooden frames, ESP32 Mini, DC-DC buck-boost converters. BetaFPV LiteRadio 3 to control the drones via Wi-Fi connection.<br> Description: 3D-printed and wooden frames, ESP32 Mini, DC-DC buck-boost converters. BetaFPV LiteRadio 3 to control the drones via Wi-Fi connection.<br>
Features: altitude hold, obstacle avoidance, autonomous flight elements.<br> Features: altitude hold, obstacle avoidance, autonomous flight elements.<br>
Some of the designed model files: see [here](https://drive.google.com/drive/folders/18YHWGquKeIevzrMH4-OUT-zKXMETTEUu?usp=share_link). Some of the designed model files: https://drive.google.com/drive/folders/18YHWGquKeIevzrMH4-OUT-zKXMETTEUu?usp=share_link.
RoboCamp took place in July 2025, Saint Petersburg, where 9 participants designed and built their own drones using the Flix project, and then modified the firmware to complete specific flight tasks. RoboCamp took place in July 2025, Saint Petersburg, where 9 participants designed and built their own drones using the Flix project, and then modified the firmware to complete specific flight tasks.

View File

@@ -8,10 +8,10 @@
#include "util.h" #include "util.h"
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern const int RAW, ACRO, STAB, AUTO; extern const int ACRO, STAB, AUTO;
extern float t, dt, loopRate; extern float t, dt, loopRate;
extern uint16_t channels[16]; extern uint16_t channels[16];
extern float controlTime; extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
extern int mode; extern int mode;
extern bool armed; extern bool armed;
@@ -35,11 +35,8 @@ const char* motd =
"imu - show IMU data\n" "imu - show IMU data\n"
"arm - arm the drone\n" "arm - arm the drone\n"
"disarm - disarm the drone\n" "disarm - disarm the drone\n"
"raw/stab/acro/auto - set mode\n" "stab/acro/auto - set mode\n"
"rc - show RC data\n" "rc - show RC data\n"
"wifi - show Wi-Fi info\n"
"ap <ssid> <password> - setup Wi-Fi access point\n"
"sta <ssid> <password> - setup Wi-Fi client mode\n"
"mot - show motor output\n" "mot - show motor output\n"
"log [dump] - print log header [and data]\n" "log [dump] - print log header [and data]\n"
"cr - calibrate RC\n" "cr - calibrate RC\n"
@@ -56,7 +53,9 @@ void print(const char* format, ...) {
vsnprintf(buf, sizeof(buf), format, args); vsnprintf(buf, sizeof(buf), format, args);
va_end(args); va_end(args);
Serial.print(buf); Serial.print(buf);
#if WIFI_ENABLED
mavlinkPrint(buf); mavlinkPrint(buf);
#endif
} }
void pause(float duration) { void pause(float duration) {
@@ -64,7 +63,9 @@ void pause(float duration) {
while (t - start < duration) { while (t - start < duration) {
step(); step();
handleInput(); handleInput();
#if WIFI_ENABLED
processMavlink(); processMavlink();
#endif
delay(50); delay(50);
} }
} }
@@ -115,8 +116,6 @@ void doCommand(String str, bool echo = false) {
armed = true; armed = true;
} else if (command == "disarm") { } else if (command == "disarm") {
armed = false; armed = false;
} else if (command == "raw") {
mode = RAW;
} else if (command == "stab") { } else if (command == "stab") {
mode = STAB; mode = STAB;
} else if (command == "acro") { } else if (command == "acro") {
@@ -130,15 +129,8 @@ void doCommand(String str, bool echo = false) {
} }
print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n", print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n",
controlRoll, controlPitch, controlYaw, controlThrottle, controlMode); controlRoll, controlPitch, controlYaw, controlThrottle, controlMode);
print("time: %.1f\n", controlTime);
print("mode: %s\n", getModeName()); print("mode: %s\n", getModeName());
print("armed: %d\n", armed); print("armed: %d\n", armed);
} else if (command == "wifi") {
printWiFiInfo();
} else if (command == "ap") {
configWiFi(true, arg0.c_str(), arg1.c_str());
} else if (command == "sta") {
configWiFi(false, arg0.c_str(), arg1.c_str());
} else if (command == "mot") { } else if (command == "mot") {
print("front-right %g front-left %g rear-right %g rear-left %g\n", print("front-right %g front-left %g rear-right %g rear-left %g\n",
motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]); motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);

View File

@@ -34,16 +34,10 @@
#define TILT_MAX radians(30) #define TILT_MAX radians(30)
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
int mode = STAB; int mode = STAB;
bool armed = false; bool armed = false;
Quaternion attitudeTarget;
Vector ratesTarget;
Vector ratesExtra; // feedforward rates
Vector torqueTarget;
float thrustTarget;
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA); PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA); PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D); PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
@@ -53,6 +47,12 @@ PID yawPID(YAW_P, 0, 0);
Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX); Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
float tiltMax = TILT_MAX; float tiltMax = TILT_MAX;
Quaternion attitudeTarget;
Vector ratesTarget;
Vector ratesExtra; // feedforward rates
Vector torqueTarget;
float thrustTarget;
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT; extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode; extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
@@ -65,6 +65,7 @@ void control() {
} }
void interpretControls() { void interpretControls() {
// NOTE: put ACRO or MANUAL modes there if you want to use them
if (controlMode < 0.25) mode = STAB; if (controlMode < 0.25) mode = STAB;
if (controlMode < 0.75) mode = STAB; if (controlMode < 0.75) mode = STAB;
if (controlMode > 0.75) mode = STAB; if (controlMode > 0.75) mode = STAB;
@@ -74,8 +75,6 @@ void interpretControls() {
if (controlThrottle < 0.05 && controlYaw > 0.95) armed = true; // arm gesture if (controlThrottle < 0.05 && controlYaw > 0.95) armed = true; // arm gesture
if (controlThrottle < 0.05 && controlYaw < -0.95) armed = false; // disarm gesture if (controlThrottle < 0.05 && controlYaw < -0.95) armed = false; // disarm gesture
if (abs(controlYaw) < 0.1) controlYaw = 0; // yaw dead zone
thrustTarget = controlThrottle; thrustTarget = controlThrottle;
if (mode == STAB) { if (mode == STAB) {
@@ -92,10 +91,10 @@ void interpretControls() {
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
} }
if (mode == RAW) { // direct torque control if (mode == MANUAL) { // passthrough mode
attitudeTarget.invalidate(); // skip attitude control attitudeTarget.invalidate(); // skip attitude control
ratesTarget.invalidate(); // skip rate control ratesTarget.invalidate(); // skip rate control
torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.1; torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
} }
} }
@@ -156,7 +155,7 @@ void controlTorque() {
const char* getModeName() { const char* getModeName() {
switch (mode) { switch (mode) {
case RAW: return "RAW"; case MANUAL: return "MANUAL";
case ACRO: return "ACRO"; case ACRO: return "ACRO";
case STAB: return "STAB"; case STAB: return "STAB";
case AUTO: return "AUTO"; case AUTO: return "AUTO";

View File

@@ -8,12 +8,8 @@
#include "lpf.h" #include "lpf.h"
#include "util.h" #include "util.h"
Vector rates; // estimated angular rates, rad/s #define WEIGHT_ACC 0.003
Quaternion attitude; // estimated attitude #define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
bool landed;
float accWeight = 0.003;
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
void estimate() { void estimate() {
applyGyro(); applyGyro();
@@ -22,6 +18,7 @@ void estimate() {
void applyGyro() { void applyGyro() {
// filter gyro to get angular rates // filter gyro to get angular rates
static LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
rates = ratesFilter.update(gyro); rates = ratesFilter.update(gyro);
// apply rates to attitude // apply rates to attitude
@@ -37,7 +34,7 @@ void applyAcc() {
// calculate accelerometer correction // calculate accelerometer correction
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude); Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
Vector correction = Vector::rotationVectorBetween(acc, up) * accWeight; Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
// apply correction // apply correction
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction)); attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));

View File

@@ -7,14 +7,18 @@
#include "quaternion.h" #include "quaternion.h"
#include "util.h" #include "util.h"
#define WIFI_ENABLED 1
extern float t, dt; float t = NAN; // current step time, s
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode; float dt; // time delta from previous step, s
extern Vector gyro, acc; float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
extern Vector rates; float controlMode = NAN;
extern Quaternion attitude; Vector gyro; // gyroscope data
extern bool landed; Vector acc; // accelerometer data, m/s/s
extern float motors[4]; Vector rates; // filtered angular rates, rad/s
Quaternion attitude; // estimated attitude
bool landed; // are we landed and stationary
float motors[4]; // normalized motors thrust in range [0..1]
void setup() { void setup() {
Serial.begin(115200); Serial.begin(115200);
@@ -24,7 +28,9 @@ void setup() {
setupLED(); setupLED();
setupMotors(); setupMotors();
setLED(true); setLED(true);
#if WIFI_ENABLED
setupWiFi(); setupWiFi();
#endif
setupIMU(); setupIMU();
setupRC(); setupRC();
setLED(false); setLED(false);
@@ -39,7 +45,9 @@ void loop() {
control(); control();
sendMotors(); sendMotors();
handleInput(); handleInput();
#if WIFI_ENABLED
processMavlink(); processMavlink();
#endif
logData(); logData();
syncParameters(); syncParameters();
} }

View File

@@ -10,14 +10,10 @@
#include "util.h" #include "util.h"
MPU9250 imu(SPI); MPU9250 imu(SPI);
Vector imuRotation(0, 0, -PI / 2); // imu orientation as Euler angles
Vector gyro; // gyroscope output, rad/s
Vector gyroBias;
Vector acc; // accelerometer output, m/s/s
Vector accBias; Vector accBias;
Vector accScale(1, 1, 1); Vector accScale(1, 1, 1);
Vector gyroBias;
void setupIMU() { void setupIMU() {
print("Setup IMU\n"); print("Setup IMU\n");
@@ -41,18 +37,24 @@ void readIMU() {
// apply scale and bias // apply scale and bias
acc = (acc - accBias) / accScale; acc = (acc - accBias) / accScale;
gyro = gyro - gyroBias; gyro = gyro - gyroBias;
// rotate to body frame // rotate
Quaternion rotation = Quaternion::fromEuler(imuRotation); rotateIMU(acc);
acc = Quaternion::rotateVector(acc, rotation.inversed()); rotateIMU(gyro);
gyro = Quaternion::rotateVector(gyro, rotation.inversed()); }
void rotateIMU(Vector& data) {
// Rotate from LFD to FLU
// NOTE: In case of using other IMU orientation, change this line:
data = Vector(data.y, data.x, -data.z);
// Axes orientation for various boards: https://github.com/okalachev/flixperiph#imu-axes-orientation
} }
void calibrateGyroOnce() { void calibrateGyroOnce() {
static Delay landedDelay(2); static Delay landedDelay(2);
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
static LowPassFilter<Vector> gyroBiasFilter(0.001); static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
gyroBias = gyroBiasFilter.update(gyro); gyroBias = gyroCalibrationFilter.update(gyro);
} }
void calibrateAccel() { void calibrateAccel() {

View File

@@ -3,16 +3,21 @@
// MAVLink communication // MAVLink communication
#if WIFI_ENABLED
#include <MAVLink.h> #include <MAVLink.h>
#include "util.h" #include "util.h"
extern float controlTime; #define SYSTEM_ID 1
#define MAVLINK_RATE_SLOW 1
#define MAVLINK_RATE_FAST 10
#define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
bool mavlinkConnected = false; bool mavlinkConnected = false;
String mavlinkPrintBuffer; String mavlinkPrintBuffer;
int mavlinkSysId = 1;
Rate telemetryFast(10); extern float controlTime;
Rate telemetrySlow(2); extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
void processMavlink() { void processMavlink() {
sendMavlink(); sendMavlink();
@@ -25,8 +30,10 @@ void sendMavlink() {
mavlink_message_t msg; mavlink_message_t msg;
uint32_t time = t * 1000; uint32_t time = t * 1000;
if (telemetrySlow) { static Rate slow(MAVLINK_RATE_SLOW), fast(MAVLINK_RATE_FAST);
mavlink_msg_heartbeat_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
if (slow) {
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
(armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) | (armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) |
((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) | ((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) |
((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED), ((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
@@ -35,27 +42,27 @@ void sendMavlink() {
if (!mavlinkConnected) return; // send only heartbeat until connected if (!mavlinkConnected) return; // send only heartbeat until connected
mavlink_msg_extended_sys_state_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR); MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
sendMessage(&msg); sendMessage(&msg);
} }
if (telemetryFast && mavlinkConnected) { if (fast && mavlinkConnected) {
const float zeroQuat[] = {0, 0, 0, 0}; const float zeroQuat[] = {0, 0, 0, 0};
mavlink_msg_attitude_quaternion_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd
sendMessage(&msg); sendMessage(&msg);
mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0, mavlink_msg_rc_channels_raw_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX); channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
float controls[8]; float controls[8];
memcpy(controls, motors, sizeof(motors)); memcpy(controls, motors, sizeof(motors));
mavlink_msg_actuator_control_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls); mavlink_msg_actuator_control_target_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
sendMessage(&msg); sendMessage(&msg);
mavlink_msg_scaled_imu_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd
gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000, gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
0, 0, 0, 0); 0, 0, 0, 0);
@@ -90,7 +97,7 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
mavlink_manual_control_t m; mavlink_manual_control_t m;
mavlink_msg_manual_control_decode(&msg, &m); mavlink_msg_manual_control_decode(&msg, &m);
if (m.target && m.target != mavlinkSysId) return; // 0 is broadcast if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast
controlThrottle = m.z / 1000.0f; controlThrottle = m.z / 1000.0f;
controlPitch = m.x / 1000.0f; controlPitch = m.x / 1000.0f;
@@ -98,16 +105,18 @@ void handleMavlink(const void *_msg) {
controlYaw = m.r / 1000.0f; controlYaw = m.r / 1000.0f;
controlMode = NAN; controlMode = NAN;
controlTime = t; controlTime = t;
if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
} }
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
mavlink_param_request_list_t m; mavlink_param_request_list_t m;
mavlink_msg_param_request_list_decode(&msg, &m); mavlink_msg_param_request_list_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != SYSTEM_ID) return;
mavlink_message_t msg; mavlink_message_t msg;
for (int i = 0; i < parametersCount(); i++) { for (int i = 0; i < parametersCount(); i++) {
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i); getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);
sendMessage(&msg); sendMessage(&msg);
} }
@@ -116,7 +125,7 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
mavlink_param_request_read_t m; mavlink_param_request_read_t m;
mavlink_msg_param_request_read_decode(&msg, &m); mavlink_msg_param_request_read_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != SYSTEM_ID) return;
char name[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1]; char name[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
@@ -125,7 +134,7 @@ void handleMavlink(const void *_msg) {
memcpy(name, getParameterName(m.param_index), 16); memcpy(name, getParameterName(m.param_index), 16);
} }
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index); name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index);
sendMessage(&msg); sendMessage(&msg);
} }
@@ -133,15 +142,14 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) { if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t m; mavlink_param_set_t m;
mavlink_msg_param_set_decode(&msg, &m); mavlink_msg_param_set_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != SYSTEM_ID) return;
char name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1]; char name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];
strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
bool success = setParameter(name, m.param_value); setParameter(name, m.param_value);
if (!success) return;
// send ack // send ack
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
sendMessage(&msg); sendMessage(&msg);
} }
@@ -149,17 +157,17 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
mavlink_mission_request_list_t m; mavlink_mission_request_list_t m;
mavlink_msg_mission_request_list_decode(&msg, &m); mavlink_msg_mission_request_list_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != SYSTEM_ID) return;
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_mission_count_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0); mavlink_msg_mission_count_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
sendMessage(&msg); sendMessage(&msg);
} }
if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) { if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {
mavlink_serial_control_t m; mavlink_serial_control_t m;
mavlink_msg_serial_control_decode(&msg, &m); mavlink_msg_serial_control_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != SYSTEM_ID) return;
char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1]; char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated
@@ -171,7 +179,7 @@ void handleMavlink(const void *_msg) {
mavlink_set_attitude_target_t m; mavlink_set_attitude_target_t m;
mavlink_msg_set_attitude_target_decode(&msg, &m); mavlink_msg_set_attitude_target_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != SYSTEM_ID) return;
// copy attitude, rates and thrust targets // copy attitude, rates and thrust targets
ratesTarget.x = m.body_roll_rate; ratesTarget.x = m.body_roll_rate;
@@ -193,7 +201,7 @@ void handleMavlink(const void *_msg) {
mavlink_set_actuator_control_target_t m; mavlink_set_actuator_control_target_t m;
mavlink_msg_set_actuator_control_target_decode(&msg, &m); mavlink_msg_set_actuator_control_target_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != SYSTEM_ID) return;
attitudeTarget.invalidate(); attitudeTarget.invalidate();
ratesTarget.invalidate(); ratesTarget.invalidate();
@@ -205,12 +213,12 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) { if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
mavlink_log_request_data_t m; mavlink_log_request_data_t m;
mavlink_msg_log_request_data_decode(&msg, &m); mavlink_msg_log_request_data_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != SYSTEM_ID) return;
// Send all log records // Send all log records
for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) { for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i, mavlink_msg_log_data_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]); sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]);
sendMessage(&msg); sendMessage(&msg);
} }
@@ -220,13 +228,13 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) { if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
mavlink_command_long_t m; mavlink_command_long_t m;
mavlink_msg_command_long_decode(&msg, &m); mavlink_msg_command_long_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return; if (m.target_system && m.target_system != SYSTEM_ID) return;
mavlink_message_t response; mavlink_message_t response;
bool accepted = false; bool accepted = false;
if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) { if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
accepted = true; accepted = true;
mavlink_msg_autopilot_version_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response, mavlink_msg_autopilot_version_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &response,
MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0); MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
sendMessage(&response); sendMessage(&response);
} }
@@ -245,7 +253,7 @@ void handleMavlink(const void *_msg) {
// send command ack // send command ack
mavlink_message_t ack; mavlink_message_t ack;
mavlink_msg_command_ack_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid); mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
sendMessage(&ack); sendMessage(&ack);
} }
} }
@@ -262,7 +270,7 @@ void sendMavlinkPrint() {
char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1]; char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
strlcpy(data, str + i, sizeof(data)); strlcpy(data, str + i, sizeof(data));
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_serial_control_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, mavlink_msg_serial_control_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
SERIAL_CONTROL_DEV_SHELL, SERIAL_CONTROL_DEV_SHELL,
i + MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN < strlen(str) ? SERIAL_CONTROL_FLAG_MULTI : 0, // more chunks to go i + MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN < strlen(str) ? SERIAL_CONTROL_FLAG_MULTI : 0, // more chunks to go
0, 0, strlen(data), (uint8_t *)data, 0, 0); 0, 0, strlen(data), (uint8_t *)data, 0, 0);
@@ -270,3 +278,5 @@ void sendMavlinkPrint() {
} }
mavlinkPrintBuffer.clear(); mavlinkPrintBuffer.clear();
} }
#endif

View File

@@ -17,8 +17,7 @@
#define PWM_MIN 0 #define PWM_MIN 0
#define PWM_MAX 1000000 / PWM_FREQUENCY #define PWM_MAX 1000000 / PWM_FREQUENCY
float motors[4]; // normalized motor thrusts in range [0..1] // Motors array indexes:
const int MOTOR_REAR_LEFT = 0; const int MOTOR_REAR_LEFT = 0;
const int MOTOR_REAR_RIGHT = 1; const int MOTOR_REAR_RIGHT = 1;
const int MOTOR_FRONT_RIGHT = 2; const int MOTOR_FRONT_RIGHT = 2;

View File

@@ -9,19 +9,13 @@
extern float channelZero[16]; extern float channelZero[16];
extern float channelMax[16]; extern float channelMax[16];
extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel; extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
extern int wifiMode, udpLocalPort, udpRemotePort;
Preferences storage; Preferences storage;
struct Parameter { struct Parameter {
const char *name; // max length is 15 (Preferences key limit) const char *name; // max length is 15 (Preferences key limit)
bool integer; float *variable;
union { float *f; int *i; }; // pointer to variable float value; // cache
float cache; // what's stored in flash
Parameter(const char *name, float *variable) : name(name), integer(false), f(variable) {};
Parameter(const char *name, int *variable) : name(name), integer(true), i(variable) {};
float getValue() const { return integer ? *i : *f; };
void setValue(const float value) { if (integer) *i = value; else *f = value; };
}; };
Parameter parameters[] = { Parameter parameters[] = {
@@ -49,18 +43,12 @@ Parameter parameters[] = {
{"CTL_Y_RATE_MAX", &maxRate.z}, {"CTL_Y_RATE_MAX", &maxRate.z},
{"CTL_TILT_MAX", &tiltMax}, {"CTL_TILT_MAX", &tiltMax},
// imu // imu
{"IMU_ROT_ROLL", &imuRotation.x},
{"IMU_ROT_PITCH", &imuRotation.y},
{"IMU_ROT_YAW", &imuRotation.z},
{"IMU_ACC_BIAS_X", &accBias.x}, {"IMU_ACC_BIAS_X", &accBias.x},
{"IMU_ACC_BIAS_Y", &accBias.y}, {"IMU_ACC_BIAS_Y", &accBias.y},
{"IMU_ACC_BIAS_Z", &accBias.z}, {"IMU_ACC_BIAS_Z", &accBias.z},
{"IMU_ACC_SCALE_X", &accScale.x}, {"IMU_ACC_SCALE_X", &accScale.x},
{"IMU_ACC_SCALE_Y", &accScale.y}, {"IMU_ACC_SCALE_Y", &accScale.y},
{"IMU_ACC_SCALE_Z", &accScale.z}, {"IMU_ACC_SCALE_Z", &accScale.z},
// estimate
{"EST_ACC_WEIGHT", &accWeight},
{"EST_RATES_LPF_A", &ratesFilter.alpha},
// rc // rc
{"RC_ZERO_0", &channelZero[0]}, {"RC_ZERO_0", &channelZero[0]},
{"RC_ZERO_1", &channelZero[1]}, {"RC_ZERO_1", &channelZero[1]},
@@ -83,14 +71,6 @@ Parameter parameters[] = {
{"RC_THROTTLE", &throttleChannel}, {"RC_THROTTLE", &throttleChannel},
{"RC_YAW", &yawChannel}, {"RC_YAW", &yawChannel},
{"RC_MODE", &modeChannel}, {"RC_MODE", &modeChannel},
// wifi
{"WIFI_MODE", &wifiMode},
{"WIFI_LOC_PORT", &udpLocalPort},
{"WIFI_REM_PORT", &udpRemotePort},
// mavlink
{"MAV_SYS_ID", &mavlinkSysId},
{"MAV_RATE_SLOW", &telemetrySlow.rate},
{"MAV_RATE_FAST", &telemetryFast.rate},
}; };
void setupParameters() { void setupParameters() {
@@ -98,10 +78,10 @@ void setupParameters() {
// Read parameters from storage // Read parameters from storage
for (auto &parameter : parameters) { for (auto &parameter : parameters) {
if (!storage.isKey(parameter.name)) { if (!storage.isKey(parameter.name)) {
storage.putFloat(parameter.name, parameter.getValue()); // store default value storage.putFloat(parameter.name, *parameter.variable);
} }
parameter.setValue(storage.getFloat(parameter.name, 0)); *parameter.variable = storage.getFloat(parameter.name, *parameter.variable);
parameter.cache = parameter.getValue(); parameter.value = *parameter.variable;
} }
} }
@@ -116,13 +96,13 @@ const char *getParameterName(int index) {
float getParameter(int index) { float getParameter(int index) {
if (index < 0 || index >= parametersCount()) return NAN; if (index < 0 || index >= parametersCount()) return NAN;
return parameters[index].getValue(); return *parameters[index].variable;
} }
float getParameter(const char *name) { float getParameter(const char *name) {
for (auto &parameter : parameters) { for (auto &parameter : parameters) {
if (strcmp(parameter.name, name) == 0) { if (strcmp(parameter.name, name) == 0) {
return parameter.getValue(); return *parameter.variable;
} }
} }
return NAN; return NAN;
@@ -131,8 +111,7 @@ float getParameter(const char *name) {
bool setParameter(const char *name, const float value) { bool setParameter(const char *name, const float value) {
for (auto &parameter : parameters) { for (auto &parameter : parameters) {
if (strcmp(parameter.name, name) == 0) { if (strcmp(parameter.name, name) == 0) {
if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf *parameter.variable = value;
parameter.setValue(value);
return true; return true;
} }
} }
@@ -145,18 +124,16 @@ void syncParameters() {
if (motorsActive()) return; // don't use flash while flying, it may cause a delay if (motorsActive()) return; // don't use flash while flying, it may cause a delay
for (auto &parameter : parameters) { for (auto &parameter : parameters) {
if (parameter.getValue() == parameter.cache) continue; // no change if (parameter.value == *parameter.variable) continue;
if (isnan(parameter.getValue()) && isnan(parameter.cache)) continue; // both are NaN if (isnan(parameter.value) && isnan(*parameter.variable)) continue; // handle NAN != NAN
if (isinf(parameter.getValue()) && isinf(parameter.cache)) continue; // both are Inf storage.putFloat(parameter.name, *parameter.variable);
parameter.value = *parameter.variable;
storage.putFloat(parameter.name, parameter.getValue());
parameter.cache = parameter.getValue(); // update cache
} }
} }
void printParameters() { void printParameters() {
for (auto &parameter : parameters) { for (auto &parameter : parameters) {
print("%s = %g\n", parameter.name, parameter.getValue()); print("%s = %g\n", parameter.name, *parameter.variable);
} }
} }

View File

@@ -6,16 +6,13 @@
#include <SBUS.h> #include <SBUS.h>
#include "util.h" #include "util.h"
SBUS rc(Serial2); SBUS rc(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
uint16_t channels[16]; // raw rc channels uint16_t channels[16]; // raw rc channels
float controlTime; // time of the last controls update
float channelZero[16]; // calibration zero values float channelZero[16]; // calibration zero values
float channelMax[16]; // calibration max values float channelMax[16]; // calibration max values
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
float controlMode = NAN; //
float controlTime; // time of the last controls update (0 when no RC)
// Channels mapping (using float to store in parameters): // Channels mapping (using float to store in parameters):
float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN; float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
@@ -41,11 +38,11 @@ void normalizeRC() {
controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1); controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
} }
// Update control values // Update control values
controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0; controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : NAN;
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0; controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0; controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0; controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
} }
void calibrateRC() { void calibrateRC() {

View File

@@ -3,8 +3,6 @@
// Time related functions // Time related functions
float t = NAN; // current time, s
float dt; // time delta with the previous step, s
float loopRate; // Hz float loopRate; // Hz
void step() { void step() {

View File

@@ -35,6 +35,7 @@ public:
z = NAN; z = NAN;
} }
float norm() const { float norm() const {
return sqrt(x * x + y * y + z * z); return sqrt(x * x + y * y + z * z);
} }

View File

@@ -1,76 +1,45 @@
// 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
// Wi-Fi connectivity // Wi-Fi support
#if WIFI_ENABLED
#include <WiFi.h> #include <WiFi.h>
#include <WiFiAP.h> #include <WiFiAP.h>
#include <WiFiUdp.h> #include <WiFiUdp.h>
#include "Preferences.h"
extern Preferences storage; // use the main preferences storage #define WIFI_AP_MODE 1
#define WIFI_AP_SSID "flix"
const int W_DISABLED = 0, W_AP = 1, W_STA = 2; #define WIFI_AP_PASSWORD "flixwifi"
int wifiMode = W_AP; #define WIFI_SSID ""
int udpLocalPort = 14550; #define WIFI_PASSWORD ""
int udpRemotePort = 14550; #define WIFI_UDP_PORT 14550
IPAddress udpRemoteIP = "255.255.255.255"; #define WIFI_UDP_REMOTE_PORT 14550
#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
WiFiUDP udp; WiFiUDP udp;
void setupWiFi() { void setupWiFi() {
print("Setup Wi-Fi\n"); print("Setup Wi-Fi\n");
if (wifiMode == W_AP) { if (WIFI_AP_MODE) {
WiFi.softAP(storage.getString("WIFI_AP_SSID", "flix").c_str(), storage.getString("WIFI_AP_PASS", "flixwifi").c_str()); WiFi.softAP(WIFI_AP_SSID, WIFI_AP_PASSWORD);
} else if (wifiMode == W_STA) { } else {
WiFi.begin(storage.getString("WIFI_STA_SSID", "").c_str(), storage.getString("WIFI_STA_PASS", "").c_str()); WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
} }
udp.begin(udpLocalPort); udp.begin(WIFI_UDP_PORT);
} }
void sendWiFi(const uint8_t *buf, int len) { void sendWiFi(const uint8_t *buf, int len) {
if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return; if (WiFi.softAPIP() == IPAddress(0, 0, 0, 0) && WiFi.status() != WL_CONNECTED) return;
udp.beginPacket(udpRemoteIP, udpRemotePort); udp.beginPacket(udp.remoteIP() ? udp.remoteIP() : WIFI_UDP_REMOTE_ADDR, WIFI_UDP_REMOTE_PORT);
udp.write(buf, len); udp.write(buf, len);
udp.endPacket(); udp.endPacket();
} }
int receiveWiFi(uint8_t *buf, int len) { int receiveWiFi(uint8_t *buf, int len) {
udp.parsePacket(); udp.parsePacket();
if (udp.remoteIP()) udpRemoteIP = udp.remoteIP();
return udp.read(buf, len); return udp.read(buf, len);
} }
void printWiFiInfo() { #endif
if (WiFi.getMode() == WIFI_MODE_AP) {
print("Mode: Access Point (AP)\n");
print("MAC: %s\n", WiFi.softAPmacAddress().c_str());
print("SSID: %s\n", WiFi.softAPSSID().c_str());
print("Password: ***\n");
print("Clients: %d\n", WiFi.softAPgetStationNum());
print("IP: %s\n", WiFi.softAPIP().toString().c_str());
} else if (WiFi.getMode() == WIFI_MODE_STA) {
print("Mode: Client (STA)\n");
print("Connected: %d\n", WiFi.isConnected());
print("MAC: %s\n", WiFi.macAddress().c_str());
print("SSID: %s\n", WiFi.SSID().c_str());
print("Password: ***\n");
print("IP: %s\n", WiFi.localIP().toString().c_str());
} else {
print("Mode: Disabled\n");
return;
}
print("Remote IP: %s\n", udpRemoteIP.toString().c_str());
print("MAVLink connected: %d\n", mavlinkConnected);
}
void configWiFi(bool ap, const char *ssid, const char *password) {
if (ap) {
storage.putString("WIFI_AP_SSID", ssid);
storage.putString("WIFI_AP_PASS", password);
} else {
storage.putString("WIFI_STA_SSID", ssid);
storage.putString("WIFI_STA_PASS", password);
}
print("✓ Reboot to apply new settings\n");
}

View File

@@ -68,9 +68,6 @@ Just like the real drone, the simulator can be controlled using a USB remote con
6. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**. 6. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
7. Use the virtual joystick to fly the drone! 7. Use the virtual joystick to fly the drone!
> [!TIP]
> Decrease `CTL_TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive.
### Control with USB remote control ### Control with USB remote control
1. Connect your USB remote control to the machine running the simulator. 1. Connect your USB remote control to the machine running the simulator.

View File

@@ -10,15 +10,18 @@
#include "Arduino.h" #include "Arduino.h"
#include "wifi.h" #include "wifi.h"
extern float t, dt; #define WIFI_ENABLED 1
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
extern Vector rates;
extern Quaternion attitude;
extern bool landed;
extern float motors[4];
Vector gyro, acc, imuRotation; float t = NAN;
Vector accBias, gyroBias, accScale(1, 1, 1); float dt;
float motors[4];
float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
float controlMode = NAN;
Vector acc;
Vector gyro;
Vector rates;
Quaternion attitude;
bool landed;
// declarations // declarations
void step(); void step();
@@ -70,5 +73,4 @@ void calibrateGyro() { print("Skip gyro calibrating\n"); };
void calibrateAccel() { print("Skip accel calibrating\n"); }; void calibrateAccel() { print("Skip accel calibrating\n"); };
void printIMUCalibration() { print("cal: N/A\n"); }; void printIMUCalibration() { print("cal: N/A\n"); };
void printIMUInfo() {}; void printIMUInfo() {};
void printWiFiInfo() {}; Vector accBias, gyroBias, accScale(1, 1, 1);
void configWiFi(bool, const char*, const char*) { print("Skip WiFi config\n"); };

View File

@@ -1,8 +1,8 @@
# Flix Python library # Flix Python library
The Flix Python library allows you to remotely connect to a Flix quadcopter. It provides access to telemetry data, supports executing console commands, and controlling the drone's flight. The Flix Python library allows you to remotely connect to a Flix quadcopter. It provides access to telemetry data, supports executing CLI commands, and controlling the drone's flight.
To use the library, connect to the drone's Wi-Fi. To use it with the simulator, ensure the script runs on the same network as the simulator. To use the library, connect to the drone's Wi-Fi. To use it with the simulator, ensure the script runs on the same local network as the simulator.
## Installation ## Installation
@@ -30,7 +30,7 @@ flix = Flix() # create a Flix object and wait for connection
### Telemetry ### Telemetry
Basic telemetry is available through object properties. The property names generally match the corresponding variables in the firmware itself: Basic telemetry is available through object properties. The properties names generally match the corresponding variables in the firmware itself:
```python ```python
print(flix.connected) # True if connected to the drone print(flix.connected) # True if connected to the drone
@@ -41,7 +41,7 @@ print(flix.attitude) # attitude quaternion [w, x, y, z]
print(flix.attitude_euler) # attitude as Euler angles [roll, pitch, yaw] print(flix.attitude_euler) # attitude as Euler angles [roll, pitch, yaw]
print(flix.rates) # angular rates [roll_rate, pitch_rate, yaw_rate] print(flix.rates) # angular rates [roll_rate, pitch_rate, yaw_rate]
print(flix.channels) # raw RC channels (list) print(flix.channels) # raw RC channels (list)
print(flix.motors) # motor outputs (list) print(flix.motors) # motors outputs (list)
print(flix.acc) # accelerometer output (list) print(flix.acc) # accelerometer output (list)
print(flix.gyro) # gyroscope output (list) print(flix.gyro) # gyroscope output (list)
``` ```
@@ -95,24 +95,24 @@ Full list of events:
|`armed`|Armed state update|Armed state (*bool*)| |`armed`|Armed state update|Armed state (*bool*)|
|`mode`|Flight mode update|Flight mode (*str*)| |`mode`|Flight mode update|Flight mode (*str*)|
|`landed`|Landed state update|Landed state (*bool*)| |`landed`|Landed state update|Landed state (*bool*)|
|`print`|The drone prints text to the console|Text| |`print`|The drone sends text to the console|Text|
|`attitude`|Attitude update|Attitude quaternion (*list*)| |`attitude`|Attitude update|Attitude quaternion (*list*)|
|`attitude_euler`|Attitude update|Euler angles (*list*)| |`attitude_euler`|Attitude update|Euler angles (*list*)|
|`rates`|Angular rates update|Angular rates (*list*)| |`rates`|Angular rates update|Angular rates (*list*)|
|`channels`|Raw RC channels update|Raw RC channels (*list*)| |`channels`|Raw RC channels update|Raw RC channels (*list*)|
|`motors`|Motor outputs update|Motor outputs (*list*)| |`motors`|Motors outputs update|Motors outputs (*list*)|
|`acc`|Accelerometer update|Accelerometer output (*list*)| |`acc`|Accelerometer update|Accelerometer output (*list*)|
|`gyro`|Gyroscope update|Gyroscope output (*list*)| |`gyro`|Gyroscope update|Gyroscope output (*list*)|
|`mavlink`|Received MAVLink message|Message object| |`mavlink`|Received MAVLink message|Message object|
|`mavlink.<message_name>`|Received specific MAVLink message|Message object| |`mavlink.<message_name>`|Received specific MAVLink message|Message object|
|`mavlink.<message_id>`|Received specific MAVLink message|Message object| |`mavlink.<message_id>`|Received specific MAVLink message|Message object|
|`value`|Named value update (see below)|Name, value| |`value`|Named value update (see below)|Name, value|
|`value.<name>`|Specific named value update (see below)|Value| |`value.<name>`|Specific named value update (see bellow)|Value|
> [!NOTE] > [!NOTE]
> Update events trigger on every new piece of data from the drone, and do not mean the value has changed. > Update events trigger on every new data from the drone, and do not mean the value has changed.
### Basic methods ### Common methods
Get and set firmware parameters using `get_param` and `set_param` methods: Get and set firmware parameters using `get_param` and `set_param` methods:
@@ -121,7 +121,7 @@ pitch_p = flix.get_param('PITCH_P') # get parameter value
flix.set_param('PITCH_P', 5) # set parameter value flix.set_param('PITCH_P', 5) # set parameter value
``` ```
Execute console commands using `cli` method. This method returns the command response: Execute console commands using `cli` method. This method returns command response:
```python ```python
imu = flix.cli('imu') # get detailed IMU data imu = flix.cli('imu') # get detailed IMU data
@@ -169,10 +169,10 @@ Setting angular rates target:
flix.set_rates([0.1, 0.2, 0.3], 0.6) # set target roll rate, pitch rate, yaw rate and thrust flix.set_rates([0.1, 0.2, 0.3], 0.6) # set target roll rate, pitch rate, yaw rate and thrust
``` ```
You also can control raw motor outputs directly: You also can control raw motors outputs directly:
```python ```python
flix.set_motors([0.5, 0.5, 0.5, 0.5]) # set motor outputs in range [0, 1] flix.set_motors([0.5, 0.5, 0.5, 0.5]) # set motors outputs in range [0, 1]
``` ```
In *AUTO* mode, the drone will arm automatically if the thrust is greater than zero, and disarm if thrust is zero. Therefore, to disarm the drone, set thrust to zero: In *AUTO* mode, the drone will arm automatically if the thrust is greater than zero, and disarm if thrust is zero. Therefore, to disarm the drone, set thrust to zero:
@@ -186,7 +186,7 @@ The following methods are in development and are not functional yet:
* `set_position` — set target position. * `set_position` — set target position.
* `set_velocity` — set target velocity. * `set_velocity` — set target velocity.
To exit *AUTO* mode move control sticks and the drone will switch to *STAB* mode. To exit from *AUTO* mode move control sticks and the drone will switch to *STAB* mode.
## Usage alongside QGroundControl ## Usage alongside QGroundControl

View File

@@ -17,7 +17,7 @@ from pymavlink.dialects.v20 import common as mavlink
logger = logging.getLogger('flix') logger = logging.getLogger('flix')
if not logger.hasHandlers(): if not logger.hasHandlers():
handler = logging.StreamHandler() handler = logging.StreamHandler()
handler.setFormatter(logging.Formatter('%(name)s: %(message)s')) handler.setFormatter(logging.Formatter('%(name)s - %(levelname)s - %(message)s'))
logger.addHandler(handler) logger.addHandler(handler)
logger.setLevel(logging.INFO) logger.setLevel(logging.INFO)
@@ -40,7 +40,7 @@ class Flix:
_connection_timeout = 3 _connection_timeout = 3
_print_buffer: str = '' _print_buffer: str = ''
_modes = ['RAW', 'ACRO', 'STAB', 'AUTO'] _modes = ['MANUAL', 'ACRO', 'STAB', 'AUTO']
def __init__(self, system_id: int=1, wait_connection: bool=True): def __init__(self, system_id: int=1, wait_connection: bool=True):
if not (0 <= system_id < 256): if not (0 <= system_id < 256):

View File

@@ -1,6 +1,6 @@
[project] [project]
name = "pyflix" name = "pyflix"
version = "0.11" version = "0.9"
description = "Python API for Flix drone" description = "Python API for Flix drone"
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }] authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
license = "MIT" license = "MIT"