Compare commits
34 Commits
e7e57d1020
...
school-548
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
3104410bb9 | ||
|
|
1551d096fc | ||
|
|
80f23ab016 | ||
|
|
e6fb264499 | ||
|
|
4d0871b00b | ||
|
|
f1b993d719 | ||
|
|
2e7330d2f5 | ||
|
|
e22df3ab01 | ||
|
|
8484854576 | ||
|
|
b9d5624f30 | ||
|
|
205270b8ec | ||
|
|
f1bedb2b10 | ||
|
|
46d1749a8c | ||
|
|
66fb7a13c3 | ||
|
|
cfef3b9c36 | ||
|
|
1338a9ea79 | ||
|
|
b60757ec1d | ||
|
|
491e054534 | ||
|
|
3eaf24f89d | ||
|
|
dc09459613 | ||
|
|
59c9ea8cb3 | ||
|
|
5bdd46c6ad | ||
|
|
5b37c87166 | ||
|
|
48ba55aa7e | ||
|
|
2708c1eafd | ||
|
|
b2c9dfe6ef | ||
|
|
0579118dd5 | ||
|
|
05818349d8 | ||
|
|
6c1d651caa | ||
|
|
49a0aa7036 | ||
|
|
bf9eeb90a4 | ||
|
|
96836b2e3e | ||
|
|
82d9d3570d | ||
|
|
d7f8c8d934 |
11
.github/workflows/tools.yml
vendored
@@ -46,3 +46,14 @@ jobs:
|
|||||||
echo -e "t,x,y,z\n0,1,2,3\n1,4,5,6" > log.csv
|
echo -e "t,x,y,z\n0,1,2,3\n1,4,5,6" > log.csv
|
||||||
./csv_to_mcap.py log.csv
|
./csv_to_mcap.py log.csv
|
||||||
test $(stat -c %s log.mcap) -eq 883
|
test $(stat -c %s log.mcap) -eq 883
|
||||||
|
|
||||||
|
sloc:
|
||||||
|
runs-on: ubuntu-latest
|
||||||
|
steps:
|
||||||
|
- uses: actions/checkout@v4
|
||||||
|
- name: Install cloc
|
||||||
|
run: sudo apt-get install -y cloc
|
||||||
|
- name: Firmware source lines count
|
||||||
|
run: cloc --by-file-by-lang flix
|
||||||
|
- name: Overall source lines count
|
||||||
|
run: cloc --by-file-by-lang --exclude-ext=svg,dae,css,hbs,md,json,yaml,yml,toml .
|
||||||
|
|||||||
@@ -7,7 +7,6 @@
|
|||||||
"MD024": false,
|
"MD024": false,
|
||||||
"MD033": false,
|
"MD033": false,
|
||||||
"MD034": false,
|
"MD034": false,
|
||||||
"MD059": false,
|
|
||||||
"MD044": {
|
"MD044": {
|
||||||
"html_elements": false,
|
"html_elements": false,
|
||||||
"code_blocks": false,
|
"code_blocks": false,
|
||||||
@@ -65,6 +64,5 @@
|
|||||||
"PX4"
|
"PX4"
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
"MD045": false,
|
"MD045": false
|
||||||
"MD060": false
|
|
||||||
}
|
}
|
||||||
|
|||||||
45
.vscode/c_cpp_properties.json
vendored
@@ -18,7 +18,20 @@
|
|||||||
"forcedInclude": [
|
"forcedInclude": [
|
||||||
"${workspaceFolder}/.vscode/intellisense.h",
|
"${workspaceFolder}/.vscode/intellisense.h",
|
||||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/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"
|
"~/.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",
|
||||||
|
"${workspaceFolder}/flix/flix.ino",
|
||||||
|
"${workspaceFolder}/flix/imu.ino",
|
||||||
|
"${workspaceFolder}/flix/led.ino",
|
||||||
|
"${workspaceFolder}/flix/log.ino",
|
||||||
|
"${workspaceFolder}/flix/mavlink.ino",
|
||||||
|
"${workspaceFolder}/flix/motors.ino",
|
||||||
|
"${workspaceFolder}/flix/rc.ino",
|
||||||
|
"${workspaceFolder}/flix/time.ino",
|
||||||
|
"${workspaceFolder}/flix/wifi.ino",
|
||||||
|
"${workspaceFolder}/flix/parameters.ino"
|
||||||
],
|
],
|
||||||
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
||||||
"cStandard": "c11",
|
"cStandard": "c11",
|
||||||
@@ -52,7 +65,20 @@
|
|||||||
"forcedInclude": [
|
"forcedInclude": [
|
||||||
"${workspaceFolder}/.vscode/intellisense.h",
|
"${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/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.2.0/variants/d1_mini32/pins_arduino.h",
|
||||||
|
"${workspaceFolder}/flix/flix.ino",
|
||||||
|
"${workspaceFolder}/flix/cli.ino",
|
||||||
|
"${workspaceFolder}/flix/control.ino",
|
||||||
|
"${workspaceFolder}/flix/estimate.ino",
|
||||||
|
"${workspaceFolder}/flix/imu.ino",
|
||||||
|
"${workspaceFolder}/flix/led.ino",
|
||||||
|
"${workspaceFolder}/flix/log.ino",
|
||||||
|
"${workspaceFolder}/flix/mavlink.ino",
|
||||||
|
"${workspaceFolder}/flix/motors.ino",
|
||||||
|
"${workspaceFolder}/flix/rc.ino",
|
||||||
|
"${workspaceFolder}/flix/time.ino",
|
||||||
|
"${workspaceFolder}/flix/wifi.ino",
|
||||||
|
"${workspaceFolder}/flix/parameters.ino"
|
||||||
],
|
],
|
||||||
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
||||||
"cStandard": "c11",
|
"cStandard": "c11",
|
||||||
@@ -87,7 +113,20 @@
|
|||||||
"forcedInclude": [
|
"forcedInclude": [
|
||||||
"${workspaceFolder}/.vscode/intellisense.h",
|
"${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/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.2.0/variants/d1_mini32/pins_arduino.h",
|
||||||
|
"${workspaceFolder}/flix/cli.ino",
|
||||||
|
"${workspaceFolder}/flix/control.ino",
|
||||||
|
"${workspaceFolder}/flix/estimate.ino",
|
||||||
|
"${workspaceFolder}/flix/flix.ino",
|
||||||
|
"${workspaceFolder}/flix/imu.ino",
|
||||||
|
"${workspaceFolder}/flix/led.ino",
|
||||||
|
"${workspaceFolder}/flix/log.ino",
|
||||||
|
"${workspaceFolder}/flix/mavlink.ino",
|
||||||
|
"${workspaceFolder}/flix/motors.ino",
|
||||||
|
"${workspaceFolder}/flix/rc.ino",
|
||||||
|
"${workspaceFolder}/flix/time.ino",
|
||||||
|
"${workspaceFolder}/flix/wifi.ino",
|
||||||
|
"${workspaceFolder}/flix/parameters.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/2411/bin/xtensa-esp32-elf-g++.exe",
|
||||||
"cStandard": "c11",
|
"cStandard": "c11",
|
||||||
|
|||||||
@@ -1,9 +1,6 @@
|
|||||||
<!-- markdownlint-disable MD041 -->
|
# Flix
|
||||||
|
|
||||||
<p align="center">
|
**Flix** (*flight + X*) — open source ESP32-based quadcopter made from scratch.
|
||||||
<img src="docs/img/flix.svg" width=180 alt="Flix logo"><br>
|
|
||||||
<b>Flix</b> (<i>flight + X</i>) — open source ESP32-based quadcopter made from scratch.
|
|
||||||
</p>
|
|
||||||
|
|
||||||
<table>
|
<table>
|
||||||
<tr>
|
<tr>
|
||||||
|
|||||||
@@ -35,7 +35,7 @@
|
|||||||
|
|
||||||
### Подсистема управления
|
### Подсистема управления
|
||||||
|
|
||||||
Состояние органов управления обрабатывается в функции `interpretControls()` и преобразуется в **команду управления**, которая включает следующее:
|
Состояние органов управления обрабатывается в функции `interpretControls()` и преобразуется в *команду управления*, которая включает следующее:
|
||||||
|
|
||||||
* `attitudeTarget` *(Quaternion)* — целевая ориентация дрона.
|
* `attitudeTarget` *(Quaternion)* — целевая ориентация дрона.
|
||||||
* `ratesTarget` *(Vector)* — целевые угловые скорости, *рад/с*.
|
* `ratesTarget` *(Vector)* — целевые угловые скорости, *рад/с*.
|
||||||
|
|||||||
@@ -38,13 +38,13 @@ Utility files:
|
|||||||
|
|
||||||
### Control subsystem
|
### Control subsystem
|
||||||
|
|
||||||
Pilot inputs are interpreted in `interpretControls()`, and then converted to the **control command**, which consists of the following:
|
Pilot inputs are interpreted in `interpretControls()`, and then converted to the *control command*, which consists of the following:
|
||||||
|
|
||||||
* `attitudeTarget` *(Quaternion)* — target attitude of the drone.
|
* `attitudeTarget` *(Quaternion)* — target attitude of the drone.
|
||||||
* `ratesTarget` *(Vector)* — target angular rates, *rad/s*.
|
* `ratesTarget` *(Vector)* — target angular rates, *rad/s*.
|
||||||
* `ratesExtra` *(Vector)* — additional (feed-forward) angular rates , used for yaw rate control in STAB mode, *rad/s*.
|
* `ratesExtra` *(Vector)* — additional (feed-forward) angular rates , used for yaw rate control in STAB mode, *rad/s*.
|
||||||
* `torqueTarget` *(Vector)* — target torque, range [-1, 1].
|
* `torqueTarget` *(Vector)* — target torque, range [-1, 1].
|
||||||
* `thrustTarget` *(float)* — collective motor thrust target, range [0, 1].
|
* `thrustTarget` *(float)* — collective thrust target, range [0, 1].
|
||||||
|
|
||||||
Control command is handled in `controlAttitude()`, `controlRates()`, `controlTorque()` functions. Each function may be skipped if the corresponding control target is set to `NAN`.
|
Control command is handled in `controlAttitude()`, `controlRates()`, `controlTorque()` functions. Each function may be skipped if the corresponding control target is set to `NAN`.
|
||||||
|
|
||||||
@@ -62,11 +62,6 @@ print("Test value: %.2f\n", testValue);
|
|||||||
|
|
||||||
In order to add a console command, modify the `doCommand()` function in `cli.ino` file.
|
In order to add a console command, modify the `doCommand()` function in `cli.ino` file.
|
||||||
|
|
||||||
> [!IMPORTANT]
|
|
||||||
> Avoid using delays in in-flight commands, it will **crash** the drone! (The design is one-threaded.)
|
|
||||||
>
|
|
||||||
> For on-the-ground commands, use `pause()` function, instead of `delay()`. This function allows to pause in a way that MAVLink connection will continue working.
|
|
||||||
|
|
||||||
## Building the firmware
|
## Building the firmware
|
||||||
|
|
||||||
See build instructions in [usage.md](usage.md).
|
See build instructions in [usage.md](usage.md).
|
||||||
|
|||||||
@@ -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 |
|
Before Width: | Height: | Size: 23 KiB |
|
Before Width: | Height: | Size: 18 KiB |
|
Before Width: | Height: | Size: 18 KiB |
|
Before Width: | Height: | Size: 17 KiB |
|
Before Width: | Height: | Size: 17 KiB |
|
Before Width: | Height: | Size: 10 KiB |
|
Before Width: | Height: | Size: 9.6 KiB |
|
Before Width: | Height: | Size: 10 KiB |
|
Before Width: | Height: | Size: 10 KiB |
|
Before Width: | Height: | Size: 68 KiB |
|
Before Width: | Height: | Size: 35 KiB |
|
Before Width: | Height: | Size: 32 KiB |
|
Before Width: | Height: | Size: 108 KiB |
|
Before Width: | Height: | Size: 93 KiB |
|
Before Width: | Height: | Size: 65 KiB |
|
Before Width: | Height: | Size: 28 KiB |
|
Before Width: | Height: | Size: 31 KiB |
|
Before Width: | Height: | Size: 42 KiB |
|
Before Width: | Height: | Size: 43 KiB |
|
Before Width: | Height: | Size: 76 KiB |
|
Before Width: | Height: | Size: 28 KiB |
|
Before Width: | Height: | Size: 44 KiB |
|
Before Width: | Height: | Size: 32 KiB |
@@ -4,7 +4,7 @@
|
|||||||
|
|
||||||
Do the following:
|
Do the following:
|
||||||
|
|
||||||
* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#building-the-firmware).
|
* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#firmware).
|
||||||
* **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU9250 or other peripherals libraries that may conflict with the ones used in the tutorial.
|
* **Check libraries**. Install all the required libraries from the tutorial. Make sure there are no MPU9250 or other peripherals libraries that may conflict with the ones used in the tutorial.
|
||||||
* **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*.
|
* **Check the chosen board**. The correct board to choose in Arduino IDE for ESP32 Mini is *WEMOS D1 MINI ESP32*.
|
||||||
|
|
||||||
@@ -25,7 +25,7 @@ Do the following:
|
|||||||
* The `accel` and `gyro` fields should change as you move the drone.
|
* The `accel` and `gyro` fields should change as you move the drone.
|
||||||
* **Calibrate the accelerometer.** if is wasn't done before. Type `ca` command in Serial Monitor and follow the instructions.
|
* **Calibrate the accelerometer.** if is wasn't done before. Type `ca` command in Serial Monitor and follow the instructions.
|
||||||
* **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct.
|
* **Check the attitude estimation**. Connect to the drone using QGroundControl. Rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct.
|
||||||
* **Check the IMU orientation is set correctly**. If the attitude estimation is rotated, set the correct IMU orientation as described in the [tutorial](usage.md#define-imu-orientation).
|
* **Check the IMU orientation is set correctly**. If the attitude estimation is rotated, make sure `rotateIMU` function is defined correctly in `imu.ino` file.
|
||||||
* **Check the motors type**. Motors with exact 3.7V voltage are needed, not ranged working voltage (3.7V — 6V).
|
* **Check the motors type**. Motors with exact 3.7V voltage are needed, not ranged working voltage (3.7V — 6V).
|
||||||
* **Check the motors**. Perform the following commands using Serial Monitor:
|
* **Check the motors**. Perform the following commands using Serial Monitor:
|
||||||
* `mfr` — should rotate front right motor (counter-clockwise).
|
* `mfr` — should rotate front right motor (counter-clockwise).
|
||||||
|
|||||||
@@ -12,7 +12,7 @@ Beginners can [download the source code as a ZIP archive](https://github.com/oka
|
|||||||
|
|
||||||
## Building the firmware
|
## Building the firmware
|
||||||
|
|
||||||
You can build and upload the firmware using either **Arduino IDE** (easier for beginners) or **command line**.
|
You can build and upload the firmware using either **Arduino IDE** (easier for beginners) or a **command line**.
|
||||||
|
|
||||||
### Arduino IDE (Windows, Linux, macOS)
|
### Arduino IDE (Windows, Linux, macOS)
|
||||||
|
|
||||||
@@ -73,6 +73,14 @@ ICM20948 imu(SPI); // For ICM-20948
|
|||||||
MPU6050 imu(Wire); // For MPU-6050
|
MPU6050 imu(Wire); // For MPU-6050
|
||||||
```
|
```
|
||||||
|
|
||||||
|
### Setup the IMU orientation
|
||||||
|
|
||||||
|
The IMU orientation is defined in `rotateIMU` function in the `imu.ino` file. Change it so it converts the IMU axes to the drone's axes correctly. **Drone axes are X forward, Y left, Z up**:
|
||||||
|
|
||||||
|
<img src="img/drone-axes.svg" width="200">
|
||||||
|
|
||||||
|
See various [IMU boards axes orientations table](https://github.com/okalachev/flixperiph/?tab=readme-ov-file#imu-axes-orientation) to help you set up the correct orientation.
|
||||||
|
|
||||||
### Connect using QGroundControl
|
### Connect using QGroundControl
|
||||||
|
|
||||||
QGroundControl is a ground control station software that can be used to monitor and control the drone.
|
QGroundControl is a ground control station software that can be used to monitor and control the drone.
|
||||||
@@ -80,7 +88,7 @@ QGroundControl is a ground control station software that can be used to monitor
|
|||||||
1. Install mobile or desktop version of [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html).
|
1. Install mobile or desktop version of [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html).
|
||||||
2. Power up the drone.
|
2. Power up the drone.
|
||||||
3. Connect your computer or smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
|
3. Connect your computer or smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
|
||||||
4. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
|
4. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically
|
||||||
|
|
||||||
### Access console
|
### Access console
|
||||||
|
|
||||||
@@ -96,37 +104,11 @@ To access the console using QGroundControl:
|
|||||||
|
|
||||||
1. Connect to the drone using QGroundControl app.
|
1. Connect to the drone using QGroundControl app.
|
||||||
2. Go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
|
2. Go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Analyze Tools* ⇒ *MAVLink Console*.
|
||||||
|
<img src="img/cli.png" width="400">
|
||||||
<img src="img/cli.png" width="400">
|
|
||||||
|
|
||||||
> [!TIP]
|
> [!TIP]
|
||||||
> Use `help` command to see the list of available commands.
|
> Use `help` command to see the list of available commands.
|
||||||
|
|
||||||
### Access parameters
|
|
||||||
|
|
||||||
The drone is configured using parameters. To access and modify them, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*:
|
|
||||||
|
|
||||||
<img src="img/parameters.png" width="400">
|
|
||||||
|
|
||||||
You can also work with parameters using `p` command in the console.
|
|
||||||
|
|
||||||
### Define IMU orientation
|
|
||||||
|
|
||||||
Use parameters, to define the IMU board axes orientation relative to the drone's axes: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
|
|
||||||
|
|
||||||
The drone has *X* axis pointing forward, *Y* axis pointing left, and *Z* axis pointing up, and the supported IMU boards have *X* axis pointing to the pins side and *Z* axis pointing up from the component side:
|
|
||||||
|
|
||||||
<img src="img/imu-axes.png" width="200">
|
|
||||||
|
|
||||||
Use the following table to set the parameters for common IMU orientations:
|
|
||||||
|
|
||||||
|Orientation|Parameters|Orientation|Parameters|
|
|
||||||
|:-:|-|-|-|
|
|
||||||
|<img src="img/imu-rot-1.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0 |<img src="img/imu-rot-5.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0|
|
|
||||||
|<img src="img/imu-rot-2.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|<img src="img/imu-rot-6.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|
|
|
||||||
|<img src="img/imu-rot-3.png" width="180">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|<img src="img/imu-rot-7.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|
|
|
||||||
|<img src="img/imu-rot-4.png" width="180"><br>☑️ **Default**|<br>`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|<img src="img/imu-rot-8.png" width="180">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|
|
|
||||||
|
|
||||||
### Calibrate accelerometer
|
### Calibrate accelerometer
|
||||||
|
|
||||||
Before flight you need to calibrate the accelerometer:
|
Before flight you need to calibrate the accelerometer:
|
||||||
@@ -154,10 +136,6 @@ Before flight you need to calibrate the accelerometer:
|
|||||||
* `mrl` — should rotate rear left motor (counter-clockwise).
|
* `mrl` — should rotate rear left motor (counter-clockwise).
|
||||||
* `mrr` — should rotate rear right motor (clockwise).
|
* `mrr` — should rotate rear right motor (clockwise).
|
||||||
|
|
||||||
Rotation diagram:
|
|
||||||
|
|
||||||
<img src="img/motors.svg" width=200>
|
|
||||||
|
|
||||||
> [!WARNING]
|
> [!WARNING]
|
||||||
> Never run the motors when powering the drone from USB, always use the battery for that.
|
> Never run the motors when powering the drone from USB, always use the battery for that.
|
||||||
|
|
||||||
@@ -165,7 +143,7 @@ Before flight you need to calibrate the accelerometer:
|
|||||||
|
|
||||||
There are several ways to control the drone's flight: using **smartphone** (Wi-Fi), using **SBUS remote control**, or using **USB remote control** (Wi-Fi).
|
There are several ways to control the drone's flight: using **smartphone** (Wi-Fi), using **SBUS remote control**, or using **USB remote control** (Wi-Fi).
|
||||||
|
|
||||||
### Control with a smartphone
|
### Control with smartphone
|
||||||
|
|
||||||
1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone.
|
1. Install [QGroundControl mobile app](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html#android) on your smartphone.
|
||||||
2. Power the drone using the battery.
|
2. Power the drone using the battery.
|
||||||
@@ -175,9 +153,9 @@ There are several ways to control the drone's flight: using **smartphone** (Wi-F
|
|||||||
6. Use the virtual joystick to fly the drone!
|
6. Use the virtual joystick to fly the drone!
|
||||||
|
|
||||||
> [!TIP]
|
> [!TIP]
|
||||||
> Decrease `CTL_TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive.
|
> Decrease `CNT_TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive.
|
||||||
|
|
||||||
### Control with a remote control
|
### Control with remote control
|
||||||
|
|
||||||
Before using remote SBUS-connected remote control, you need to calibrate it:
|
Before using remote SBUS-connected remote control, you need to calibrate it:
|
||||||
|
|
||||||
@@ -185,7 +163,7 @@ Before using remote SBUS-connected remote control, you need to calibrate it:
|
|||||||
2. Type `cr` command and follow the instructions.
|
2. Type `cr` command and follow the instructions.
|
||||||
3. Use the remote control to fly the drone!
|
3. Use the remote control to fly the drone!
|
||||||
|
|
||||||
### Control with a USB remote control
|
### Control with USB remote control
|
||||||
|
|
||||||
If your drone doesn't have RC receiver installed, you can use USB remote control and QGroundControl app to fly it.
|
If your drone doesn't have RC receiver installed, you can use USB remote control and QGroundControl app to fly it.
|
||||||
|
|
||||||
@@ -233,9 +211,9 @@ The default mode is *STAB*. In this mode, the drone stabilizes its attitude (ori
|
|||||||
|
|
||||||
In this mode, the pilot controls the angular rates. This control method is difficult to fly and mostly used in FPV racing.
|
In this mode, the pilot controls the angular rates. This control method is difficult to fly and mostly used in FPV racing.
|
||||||
|
|
||||||
#### RAW
|
#### MANUAL
|
||||||
|
|
||||||
*RAW* mode disables all the stabilization, and the pilot inputs are mixed directly to the motors. The IMU sensor is not involved. This mode is intended for testing and demonstration purposes only, and basically the drone **cannot fly in this mode**.
|
Manual mode disables all the stabilization, and the pilot inputs are passed directly to the motors. This mode is intended for testing and demonstration purposes only, and basically the drone **cannot fly in this mode**.
|
||||||
|
|
||||||
#### AUTO
|
#### AUTO
|
||||||
|
|
||||||
@@ -243,6 +221,12 @@ In this mode, the pilot inputs are ignored (except the mode switch, if configure
|
|||||||
|
|
||||||
If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
|
If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
|
||||||
|
|
||||||
|
## Adjusting parameters
|
||||||
|
|
||||||
|
You can adjust some of the drone's parameters (include PID coefficients) in QGroundControl. In order to do that, go to the QGroundControl menu ⇒ *Vehicle Setup* ⇒ *Parameters*.
|
||||||
|
|
||||||
|
<img src="img/parameters.png" width="400">
|
||||||
|
|
||||||
## Flight log
|
## 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 for that:
|
||||||
|
|||||||
57
docs/user.md
@@ -4,67 +4,12 @@ This page contains user-built drones based on the Flix project. Publish your pro
|
|||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
Author: [goldarte](https://t.me/goldarte).<br>
|
|
||||||
|
|
||||||
<img src="img/user/goldarte/1.jpg" height=150> <img src="img/user/goldarte/2.jpg" height=150>
|
|
||||||
|
|
||||||
**Flight video:**
|
|
||||||
|
|
||||||
<a href="https://drive.google.com/file/d/1nQtFjEcGGLx-l4xkL5ko9ZpOTVU-WDjL/view?usp=sharing"><img height=200 src="img/user/goldarte/video.jpg"></a>
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
## School 548 course
|
|
||||||
|
|
||||||
Special course on quadcopter design and engineering took place in october-november 2025 in School 548, Moscow. The course included UAV control theory, electronics, drone assembly and setup practice, using the Flix project.
|
|
||||||
|
|
||||||
<img height=200 src="img/user/school548/1.jpg"> <img height=200 src="img/user/school548/2.jpg"> <img height=200 src="img/user/school548/3.jpg">
|
|
||||||
|
|
||||||
STL files and other materials: see [here](https://drive.google.com/drive/folders/1wTUzj087LjKQQl3Lz5CjHCuobxoykhyp?usp=share_link).
|
|
||||||
|
|
||||||
### Selected works
|
|
||||||
|
|
||||||
Author: [KiraFlux](https://t.me/@kiraflux_0XC0000005).<br>
|
|
||||||
Description: **custom ESPNOW remote control** was implemented, modified firmware to support ESPNOW protocol.<br>
|
|
||||||
Telegram posts: [1](https://t.me/opensourcequadcopter/106), [2](https://t.me/opensourcequadcopter/114).<br>
|
|
||||||
Modified Flix firmware: https://github.com/KiraFlux/flix/tree/klyax.<br>
|
|
||||||
Remote control project: https://github.com/KiraFlux/ESP32-DJC.<br>
|
|
||||||
Drone design: https://github.com/KiraFlux/Klyax.<br>
|
|
||||||
|
|
||||||
<img src="img/user/school548/kiraflux1.jpg" height=150> <img src="img/user/school548/kiraflux2.jpg" height=150>
|
|
||||||
|
|
||||||
**ESPNOW remote control demonstration**:
|
|
||||||
|
|
||||||
<img height=200 src="img/user/school548/kiraflux-video.jpg"><a href="https://drive.google.com/file/d/1soHDAeHQWnm97Y4dg4nWevJuMiTdJJXW/view?usp=sharing"></a>
|
|
||||||
|
|
||||||
Author: [tolyan4krut](https://t.me/tolyan4krut).<br>
|
|
||||||
Description: the first drone based on ESP32-S3-CAM board **with a camera**, implementing Wi-Fi video streaming. Runs HTTP server and HTTP video stream.<br>
|
|
||||||
Modified Flix firmware: https://github.com/CatRey/Flix-Camera-Streaming.<br>
|
|
||||||
[Telegram post](https://t.me/opensourcequadcopter/117).
|
|
||||||
|
|
||||||
<img src="img/user/school548/tolyan4krut.jpg" height=150>
|
|
||||||
|
|
||||||
**Video streaming and flight demonstration**:
|
|
||||||
|
|
||||||
<a href="https://drive.google.com/file/d/1KuOBsujLsk7q8FoqKD8u7uoq4ptS5onp/view?usp=sharing"><img height=200 src="img/user/school548/tolyan4krut-video.jpg"></a>
|
|
||||||
|
|
||||||
Author: [Vlad Tolshinov](https://t.me/Vlad_Tolshinov).<br>
|
|
||||||
Description: custom frame with enlarged arm length, which provides very high flight stability, 65 mm props.
|
|
||||||
|
|
||||||
<img src="img/user/school548/vlad_tolshinov1.jpg" height=150> <img src="img/user/school548/vlad_tolshinov2.jpg" height=150>
|
|
||||||
|
|
||||||
**Flight video**:
|
|
||||||
|
|
||||||
<a href="https://drive.google.com/file/d/1zu00DZxhC7DJ9Z2mYjtxdNQqOOLAyYbp/view?usp=sharing"><img height=200 src="img/user/school548/vlad_tolshinov-video.jpg"></a>
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
## RoboCamp
|
## RoboCamp
|
||||||
|
|
||||||
Author: RoboCamp participants.<br>
|
Author: RoboCamp participants.<br>
|
||||||
Description: 3D-printed and wooden frames, ESP32 Mini, DC-DC buck-boost converters. BetaFPV LiteRadio 3 to control the drones via Wi-Fi connection.<br>
|
Description: 3D-printed and wooden frames, ESP32 Mini, DC-DC buck-boost converters. BetaFPV LiteRadio 3 to control the drones via Wi-Fi connection.<br>
|
||||||
Features: altitude hold, obstacle avoidance, autonomous flight elements.<br>
|
Features: altitude hold, obstacle avoidance, autonomous flight elements.<br>
|
||||||
Some of the designed model files: see [here](https://drive.google.com/drive/folders/18YHWGquKeIevzrMH4-OUT-zKXMETTEUu?usp=share_link).
|
Some of the designed model files: https://drive.google.com/drive/folders/18YHWGquKeIevzrMH4-OUT-zKXMETTEUu?usp=share_link.
|
||||||
|
|
||||||
RoboCamp took place in July 2025, Saint Petersburg, where 9 participants designed and built their own drones using the Flix project, and then modified the firmware to complete specific flight tasks.
|
RoboCamp took place in July 2025, Saint Petersburg, where 9 participants designed and built their own drones using the Flix project, and then modified the firmware to complete specific flight tasks.
|
||||||
|
|
||||||
|
|||||||
@@ -3,17 +3,15 @@
|
|||||||
|
|
||||||
// Implementation of command line interface
|
// Implementation of command line interface
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
#include "flix.h"
|
|
||||||
#include "pid.h"
|
#include "pid.h"
|
||||||
#include "vector.h"
|
#include "vector.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
|
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
|
||||||
extern const int RAW, ACRO, STAB, AUTO;
|
extern const int ACRO, STAB, AUTO;
|
||||||
extern float t, dt, loopRate;
|
extern float t, dt, loopRate;
|
||||||
extern uint16_t channels[16];
|
extern uint16_t channels[16];
|
||||||
extern float controlTime;
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
||||||
extern int mode;
|
extern int mode;
|
||||||
extern bool armed;
|
extern bool armed;
|
||||||
|
|
||||||
@@ -37,9 +35,8 @@ const char* motd =
|
|||||||
"imu - show IMU data\n"
|
"imu - show IMU data\n"
|
||||||
"arm - arm the drone\n"
|
"arm - arm the drone\n"
|
||||||
"disarm - disarm the drone\n"
|
"disarm - disarm the drone\n"
|
||||||
"raw/stab/acro/auto - set mode\n"
|
"stab/acro/auto - set mode\n"
|
||||||
"rc - show RC data\n"
|
"rc - show RC data\n"
|
||||||
"wifi - show Wi-Fi info\n"
|
|
||||||
"mot - show motor output\n"
|
"mot - show motor output\n"
|
||||||
"log [dump] - print log header [and data]\n"
|
"log [dump] - print log header [and data]\n"
|
||||||
"cr - calibrate RC\n"
|
"cr - calibrate RC\n"
|
||||||
@@ -73,7 +70,7 @@ void pause(float duration) {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void doCommand(String str, bool echo) {
|
void doCommand(String str, bool echo = false) {
|
||||||
// parse command
|
// parse command
|
||||||
String command, arg0, arg1;
|
String command, arg0, arg1;
|
||||||
splitString(str, command, arg0, arg1);
|
splitString(str, command, arg0, arg1);
|
||||||
@@ -119,8 +116,6 @@ void doCommand(String str, bool echo) {
|
|||||||
armed = true;
|
armed = true;
|
||||||
} else if (command == "disarm") {
|
} else if (command == "disarm") {
|
||||||
armed = false;
|
armed = false;
|
||||||
} else if (command == "raw") {
|
|
||||||
mode = RAW;
|
|
||||||
} else if (command == "stab") {
|
} else if (command == "stab") {
|
||||||
mode = STAB;
|
mode = STAB;
|
||||||
} else if (command == "acro") {
|
} else if (command == "acro") {
|
||||||
@@ -134,13 +129,8 @@ void doCommand(String str, bool echo) {
|
|||||||
}
|
}
|
||||||
print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n",
|
print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n",
|
||||||
controlRoll, controlPitch, controlYaw, controlThrottle, controlMode);
|
controlRoll, controlPitch, controlYaw, controlThrottle, controlMode);
|
||||||
print("time: %.1f\n", controlTime);
|
|
||||||
print("mode: %s\n", getModeName());
|
print("mode: %s\n", getModeName());
|
||||||
print("armed: %d\n", armed);
|
print("armed: %d\n", armed);
|
||||||
} else if (command == "wifi") {
|
|
||||||
#if WIFI_ENABLED
|
|
||||||
printWiFiInfo();
|
|
||||||
#endif
|
|
||||||
} else if (command == "mot") {
|
} else if (command == "mot") {
|
||||||
print("front-right %g front-left %g rear-right %g rear-left %g\n",
|
print("front-right %g front-left %g rear-right %g rear-left %g\n",
|
||||||
motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);
|
motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);
|
||||||
@@ -1,55 +0,0 @@
|
|||||||
// Wi-Fi
|
|
||||||
#define WIFI_ENABLED 1
|
|
||||||
#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"
|
|
||||||
|
|
||||||
// Motors
|
|
||||||
#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
|
|
||||||
|
|
||||||
// Control
|
|
||||||
#define PITCHRATE_P 0.05
|
|
||||||
#define PITCHRATE_I 0.2
|
|
||||||
#define PITCHRATE_D 0.001
|
|
||||||
#define PITCHRATE_I_LIM 0.3
|
|
||||||
#define ROLLRATE_P PITCHRATE_P
|
|
||||||
#define ROLLRATE_I PITCHRATE_I
|
|
||||||
#define ROLLRATE_D PITCHRATE_D
|
|
||||||
#define ROLLRATE_I_LIM PITCHRATE_I_LIM
|
|
||||||
#define YAWRATE_P 0.3
|
|
||||||
#define YAWRATE_I 0.0
|
|
||||||
#define YAWRATE_D 0.0
|
|
||||||
#define YAWRATE_I_LIM 0.3
|
|
||||||
#define ROLL_P 6
|
|
||||||
#define ROLL_I 0
|
|
||||||
#define ROLL_D 0
|
|
||||||
#define PITCH_P ROLL_P
|
|
||||||
#define PITCH_I ROLL_I
|
|
||||||
#define PITCH_D ROLL_D
|
|
||||||
#define YAW_P 3
|
|
||||||
#define PITCHRATE_MAX radians(360)
|
|
||||||
#define ROLLRATE_MAX radians(360)
|
|
||||||
#define YAWRATE_MAX radians(300)
|
|
||||||
#define TILT_MAX radians(30)
|
|
||||||
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
|
||||||
|
|
||||||
// Estimation
|
|
||||||
#define WEIGHT_ACC 0.003
|
|
||||||
#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
|
||||||
|
|
||||||
// MAVLink
|
|
||||||
#define SYSTEM_ID 1
|
|
||||||
|
|
||||||
// Safety
|
|
||||||
#define RC_LOSS_TIMEOUT 1
|
|
||||||
#define DESCEND_TIME 10
|
|
||||||
@@ -3,25 +3,41 @@
|
|||||||
|
|
||||||
// Flight control
|
// Flight control
|
||||||
|
|
||||||
#include "config.h"
|
|
||||||
#include "flix.h"
|
|
||||||
#include "vector.h"
|
#include "vector.h"
|
||||||
#include "quaternion.h"
|
#include "quaternion.h"
|
||||||
#include "pid.h"
|
#include "pid.h"
|
||||||
#include "lpf.h"
|
#include "lpf.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
extern const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
|
#define PITCHRATE_P 0.05
|
||||||
|
#define PITCHRATE_I 0.2
|
||||||
|
#define PITCHRATE_D 0.001
|
||||||
|
#define PITCHRATE_I_LIM 0.3
|
||||||
|
#define ROLLRATE_P PITCHRATE_P
|
||||||
|
#define ROLLRATE_I PITCHRATE_I
|
||||||
|
#define ROLLRATE_D PITCHRATE_D
|
||||||
|
#define ROLLRATE_I_LIM PITCHRATE_I_LIM
|
||||||
|
#define YAWRATE_P 0.3
|
||||||
|
#define YAWRATE_I 0.0
|
||||||
|
#define YAWRATE_D 0.0
|
||||||
|
#define YAWRATE_I_LIM 0.3
|
||||||
|
#define ROLL_P 6
|
||||||
|
#define ROLL_I 0
|
||||||
|
#define ROLL_D 0
|
||||||
|
#define PITCH_P ROLL_P
|
||||||
|
#define PITCH_I ROLL_I
|
||||||
|
#define PITCH_D ROLL_D
|
||||||
|
#define YAW_P 3
|
||||||
|
#define PITCHRATE_MAX radians(360)
|
||||||
|
#define ROLLRATE_MAX radians(360)
|
||||||
|
#define YAWRATE_MAX radians(300)
|
||||||
|
#define TILT_MAX radians(30)
|
||||||
|
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
||||||
|
|
||||||
|
const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
|
||||||
int mode = STAB;
|
int mode = STAB;
|
||||||
bool armed = false;
|
bool armed = false;
|
||||||
|
|
||||||
Quaternion attitudeTarget;
|
|
||||||
Vector ratesTarget;
|
|
||||||
Vector ratesExtra; // feedforward rates
|
|
||||||
Vector torqueTarget;
|
|
||||||
float thrustTarget;
|
|
||||||
|
|
||||||
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
|
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
|
||||||
PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
|
PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
|
||||||
PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
|
PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
|
||||||
@@ -31,6 +47,12 @@ PID yawPID(YAW_P, 0, 0);
|
|||||||
Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
|
Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
|
||||||
float tiltMax = TILT_MAX;
|
float tiltMax = TILT_MAX;
|
||||||
|
|
||||||
|
Quaternion attitudeTarget;
|
||||||
|
Vector ratesTarget;
|
||||||
|
Vector ratesExtra; // feedforward rates
|
||||||
|
Vector torqueTarget;
|
||||||
|
float thrustTarget;
|
||||||
|
|
||||||
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
|
extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
|
||||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
||||||
|
|
||||||
@@ -43,6 +65,7 @@ void control() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void interpretControls() {
|
void interpretControls() {
|
||||||
|
// NOTE: put ACRO or MANUAL modes there if you want to use them
|
||||||
if (controlMode < 0.25) mode = STAB;
|
if (controlMode < 0.25) mode = STAB;
|
||||||
if (controlMode < 0.75) mode = STAB;
|
if (controlMode < 0.75) mode = STAB;
|
||||||
if (controlMode > 0.75) mode = STAB;
|
if (controlMode > 0.75) mode = STAB;
|
||||||
@@ -52,8 +75,6 @@ void interpretControls() {
|
|||||||
if (controlThrottle < 0.05 && controlYaw > 0.95) armed = true; // arm gesture
|
if (controlThrottle < 0.05 && controlYaw > 0.95) armed = true; // arm gesture
|
||||||
if (controlThrottle < 0.05 && controlYaw < -0.95) armed = false; // disarm gesture
|
if (controlThrottle < 0.05 && controlYaw < -0.95) armed = false; // disarm gesture
|
||||||
|
|
||||||
if (abs(controlYaw) < 0.1) controlYaw = 0; // yaw dead zone
|
|
||||||
|
|
||||||
thrustTarget = controlThrottle;
|
thrustTarget = controlThrottle;
|
||||||
|
|
||||||
if (mode == STAB) {
|
if (mode == STAB) {
|
||||||
@@ -70,10 +91,10 @@ void interpretControls() {
|
|||||||
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
|
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mode == RAW) { // direct torque control
|
if (mode == MANUAL) { // passthrough mode
|
||||||
attitudeTarget.invalidate(); // skip attitude control
|
attitudeTarget.invalidate(); // skip attitude control
|
||||||
ratesTarget.invalidate(); // skip rate control
|
ratesTarget.invalidate(); // skip rate control
|
||||||
torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.1;
|
torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -134,7 +155,7 @@ void controlTorque() {
|
|||||||
|
|
||||||
const char* getModeName() {
|
const char* getModeName() {
|
||||||
switch (mode) {
|
switch (mode) {
|
||||||
case RAW: return "RAW";
|
case MANUAL: return "MANUAL";
|
||||||
case ACRO: return "ACRO";
|
case ACRO: return "ACRO";
|
||||||
case STAB: return "STAB";
|
case STAB: return "STAB";
|
||||||
case AUTO: return "AUTO";
|
case AUTO: return "AUTO";
|
||||||
@@ -3,19 +3,13 @@
|
|||||||
|
|
||||||
// Attitude estimation from gyro and accelerometer
|
// Attitude estimation from gyro and accelerometer
|
||||||
|
|
||||||
#include "config.h"
|
|
||||||
#include "flix.h"
|
|
||||||
#include "quaternion.h"
|
#include "quaternion.h"
|
||||||
#include "vector.h"
|
#include "vector.h"
|
||||||
#include "lpf.h"
|
#include "lpf.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
Vector rates; // estimated angular rates, rad/s
|
#define WEIGHT_ACC 0.003
|
||||||
Quaternion attitude; // estimated attitude
|
#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
||||||
bool landed;
|
|
||||||
|
|
||||||
float accWeight = 0.003;
|
|
||||||
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
|
|
||||||
|
|
||||||
void estimate() {
|
void estimate() {
|
||||||
applyGyro();
|
applyGyro();
|
||||||
@@ -24,6 +18,7 @@ void estimate() {
|
|||||||
|
|
||||||
void applyGyro() {
|
void applyGyro() {
|
||||||
// filter gyro to get angular rates
|
// filter gyro to get angular rates
|
||||||
|
static LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
|
||||||
rates = ratesFilter.update(gyro);
|
rates = ratesFilter.update(gyro);
|
||||||
|
|
||||||
// apply rates to attitude
|
// apply rates to attitude
|
||||||
@@ -39,7 +34,7 @@ void applyAcc() {
|
|||||||
|
|
||||||
// calculate accelerometer correction
|
// calculate accelerometer correction
|
||||||
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
|
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
|
||||||
Vector correction = Vector::rotationVectorBetween(acc, up) * accWeight;
|
Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
|
||||||
|
|
||||||
// apply correction
|
// apply correction
|
||||||
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
||||||
90
flix/flix.h
@@ -1,90 +0,0 @@
|
|||||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
||||||
// Repository: https://github.com/okalachev/flix
|
|
||||||
|
|
||||||
// All-in-one header file
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
#include "vector.h"
|
|
||||||
#include "quaternion.h"
|
|
||||||
|
|
||||||
extern float t, dt;
|
|
||||||
extern float loopRate;
|
|
||||||
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
|
||||||
extern Vector gyro, acc;
|
|
||||||
extern Vector rates;
|
|
||||||
extern Quaternion attitude;
|
|
||||||
extern bool landed;
|
|
||||||
extern int mode;
|
|
||||||
extern bool armed;
|
|
||||||
extern Quaternion attitudeTarget;
|
|
||||||
extern Vector ratesTarget, ratesExtra, torqueTarget;
|
|
||||||
extern float thrustTarget;
|
|
||||||
extern float motors[4];
|
|
||||||
|
|
||||||
void print(const char* format, ...);
|
|
||||||
void pause(float duration);
|
|
||||||
void doCommand(String str, bool echo = false);
|
|
||||||
void handleInput();
|
|
||||||
void control();
|
|
||||||
void interpretControls();
|
|
||||||
void controlAttitude();
|
|
||||||
void controlRates();
|
|
||||||
void controlTorque();
|
|
||||||
const char *getModeName();
|
|
||||||
void estimate();
|
|
||||||
void applyGyro();
|
|
||||||
void applyAcc();
|
|
||||||
void setupIMU();
|
|
||||||
void configureIMU();
|
|
||||||
void readIMU();
|
|
||||||
void rotateIMU(Vector& data);
|
|
||||||
void calibrateGyroOnce();
|
|
||||||
void calibrateAccel();
|
|
||||||
void calibrateAccelOnce();
|
|
||||||
void printIMUCalibration();
|
|
||||||
void printIMUInfo();
|
|
||||||
void setupLED();
|
|
||||||
void setLED(bool on);
|
|
||||||
void blinkLED();
|
|
||||||
void prepareLogData();
|
|
||||||
void logData();
|
|
||||||
void printLogHeader();
|
|
||||||
void printLogData();
|
|
||||||
void processMavlink();
|
|
||||||
void sendMavlink();
|
|
||||||
void sendMessage(const void *msg);
|
|
||||||
void receiveMavlink();
|
|
||||||
void handleMavlink(const void *_msg);
|
|
||||||
void mavlinkPrint(const char* str);
|
|
||||||
void sendMavlinkPrint();
|
|
||||||
void setupMotors();
|
|
||||||
int getDutyCycle(float value);
|
|
||||||
void sendMotors();
|
|
||||||
bool motorsActive();
|
|
||||||
void testMotor(int n);
|
|
||||||
void setupParameters();
|
|
||||||
int parametersCount();
|
|
||||||
const char *getParameterName(int index);
|
|
||||||
float getParameter(int index);
|
|
||||||
float getParameter(const char *name);
|
|
||||||
bool setParameter(const char *name, const float value);
|
|
||||||
void syncParameters();
|
|
||||||
void printParameters();
|
|
||||||
void resetParameters();
|
|
||||||
void setupRC();
|
|
||||||
bool readRC();
|
|
||||||
void normalizeRC();
|
|
||||||
void calibrateRC();
|
|
||||||
void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str);
|
|
||||||
void printRCCalibration();
|
|
||||||
void failsafe();
|
|
||||||
void rcLossFailsafe();
|
|
||||||
void descend();
|
|
||||||
void autoFailsafe();
|
|
||||||
void step();
|
|
||||||
void computeLoopRate();
|
|
||||||
void setupWiFi();
|
|
||||||
void sendWiFi(const uint8_t *buf, int len);
|
|
||||||
int receiveWiFi(uint8_t *buf, int len);
|
|
||||||
@@ -3,11 +3,22 @@
|
|||||||
|
|
||||||
// Main firmware file
|
// Main firmware file
|
||||||
|
|
||||||
#include "config.h"
|
|
||||||
#include "vector.h"
|
#include "vector.h"
|
||||||
#include "quaternion.h"
|
#include "quaternion.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
#include "flix.h"
|
|
||||||
|
#define WIFI_ENABLED 1
|
||||||
|
|
||||||
|
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() {
|
void setup() {
|
||||||
Serial.begin(115200);
|
Serial.begin(115200);
|
||||||
|
|||||||
@@ -10,14 +10,10 @@
|
|||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
MPU9250 imu(SPI);
|
MPU9250 imu(SPI);
|
||||||
Vector imuRotation(0, 0, -PI / 2); // imu orientation as Euler angles
|
|
||||||
|
|
||||||
Vector gyro; // gyroscope output, rad/s
|
|
||||||
Vector gyroBias;
|
|
||||||
|
|
||||||
Vector acc; // accelerometer output, m/s/s
|
|
||||||
Vector accBias;
|
Vector accBias;
|
||||||
Vector accScale(1, 1, 1);
|
Vector accScale(1, 1, 1);
|
||||||
|
Vector gyroBias;
|
||||||
|
|
||||||
void setupIMU() {
|
void setupIMU() {
|
||||||
print("Setup IMU\n");
|
print("Setup IMU\n");
|
||||||
@@ -41,18 +37,24 @@ void readIMU() {
|
|||||||
// apply scale and bias
|
// apply scale and bias
|
||||||
acc = (acc - accBias) / accScale;
|
acc = (acc - accBias) / accScale;
|
||||||
gyro = gyro - gyroBias;
|
gyro = gyro - gyroBias;
|
||||||
// rotate to body frame
|
// rotate
|
||||||
Quaternion rotation = Quaternion::fromEuler(imuRotation);
|
rotateIMU(acc);
|
||||||
acc = Quaternion::rotateVector(acc, rotation.inversed());
|
rotateIMU(gyro);
|
||||||
gyro = Quaternion::rotateVector(gyro, rotation.inversed());
|
}
|
||||||
|
|
||||||
|
void rotateIMU(Vector& data) {
|
||||||
|
// Rotate from LFD to FLU
|
||||||
|
// NOTE: In case of using other IMU orientation, change this line:
|
||||||
|
data = Vector(data.y, data.x, -data.z);
|
||||||
|
// Axes orientation for various boards: https://github.com/okalachev/flixperiph#imu-axes-orientation
|
||||||
}
|
}
|
||||||
|
|
||||||
void calibrateGyroOnce() {
|
void calibrateGyroOnce() {
|
||||||
static Delay landedDelay(2);
|
static Delay landedDelay(2);
|
||||||
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
|
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
|
||||||
|
|
||||||
static LowPassFilter<Vector> gyroBiasFilter(0.001);
|
static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
|
||||||
gyroBias = gyroBiasFilter.update(gyro);
|
gyroBias = gyroCalibrationFilter.update(gyro);
|
||||||
}
|
}
|
||||||
|
|
||||||
void calibrateAccel() {
|
void calibrateAccel() {
|
||||||
@@ -3,8 +3,6 @@
|
|||||||
|
|
||||||
// Board's LED control
|
// Board's LED control
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
|
|
||||||
#define BLINK_PERIOD 500000
|
#define BLINK_PERIOD 500000
|
||||||
|
|
||||||
#ifndef LED_BUILTIN
|
#ifndef LED_BUILTIN
|
||||||
@@ -3,7 +3,6 @@
|
|||||||
|
|
||||||
// In-RAM logging
|
// In-RAM logging
|
||||||
|
|
||||||
#include "flix.h"
|
|
||||||
#include "vector.h"
|
#include "vector.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
@@ -5,8 +5,6 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
|
|
||||||
template <typename T> // Using template to make the filter usable for scalar and vector values
|
template <typename T> // Using template to make the filter usable for scalar and vector values
|
||||||
class LowPassFilter {
|
class LowPassFilter {
|
||||||
public:
|
public:
|
||||||
|
|||||||
@@ -3,10 +3,6 @@
|
|||||||
|
|
||||||
// MAVLink communication
|
// MAVLink communication
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
#include "config.h"
|
|
||||||
#include "flix.h"
|
|
||||||
|
|
||||||
#if WIFI_ENABLED
|
#if WIFI_ENABLED
|
||||||
|
|
||||||
#include <MAVLink.h>
|
#include <MAVLink.h>
|
||||||
@@ -15,14 +11,14 @@
|
|||||||
#define SYSTEM_ID 1
|
#define SYSTEM_ID 1
|
||||||
#define MAVLINK_RATE_SLOW 1
|
#define MAVLINK_RATE_SLOW 1
|
||||||
#define MAVLINK_RATE_FAST 10
|
#define MAVLINK_RATE_FAST 10
|
||||||
|
#define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
|
||||||
extern const int AUTO, STAB;
|
|
||||||
extern uint16_t channels[16];
|
|
||||||
extern float controlTime;
|
|
||||||
|
|
||||||
bool mavlinkConnected = false;
|
bool mavlinkConnected = false;
|
||||||
String mavlinkPrintBuffer;
|
String mavlinkPrintBuffer;
|
||||||
|
|
||||||
|
extern float controlTime;
|
||||||
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
||||||
|
|
||||||
void processMavlink() {
|
void processMavlink() {
|
||||||
sendMavlink();
|
sendMavlink();
|
||||||
receiveMavlink();
|
receiveMavlink();
|
||||||
@@ -109,6 +105,8 @@ void handleMavlink(const void *_msg) {
|
|||||||
controlYaw = m.r / 1000.0f;
|
controlYaw = m.r / 1000.0f;
|
||||||
controlMode = NAN;
|
controlMode = NAN;
|
||||||
controlTime = t;
|
controlTime = t;
|
||||||
|
|
||||||
|
if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
|
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
|
||||||
@@ -212,7 +210,6 @@ void handleMavlink(const void *_msg) {
|
|||||||
armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
|
armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* TODO:
|
|
||||||
if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
|
if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
|
||||||
mavlink_log_request_data_t m;
|
mavlink_log_request_data_t m;
|
||||||
mavlink_msg_log_request_data_decode(&msg, &m);
|
mavlink_msg_log_request_data_decode(&msg, &m);
|
||||||
@@ -226,7 +223,6 @@ void handleMavlink(const void *_msg) {
|
|||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
*/
|
|
||||||
|
|
||||||
// Handle commands
|
// Handle commands
|
||||||
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
|
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
|
||||||
@@ -4,17 +4,24 @@
|
|||||||
// Motors output control using MOSFETs
|
// 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)
|
// In case of using ESCs, change PWM_STOP, PWM_MIN and PWM_MAX to appropriate values in μs, decrease PWM_FREQUENCY (to 400)
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
#include "config.h"
|
|
||||||
#include "flix.h"
|
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
float motors[4]; // normalized motor thrusts in range [0..1]
|
#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
|
||||||
|
|
||||||
extern const int MOTOR_REAR_LEFT = 0;
|
#define PWM_FREQUENCY 78000
|
||||||
extern const int MOTOR_REAR_RIGHT = 1;
|
#define PWM_RESOLUTION 10
|
||||||
extern const int MOTOR_FRONT_RIGHT = 2;
|
#define PWM_STOP 0
|
||||||
extern const int MOTOR_FRONT_LEFT = 3;
|
#define PWM_MIN 0
|
||||||
|
#define PWM_MAX 1000000 / PWM_FREQUENCY
|
||||||
|
|
||||||
|
// Motors array indexes:
|
||||||
|
const int MOTOR_REAR_LEFT = 0;
|
||||||
|
const int MOTOR_REAR_RIGHT = 1;
|
||||||
|
const int MOTOR_FRONT_RIGHT = 2;
|
||||||
|
const int MOTOR_FRONT_LEFT = 3;
|
||||||
|
|
||||||
void setupMotors() {
|
void setupMotors() {
|
||||||
print("Setup Motors\n");
|
print("Setup Motors\n");
|
||||||
@@ -4,22 +4,11 @@
|
|||||||
// Parameters storage in flash memory
|
// Parameters storage in flash memory
|
||||||
|
|
||||||
#include <Preferences.h>
|
#include <Preferences.h>
|
||||||
#include "flix.h"
|
|
||||||
#include "pid.h"
|
|
||||||
#include "lpf.h"
|
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
extern float channelZero[16];
|
extern float channelZero[16];
|
||||||
extern float channelMax[16];
|
extern float channelMax[16];
|
||||||
extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
|
extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
|
||||||
extern float tiltMax;
|
|
||||||
extern PID rollPID, pitchPID, yawPID;
|
|
||||||
extern PID rollRatePID, pitchRatePID, yawRatePID;
|
|
||||||
extern Vector maxRate;
|
|
||||||
extern Vector imuRotation;
|
|
||||||
extern Vector accBias, accScale;
|
|
||||||
extern float accWeight;
|
|
||||||
extern LowPassFilter<Vector> ratesFilter;
|
|
||||||
|
|
||||||
Preferences storage;
|
Preferences storage;
|
||||||
|
|
||||||
@@ -54,18 +43,12 @@ Parameter parameters[] = {
|
|||||||
{"CTL_Y_RATE_MAX", &maxRate.z},
|
{"CTL_Y_RATE_MAX", &maxRate.z},
|
||||||
{"CTL_TILT_MAX", &tiltMax},
|
{"CTL_TILT_MAX", &tiltMax},
|
||||||
// imu
|
// imu
|
||||||
{"IMU_ROT_ROLL", &imuRotation.x},
|
|
||||||
{"IMU_ROT_PITCH", &imuRotation.y},
|
|
||||||
{"IMU_ROT_YAW", &imuRotation.z},
|
|
||||||
{"IMU_ACC_BIAS_X", &accBias.x},
|
{"IMU_ACC_BIAS_X", &accBias.x},
|
||||||
{"IMU_ACC_BIAS_Y", &accBias.y},
|
{"IMU_ACC_BIAS_Y", &accBias.y},
|
||||||
{"IMU_ACC_BIAS_Z", &accBias.z},
|
{"IMU_ACC_BIAS_Z", &accBias.z},
|
||||||
{"IMU_ACC_SCALE_X", &accScale.x},
|
{"IMU_ACC_SCALE_X", &accScale.x},
|
||||||
{"IMU_ACC_SCALE_Y", &accScale.y},
|
{"IMU_ACC_SCALE_Y", &accScale.y},
|
||||||
{"IMU_ACC_SCALE_Z", &accScale.z},
|
{"IMU_ACC_SCALE_Z", &accScale.z},
|
||||||
// estimate
|
|
||||||
{"EST_ACC_WEIGHT", &accWeight},
|
|
||||||
{"EST_RATES_LPF_A", &ratesFilter.alpha},
|
|
||||||
// rc
|
// rc
|
||||||
{"RC_ZERO_0", &channelZero[0]},
|
{"RC_ZERO_0", &channelZero[0]},
|
||||||
{"RC_ZERO_1", &channelZero[1]},
|
{"RC_ZERO_1", &channelZero[1]},
|
||||||
@@ -5,8 +5,6 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "Arduino.h"
|
|
||||||
#include "flix.h"
|
|
||||||
#include "lpf.h"
|
#include "lpf.h"
|
||||||
|
|
||||||
class PID {
|
class PID {
|
||||||
|
|||||||
@@ -5,7 +5,6 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
#include "vector.h"
|
#include "vector.h"
|
||||||
|
|
||||||
class Quaternion : public Printable {
|
class Quaternion : public Printable {
|
||||||
|
|||||||
@@ -6,16 +6,13 @@
|
|||||||
#include <SBUS.h>
|
#include <SBUS.h>
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
SBUS rc(Serial2);
|
SBUS rc(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
|
||||||
|
|
||||||
uint16_t channels[16]; // raw rc channels
|
uint16_t channels[16]; // raw rc channels
|
||||||
|
float controlTime; // time of the last controls update
|
||||||
float channelZero[16]; // calibration zero values
|
float channelZero[16]; // calibration zero values
|
||||||
float channelMax[16]; // calibration max values
|
float channelMax[16]; // calibration max values
|
||||||
|
|
||||||
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
|
||||||
float controlMode = NAN; //
|
|
||||||
float controlTime; // time of the last controls update (0 when no RC)
|
|
||||||
|
|
||||||
// Channels mapping (using float to store in parameters):
|
// Channels mapping (using float to store in parameters):
|
||||||
float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
|
float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
|
||||||
|
|
||||||
@@ -41,11 +38,11 @@ void normalizeRC() {
|
|||||||
controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
|
controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
|
||||||
}
|
}
|
||||||
// Update control values
|
// Update control values
|
||||||
controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;
|
controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : NAN;
|
||||||
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
|
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
|
||||||
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
|
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
|
||||||
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
|
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
|
||||||
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
|
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
|
||||||
}
|
}
|
||||||
|
|
||||||
void calibrateRC() {
|
void calibrateRC() {
|
||||||
@@ -3,11 +3,11 @@
|
|||||||
|
|
||||||
// Fail-safe functions
|
// Fail-safe functions
|
||||||
|
|
||||||
#include "config.h"
|
#define RC_LOSS_TIMEOUT 1
|
||||||
#include "flix.h"
|
#define DESCEND_TIME 10
|
||||||
|
|
||||||
extern float controlTime;
|
extern float controlTime;
|
||||||
extern const int AUTO, STAB;
|
extern float controlRoll, controlPitch, controlThrottle, controlYaw;
|
||||||
|
|
||||||
void failsafe() {
|
void failsafe() {
|
||||||
rcLossFailsafe();
|
rcLossFailsafe();
|
||||||
@@ -3,11 +3,6 @@
|
|||||||
|
|
||||||
// Time related functions
|
// Time related functions
|
||||||
|
|
||||||
#include "Arduino.h"
|
|
||||||
#include "flix.h"
|
|
||||||
|
|
||||||
float t = NAN; // current time, s
|
|
||||||
float dt; // time delta with the previous step, s
|
|
||||||
float loopRate; // Hz
|
float loopRate; // Hz
|
||||||
|
|
||||||
void step() {
|
void step() {
|
||||||
14
flix/util.h
@@ -8,24 +8,24 @@
|
|||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <soc/soc.h>
|
#include <soc/soc.h>
|
||||||
#include <soc/rtc_cntl_reg.h>
|
#include <soc/rtc_cntl_reg.h>
|
||||||
#include "flix.h"
|
|
||||||
|
|
||||||
const float ONE_G = 9.80665;
|
const float ONE_G = 9.80665;
|
||||||
|
extern float t;
|
||||||
|
|
||||||
inline float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
|
float mapf(float x, float in_min, float in_max, float out_min, float out_max) {
|
||||||
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool invalid(float x) {
|
bool invalid(float x) {
|
||||||
return !isfinite(x);
|
return !isfinite(x);
|
||||||
}
|
}
|
||||||
|
|
||||||
inline bool valid(float x) {
|
bool valid(float x) {
|
||||||
return isfinite(x);
|
return isfinite(x);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Wrap angle to [-PI, PI)
|
// Wrap angle to [-PI, PI)
|
||||||
inline float wrapAngle(float angle) {
|
float wrapAngle(float angle) {
|
||||||
angle = fmodf(angle, 2 * PI);
|
angle = fmodf(angle, 2 * PI);
|
||||||
if (angle > PI) {
|
if (angle > PI) {
|
||||||
angle -= 2 * PI;
|
angle -= 2 * PI;
|
||||||
@@ -36,12 +36,12 @@ inline float wrapAngle(float angle) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Disable reset on low voltage
|
// Disable reset on low voltage
|
||||||
inline void disableBrownOut() {
|
void disableBrownOut() {
|
||||||
REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
|
REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Trim and split string by spaces
|
// Trim and split string by spaces
|
||||||
inline void splitString(String& str, String& token0, String& token1, String& token2) {
|
void splitString(String& str, String& token0, String& token1, String& token2) {
|
||||||
str.trim();
|
str.trim();
|
||||||
char chars[str.length() + 1];
|
char chars[str.length() + 1];
|
||||||
str.toCharArray(chars, str.length() + 1);
|
str.toCharArray(chars, str.length() + 1);
|
||||||
|
|||||||
@@ -5,8 +5,6 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
|
|
||||||
class Vector : public Printable {
|
class Vector : public Printable {
|
||||||
public:
|
public:
|
||||||
float x, y, z;
|
float x, y, z;
|
||||||
@@ -37,6 +35,7 @@ public:
|
|||||||
z = NAN;
|
z = NAN;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
float norm() const {
|
float norm() const {
|
||||||
return sqrt(x * x + y * y + z * z);
|
return sqrt(x * x + y * y + z * z);
|
||||||
}
|
}
|
||||||
@@ -125,5 +124,5 @@ public:
|
|||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
inline Vector operator * (const float a, const Vector& b) { return b * a; }
|
Vector operator * (const float a, const Vector& b) { return b * a; }
|
||||||
inline Vector operator + (const float a, const Vector& b) { return b + a; }
|
Vector operator + (const float a, const Vector& b) { return b + a; }
|
||||||
|
|||||||
@@ -3,22 +3,30 @@
|
|||||||
|
|
||||||
// Wi-Fi support
|
// Wi-Fi support
|
||||||
|
|
||||||
#include "config.h"
|
|
||||||
#include "flix.h"
|
|
||||||
|
|
||||||
#if WIFI_ENABLED
|
#if WIFI_ENABLED
|
||||||
|
|
||||||
#include <WiFi.h>
|
#include <WiFi.h>
|
||||||
#include <WiFiAP.h>
|
#include <WiFiAP.h>
|
||||||
#include <WiFiUdp.h>
|
#include <WiFiUdp.h>
|
||||||
|
|
||||||
WiFiUDP udp;
|
#define WIFI_AP_MODE 1
|
||||||
|
#define WIFI_AP_SSID "flix"
|
||||||
|
#define WIFI_AP_PASSWORD "flixwifi"
|
||||||
|
#define WIFI_SSID ""
|
||||||
|
#define WIFI_PASSWORD ""
|
||||||
|
#define WIFI_UDP_PORT 14550
|
||||||
|
#define WIFI_UDP_REMOTE_PORT 14550
|
||||||
|
#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
|
||||||
|
|
||||||
extern bool mavlinkConnected;
|
WiFiUDP udp;
|
||||||
|
|
||||||
void setupWiFi() {
|
void setupWiFi() {
|
||||||
print("Setup Wi-Fi\n");
|
print("Setup Wi-Fi\n");
|
||||||
WiFi.softAP(WIFI_SSID, WIFI_PASSWORD);
|
if (WIFI_AP_MODE) {
|
||||||
|
WiFi.softAP(WIFI_AP_SSID, WIFI_AP_PASSWORD);
|
||||||
|
} else {
|
||||||
|
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
|
||||||
|
}
|
||||||
udp.begin(WIFI_UDP_PORT);
|
udp.begin(WIFI_UDP_PORT);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -34,15 +42,4 @@ int receiveWiFi(uint8_t *buf, int len) {
|
|||||||
return udp.read(buf, len);
|
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());
|
|
||||||
print("MAVLink connected: %d\n", mavlinkConnected);
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
@@ -68,9 +68,6 @@ Just like the real drone, the simulator can be controlled using a USB remote con
|
|||||||
6. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
|
6. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
|
||||||
7. Use the virtual joystick to fly the drone!
|
7. Use the virtual joystick to fly the drone!
|
||||||
|
|
||||||
> [!TIP]
|
|
||||||
> Decrease `CTL_TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive.
|
|
||||||
|
|
||||||
### Control with USB remote control
|
### Control with USB remote control
|
||||||
|
|
||||||
1. Connect your USB remote control to the machine running the simulator.
|
1. Connect your USB remote control to the machine running the simulator.
|
||||||
|
|||||||
@@ -12,15 +12,16 @@
|
|||||||
|
|
||||||
#define WIFI_ENABLED 1
|
#define WIFI_ENABLED 1
|
||||||
|
|
||||||
extern float t, dt;
|
float t = NAN;
|
||||||
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
float dt;
|
||||||
extern Vector rates;
|
float motors[4];
|
||||||
extern Quaternion attitude;
|
float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
|
||||||
extern bool landed;
|
float controlMode = NAN;
|
||||||
extern float motors[4];
|
Vector acc;
|
||||||
|
Vector gyro;
|
||||||
Vector gyro, acc, imuRotation;
|
Vector rates;
|
||||||
Vector accBias, gyroBias, accScale(1, 1, 1);
|
Quaternion attitude;
|
||||||
|
bool landed;
|
||||||
|
|
||||||
// declarations
|
// declarations
|
||||||
void step();
|
void step();
|
||||||
@@ -72,4 +73,4 @@ void calibrateGyro() { print("Skip gyro calibrating\n"); };
|
|||||||
void calibrateAccel() { print("Skip accel calibrating\n"); };
|
void calibrateAccel() { print("Skip accel calibrating\n"); };
|
||||||
void printIMUCalibration() { print("cal: N/A\n"); };
|
void printIMUCalibration() { print("cal: N/A\n"); };
|
||||||
void printIMUInfo() {};
|
void printIMUInfo() {};
|
||||||
void printWiFiInfo() {};
|
Vector accBias, gyroBias, accScale(1, 1, 1);
|
||||||
|
|||||||
@@ -1,8 +1,8 @@
|
|||||||
# Flix Python library
|
# Flix Python library
|
||||||
|
|
||||||
The Flix Python library allows you to remotely connect to a Flix quadcopter. It provides access to telemetry data, supports executing console commands, and controlling the drone's flight.
|
The Flix Python library allows you to remotely connect to a Flix quadcopter. It provides access to telemetry data, supports executing CLI commands, and controlling the drone's flight.
|
||||||
|
|
||||||
To use the library, connect to the drone's Wi-Fi. To use it with the simulator, ensure the script runs on the same network as the simulator.
|
To use the library, connect to the drone's Wi-Fi. To use it with the simulator, ensure the script runs on the same local network as the simulator.
|
||||||
|
|
||||||
## Installation
|
## Installation
|
||||||
|
|
||||||
@@ -30,7 +30,7 @@ flix = Flix() # create a Flix object and wait for connection
|
|||||||
|
|
||||||
### Telemetry
|
### Telemetry
|
||||||
|
|
||||||
Basic telemetry is available through object properties. The property names generally match the corresponding variables in the firmware itself:
|
Basic telemetry is available through object properties. The properties names generally match the corresponding variables in the firmware itself:
|
||||||
|
|
||||||
```python
|
```python
|
||||||
print(flix.connected) # True if connected to the drone
|
print(flix.connected) # True if connected to the drone
|
||||||
@@ -41,7 +41,7 @@ print(flix.attitude) # attitude quaternion [w, x, y, z]
|
|||||||
print(flix.attitude_euler) # attitude as Euler angles [roll, pitch, yaw]
|
print(flix.attitude_euler) # attitude as Euler angles [roll, pitch, yaw]
|
||||||
print(flix.rates) # angular rates [roll_rate, pitch_rate, yaw_rate]
|
print(flix.rates) # angular rates [roll_rate, pitch_rate, yaw_rate]
|
||||||
print(flix.channels) # raw RC channels (list)
|
print(flix.channels) # raw RC channels (list)
|
||||||
print(flix.motors) # motor outputs (list)
|
print(flix.motors) # motors outputs (list)
|
||||||
print(flix.acc) # accelerometer output (list)
|
print(flix.acc) # accelerometer output (list)
|
||||||
print(flix.gyro) # gyroscope output (list)
|
print(flix.gyro) # gyroscope output (list)
|
||||||
```
|
```
|
||||||
@@ -95,24 +95,24 @@ Full list of events:
|
|||||||
|`armed`|Armed state update|Armed state (*bool*)|
|
|`armed`|Armed state update|Armed state (*bool*)|
|
||||||
|`mode`|Flight mode update|Flight mode (*str*)|
|
|`mode`|Flight mode update|Flight mode (*str*)|
|
||||||
|`landed`|Landed state update|Landed state (*bool*)|
|
|`landed`|Landed state update|Landed state (*bool*)|
|
||||||
|`print`|The drone prints text to the console|Text|
|
|`print`|The drone sends text to the console|Text|
|
||||||
|`attitude`|Attitude update|Attitude quaternion (*list*)|
|
|`attitude`|Attitude update|Attitude quaternion (*list*)|
|
||||||
|`attitude_euler`|Attitude update|Euler angles (*list*)|
|
|`attitude_euler`|Attitude update|Euler angles (*list*)|
|
||||||
|`rates`|Angular rates update|Angular rates (*list*)|
|
|`rates`|Angular rates update|Angular rates (*list*)|
|
||||||
|`channels`|Raw RC channels update|Raw RC channels (*list*)|
|
|`channels`|Raw RC channels update|Raw RC channels (*list*)|
|
||||||
|`motors`|Motor outputs update|Motor outputs (*list*)|
|
|`motors`|Motors outputs update|Motors outputs (*list*)|
|
||||||
|`acc`|Accelerometer update|Accelerometer output (*list*)|
|
|`acc`|Accelerometer update|Accelerometer output (*list*)|
|
||||||
|`gyro`|Gyroscope update|Gyroscope output (*list*)|
|
|`gyro`|Gyroscope update|Gyroscope output (*list*)|
|
||||||
|`mavlink`|Received MAVLink message|Message object|
|
|`mavlink`|Received MAVLink message|Message object|
|
||||||
|`mavlink.<message_name>`|Received specific MAVLink message|Message object|
|
|`mavlink.<message_name>`|Received specific MAVLink message|Message object|
|
||||||
|`mavlink.<message_id>`|Received specific MAVLink message|Message object|
|
|`mavlink.<message_id>`|Received specific MAVLink message|Message object|
|
||||||
|`value`|Named value update (see below)|Name, value|
|
|`value`|Named value update (see below)|Name, value|
|
||||||
|`value.<name>`|Specific named value update (see below)|Value|
|
|`value.<name>`|Specific named value update (see bellow)|Value|
|
||||||
|
|
||||||
> [!NOTE]
|
> [!NOTE]
|
||||||
> Update events trigger on every new piece of data from the drone, and do not mean the value has changed.
|
> Update events trigger on every new data from the drone, and do not mean the value has changed.
|
||||||
|
|
||||||
### Basic methods
|
### Common methods
|
||||||
|
|
||||||
Get and set firmware parameters using `get_param` and `set_param` methods:
|
Get and set firmware parameters using `get_param` and `set_param` methods:
|
||||||
|
|
||||||
@@ -121,7 +121,7 @@ pitch_p = flix.get_param('PITCH_P') # get parameter value
|
|||||||
flix.set_param('PITCH_P', 5) # set parameter value
|
flix.set_param('PITCH_P', 5) # set parameter value
|
||||||
```
|
```
|
||||||
|
|
||||||
Execute console commands using `cli` method. This method returns the command response:
|
Execute console commands using `cli` method. This method returns command response:
|
||||||
|
|
||||||
```python
|
```python
|
||||||
imu = flix.cli('imu') # get detailed IMU data
|
imu = flix.cli('imu') # get detailed IMU data
|
||||||
@@ -169,10 +169,10 @@ Setting angular rates target:
|
|||||||
flix.set_rates([0.1, 0.2, 0.3], 0.6) # set target roll rate, pitch rate, yaw rate and thrust
|
flix.set_rates([0.1, 0.2, 0.3], 0.6) # set target roll rate, pitch rate, yaw rate and thrust
|
||||||
```
|
```
|
||||||
|
|
||||||
You also can control raw motor outputs directly:
|
You also can control raw motors outputs directly:
|
||||||
|
|
||||||
```python
|
```python
|
||||||
flix.set_motors([0.5, 0.5, 0.5, 0.5]) # set motor outputs in range [0, 1]
|
flix.set_motors([0.5, 0.5, 0.5, 0.5]) # set motors outputs in range [0, 1]
|
||||||
```
|
```
|
||||||
|
|
||||||
In *AUTO* mode, the drone will arm automatically if the thrust is greater than zero, and disarm if thrust is zero. Therefore, to disarm the drone, set thrust to zero:
|
In *AUTO* mode, the drone will arm automatically if the thrust is greater than zero, and disarm if thrust is zero. Therefore, to disarm the drone, set thrust to zero:
|
||||||
@@ -186,7 +186,7 @@ The following methods are in development and are not functional yet:
|
|||||||
* `set_position` — set target position.
|
* `set_position` — set target position.
|
||||||
* `set_velocity` — set target velocity.
|
* `set_velocity` — set target velocity.
|
||||||
|
|
||||||
To exit *AUTO* mode move control sticks and the drone will switch to *STAB* mode.
|
To exit from *AUTO* mode move control sticks and the drone will switch to *STAB* mode.
|
||||||
|
|
||||||
## Usage alongside QGroundControl
|
## Usage alongside QGroundControl
|
||||||
|
|
||||||
|
|||||||
@@ -17,7 +17,7 @@ from pymavlink.dialects.v20 import common as mavlink
|
|||||||
logger = logging.getLogger('flix')
|
logger = logging.getLogger('flix')
|
||||||
if not logger.hasHandlers():
|
if not logger.hasHandlers():
|
||||||
handler = logging.StreamHandler()
|
handler = logging.StreamHandler()
|
||||||
handler.setFormatter(logging.Formatter('%(name)s: %(message)s'))
|
handler.setFormatter(logging.Formatter('%(name)s - %(levelname)s - %(message)s'))
|
||||||
logger.addHandler(handler)
|
logger.addHandler(handler)
|
||||||
logger.setLevel(logging.INFO)
|
logger.setLevel(logging.INFO)
|
||||||
|
|
||||||
@@ -40,7 +40,7 @@ class Flix:
|
|||||||
|
|
||||||
_connection_timeout = 3
|
_connection_timeout = 3
|
||||||
_print_buffer: str = ''
|
_print_buffer: str = ''
|
||||||
_modes = ['RAW', 'ACRO', 'STAB', 'AUTO']
|
_modes = ['MANUAL', 'ACRO', 'STAB', 'AUTO']
|
||||||
|
|
||||||
def __init__(self, system_id: int=1, wait_connection: bool=True):
|
def __init__(self, system_id: int=1, wait_connection: bool=True):
|
||||||
if not (0 <= system_id < 256):
|
if not (0 <= system_id < 256):
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
[project]
|
[project]
|
||||||
name = "pyflix"
|
name = "pyflix"
|
||||||
version = "0.11"
|
version = "0.9"
|
||||||
description = "Python API for Flix drone"
|
description = "Python API for Flix drone"
|
||||||
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
|
authors = [{ name="Oleg Kalachev", email="okalachev@gmail.com" }]
|
||||||
license = "MIT"
|
license = "MIT"
|
||||||
|
|||||||