mirror of
https://github.com/okalachev/flix.git
synced 2026-03-29 19:43:31 +00:00
Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
0908674a60 | ||
|
|
13a7e67b92 |
4
.github/workflows/build.yml
vendored
4
.github/workflows/build.yml
vendored
@@ -25,8 +25,8 @@ jobs:
|
||||
path: flix/build
|
||||
- name: Build firmware for ESP32-S3
|
||||
run: make BOARD=esp32:esp32:esp32s3
|
||||
- name: Build firmware for ESP32-C3
|
||||
run: make BOARD=esp32:esp32:esp32c3
|
||||
- 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
|
||||
run: tools/check_c_cpp_properties.py
|
||||
|
||||
|
||||
@@ -7,7 +7,6 @@
|
||||
"MD024": false,
|
||||
"MD033": false,
|
||||
"MD034": false,
|
||||
"MD040": false,
|
||||
"MD059": false,
|
||||
"MD044": {
|
||||
"html_elements": false,
|
||||
|
||||
54
.vscode/c_cpp_properties.json
vendored
54
.vscode/c_cpp_properties.json
vendored
@@ -6,18 +6,19 @@
|
||||
"${workspaceFolder}/flix",
|
||||
"${workspaceFolder}/gazebo",
|
||||
"${workspaceFolder}/tools/**",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
|
||||
"~/.arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
|
||||
"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
|
||||
"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
|
||||
"~/Arduino/libraries/**",
|
||||
"/usr/include/gazebo-11/",
|
||||
"/usr/include/ignition/math6/"
|
||||
],
|
||||
"forcedInclude": [
|
||||
"${workspaceFolder}/.vscode/intellisense.h",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
|
||||
"${workspaceFolder}/flix/cli.ino",
|
||||
"${workspaceFolder}/flix/control.ino",
|
||||
"${workspaceFolder}/flix/estimate.ino",
|
||||
@@ -30,10 +31,9 @@
|
||||
"${workspaceFolder}/flix/rc.ino",
|
||||
"${workspaceFolder}/flix/time.ino",
|
||||
"${workspaceFolder}/flix/wifi.ino",
|
||||
"${workspaceFolder}/flix/parameters.ino",
|
||||
"${workspaceFolder}/flix/safety.ino"
|
||||
"${workspaceFolder}/flix/parameters.ino"
|
||||
],
|
||||
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
|
||||
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++17",
|
||||
"defines": [
|
||||
@@ -53,18 +53,19 @@
|
||||
"name": "Mac",
|
||||
"includePath": [
|
||||
"${workspaceFolder}/flix",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
|
||||
"~/Library/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
|
||||
"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/include/**",
|
||||
"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
|
||||
"~/Documents/Arduino/libraries/**",
|
||||
"/opt/homebrew/include/gazebo-11/",
|
||||
"/opt/homebrew/include/ignition/math6/"
|
||||
],
|
||||
"forcedInclude": [
|
||||
"${workspaceFolder}/.vscode/intellisense.h",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
|
||||
"${workspaceFolder}/flix/flix.ino",
|
||||
"${workspaceFolder}/flix/cli.ino",
|
||||
"${workspaceFolder}/flix/control.ino",
|
||||
@@ -77,10 +78,9 @@
|
||||
"${workspaceFolder}/flix/rc.ino",
|
||||
"${workspaceFolder}/flix/time.ino",
|
||||
"${workspaceFolder}/flix/wifi.ino",
|
||||
"${workspaceFolder}/flix/parameters.ino",
|
||||
"${workspaceFolder}/flix/safety.ino"
|
||||
"${workspaceFolder}/flix/parameters.ino"
|
||||
],
|
||||
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
|
||||
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++17",
|
||||
"defines": [
|
||||
@@ -103,16 +103,17 @@
|
||||
"${workspaceFolder}/flix",
|
||||
"${workspaceFolder}/gazebo",
|
||||
"${workspaceFolder}/tools/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
|
||||
"~/Documents/Arduino/libraries/**"
|
||||
],
|
||||
"forcedInclude": [
|
||||
"${workspaceFolder}/.vscode/intellisense.h",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
|
||||
"${workspaceFolder}/flix/cli.ino",
|
||||
"${workspaceFolder}/flix/control.ino",
|
||||
"${workspaceFolder}/flix/estimate.ino",
|
||||
@@ -125,10 +126,9 @@
|
||||
"${workspaceFolder}/flix/rc.ino",
|
||||
"${workspaceFolder}/flix/time.ino",
|
||||
"${workspaceFolder}/flix/wifi.ino",
|
||||
"${workspaceFolder}/flix/parameters.ino",
|
||||
"${workspaceFolder}/flix/safety.ino"
|
||||
"${workspaceFolder}/flix/parameters.ino"
|
||||
],
|
||||
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++.exe",
|
||||
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++17",
|
||||
"defines": [
|
||||
|
||||
4
Makefile
4
Makefile
@@ -13,10 +13,10 @@ monitor:
|
||||
|
||||
dependencies .dependencies:
|
||||
arduino-cli core update-index --config-file arduino-cli.yaml
|
||||
arduino-cli core install esp32:esp32@3.3.6 --config-file arduino-cli.yaml
|
||||
arduino-cli core install esp32:esp32@3.2.0 --config-file arduino-cli.yaml
|
||||
arduino-cli lib update-index
|
||||
arduino-cli lib install "FlixPeriph"
|
||||
arduino-cli lib install "MAVLink"@2.0.25
|
||||
arduino-cli lib install "MAVLink"@2.0.16
|
||||
touch .dependencies
|
||||
|
||||
gazebo/build cmake: gazebo/CMakeLists.txt
|
||||
|
||||
15
README.md
15
README.md
@@ -1,9 +1,6 @@
|
||||
<!-- markdownlint-disable MD041 -->
|
||||
# Flix
|
||||
|
||||
<p align="center">
|
||||
<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>
|
||||
**Flix** (*flight + X*) — open source ESP32-based quadcopter made from scratch.
|
||||
|
||||
<table>
|
||||
<tr>
|
||||
@@ -21,13 +18,15 @@
|
||||
* Dedicated for education and research.
|
||||
* Made from general-purpose components.
|
||||
* 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.
|
||||
* Wi-Fi and MAVLink support.
|
||||
* Wireless command line interface and analyzing.
|
||||
* 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)).
|
||||
* *Position control (planned)*.
|
||||
* *Position control (using external camera) and autonomous flights¹*.
|
||||
|
||||
*¹ — planned.*
|
||||
|
||||
## It actually flies
|
||||
|
||||
|
||||
@@ -35,7 +35,7 @@
|
||||
|
||||
### Подсистема управления
|
||||
|
||||
Состояние органов управления обрабатывается в функции `interpretControls()` и преобразуется в **команду управления**, которая включает следующее:
|
||||
Состояние органов управления обрабатывается в функции `interpretControls()` и преобразуется в *команду управления*, которая включает следующее:
|
||||
|
||||
* `attitudeTarget` *(Quaternion)* — целевая ориентация дрона.
|
||||
* `ratesTarget` *(Vector)* — целевые угловые скорости, *рад/с*.
|
||||
|
||||
@@ -87,13 +87,13 @@ Flix поддерживает следующие модели IMU:
|
||||
#include <FlixPeriph.h>
|
||||
#include <SPI.h>
|
||||
|
||||
MPU9250 imu(SPI);
|
||||
MPU9250 IMU(SPI);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
bool success = imu.begin();
|
||||
bool success = IMU.begin();
|
||||
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 <SPI.h>
|
||||
|
||||
MPU9250 imu(SPI);
|
||||
MPU9250 IMU(SPI);
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
bool success = imu.begin();
|
||||
bool success = IMU.begin();
|
||||
if (!success) {
|
||||
Serial.println("Failed to initialize the IMU");
|
||||
Serial.println("Failed to initialize IMU");
|
||||
}
|
||||
}
|
||||
|
||||
void loop() {
|
||||
imu.waitForData();
|
||||
IMU.waitForData();
|
||||
|
||||
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);
|
||||
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
|
||||
IMU.setRate(IMU.RATE_1KHZ_APPROX);
|
||||
```
|
||||
|
||||
Поскольку не все поддерживаемые IMU могут работать строго на частоте 1 кГц, в библиотеке FlixPeriph существует возможность приближенной настройки частоты сэмплирования. Например, у IMU ICM-20948 при такой настройке реальная частота сэмплирования будет равна 1125 Гц.
|
||||
Поскольку не все поддерживаемые IMU могут работать строго на частоте 1 кГц, в библиотеке FlixPeriph существует возможность приближенной настройки частоты сэмплов. Например, у IMU ICM-20948 при такой настройке реальная частота сэмплирования будет равна 1125 Гц.
|
||||
|
||||
Другие доступные для установки в библиотеке FlixPeriph частоты сэмплирования:
|
||||
|
||||
* `RATE_MIN` — минимальная частота для конкретного IMU.
|
||||
* `RATE_MIN` — минимальная частота сэмплов для конкретного IMU.
|
||||
* `RATE_50HZ_APPROX` — значение, близкое к 50 Гц.
|
||||
* `RATE_1KHZ_APPROX` — значение, близкое к 1 кГц.
|
||||
* `RATE_8KHZ_APPROX` — значение, близкое к 8 кГц.
|
||||
* `RATE_MAX` — максимальная частота для конкретного IMU.
|
||||
* `RATE_MAX` — максимальная частота сэмплов для конкретного IMU.
|
||||
|
||||
#### Диапазон измерений
|
||||
|
||||
Большинство MEMS-гироскопов поддерживают несколько диапазонов измерений угловой скорости. Главное преимущество выбора меньшего диапазона — бо́льшая чувствительность. В полетных контроллерах обычно выбирается максимальный диапазон измерений от –2000 до 2000 градусов в секунду, чтобы обеспечить возможность быстрых маневров.
|
||||
Большинство MEMS-гироскопов поддерживают несколько диапазонов измерений угловой скорости. Главное преимущество выбора меньшего диапазона — бо́льшая чувствительность. В полетных контроллерах обычно выбирается максимальный диапазон измерений от –2000 до 2000 градусов в секунду, чтобы обеспечить возможность динамичных маневров.
|
||||
|
||||
В библиотеке FlixPeriph диапазон измерений гироскопа устанавливается методом `setGyroRange()`:
|
||||
|
||||
```cpp
|
||||
imu.setGyroRange(imu.GYRO_RANGE_2000DPS);
|
||||
IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS);
|
||||
```
|
||||
|
||||
### LPF-фильтр
|
||||
@@ -172,7 +172,7 @@ imu.setGyroRange(imu.GYRO_RANGE_2000DPS);
|
||||
IMU InvenSense могут фильтровать измерения на аппаратном уровне при помощи фильтра нижних частот (LPF). Flix реализует собственный фильтр для гироскопа, чтобы иметь больше гибкости при поддержке разных IMU. Поэтому для встроенного LPF устанавливается максимальная частота среза:
|
||||
|
||||
```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 \\]
|
||||
|
||||
Для точной работы подсистемы оценки ориентации и управления дроном необходимо оценить *bias* гироскопа и учесть его в вычислениях. Для этого при запуске программы производится калибровка гироскопа, которая реализована в функции `calibrateGyro()`. Эта функция считывает данные с гироскопа в состоянии покоя 1000 раз и усредняет их. Полученные значения считаются *bias* гироскопа и в дальнейшем вычитаются из измерений.
|
||||
Для качественной работы подсистемы оценки ориентации и управления дроном необходимо оценить *bias* гироскопа и учесть его в вычислениях. Для этого при запуске программы производится калибровка гироскопа, которая реализована в функции `calibrateGyro()`. Эта функция считывает данные с гироскопа в состоянии покоя 1000 раз и усредняет их. Полученные значения считаются *bias* гироскопа и в дальнейшем вычитаются из измерений.
|
||||
|
||||
Программа для вывода данных с гироскопа с калибровкой:
|
||||
|
||||
@@ -189,23 +189,23 @@ imu.setDLPF(imu.DLPF_MAX);
|
||||
#include <FlixPeriph.h>
|
||||
#include <SPI.h>
|
||||
|
||||
MPU9250 imu(SPI);
|
||||
MPU9250 IMU(SPI);
|
||||
|
||||
float gyroBiasX, gyroBiasY, gyroBiasZ; // bias гироскопа
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
bool success = imu.begin();
|
||||
bool success = IMU.begin();
|
||||
if (!success) {
|
||||
Serial.println("Failed to initialize the IMU");
|
||||
Serial.println("Failed to initialize IMU");
|
||||
}
|
||||
calibrateGyro();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
float gx, gy, gz;
|
||||
imu.waitForData();
|
||||
imu.getGyro(gx, gy, gz);
|
||||
IMU.waitForData();
|
||||
IMU.getGyro(gx, gy, gz);
|
||||
|
||||
// Устранение bias гироскопа
|
||||
gx -= gyroBiasX;
|
||||
@@ -226,9 +226,9 @@ void calibrateGyro() {
|
||||
|
||||
// Получение 1000 измерений гироскопа
|
||||
for (int i = 0; i < samples; i++) {
|
||||
imu.waitForData();
|
||||
IMU.waitForData();
|
||||
float gx, gy, gz;
|
||||
imu.getGyro(gx, gy, gz);
|
||||
IMU.getGyro(gx, gy, gz);
|
||||
gyroBiasX += gx;
|
||||
gyroBiasY += gy;
|
||||
gyroBiasZ += gz;
|
||||
|
||||
@@ -38,13 +38,13 @@ Utility files:
|
||||
|
||||
### 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.
|
||||
* `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].
|
||||
* `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`.
|
||||
|
||||
@@ -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.
|
||||
|
||||
> [!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
|
||||
|
||||
See build instructions in [usage.md](usage.md).
|
||||
|
||||
@@ -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 |
@@ -4,7 +4,7 @@
|
||||
|
||||
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 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.
|
||||
* **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 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**. Perform the following commands using Serial Monitor:
|
||||
* `mfr` — should rotate front right motor (counter-clockwise).
|
||||
|
||||
@@ -20,10 +20,10 @@ You can build and upload the firmware using either **Arduino IDE** (easier for b
|
||||
|
||||
1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
|
||||
2. *Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers).*
|
||||
3. Install ESP32 core, version 3.3.6. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
|
||||
3. Install ESP32 core, version 3.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
|
||||
4. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
|
||||
* `FlixPeriph`, the latest version.
|
||||
* `MAVLink`, version 2.0.25.
|
||||
* `MAVLink`, version 2.0.16.
|
||||
5. Open the `flix/flix.ino` sketch from downloaded firmware sources in Arduino IDE.
|
||||
6. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port.
|
||||
7. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
|
||||
@@ -80,7 +80,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).
|
||||
2. Power up the drone.
|
||||
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
|
||||
|
||||
@@ -108,13 +108,11 @@ The drone is configured using parameters. To access and modify them, go to the Q
|
||||
|
||||
<img src="img/parameters.png" width="400">
|
||||
|
||||
You can also work with parameters using `p` command in the console. Parameter names are case-insensitive.
|
||||
|
||||
### 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:
|
||||
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 side with the components:
|
||||
|
||||
<img src="img/imu-axes.png" width="200">
|
||||
|
||||
@@ -122,10 +120,10 @@ 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|
|
||||
|<img src="img/imu-rot-1.png" width="200">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0 |<img src="img/imu-rot-5.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0|
|
||||
|<img src="img/imu-rot-2.png" width="200">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|<img src="img/imu-rot-6.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|
|
||||
|<img src="img/imu-rot-3.png" width="200">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|<img src="img/imu-rot-7.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|
|
||||
|<img src="img/imu-rot-4.png" width="200"><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="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|
|
||||
|
||||
### Calibrate accelerometer
|
||||
|
||||
@@ -143,9 +141,9 @@ Before flight you need to calibrate the accelerometer:
|
||||
* 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.
|
||||
|
||||
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!**
|
||||
|
||||
@@ -154,10 +152,6 @@ Before flight you need to calibrate the accelerometer:
|
||||
* `mrl` — should rotate rear left motor (counter-clockwise).
|
||||
* `mrr` — should rotate rear right motor (clockwise).
|
||||
|
||||
Rotation diagram:
|
||||
|
||||
<img src="img/motors.svg" width=200>
|
||||
|
||||
> [!WARNING]
|
||||
> Never run the motors when powering the drone from USB, always use the battery for that.
|
||||
|
||||
@@ -165,7 +159,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).
|
||||
|
||||
### 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.
|
||||
2. Power the drone using the battery.
|
||||
@@ -177,7 +171,7 @@ There are several ways to control the drone's flight: using **smartphone** (Wi-F
|
||||
> [!TIP]
|
||||
> Decrease `CTL_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:
|
||||
|
||||
@@ -185,7 +179,7 @@ Before using remote SBUS-connected remote control, you need to calibrate it:
|
||||
2. Type `cr` command and follow the instructions.
|
||||
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.
|
||||
|
||||
@@ -243,43 +237,11 @@ 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.
|
||||
|
||||
## Wi-Fi configuration
|
||||
|
||||
You can configure the Wi-Fi using parameters and console commands.
|
||||
|
||||
The Wi-Fi mode is chosen using `WIFI_MODE` parameter in QGroundControl or in the console:
|
||||
|
||||
* `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
|
||||
```
|
||||
|
||||
Disabling Wi-Fi:
|
||||
|
||||
```
|
||||
p WIFI_MODE 0
|
||||
```
|
||||
<img src="img/parameters.png" width="400">
|
||||
|
||||
## 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
|
||||
make log
|
||||
|
||||
@@ -16,7 +16,7 @@ Author: [goldarte](https://t.me/goldarte).<br>
|
||||
|
||||
## 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.
|
||||
Special quadcopter design and engineering course took place in october-november 2025 in School 548, Moscow. Course included UAV control theory, electronics, and practical drone assembly and setup 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">
|
||||
|
||||
@@ -25,7 +25,7 @@ STL files and other materials: see [here](https://drive.google.com/drive/folders
|
||||
### Selected works
|
||||
|
||||
Author: [KiraFlux](https://t.me/@kiraflux_0XC0000005).<br>
|
||||
Description: **custom ESPNOW remote control** was implemented, modified firmware to support ESPNOW protocol.<br>
|
||||
Description: **custom ESPNOW remote control** is implemented, firmware modified 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>
|
||||
|
||||
18
flix/cli.ino
18
flix/cli.ino
@@ -11,7 +11,7 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO
|
||||
extern const int RAW, ACRO, STAB, AUTO;
|
||||
extern float t, dt, loopRate;
|
||||
extern uint16_t channels[16];
|
||||
extern float controlTime;
|
||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
||||
extern int mode;
|
||||
extern bool armed;
|
||||
|
||||
@@ -38,12 +38,11 @@ const char* motd =
|
||||
"raw/stab/acro/auto - set mode\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"
|
||||
"log [dump] - print log header [and data]\n"
|
||||
"cr - calibrate RC\n"
|
||||
"ca - calibrate accel\n"
|
||||
"cl - calibrate level\n"
|
||||
"mfr, mfl, mrr, mrl - test motor (remove props)\n"
|
||||
"sys - show system info\n"
|
||||
"reset - reset drone's state\n"
|
||||
@@ -56,7 +55,9 @@ void print(const char* format, ...) {
|
||||
vsnprintf(buf, sizeof(buf), format, args);
|
||||
va_end(args);
|
||||
Serial.print(buf);
|
||||
#if WIFI_ENABLED
|
||||
mavlinkPrint(buf);
|
||||
#endif
|
||||
}
|
||||
|
||||
void pause(float duration) {
|
||||
@@ -64,7 +65,9 @@ void pause(float duration) {
|
||||
while (t - start < duration) {
|
||||
step();
|
||||
handleInput();
|
||||
#if WIFI_ENABLED
|
||||
processMavlink();
|
||||
#endif
|
||||
delay(50);
|
||||
}
|
||||
}
|
||||
@@ -130,15 +133,12 @@ void doCommand(String str, bool echo = false) {
|
||||
}
|
||||
print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n",
|
||||
controlRoll, controlPitch, controlYaw, controlThrottle, controlMode);
|
||||
print("time: %.1f\n", controlTime);
|
||||
print("mode: %s\n", getModeName());
|
||||
print("armed: %d\n", armed);
|
||||
} else if (command == "wifi") {
|
||||
#if WIFI_ENABLED
|
||||
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());
|
||||
#endif
|
||||
} else if (command == "mot") {
|
||||
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]);
|
||||
@@ -149,6 +149,8 @@ void doCommand(String str, bool echo = false) {
|
||||
calibrateRC();
|
||||
} else if (command == "ca") {
|
||||
calibrateAccel();
|
||||
} else if (command == "cl") {
|
||||
calibrateLevel();
|
||||
} else if (command == "mfr") {
|
||||
testMotor(MOTOR_FRONT_RIGHT);
|
||||
} else if (command == "mfl") {
|
||||
|
||||
@@ -38,12 +38,6 @@ const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
|
||||
int mode = STAB;
|
||||
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 pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
|
||||
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);
|
||||
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 float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
||||
|
||||
|
||||
@@ -8,10 +8,6 @@
|
||||
#include "lpf.h"
|
||||
#include "util.h"
|
||||
|
||||
Vector rates; // estimated angular rates, rad/s
|
||||
Quaternion attitude; // estimated attitude
|
||||
bool landed;
|
||||
|
||||
float accWeight = 0.003;
|
||||
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
|
||||
|
||||
|
||||
@@ -7,14 +7,18 @@
|
||||
#include "quaternion.h"
|
||||
#include "util.h"
|
||||
|
||||
#define WIFI_ENABLED 1
|
||||
|
||||
extern float t, dt;
|
||||
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
||||
extern Vector gyro, acc;
|
||||
extern Vector rates;
|
||||
extern Quaternion attitude;
|
||||
extern bool landed;
|
||||
extern float motors[4];
|
||||
float t = NAN; // current step time, s
|
||||
float dt; // time delta from previous step, s
|
||||
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
||||
float controlMode = NAN;
|
||||
Vector gyro; // gyroscope data
|
||||
Vector acc; // accelerometer data, m/s/s
|
||||
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() {
|
||||
Serial.begin(115200);
|
||||
@@ -24,7 +28,9 @@ void setup() {
|
||||
setupLED();
|
||||
setupMotors();
|
||||
setLED(true);
|
||||
#if WIFI_ENABLED
|
||||
setupWiFi();
|
||||
#endif
|
||||
setupIMU();
|
||||
setupRC();
|
||||
setLED(false);
|
||||
@@ -39,7 +45,9 @@ void loop() {
|
||||
control();
|
||||
sendMotors();
|
||||
handleInput();
|
||||
#if WIFI_ENABLED
|
||||
processMavlink();
|
||||
#endif
|
||||
logData();
|
||||
syncParameters();
|
||||
}
|
||||
|
||||
13
flix/imu.ino
13
flix/imu.ino
@@ -12,12 +12,9 @@
|
||||
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 accScale(1, 1, 1);
|
||||
Vector gyroBias;
|
||||
|
||||
void setupIMU() {
|
||||
print("Setup IMU\n");
|
||||
@@ -110,6 +107,14 @@ void calibrateAccelOnce() {
|
||||
accBias = (accMax + accMin) / 2;
|
||||
}
|
||||
|
||||
void calibrateLevel() {
|
||||
print("Place perfectly level [1 sec]\n");
|
||||
pause(1);
|
||||
Quaternion correction = Quaternion::fromBetweenVectors(Quaternion::rotateVector(Vector(0, 0, 1), attitude), Vector(0, 0, 1));
|
||||
imuRotation = Quaternion::rotate(correction, Quaternion::fromEuler(imuRotation)).toEuler();
|
||||
print("✓ Done: %.3f %.3f %.3f\n", degrees(imuRotation.x), degrees(imuRotation.y), degrees(imuRotation.z));
|
||||
}
|
||||
|
||||
void printIMUCalibration() {
|
||||
print("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
|
||||
print("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);
|
||||
|
||||
@@ -3,16 +3,20 @@
|
||||
|
||||
// MAVLink communication
|
||||
|
||||
#if WIFI_ENABLED
|
||||
|
||||
#include <MAVLink.h>
|
||||
#include "util.h"
|
||||
|
||||
extern float controlTime;
|
||||
#define SYSTEM_ID 1
|
||||
#define MAVLINK_RATE_SLOW 1
|
||||
#define MAVLINK_RATE_FAST 10
|
||||
|
||||
bool mavlinkConnected = false;
|
||||
String mavlinkPrintBuffer;
|
||||
int mavlinkSysId = 1;
|
||||
Rate telemetryFast(10);
|
||||
Rate telemetrySlow(2);
|
||||
|
||||
extern float controlTime;
|
||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
||||
|
||||
void processMavlink() {
|
||||
sendMavlink();
|
||||
@@ -25,8 +29,10 @@ void sendMavlink() {
|
||||
mavlink_message_t msg;
|
||||
uint32_t time = t * 1000;
|
||||
|
||||
if (telemetrySlow) {
|
||||
mavlink_msg_heartbeat_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
|
||||
static Rate slow(MAVLINK_RATE_SLOW), fast(MAVLINK_RATE_FAST);
|
||||
|
||||
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) |
|
||||
((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) |
|
||||
((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
|
||||
@@ -35,27 +41,27 @@ void sendMavlink() {
|
||||
|
||||
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);
|
||||
sendMessage(&msg);
|
||||
}
|
||||
|
||||
if (telemetryFast && mavlinkConnected) {
|
||||
if (fast && mavlinkConnected) {
|
||||
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
|
||||
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);
|
||||
if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
|
||||
|
||||
float controls[8];
|
||||
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);
|
||||
|
||||
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
|
||||
gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
|
||||
0, 0, 0, 0);
|
||||
@@ -90,7 +96,7 @@ void handleMavlink(const void *_msg) {
|
||||
if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
|
||||
mavlink_manual_control_t 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;
|
||||
controlPitch = m.x / 1000.0f;
|
||||
@@ -103,11 +109,11 @@ void handleMavlink(const void *_msg) {
|
||||
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
|
||||
mavlink_param_request_list_t 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;
|
||||
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);
|
||||
sendMessage(&msg);
|
||||
}
|
||||
@@ -116,7 +122,7 @@ void handleMavlink(const void *_msg) {
|
||||
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
|
||||
mavlink_param_request_read_t 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];
|
||||
strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
|
||||
@@ -125,7 +131,7 @@ void handleMavlink(const void *_msg) {
|
||||
memcpy(name, getParameterName(m.param_index), 16);
|
||||
}
|
||||
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);
|
||||
sendMessage(&msg);
|
||||
}
|
||||
@@ -133,15 +139,14 @@ void handleMavlink(const void *_msg) {
|
||||
if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) {
|
||||
mavlink_param_set_t 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];
|
||||
strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated
|
||||
bool success = setParameter(name, m.param_value);
|
||||
if (!success) return;
|
||||
setParameter(name, m.param_value);
|
||||
// send ack
|
||||
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
|
||||
sendMessage(&msg);
|
||||
}
|
||||
@@ -149,17 +154,17 @@ void handleMavlink(const void *_msg) {
|
||||
if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
|
||||
mavlink_mission_request_list_t 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_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);
|
||||
}
|
||||
|
||||
if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {
|
||||
mavlink_serial_control_t 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];
|
||||
strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated
|
||||
@@ -171,7 +176,7 @@ void handleMavlink(const void *_msg) {
|
||||
|
||||
mavlink_set_attitude_target_t 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
|
||||
ratesTarget.x = m.body_roll_rate;
|
||||
@@ -193,7 +198,7 @@ void handleMavlink(const void *_msg) {
|
||||
|
||||
mavlink_set_actuator_control_target_t 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();
|
||||
ratesTarget.invalidate();
|
||||
@@ -205,12 +210,12 @@ void handleMavlink(const void *_msg) {
|
||||
if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
|
||||
mavlink_log_request_data_t 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
|
||||
for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) {
|
||||
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]);
|
||||
sendMessage(&msg);
|
||||
}
|
||||
@@ -220,13 +225,13 @@ void handleMavlink(const void *_msg) {
|
||||
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
|
||||
mavlink_command_long_t 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;
|
||||
bool accepted = false;
|
||||
|
||||
if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
|
||||
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);
|
||||
sendMessage(&response);
|
||||
}
|
||||
@@ -245,7 +250,7 @@ void handleMavlink(const void *_msg) {
|
||||
|
||||
// send command 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);
|
||||
}
|
||||
}
|
||||
@@ -262,7 +267,7 @@ void sendMavlinkPrint() {
|
||||
char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
|
||||
strlcpy(data, str + i, sizeof(data));
|
||||
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,
|
||||
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);
|
||||
@@ -270,3 +275,5 @@ void sendMavlinkPrint() {
|
||||
}
|
||||
mavlinkPrintBuffer.clear();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@@ -17,8 +17,7 @@
|
||||
#define PWM_MIN 0
|
||||
#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_RIGHT = 1;
|
||||
const int MOTOR_FRONT_RIGHT = 2;
|
||||
|
||||
@@ -9,20 +9,13 @@
|
||||
extern float channelZero[16];
|
||||
extern float channelMax[16];
|
||||
extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
|
||||
extern int wifiMode, udpLocalPort, udpRemotePort;
|
||||
extern float rcLossTimeout, descendTime;
|
||||
|
||||
Preferences storage;
|
||||
|
||||
struct Parameter {
|
||||
const char *name; // max length is 15 (Preferences key limit)
|
||||
bool integer;
|
||||
union { float *f; int *i; }; // pointer to variable
|
||||
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; };
|
||||
float *variable;
|
||||
float value; // cache
|
||||
};
|
||||
|
||||
Parameter parameters[] = {
|
||||
@@ -84,17 +77,6 @@ Parameter parameters[] = {
|
||||
{"RC_THROTTLE", &throttleChannel},
|
||||
{"RC_YAW", &yawChannel},
|
||||
{"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},
|
||||
// safety
|
||||
{"SF_RC_LOSS_TIME", &rcLossTimeout},
|
||||
{"SF_DESCEND_TIME", &descendTime},
|
||||
};
|
||||
|
||||
void setupParameters() {
|
||||
@@ -102,10 +84,10 @@ void setupParameters() {
|
||||
// Read parameters from storage
|
||||
for (auto ¶meter : parameters) {
|
||||
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.cache = parameter.getValue();
|
||||
*parameter.variable = storage.getFloat(parameter.name, *parameter.variable);
|
||||
parameter.value = *parameter.variable;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -120,13 +102,13 @@ const char *getParameterName(int index) {
|
||||
|
||||
float getParameter(int index) {
|
||||
if (index < 0 || index >= parametersCount()) return NAN;
|
||||
return parameters[index].getValue();
|
||||
return *parameters[index].variable;
|
||||
}
|
||||
|
||||
float getParameter(const char *name) {
|
||||
for (auto ¶meter : parameters) {
|
||||
if (strcasecmp(parameter.name, name) == 0) {
|
||||
return parameter.getValue();
|
||||
if (strcmp(parameter.name, name) == 0) {
|
||||
return *parameter.variable;
|
||||
}
|
||||
}
|
||||
return NAN;
|
||||
@@ -134,9 +116,8 @@ float getParameter(const char *name) {
|
||||
|
||||
bool setParameter(const char *name, const float value) {
|
||||
for (auto ¶meter : parameters) {
|
||||
if (strcasecmp(parameter.name, name) == 0) {
|
||||
if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
|
||||
parameter.setValue(value);
|
||||
if (strcmp(parameter.name, name) == 0) {
|
||||
*parameter.variable = value;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -149,18 +130,16 @@ void syncParameters() {
|
||||
if (motorsActive()) return; // don't use flash while flying, it may cause a delay
|
||||
|
||||
for (auto ¶meter : parameters) {
|
||||
if (parameter.getValue() == parameter.cache) continue; // no change
|
||||
if (isnan(parameter.getValue()) && isnan(parameter.cache)) continue; // both are NaN
|
||||
if (isinf(parameter.getValue()) && isinf(parameter.cache)) continue; // both are Inf
|
||||
|
||||
storage.putFloat(parameter.name, parameter.getValue());
|
||||
parameter.cache = parameter.getValue(); // update cache
|
||||
if (parameter.value == *parameter.variable) continue;
|
||||
if (isnan(parameter.value) && isnan(*parameter.variable)) continue; // handle NAN != NAN
|
||||
storage.putFloat(parameter.name, *parameter.variable);
|
||||
parameter.value = *parameter.variable;
|
||||
}
|
||||
}
|
||||
|
||||
void printParameters() {
|
||||
for (auto ¶meter : parameters) {
|
||||
print("%s = %g\n", parameter.name, parameter.getValue());
|
||||
print("%s = %g\n", parameter.name, *parameter.variable);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
23
flix/rc.ino
23
flix/rc.ino
@@ -6,21 +6,14 @@
|
||||
#include <SBUS.h>
|
||||
#include "util.h"
|
||||
|
||||
#ifdef ESP32C3
|
||||
SBUS rc(Serial1);
|
||||
#else
|
||||
SBUS rc(Serial2);
|
||||
#endif
|
||||
SBUS rc(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
|
||||
|
||||
uint16_t channels[16]; // raw rc channels
|
||||
float controlTime; // time of the last controls update
|
||||
float channelZero[16]; // calibration zero values
|
||||
float channelMax[16]; // calibration max values
|
||||
|
||||
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
||||
float controlMode = NAN;
|
||||
float controlTime = NAN; // time of the last controls update
|
||||
|
||||
// Channels mapping (nan means not assigned):
|
||||
// Channels mapping (using float to store in parameters):
|
||||
float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
|
||||
|
||||
void setupRC() {
|
||||
@@ -45,11 +38,11 @@ void normalizeRC() {
|
||||
controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
|
||||
}
|
||||
// Update control values
|
||||
controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;
|
||||
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
|
||||
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
|
||||
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
|
||||
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
|
||||
controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : NAN;
|
||||
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
|
||||
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
|
||||
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
|
||||
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
|
||||
}
|
||||
|
||||
void calibrateRC() {
|
||||
|
||||
@@ -3,12 +3,12 @@
|
||||
|
||||
// Fail-safe functions
|
||||
|
||||
#define RC_LOSS_TIMEOUT 1
|
||||
#define DESCEND_TIME 10
|
||||
|
||||
extern float controlTime;
|
||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw;
|
||||
|
||||
float rcLossTimeout = 1;
|
||||
float descendTime = 10;
|
||||
|
||||
void failsafe() {
|
||||
rcLossFailsafe();
|
||||
autoFailsafe();
|
||||
@@ -16,8 +16,9 @@ void failsafe() {
|
||||
|
||||
// RC loss failsafe
|
||||
void rcLossFailsafe() {
|
||||
if (controlTime == 0) return; // no RC at all
|
||||
if (!armed) return;
|
||||
if (t - controlTime > rcLossTimeout) {
|
||||
if (t - controlTime > RC_LOSS_TIMEOUT) {
|
||||
descend();
|
||||
}
|
||||
}
|
||||
@@ -26,7 +27,7 @@ void rcLossFailsafe() {
|
||||
void descend() {
|
||||
mode = AUTO;
|
||||
attitudeTarget = Quaternion();
|
||||
thrustTarget -= dt / descendTime;
|
||||
thrustTarget -= dt / DESCEND_TIME;
|
||||
if (thrustTarget < 0) {
|
||||
thrustTarget = 0;
|
||||
armed = false;
|
||||
|
||||
@@ -3,8 +3,6 @@
|
||||
|
||||
// Time related functions
|
||||
|
||||
float t = NAN; // current time, s
|
||||
float dt; // time delta with the previous step, s
|
||||
float loopRate; // Hz
|
||||
|
||||
void step() {
|
||||
|
||||
@@ -35,6 +35,7 @@ public:
|
||||
z = NAN;
|
||||
}
|
||||
|
||||
|
||||
float norm() const {
|
||||
return sqrt(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
@@ -1,76 +1,49 @@
|
||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||
// Repository: https://github.com/okalachev/flix
|
||||
|
||||
// Wi-Fi communication
|
||||
// Wi-Fi support
|
||||
|
||||
#if WIFI_ENABLED
|
||||
|
||||
#include <WiFi.h>
|
||||
#include <WiFiAP.h>
|
||||
#include <WiFiUdp.h>
|
||||
#include "Preferences.h"
|
||||
|
||||
extern Preferences storage; // use the main preferences storage
|
||||
|
||||
const int W_DISABLED = 0, W_AP = 1, W_STA = 2;
|
||||
int wifiMode = W_AP;
|
||||
int udpLocalPort = 14550;
|
||||
int udpRemotePort = 14550;
|
||||
IPAddress udpRemoteIP = "255.255.255.255";
|
||||
#define WIFI_SSID "flix"
|
||||
#define WIFI_PASSWORD "flixwifi"
|
||||
#define WIFI_UDP_PORT 14550
|
||||
#define WIFI_UDP_REMOTE_PORT 14550
|
||||
#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
|
||||
|
||||
WiFiUDP udp;
|
||||
|
||||
void setupWiFi() {
|
||||
print("Setup Wi-Fi\n");
|
||||
if (wifiMode == W_AP) {
|
||||
WiFi.softAP(storage.getString("WIFI_AP_SSID", "flix").c_str(), storage.getString("WIFI_AP_PASS", "flixwifi").c_str());
|
||||
} else if (wifiMode == W_STA) {
|
||||
WiFi.begin(storage.getString("WIFI_STA_SSID", "").c_str(), storage.getString("WIFI_STA_PASS", "").c_str());
|
||||
}
|
||||
udp.begin(udpLocalPort);
|
||||
WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
|
||||
udp.begin(WIFI_UDP_PORT);
|
||||
}
|
||||
|
||||
void sendWiFi(const uint8_t *buf, int len) {
|
||||
if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return;
|
||||
udp.beginPacket(udpRemoteIP, udpRemotePort);
|
||||
if (WiFi.softAPIP() == IPAddress(0, 0, 0, 0) && WiFi.status() != WL_CONNECTED) return;
|
||||
udp.beginPacket(udp.remoteIP() ? udp.remoteIP() : WIFI_UDP_REMOTE_ADDR, WIFI_UDP_REMOTE_PORT);
|
||||
udp.write(buf, len);
|
||||
udp.endPacket();
|
||||
}
|
||||
|
||||
int receiveWiFi(uint8_t *buf, int len) {
|
||||
udp.parsePacket();
|
||||
if (udp.remoteIP()) udpRemoteIP = udp.remoteIP();
|
||||
return udp.read(buf, len);
|
||||
}
|
||||
|
||||
void printWiFiInfo() {
|
||||
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("MAC: %s\n", WiFi.softAPmacAddress().c_str());
|
||||
print("SSID: %s\n", WiFi.softAPSSID().c_str());
|
||||
print("Password: %s\n", WIFI_PASSWORD);
|
||||
print("Clients: %d\n", WiFi.softAPgetStationNum());
|
||||
print("Status: %d\n", WiFi.status());
|
||||
print("IP: %s\n", WiFi.softAPIP().toString().c_str());
|
||||
print("Remote IP: %s\n", udp.remoteIP().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");
|
||||
}
|
||||
#endif
|
||||
|
||||
@@ -10,15 +10,19 @@
|
||||
#include "Arduino.h"
|
||||
#include "wifi.h"
|
||||
|
||||
extern float t, dt;
|
||||
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
||||
extern Vector rates;
|
||||
extern Quaternion attitude;
|
||||
extern bool landed;
|
||||
extern float motors[4];
|
||||
#define WIFI_ENABLED 1
|
||||
|
||||
Vector gyro, acc, imuRotation;
|
||||
Vector accBias, gyroBias, accScale(1, 1, 1);
|
||||
float t = NAN;
|
||||
float dt;
|
||||
float motors[4];
|
||||
float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
|
||||
float controlMode = NAN;
|
||||
Vector acc;
|
||||
Vector gyro;
|
||||
Vector rates;
|
||||
Quaternion attitude;
|
||||
bool landed;
|
||||
Vector imuRotation;
|
||||
|
||||
// declarations
|
||||
void step();
|
||||
@@ -68,7 +72,8 @@ void resetParameters();
|
||||
void setLED(bool on) {};
|
||||
void calibrateGyro() { print("Skip gyro calibrating\n"); };
|
||||
void calibrateAccel() { print("Skip accel calibrating\n"); };
|
||||
void calibrateLevel() { print("Skip level calibrating\n"); };
|
||||
void printIMUCalibration() { print("cal: N/A\n"); };
|
||||
void printIMUInfo() {};
|
||||
void printWiFiInfo() {};
|
||||
void configWiFi(bool, const char*, const char*) { print("Skip WiFi config\n"); };
|
||||
Vector accBias, gyroBias, accScale(1, 1, 1);
|
||||
|
||||
@@ -43,7 +43,6 @@ records = [record for record in records if record[0] != 0]
|
||||
|
||||
print(f'Received records: {len(records)}')
|
||||
|
||||
os.makedirs(f'{DIR}/log', exist_ok=True)
|
||||
log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
|
||||
log.write(header.encode() + b'\n')
|
||||
for record in records:
|
||||
|
||||
@@ -95,7 +95,7 @@ Full list of events:
|
||||
|`armed`|Armed state update|Armed state (*bool*)|
|
||||
|`mode`|Flight mode update|Flight mode (*str*)|
|
||||
|`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_euler`|Attitude update|Euler angles (*list*)|
|
||||
|`rates`|Angular rates update|Angular rates (*list*)|
|
||||
@@ -112,7 +112,7 @@ Full list of events:
|
||||
> [!NOTE]
|
||||
> Update events trigger on every new piece of 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:
|
||||
|
||||
|
||||
Reference in New Issue
Block a user