18 Commits

Author SHA1 Message Date
Oleg Kalachev
f8f746b0cd Add level calibration 2026-01-30 07:49:26 +03:00
Oleg Kalachev
3dde380bb7 Add parameters for list of modes bound to rc switch
Parameters: CTL_FLT_MODE_0, CTL_FLT_MODE_1, CTL_FLT_MODE_2.
Also fix a bug with incorrect choosing the mode from controlMode.
2026-01-27 16:38:20 +03:00
Oleg Kalachev
377b21429b Fix error when launching the sim
Also make the parameters WIFI_LOC_PORT and WIFI_REM_PORT work in the sim.
2026-01-27 16:32:52 +03:00
Oleg Kalachev
1ac443d6f8 Add a build by Arky Matsekh 2026-01-27 15:17:58 +03:00
Oleg Kalachev
964c0f7bc1 Make setting parameter in console printing actual parameter value.
In some cases, it would not be equal to the requested value.
2026-01-27 09:28:01 +03:00
Oleg Kalachev
40bdaacedb Make motor subsystem configurable using parameters
Motor pins: MOT_PIN_FL, MOT_PIN_FR, MOT_PIN_RL, MOT_PIN_RR.
PWM configuration: MOT_PWM_FREQ, MOT_PWM_RES, MOT_PWM_STOP, MOT_PWM_MIN, MOT_PWM_MAX.
MOT_PWM_MAX = -1 chooses duty cycle mode for brushed motors (default).
2026-01-27 08:40:52 +03:00
Oleg Kalachev
7d74f3d5cd Minor docs fixes 2026-01-27 07:21:21 +03:00
Oleg Kalachev
9fd35ba361 Simplify lpf filter code
Begin with zero instead of the initializing value, as the latter doesn't make much sense in practice, but complicates the code much.
2026-01-24 09:43:46 +03:00
Oleg Kalachev
ca50f75576 Various minor fixes 2026-01-24 09:34:16 +03:00
Oleg Kalachev
e47a31f981 Fix mavlink parameter set acknowledgement value
If the parameter is integer the acknowledgement should contain the rounded value.
2026-01-24 09:32:49 +03:00
Oleg Kalachev
7ad3022798 Add parameter for configuring gyro bias lpf
+ reset the filter on `reset` command
2026-01-24 09:31:32 +03:00
Oleg Kalachev
5b654e4d8e Update ESP32-Core to 3.3.6 2026-01-23 02:41:43 +03:00
Oleg Kalachev
cf10ec6161 Update MAVLink-Arduino to 2.0.16 2026-01-23 01:11:35 +03:00
Oleg Kalachev
6d01cd2e79 Make failsafe configurable using parameters
SF_RC_LOSS_TIME - time without rc to activate failsafe.
SD_DESCEND_TIME - total time to decrease the throttle to zero.
Make controlTime nan on the start to simplify the logic.
2026-01-22 23:57:52 +03:00
Oleg Kalachev
0abb18c616 Make parameter names case-insensitive
+ minor fix
2026-01-22 23:11:47 +03:00
Oleg Kalachev
30326a5662 Add parameters for configuring the mavlink subsystem
MAV_SYS_ID - mavlink system id.
MAV_RATE_SLOW - rate of slow telemetry (e. g. heartbeats).
MAV_RATE_FAST - rate of fast telemetry (e. g. attitude, imu data).
2026-01-22 23:04:45 +03:00
Oleg Kalachev
dd3575174b Add wifi configuration using parameters and cli
Add console commands to setup wifi.
Add a parameter for choosing between STA and AP mode.
Add parameters for udp ports.
Remove WIFI_ENABLED macro.
2026-01-22 22:58:43 +03:00
Oleg Kalachev
c0f3301da4 Support integer parameters in addition to floats
The variable pointer is stored as a union field.
If `.integer` field is true, then integer pointer should be used.
Interfaces to parameters (cli and mavlink) keep working using floats.
Setting a non-finite value to int parameter will cause an error.
`.value` field is renamed to `.cache`.
2026-01-22 22:54:05 +03:00
30 changed files with 328 additions and 223 deletions

View File

@@ -25,8 +25,6 @@ jobs:
path: flix/build
- name: Build firmware for ESP32-S3
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
run: tools/check_c_cpp_properties.py

View File

@@ -7,6 +7,7 @@
"MD024": false,
"MD033": false,
"MD034": false,
"MD040": false,
"MD059": false,
"MD044": {
"html_elements": false,

View File

@@ -6,19 +6,18 @@
"${workspaceFolder}/flix",
"${workspaceFolder}/gazebo",
"${workspaceFolder}/tools/**",
"~/.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",
"~/.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/**",
"~/Arduino/libraries/**",
"/usr/include/gazebo-11/",
"/usr/include/ignition/math6/"
],
"forcedInclude": [
"${workspaceFolder}/.vscode/intellisense.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",
"~/.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",
"${workspaceFolder}/flix/cli.ino",
"${workspaceFolder}/flix/control.ino",
"${workspaceFolder}/flix/estimate.ino",
@@ -31,9 +30,10 @@
"${workspaceFolder}/flix/rc.ino",
"${workspaceFolder}/flix/time.ino",
"${workspaceFolder}/flix/wifi.ino",
"${workspaceFolder}/flix/parameters.ino"
"${workspaceFolder}/flix/parameters.ino",
"${workspaceFolder}/flix/safety.ino"
],
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
"cStandard": "c11",
"cppStandard": "c++17",
"defines": [
@@ -53,19 +53,18 @@
"name": "Mac",
"includePath": [
"${workspaceFolder}/flix",
"~/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",
"~/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/**",
"~/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.2.0/cores/esp32/Arduino.h",
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.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",
"${workspaceFolder}/flix/flix.ino",
"${workspaceFolder}/flix/cli.ino",
"${workspaceFolder}/flix/control.ino",
@@ -78,9 +77,10 @@
"${workspaceFolder}/flix/rc.ino",
"${workspaceFolder}/flix/time.ino",
"${workspaceFolder}/flix/wifi.ino",
"${workspaceFolder}/flix/parameters.ino"
"${workspaceFolder}/flix/parameters.ino",
"${workspaceFolder}/flix/safety.ino"
],
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
"cStandard": "c11",
"cppStandard": "c++17",
"defines": [
@@ -103,17 +103,16 @@
"${workspaceFolder}/flix",
"${workspaceFolder}/gazebo",
"${workspaceFolder}/tools/**",
"~/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",
"~/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/**",
"~/Documents/Arduino/libraries/**"
],
"forcedInclude": [
"${workspaceFolder}/.vscode/intellisense.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",
"~/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",
"${workspaceFolder}/flix/cli.ino",
"${workspaceFolder}/flix/control.ino",
"${workspaceFolder}/flix/estimate.ino",
@@ -126,9 +125,10 @@
"${workspaceFolder}/flix/rc.ino",
"${workspaceFolder}/flix/time.ino",
"${workspaceFolder}/flix/wifi.ino",
"${workspaceFolder}/flix/parameters.ino"
"${workspaceFolder}/flix/parameters.ino",
"${workspaceFolder}/flix/safety.ino"
],
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++.exe",
"cStandard": "c11",
"cppStandard": "c++17",
"defines": [

View File

@@ -13,10 +13,10 @@ monitor:
dependencies .dependencies:
arduino-cli core update-index --config-file arduino-cli.yaml
arduino-cli core install esp32:esp32@3.2.0 --config-file arduino-cli.yaml
arduino-cli core install esp32:esp32@3.3.6 --config-file arduino-cli.yaml
arduino-cli lib update-index
arduino-cli lib install "FlixPeriph"
arduino-cli lib install "MAVLink"@2.0.16
arduino-cli lib install "MAVLink"@2.0.25
touch .dependencies
gazebo/build cmake: gazebo/CMakeLists.txt

View File

@@ -138,10 +138,10 @@ You can see a user-contributed [variant of complete circuit diagram](https://mir
|Motor|Position|Direction|Prop type|Motor wires|GPIO|
|-|-|-|-|-|-|
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 (*TDI*)|
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 (*TCK*)|
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 (*TMS*)|
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 (*TD0*)|
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 *(TDI)*|
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 *(TCK)*|
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 *(TMS)*|
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 *(TD0)*|
Clockwise motors have blue & red wires and correspond to propeller type A (marked on the propeller).
Counter-clockwise motors have black & white wires correspond to propeller type B.

View File

@@ -41,10 +41,10 @@ Motors connection table:
|Motor|Position|Direction|Prop type|Motor wires|GPIO|
|-|-|-|-|-|-|
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 (*TDI*)|
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 (*TCK*)|
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 (*TMS*)|
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 (*TD0*)|
|Motor 0|Rear left|Counter-clockwise|B|Black & White|GPIO12 *(TDI)*|
|Motor 1|Rear right|Clockwise|A|Blue & Red|GPIO13 *(TCK)*|
|Motor 2|Front right|Counter-clockwise|B|Black & White|GPIO14 *(TMS)*|
|Motor 3|Front left|Clockwise|A|Blue & Red|GPIO15 *(TD0)*|
## Motors tightening

View File

@@ -110,7 +110,7 @@ float angle = Vector::angleBetween(a, b); // 1.57 (90 градусов)
#### Скалярное произведение
Скалярное произведение векторов (*dot product*) — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*.
Скалярное произведение векторов *(dot product)* — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*.
В Flix используется статический метод `Vector::dot()`:
@@ -124,7 +124,7 @@ float dotProduct = Vector::dot(a, b); // 32
#### Векторное произведение
Векторное произведение (*cross product*) позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке используется статический метод `Vector::cross()`:
Векторное произведение *(cross product)* позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке используется статический метод `Vector::cross()`:
```cpp
Vector a(1, 2, 3);
@@ -144,9 +144,9 @@ Vector crossProduct = Vector::cross(a, b); // -3, 6, -3
В прошивке углы Эйлера сохраняются в обычный объект `Vector` (хоть и, строго говоря, не являются вектором):
* Угол по крену (*roll*) — `vector.x`.
* Угол по тангажу (*pitch*) — `vector.y`.
* Угол по рысканию (*yaw*) — `vector.z`.
* Угол по крену *(roll)* — `vector.x`.
* Угол по тангажу *(pitch)* — `vector.y`.
* Угол по рысканию *(yaw)* — `vector.z`.
Особенности углов Эйлера:
@@ -162,8 +162,8 @@ Vector crossProduct = Vector::cross(a, b); // -3, 6, -3
Помимо углов Эйлера, любую ориентацию в трехмерном пространстве можно представить в виде вращения вокруг некоторой оси на некоторый угол. В геометрии это доказывается, как **теорема вращения Эйлера**. В таком представлении ориентация задается двумя величинами:
* **Ось вращения** (*axis*) — единичный вектор, определяющий ось вращения.
* **Угол поворота** (*angle* или *θ*) — угол, на который нужно повернуть объект вокруг этой оси.
* **Ось вращения** *(axis)* — единичный вектор, определяющий ось вращения.
* **Угол поворота** *(angle* или *θ)* — угол, на который нужно повернуть объект вокруг этой оси.
В Flix ось вращения задается объектом `Vector`, а угол поворота — числом типа `float` в радианах:
@@ -177,7 +177,7 @@ float angle = radians(45);
### Вектор вращения
Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** (*rotation vector*). Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.
Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** *(rotation vector)*. Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.
Вектор вращения обладает замечательным свойством: если угловые скорости объекта (в собственной системе координат) в каждый момент времени совпадают с компонентами этого вектора, то за единичное время объект придет к заданной этим вектором ориентации. Это свойство позволяет использовать вектор вращения для управления ориентацией объекта посредством управления угловыми скоростями.
@@ -198,7 +198,7 @@ Vector rotation = radians(45) * Vector(1, 2, 3);
<a href="https://github.com/okalachev/flix/blob/master/flix/quaternion.h"><code>quaternion.h</code></a>.<br>
</div>
Вектор вращения удобен, но еще удобнее использовать **кватернион**. В Flix кватернионы задаются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения (*axis*) и угла поворота (*θ*) по формуле:
Вектор вращения удобен, но еще удобнее использовать **кватернион**. В Flix кватернионы задаются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения *(axis)* и угла поворота *(θ)* по формуле:
\\[ q = \left( \begin{array}{c} w \\\\ x \\\\ y \\\\ z \end{array} \right) = \left( \begin{array}{c} \cos\left(\frac{\theta}{2}\right) \\\\ axis\_x \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_y \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_z \cdot \sin\left(\frac{\theta}{2}\right) \end{array} \right) \\]

View File

@@ -177,7 +177,7 @@ imu.setDLPF(imu.DLPF_MAX);
## Калибровка гироскопа
Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения (*bias*) и случайный шум (*noise*):
Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения *(bias)* и случайный шум *(noise)*:
\\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\]

Binary file not shown.

After

Width:  |  Height:  |  Size: 62 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 48 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 50 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 17 KiB

View File

@@ -13,10 +13,10 @@ Do the following:
Do the following:
* **Check the battery voltage**. Use a multimeter to measure the battery voltage. It should be in range of 3.7-4.2 V.
* **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 output.
* **Check if there are some startup errors**. Connect the ESP32 to the computer and check the Serial Monitor output. Use the Reset button to make sure you see the whole ESP32 startup output.
* **Check the baudrate is correct**. If you see garbage characters in the Serial Monitor, make sure the baudrate is set to 115200.
* **Make sure correct IMU model is chosen**. If using ICM-20948/MPU-6050 board, change `MPU9250` to `ICM20948`/`MPU6050` in the `imu.ino` file.
* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup**Analyze Tools**MAVLink Console*).
* **Check if the console is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the console using QGroundControl *(Vehicle Setup**Analyze Tools**MAVLink Console)*.
* **Configure QGroundControl correctly before connecting to the drone** if you use it to control the drone. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
* **If QGroundControl doesn't connect**, you might need to disable the firewall and/or VPN on your computer.
* **Check the IMU is working**. Perform `imu` command and check its output:

View File

@@ -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.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.
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.
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.16.
* `MAVLink`, version 2.0.25.
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.
@@ -108,7 +108,7 @@ 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.
You can also work with parameters using `p` command in the console. Parameter names are case-insensitive.
### Define IMU orientation
@@ -134,6 +134,20 @@ Before flight you need to calibrate the accelerometer:
1. Access the console using QGroundControl (recommended) or Serial Monitor.
2. Type `ca` command there and follow the instructions.
### Setup motors
If using non-default motor pins, set the pin numbers using the parameters: `MOTOR_PIN_FL`, `MOTOR_PIN_FR`, `MOTOR_PIN_RL`, `MOTOR_PIN_RR` (front-left, front-right, rear-left, rear-right respectively).
If using brushless motors and ESCs:
1. Set the appropriate PWM using the parameters: `MOT_PWM_STOP`, `MOT_PWM_MIN`, and `MOT_PWM_MAX` (1000, 1000, and 2000 is typical).
2. Decrease the PWM frequency using the `MOT_PWM_FREQ` parameter (400 is typical).
Reboot the drone to apply the changes.
> [!CAUTION]
> **Remove the props when configuring the motors!** If improperly configured, you may not be able to stop them.
### Check everything works
1. Check the IMU is working: perform `imu` command and check its output:
@@ -243,9 +257,43 @@ 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
```
## Flight log
After the flight, you can download the flight log for analysis wirelessly. Use the following for that:
After the flight, you can download the flight log for analysis wirelessly. Use the following command on your computer for that:
```bash
make log

View File

@@ -4,6 +4,17 @@ This page contains user-built drones based on the Flix project. Publish your pro
---
Author: **Arkadiy "Arky" Matsekh**, Foucault Dynamics, Gold Coast, Australia.<br>
The drone was built for the University of Queensland industry-led Master's capstone project.
**Flight video:**
<a href="https://drive.google.com/file/d/1NNYSVXBY-w0JjCo07D8-PgnVq3ca9plj/view?usp=sharing"><img height=300 src="img/user/arkymatsekh/video.jpg"></a>
<img src="img/user/arkymatsekh/1.jpg" height=150> <img src="img/user/arkymatsekh/2.jpg" height=150> <img src="img/user/arkymatsekh/3.jpg" height=150>
---
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>

View File

@@ -6,6 +6,7 @@
#include "pid.h"
#include "vector.h"
#include "util.h"
#include "lpf.h"
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern const int RAW, ACRO, STAB, AUTO;
@@ -14,6 +15,7 @@ extern uint16_t channels[16];
extern float controlTime;
extern int mode;
extern bool armed;
extern LowPassFilter<Vector> gyroBiasFilter;
const char* motd =
"\nWelcome to\n"
@@ -38,10 +40,13 @@ 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"
@@ -54,9 +59,7 @@ 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,9 +67,7 @@ void pause(float duration) {
while (t - start < duration) {
step();
handleInput();
#if WIFI_ENABLED
processMavlink();
#endif
delay(50);
}
}
@@ -94,7 +95,7 @@ void doCommand(String str, bool echo = false) {
} else if (command == "p") {
bool success = setParameter(arg0.c_str(), arg1.toFloat());
if (success) {
print("%s = %g\n", arg0.c_str(), arg1.toFloat());
print("%s = %g\n", arg0.c_str(), getParameter(arg0.c_str()));
} else {
print("Parameter not found: %s\n", arg0.c_str());
}
@@ -136,9 +137,11 @@ void doCommand(String str, bool echo = false) {
print("mode: %s\n", getModeName());
print("armed: %d\n", armed);
} else if (command == "wifi") {
#if WIFI_ENABLED
printWiFiInfo();
#endif
} 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") {
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 +152,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") {
@@ -178,6 +183,7 @@ void doCommand(String str, bool echo = false) {
#endif
} else if (command == "reset") {
attitude = Quaternion();
gyroBiasFilter.reset();
} else if (command == "reboot") {
ESP.restart();
} else {

View File

@@ -52,6 +52,7 @@ PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
PID yawPID(YAW_P, 0, 0);
Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
float tiltMax = TILT_MAX;
int flightModes[] = {STAB, STAB, STAB}; // map for rc mode switch
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
@@ -65,9 +66,9 @@ void control() {
}
void interpretControls() {
if (controlMode < 0.25) mode = STAB;
if (controlMode < 0.75) mode = STAB;
if (controlMode > 0.75) mode = STAB;
if (controlMode < 0.25) mode = flightModes[0];
else if (controlMode < 0.75) mode = flightModes[1];
else if (controlMode > 0.75) mode = flightModes[2];
if (mode == AUTO) return; // pilot is not effective in AUTO mode

View File

@@ -7,8 +7,6 @@
#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;
@@ -25,9 +23,7 @@ void setup() {
setupLED();
setupMotors();
setLED(true);
#if WIFI_ENABLED
setupWiFi();
#endif
setupIMU();
setupRC();
setLED(false);
@@ -42,9 +38,7 @@ void loop() {
control();
sendMotors();
handleInput();
#if WIFI_ENABLED
processMavlink();
#endif
logData();
syncParameters();
}

View File

@@ -19,6 +19,8 @@ Vector acc; // accelerometer output, m/s/s
Vector accBias;
Vector accScale(1, 1, 1);
LowPassFilter<Vector> gyroBiasFilter(0.001);
void setupIMU() {
print("Setup IMU\n");
imu.begin();
@@ -50,8 +52,6 @@ void readIMU() {
void calibrateGyroOnce() {
static Delay landedDelay(2);
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
static LowPassFilter<Vector> gyroBiasFilter(0.001);
gyroBias = gyroBiasFilter.update(gyro);
}
@@ -110,6 +110,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);

View File

@@ -14,15 +14,6 @@ public:
LowPassFilter(float alpha): alpha(alpha) {};
T update(const T input) {
if (alpha == 1) { // filter disabled
return input;
}
if (!initialized) {
output = input;
initialized = true;
}
return output += alpha * (input - output);
}
@@ -31,9 +22,6 @@ public:
}
void reset() {
initialized = false;
output = T(); // set to zero
}
private:
bool initialized = false;
};

View File

@@ -3,19 +3,16 @@
// MAVLink communication
#if WIFI_ENABLED
#include <MAVLink.h>
#include "util.h"
#define SYSTEM_ID 1
#define MAVLINK_RATE_SLOW 1
#define MAVLINK_RATE_FAST 10
extern float controlTime;
bool mavlinkConnected = false;
String mavlinkPrintBuffer;
int mavlinkSysId = 1;
Rate telemetryFast(10);
Rate telemetrySlow(2);
void processMavlink() {
sendMavlink();
@@ -28,10 +25,8 @@ void sendMavlink() {
mavlink_message_t msg;
uint32_t time = t * 1000;
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,
if (telemetrySlow) {
mavlink_msg_heartbeat_pack(mavlinkSysId, 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),
@@ -40,27 +35,27 @@ void sendMavlink() {
if (!mavlinkConnected) return; // send only heartbeat until connected
mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
mavlink_msg_extended_sys_state_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
sendMessage(&msg);
}
if (fast && mavlinkConnected) {
if (telemetryFast && mavlinkConnected) {
const float zeroQuat[] = {0, 0, 0, 0};
mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
mavlink_msg_attitude_quaternion_pack(mavlinkSysId, 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(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
mavlink_msg_rc_channels_raw_pack(mavlinkSysId, 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(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
mavlink_msg_actuator_control_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
sendMessage(&msg);
mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
mavlink_msg_scaled_imu_pack(mavlinkSysId, 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);
@@ -95,7 +90,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 != SYSTEM_ID) return; // 0 is broadcast
if (m.target && m.target != mavlinkSysId) return; // 0 is broadcast
controlThrottle = m.z / 1000.0f;
controlPitch = m.x / 1000.0f;
@@ -108,11 +103,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 != SYSTEM_ID) return;
if (m.target_system && m.target_system != mavlinkSysId) return;
mavlink_message_t msg;
for (int i = 0; i < parametersCount(); i++) {
mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);
sendMessage(&msg);
}
@@ -121,7 +116,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 != SYSTEM_ID) return;
if (m.target_system && m.target_system != mavlinkSysId) 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
@@ -130,7 +125,7 @@ void handleMavlink(const void *_msg) {
memcpy(name, getParameterName(m.param_index), 16);
}
mavlink_message_t msg;
mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index);
sendMessage(&msg);
}
@@ -138,32 +133,33 @@ 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 != SYSTEM_ID) return;
if (m.target_system && m.target_system != mavlinkSysId) 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
setParameter(name, m.param_value);
bool success = setParameter(name, m.param_value);
if (!success) return;
// send ack
mavlink_message_t 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
mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg,
m.param_id, getParameter(name), MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
sendMessage(&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 != SYSTEM_ID) return;
if (m.target_system && m.target_system != mavlinkSysId) return;
mavlink_message_t msg;
mavlink_msg_mission_count_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);
mavlink_msg_mission_count_pack(mavlinkSysId, 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 != SYSTEM_ID) return;
if (m.target_system && m.target_system != mavlinkSysId) 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
@@ -175,7 +171,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 != SYSTEM_ID) return;
if (m.target_system && m.target_system != mavlinkSysId) return;
// copy attitude, rates and thrust targets
ratesTarget.x = m.body_roll_rate;
@@ -197,7 +193,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 != SYSTEM_ID) return;
if (m.target_system && m.target_system != mavlinkSysId) return;
attitudeTarget.invalidate();
ratesTarget.invalidate();
@@ -209,12 +205,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 != SYSTEM_ID) return;
if (m.target_system && m.target_system != mavlinkSysId) 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(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i,
sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]);
sendMessage(&msg);
}
@@ -224,13 +220,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 != SYSTEM_ID) return;
if (m.target_system && m.target_system != mavlinkSysId) 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(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &response,
mavlink_msg_autopilot_version_pack(mavlinkSysId, 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);
}
@@ -249,7 +245,7 @@ void handleMavlink(const void *_msg) {
// send command ack
mavlink_message_t ack;
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);
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);
sendMessage(&ack);
}
}
@@ -266,7 +262,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(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
mavlink_msg_serial_control_pack(mavlinkSysId, 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);
@@ -274,5 +270,3 @@ void sendMavlinkPrint() {
}
mavlinkPrintBuffer.clear();
}
#endif

View File

@@ -1,24 +1,19 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Motors output control using MOSFETs
// In case of using ESCs, change PWM_STOP, PWM_MIN and PWM_MAX to appropriate values in μs, decrease PWM_FREQUENCY (to 400)
// PWM control for motors
#include "util.h"
#define MOTOR_0_PIN 12 // rear left
#define MOTOR_1_PIN 13 // rear right
#define MOTOR_2_PIN 14 // front right
#define MOTOR_3_PIN 15 // front left
#define PWM_FREQUENCY 78000
#define PWM_RESOLUTION 10
#define PWM_STOP 0
#define PWM_MIN 0
#define PWM_MAX 1000000 / PWM_FREQUENCY
float motors[4]; // normalized motor thrusts in range [0..1]
int motorPins[4] = {12, 13, 14, 15}; // default pin numbers
int pwmFrequency = 78000;
int pwmResolution = 10;
int pwmStop = 0;
int pwmMin = 0;
int pwmMax = -1; // -1 means duty cycle mode
const int MOTOR_REAR_LEFT = 0;
const int MOTOR_REAR_RIGHT = 1;
const int MOTOR_FRONT_RIGHT = 2;
@@ -26,30 +21,30 @@ const int MOTOR_FRONT_LEFT = 3;
void setupMotors() {
print("Setup Motors\n");
// configure pins
ledcAttach(MOTOR_0_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
ledcAttach(MOTOR_1_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
ledcAttach(MOTOR_2_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
ledcAttach(MOTOR_3_PIN, PWM_FREQUENCY, PWM_RESOLUTION);
for (int i = 0; i < 4; i++) {
ledcAttach(motorPins[i], pwmFrequency, pwmResolution);
}
sendMotors();
print("Motors initialized\n");
}
int getDutyCycle(float value) {
value = constrain(value, 0, 1);
float pwm = mapf(value, 0, 1, PWM_MIN, PWM_MAX);
if (value == 0) pwm = PWM_STOP;
float duty = mapf(pwm, 0, 1000000 / PWM_FREQUENCY, 0, (1 << PWM_RESOLUTION) - 1);
return round(duty);
void sendMotors() {
for (int i = 0; i < 4; i++) {
ledcWrite(motorPins[i], getDutyCycle(motors[i]));
}
}
void sendMotors() {
ledcWrite(MOTOR_0_PIN, getDutyCycle(motors[0]));
ledcWrite(MOTOR_1_PIN, getDutyCycle(motors[1]));
ledcWrite(MOTOR_2_PIN, getDutyCycle(motors[2]));
ledcWrite(MOTOR_3_PIN, getDutyCycle(motors[3]));
int getDutyCycle(float value) {
value = constrain(value, 0, 1);
if (pwmMax >= 0) { // pwm mode
float pwm = mapf(value, 0, 1, pwmMin, pwmMax);
if (value == 0) pwm = pwmStop;
float duty = mapf(pwm, 0, 1000000 / pwmFrequency, 0, (1 << pwmResolution) - 1);
return round(duty);
} else { // duty cycle mode
return round(value * ((1 << pwmResolution) - 1));
}
}
bool motorsActive() {

View File

@@ -9,13 +9,20 @@
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)
float *variable;
float value; // cache
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; };
};
Parameter parameters[] = {
@@ -42,6 +49,9 @@ Parameter parameters[] = {
{"CTL_R_RATE_MAX", &maxRate.x},
{"CTL_Y_RATE_MAX", &maxRate.z},
{"CTL_TILT_MAX", &tiltMax},
{"CTL_FLT_MODE_0", &flightModes[0]},
{"CTL_FLT_MODE_1", &flightModes[1]},
{"CTL_FLT_MODE_2", &flightModes[2]},
// imu
{"IMU_ROT_ROLL", &imuRotation.x},
{"IMU_ROT_PITCH", &imuRotation.y},
@@ -52,9 +62,20 @@ Parameter parameters[] = {
{"IMU_ACC_SCALE_X", &accScale.x},
{"IMU_ACC_SCALE_Y", &accScale.y},
{"IMU_ACC_SCALE_Z", &accScale.z},
{"IMU_GYRO_BIAS_A", &gyroBiasFilter.alpha},
// estimate
{"EST_ACC_WEIGHT", &accWeight},
{"EST_RATES_LPF_A", &ratesFilter.alpha},
// motors
{"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT]},
{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT]},
{"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT]},
{"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT]},
{"MOT_PWM_FREQ", &pwmFrequency},
{"MOT_PWM_RES", &pwmResolution},
{"MOT_PWM_STOP", &pwmStop},
{"MOT_PWM_MIN", &pwmMin},
{"MOT_PWM_MAX", &pwmMax},
// rc
{"RC_ZERO_0", &channelZero[0]},
{"RC_ZERO_1", &channelZero[1]},
@@ -77,6 +98,17 @@ 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() {
@@ -84,10 +116,10 @@ void setupParameters() {
// Read parameters from storage
for (auto &parameter : parameters) {
if (!storage.isKey(parameter.name)) {
storage.putFloat(parameter.name, *parameter.variable);
storage.putFloat(parameter.name, parameter.getValue()); // store default value
}
*parameter.variable = storage.getFloat(parameter.name, *parameter.variable);
parameter.value = *parameter.variable;
parameter.setValue(storage.getFloat(parameter.name, 0));
parameter.cache = parameter.getValue();
}
}
@@ -102,13 +134,13 @@ const char *getParameterName(int index) {
float getParameter(int index) {
if (index < 0 || index >= parametersCount()) return NAN;
return *parameters[index].variable;
return parameters[index].getValue();
}
float getParameter(const char *name) {
for (auto &parameter : parameters) {
if (strcmp(parameter.name, name) == 0) {
return *parameter.variable;
if (strcasecmp(parameter.name, name) == 0) {
return parameter.getValue();
}
}
return NAN;
@@ -116,8 +148,9 @@ float getParameter(const char *name) {
bool setParameter(const char *name, const float value) {
for (auto &parameter : parameters) {
if (strcmp(parameter.name, name) == 0) {
*parameter.variable = value;
if (strcasecmp(parameter.name, name) == 0) {
if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
parameter.setValue(value);
return true;
}
}
@@ -130,16 +163,18 @@ void syncParameters() {
if (motorsActive()) return; // don't use flash while flying, it may cause a delay
for (auto &parameter : parameters) {
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;
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
}
}
void printParameters() {
for (auto &parameter : parameters) {
print("%s = %g\n", parameter.name, *parameter.variable);
print("%s = %g\n", parameter.name, parameter.getValue());
}
}

View File

@@ -13,10 +13,10 @@ 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; // time of the last controls update (0 when no RC)
float controlMode = NAN;
float controlTime = NAN; // time of the last controls update
// Channels mapping (using float to store in parameters):
// Channels mapping (nan means not assigned):
float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
void setupRC() {

View File

@@ -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,9 +16,8 @@ void failsafe() {
// RC loss failsafe
void rcLossFailsafe() {
if (controlTime == 0) return; // no RC at all
if (!armed) return;
if (t - controlTime > RC_LOSS_TIMEOUT) {
if (t - controlTime > rcLossTimeout) {
descend();
}
}
@@ -27,7 +26,7 @@ void rcLossFailsafe() {
void descend() {
mode = AUTO;
attitudeTarget = Quaternion();
thrustTarget -= dt / DESCEND_TIME;
thrustTarget -= dt / descendTime;
if (thrustTarget < 0) {
thrustTarget = 0;
armed = false;

View File

@@ -1,49 +1,76 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Wi-Fi support
#if WIFI_ENABLED
// Wi-Fi communication
#include <WiFi.h>
#include <WiFiAP.h>
#include <WiFiUdp.h>
#include "Preferences.h"
#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"
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";
WiFiUDP udp;
void setupWiFi() {
print("Setup Wi-Fi\n");
WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
udp.begin(WIFI_UDP_PORT);
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);
}
void sendWiFi(const uint8_t *buf, int len) {
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);
if (WiFi.softAPgetStationNum() == 0 && !WiFi.isConnected()) return;
udp.beginPacket(udpRemoteIP, udpRemotePort);
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() {
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());
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);
}
#endif
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

@@ -9,8 +9,7 @@
#include "quaternion.h"
#include "Arduino.h"
#include "wifi.h"
#define WIFI_ENABLED 1
#include "lpf.h"
extern float t, dt;
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
@@ -21,6 +20,7 @@ extern float motors[4];
Vector gyro, acc, imuRotation;
Vector accBias, gyroBias, accScale(1, 1, 1);
LowPassFilter<Vector> gyroBiasFilter(0);
// declarations
void step();
@@ -34,6 +34,7 @@ void controlRates();
void controlTorque();
const char* getModeName();
void sendMotors();
int getDutyCycle(float value);
bool motorsActive();
void testMotor(int n);
void print(const char* format, ...);
@@ -70,6 +71,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"); };

View File

@@ -11,9 +11,10 @@
#include <sys/poll.h>
#include <gazebo/gazebo.hh>
#define WIFI_UDP_PORT 14580
#define WIFI_UDP_REMOTE_PORT 14550
#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
int wifiMode = 1; // mock
int udpLocalPort = 14580;
int udpRemotePort = 14550;
const char *udpRemoteIP = "255.255.255.255";
int wifiSocket;
@@ -22,22 +23,22 @@ void setupWiFi() {
sockaddr_in addr; // local address
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = INADDR_ANY;
addr.sin_port = htons(WIFI_UDP_PORT);
addr.sin_port = htons(udpLocalPort);
if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) {
gzerr << "Failed to bind WiFi UDP socket on port " << WIFI_UDP_PORT << std::endl;
gzerr << "Failed to bind WiFi UDP socket on port " << udpLocalPort << std::endl;
return;
}
int broadcast = 1;
setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT << " (remote port " << WIFI_UDP_REMOTE_PORT << ")" << std::endl;
gzmsg << "WiFi UDP socket initialized on port " << udpLocalPort << " (remote port " << udpRemotePort << ")" << std::endl;
}
void sendWiFi(const uint8_t *buf, int len) {
if (wifiSocket == 0) setupWiFi();
sockaddr_in addr; // remote address
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = inet_addr(WIFI_UDP_REMOTE_ADDR);
addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
addr.sin_addr.s_addr = inet_addr(udpRemoteIP);
addr.sin_port = htons(udpRemotePort);
sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
}

View File

@@ -13,7 +13,7 @@ lines = []
print('Downloading log...')
count = 0
dev.write('log\n'.encode())
dev.write('log dump\n'.encode())
while True:
line = dev.readline()
if not line:

View File

@@ -92,17 +92,17 @@ Full list of events:
|-----|-----------|----------------|
|`connected`|Connected to the drone||
|`disconnected`|Connection is lost||
|`armed`|Armed state update|Armed state (*bool*)|
|`mode`|Flight mode update|Flight mode (*str*)|
|`landed`|Landed state update|Landed state (*bool*)|
|`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|
|`attitude`|Attitude update|Attitude quaternion (*list*)|
|`attitude_euler`|Attitude update|Euler angles (*list*)|
|`rates`|Angular rates update|Angular rates (*list*)|
|`channels`|Raw RC channels update|Raw RC channels (*list*)|
|`motors`|Motor outputs update|Motor outputs (*list*)|
|`acc`|Accelerometer update|Accelerometer output (*list*)|
|`gyro`|Gyroscope update|Gyroscope output (*list*)|
|`attitude`|Attitude update|Attitude quaternion *(list)*|
|`attitude_euler`|Attitude update|Euler angles *(list)*|
|`rates`|Angular rates update|Angular rates *(list)*|
|`channels`|Raw RC channels update|Raw RC channels *(list)*|
|`motors`|Motor outputs update|Motor outputs *(list)*|
|`acc`|Accelerometer update|Accelerometer output *(list)*|
|`gyro`|Gyroscope update|Gyroscope output *(list)*|
|`mavlink`|Received MAVLink message|Message object|
|`mavlink.<message_name>`|Received specific MAVLink message|Message object|
|`mavlink.<message_id>`|Received specific MAVLink message|Message object|
@@ -277,7 +277,3 @@ logger = logging.getLogger('flix')
logger.setLevel(logging.DEBUG) # be more verbose
logger.setLevel(logging.WARNING) # be less verbose
```
## Stability
The library is in development stage. The API is not stable.