Compare commits
5 Commits
0730ceeffa
...
wifi-confi
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
8a12d1fa70 | ||
|
|
a7cd6473fd | ||
|
|
5960e85a74 | ||
|
|
cef1834ea3 | ||
|
|
6548ae5708 |
54
.vscode/c_cpp_properties.json
vendored
@@ -6,18 +6,19 @@
|
||||
"${workspaceFolder}/flix",
|
||||
"${workspaceFolder}/gazebo",
|
||||
"${workspaceFolder}/tools/**",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
|
||||
"~/.arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
|
||||
"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
|
||||
"~/.arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
|
||||
"~/Arduino/libraries/**",
|
||||
"/usr/include/gazebo-11/",
|
||||
"/usr/include/ignition/math6/"
|
||||
],
|
||||
"forcedInclude": [
|
||||
"${workspaceFolder}/.vscode/intellisense.h",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
|
||||
"~/.arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
|
||||
"${workspaceFolder}/flix/cli.ino",
|
||||
"${workspaceFolder}/flix/control.ino",
|
||||
"${workspaceFolder}/flix/estimate.ino",
|
||||
@@ -30,10 +31,9 @@
|
||||
"${workspaceFolder}/flix/rc.ino",
|
||||
"${workspaceFolder}/flix/time.ino",
|
||||
"${workspaceFolder}/flix/wifi.ino",
|
||||
"${workspaceFolder}/flix/parameters.ino",
|
||||
"${workspaceFolder}/flix/safety.ino"
|
||||
"${workspaceFolder}/flix/parameters.ino"
|
||||
],
|
||||
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
|
||||
"compilerPath": "~/.arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++17",
|
||||
"defines": [
|
||||
@@ -53,18 +53,19 @@
|
||||
"name": "Mac",
|
||||
"includePath": [
|
||||
"${workspaceFolder}/flix",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
|
||||
"~/Library/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
|
||||
"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/include/**",
|
||||
"~/Library/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
|
||||
"~/Documents/Arduino/libraries/**",
|
||||
"/opt/homebrew/include/gazebo-11/",
|
||||
"/opt/homebrew/include/ignition/math6/"
|
||||
],
|
||||
"forcedInclude": [
|
||||
"${workspaceFolder}/.vscode/intellisense.h",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
|
||||
"~/Library/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
|
||||
"${workspaceFolder}/flix/flix.ino",
|
||||
"${workspaceFolder}/flix/cli.ino",
|
||||
"${workspaceFolder}/flix/control.ino",
|
||||
@@ -77,10 +78,9 @@
|
||||
"${workspaceFolder}/flix/rc.ino",
|
||||
"${workspaceFolder}/flix/time.ino",
|
||||
"${workspaceFolder}/flix/wifi.ino",
|
||||
"${workspaceFolder}/flix/parameters.ino",
|
||||
"${workspaceFolder}/flix/safety.ino"
|
||||
"${workspaceFolder}/flix/parameters.ino"
|
||||
],
|
||||
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++",
|
||||
"compilerPath": "~/Library/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++17",
|
||||
"defines": [
|
||||
@@ -103,16 +103,17 @@
|
||||
"${workspaceFolder}/flix",
|
||||
"${workspaceFolder}/gazebo",
|
||||
"${workspaceFolder}/tools/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/libraries/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-libs/3.3.6/include/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/libraries/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/**",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/tools/esp32-arduino-libs/idf-release_v5.4-2f7dcd86-v1/esp32/dio_qspi/include",
|
||||
"~/Documents/Arduino/libraries/**"
|
||||
],
|
||||
"forcedInclude": [
|
||||
"${workspaceFolder}/.vscode/intellisense.h",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/cores/esp32/Arduino.h",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.3.6/variants/d1_mini32/pins_arduino.h",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/cores/esp32/Arduino.h",
|
||||
"~/AppData/Local/Arduino15/packages/esp32/hardware/esp32/3.2.0/variants/d1_mini32/pins_arduino.h",
|
||||
"${workspaceFolder}/flix/cli.ino",
|
||||
"${workspaceFolder}/flix/control.ino",
|
||||
"${workspaceFolder}/flix/estimate.ino",
|
||||
@@ -125,10 +126,9 @@
|
||||
"${workspaceFolder}/flix/rc.ino",
|
||||
"${workspaceFolder}/flix/time.ino",
|
||||
"${workspaceFolder}/flix/wifi.ino",
|
||||
"${workspaceFolder}/flix/parameters.ino",
|
||||
"${workspaceFolder}/flix/safety.ino"
|
||||
"${workspaceFolder}/flix/parameters.ino"
|
||||
],
|
||||
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2511/bin/xtensa-esp32-elf-g++.exe",
|
||||
"compilerPath": "~/AppData/Local/Arduino15/packages/esp32/tools/esp-x32/2411/bin/xtensa-esp32-elf-g++.exe",
|
||||
"cStandard": "c11",
|
||||
"cppStandard": "c++17",
|
||||
"defines": [
|
||||
|
||||
4
Makefile
@@ -13,10 +13,10 @@ monitor:
|
||||
|
||||
dependencies .dependencies:
|
||||
arduino-cli core update-index --config-file arduino-cli.yaml
|
||||
arduino-cli core install esp32:esp32@3.3.6 --config-file arduino-cli.yaml
|
||||
arduino-cli core install esp32:esp32@3.2.0 --config-file arduino-cli.yaml
|
||||
arduino-cli lib update-index
|
||||
arduino-cli lib install "FlixPeriph"
|
||||
arduino-cli lib install "MAVLink"@2.0.25
|
||||
arduino-cli lib install "MAVLink"@2.0.16
|
||||
touch .dependencies
|
||||
|
||||
gazebo/build cmake: gazebo/CMakeLists.txt
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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) \\]
|
||||
|
||||
|
||||
@@ -177,7 +177,7 @@ imu.setDLPF(imu.DLPF_MAX);
|
||||
|
||||
## Калибровка гироскопа
|
||||
|
||||
Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения *(bias)* и случайный шум *(noise)*:
|
||||
Как и любое измерительное устройство, гироскоп вносит искажения в измерения. Наиболее простая модель этих искажений делит их на статические смещения (*bias*) и случайный шум (*noise*):
|
||||
|
||||
\\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\]
|
||||
|
||||
|
||||
|
Before Width: | Height: | Size: 62 KiB |
|
Before Width: | Height: | Size: 48 KiB |
|
Before Width: | Height: | Size: 50 KiB |
|
Before Width: | Height: | Size: 17 KiB |
|
Before Width: | Height: | Size: 44 KiB |
|
Before Width: | Height: | Size: 55 KiB |
|
Before Width: | Height: | Size: 105 KiB |
|
Before Width: | Height: | Size: 34 KiB |
|
Before Width: | Height: | Size: 36 KiB |
@@ -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 startup 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 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 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)*.
|
||||
* **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*).
|
||||
* **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:
|
||||
|
||||
@@ -20,10 +20,10 @@ You can build and upload the firmware using either **Arduino IDE** (easier for b
|
||||
|
||||
1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
|
||||
2. *Windows users might need to install [USB to UART bridge driver from Silicon Labs](https://www.silabs.com/developers/usb-to-uart-bridge-vcp-drivers).*
|
||||
3. Install ESP32 core, version 3.3.6. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
|
||||
3. Install ESP32 core, version 3.2.0. See the [official Espressif's instructions](https://docs.espressif.com/projects/arduino-esp32/en/latest/installing.html#installing-using-arduino-ide) on installing ESP32 Core in Arduino IDE.
|
||||
4. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
|
||||
* `FlixPeriph`, the latest version.
|
||||
* `MAVLink`, version 2.0.25.
|
||||
* `MAVLink`, version 2.0.16.
|
||||
5. Open the `flix/flix.ino` sketch from downloaded firmware sources in Arduino IDE.
|
||||
6. Connect your ESP32 board to the computer and choose correct board type in Arduino IDE (*WEMOS D1 MINI ESP32* for ESP32 Mini) and the port.
|
||||
7. [Build and upload](https://docs.arduino.cc/software/ide-v2/tutorials/getting-started/ide-v2-uploading-a-sketch) the firmware using Arduino IDE.
|
||||
@@ -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. Parameter names are case-insensitive.
|
||||
You can also work with parameters using `p` command in the console.
|
||||
|
||||
### Define IMU orientation
|
||||
|
||||
@@ -134,43 +134,27 @@ 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
|
||||
### Check everything works
|
||||
|
||||
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.
|
||||
|
||||
### Important: check everything works
|
||||
|
||||
1. Check the IMU is working: perform `imu` command in the console and check the output:
|
||||
1. Check the IMU is working: perform `imu` command and check its output:
|
||||
|
||||
* The `status` field should be `OK`.
|
||||
* The `rate` field should be about 1000 (Hz).
|
||||
* The `accel` and `gyro` fields should change as you move the drone.
|
||||
* The `accel bias` and `accel scale` fields should contain calibration parameters (not zeros and ones).
|
||||
* The `gyro bias` field should contain estimated gyro bias (not zeros).
|
||||
* The `landed` field should be `1` when the drone is still on the ground and `0` when you lift it up.
|
||||
|
||||
2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Compare your attitude indicator (in the *large vertical* mode) to the video:
|
||||
|
||||
<a href="https://youtu.be/yVRN23-GISU"><img width=300 src="https://i3.ytimg.com/vi/yVRN23-GISU/maxresdefault.jpg"></a>
|
||||
|
||||
3. Perform motor tests. Use the following commands **— remove the propellers before running the tests!**
|
||||
3. Perform motor tests in the console. Use the following commands **— remove the propellers before running the tests!**
|
||||
|
||||
* `mfr` — rotate front right motor (counter-clockwise).
|
||||
* `mfl` — rotate front left motor (clockwise).
|
||||
* `mrl` — rotate rear left motor (counter-clockwise).
|
||||
* `mrr` — rotate rear right motor (clockwise).
|
||||
* `mfr` — should rotate front right motor (counter-clockwise).
|
||||
* `mfl` — should rotate front left motor (clockwise).
|
||||
* `mrl` — should rotate rear left motor (counter-clockwise).
|
||||
* `mrr` — should rotate rear right motor (clockwise).
|
||||
|
||||
Make sure rotation directions and propeller types match the following diagram:
|
||||
Rotation diagram:
|
||||
|
||||
<img src="img/motors.svg" width=200>
|
||||
|
||||
@@ -236,11 +220,11 @@ When finished flying, **disarm** the drone, moving the left stick to the bottom
|
||||
|
||||
### Flight modes
|
||||
|
||||
Flight mode is changed using mode switch on the remote control (if configured) or using the console commands. The main flight mode is *STAB*.
|
||||
Flight mode is changed using mode switch on the remote control or using the command line.
|
||||
|
||||
#### STAB
|
||||
|
||||
In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
|
||||
The default mode is *STAB*. In this mode, the drone stabilizes its attitude (orientation). The left stick controls throttle and yaw rate, the right stick controls pitch and roll angles.
|
||||
|
||||
> [!IMPORTANT]
|
||||
> The drone doesn't stabilize its position, so slight drift is possible. The pilot should compensate it manually.
|
||||
@@ -255,7 +239,7 @@ In this mode, the pilot controls the angular rates. This control method is diffi
|
||||
|
||||
#### AUTO
|
||||
|
||||
In this mode, the pilot inputs are ignored (except the mode switch). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed behavior.
|
||||
In this mode, the pilot inputs are ignored (except the mode switch, if configured). The drone can be controlled using [pyflix](../tools/pyflix/) Python library, or by modifying the firmware to implement the needed autonomous behavior.
|
||||
|
||||
If the pilot moves the control sticks, the drone will switch back to *STAB* mode.
|
||||
|
||||
@@ -266,8 +250,8 @@ 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.
|
||||
* `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]
|
||||
@@ -287,12 +271,6 @@ 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 command on your computer for that:
|
||||
|
||||
27
docs/user.md
@@ -4,33 +4,6 @@ This page contains user-built drones based on the Flix project. Publish your pro
|
||||
|
||||
---
|
||||
|
||||
Author: [FanBy0ru](https://https://github.com/FanBy0ru).<br>
|
||||
Description: custom 3D-printed frame.<br>
|
||||
Frame STLs and flight validation: https://cults3d.com/en/3d-model/gadget/armature-pour-flix-drone.
|
||||
|
||||
<img src="img/user/fanby0ru/1.jpg" height=200> <img src="img/user/fanby0ru/2.jpg" height=200>
|
||||
|
||||
---
|
||||
|
||||
Author: Ivan44 Phalko.<br>
|
||||
Description: custom PCB, cusom test bench.<br>
|
||||
[Flight validation](https://drive.google.com/file/d/17DNDJ1gPmCmDRAwjedCbJ9RXAyqMqqcX/view?usp=sharing).
|
||||
|
||||
<img src="img/user/phalko/1.jpg" height=200> <img src="img/user/phalko/2.jpg" height=200> <img src="img/user/phalko/3.jpg" height=200>
|
||||
|
||||
---
|
||||
|
||||
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>
|
||||
|
||||
@@ -6,7 +6,6 @@
|
||||
#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;
|
||||
@@ -15,7 +14,6 @@ extern uint16_t channels[16];
|
||||
extern float controlTime;
|
||||
extern int mode;
|
||||
extern bool armed;
|
||||
extern LowPassFilter<Vector> gyroBiasFilter;
|
||||
|
||||
const char* motd =
|
||||
"\nWelcome to\n"
|
||||
@@ -94,7 +92,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(), getParameter(arg0.c_str()));
|
||||
print("%s = %g\n", arg0.c_str(), arg1.toFloat());
|
||||
} else {
|
||||
print("Parameter not found: %s\n", arg0.c_str());
|
||||
}
|
||||
@@ -180,7 +178,6 @@ void doCommand(String str, bool echo = false) {
|
||||
#endif
|
||||
} else if (command == "reset") {
|
||||
attitude = Quaternion();
|
||||
gyroBiasFilter.reset();
|
||||
} else if (command == "reboot") {
|
||||
ESP.restart();
|
||||
} else {
|
||||
|
||||
@@ -52,7 +52,6 @@ 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;
|
||||
@@ -66,9 +65,9 @@ void control() {
|
||||
}
|
||||
|
||||
void interpretControls() {
|
||||
if (controlMode < 0.25) mode = flightModes[0];
|
||||
else if (controlMode < 0.75) mode = flightModes[1];
|
||||
else if (controlMode > 0.75) mode = flightModes[2];
|
||||
if (controlMode < 0.25) mode = STAB;
|
||||
if (controlMode < 0.75) mode = STAB;
|
||||
if (controlMode > 0.75) mode = STAB;
|
||||
|
||||
if (mode == AUTO) return; // pilot is not effective in AUTO mode
|
||||
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||
// Repository: https://github.com/okalachev/flix
|
||||
|
||||
// Attitude estimation using gyro and accelerometer
|
||||
// Attitude estimation from gyro and accelerometer
|
||||
|
||||
#include "quaternion.h"
|
||||
#include "vector.h"
|
||||
|
||||
@@ -7,6 +7,7 @@
|
||||
#include "quaternion.h"
|
||||
#include "util.h"
|
||||
|
||||
|
||||
extern float t, dt;
|
||||
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
||||
extern Vector gyro, acc;
|
||||
@@ -21,8 +22,8 @@ void setup() {
|
||||
disableBrownOut();
|
||||
setupParameters();
|
||||
setupLED();
|
||||
setLED(true);
|
||||
setupMotors();
|
||||
setLED(true);
|
||||
setupWiFi();
|
||||
setupIMU();
|
||||
setupRC();
|
||||
|
||||
@@ -19,8 +19,6 @@ 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();
|
||||
@@ -52,6 +50,8 @@ 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);
|
||||
}
|
||||
|
||||
|
||||
14
flix/lpf.h
@@ -14,6 +14,15 @@ 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);
|
||||
}
|
||||
|
||||
@@ -22,6 +31,9 @@ public:
|
||||
}
|
||||
|
||||
void reset() {
|
||||
output = T(); // set to zero
|
||||
initialized = false;
|
||||
}
|
||||
|
||||
private:
|
||||
bool initialized = false;
|
||||
};
|
||||
|
||||
@@ -8,13 +8,12 @@
|
||||
|
||||
extern float controlTime;
|
||||
|
||||
bool mavlinkConnected = false;
|
||||
String mavlinkPrintBuffer;
|
||||
int mavlinkSysId = 1;
|
||||
Rate telemetryFast(10);
|
||||
Rate telemetrySlow(2);
|
||||
|
||||
bool mavlinkConnected = false;
|
||||
String mavlinkPrintBuffer;
|
||||
|
||||
void processMavlink() {
|
||||
sendMavlink();
|
||||
receiveMavlink();
|
||||
@@ -42,9 +41,9 @@ void sendMavlink() {
|
||||
}
|
||||
|
||||
if (telemetryFast && mavlinkConnected) {
|
||||
const float offset[] = {0, 0, 0, 0};
|
||||
const float zeroQuat[] = {0, 0, 0, 0};
|
||||
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, offset); // convert to frd
|
||||
time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd
|
||||
sendMessage(&msg);
|
||||
|
||||
mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,
|
||||
@@ -143,7 +142,7 @@ void handleMavlink(const void *_msg) {
|
||||
// send ack
|
||||
mavlink_message_t msg;
|
||||
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
|
||||
m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
|
||||
sendMessage(&msg);
|
||||
}
|
||||
|
||||
|
||||
@@ -1,18 +1,23 @@
|
||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||
// Repository: https://github.com/okalachev/flix
|
||||
|
||||
// PWM control for motors
|
||||
// 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)
|
||||
|
||||
#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
|
||||
|
||||
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
|
||||
#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]
|
||||
|
||||
const int MOTOR_REAR_LEFT = 0;
|
||||
const int MOTOR_REAR_RIGHT = 1;
|
||||
@@ -21,31 +26,30 @@ const int MOTOR_FRONT_LEFT = 3;
|
||||
|
||||
void setupMotors() {
|
||||
print("Setup Motors\n");
|
||||
|
||||
// configure pins
|
||||
for (int i = 0; i < 4; i++) {
|
||||
ledcAttach(motorPins[i], pwmFrequency, pwmResolution);
|
||||
pwmFrequency = ledcChangeFrequency(motorPins[i], pwmFrequency, pwmResolution); // when reconfiguring
|
||||
}
|
||||
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);
|
||||
|
||||
sendMotors();
|
||||
print("Motors initialized\n");
|
||||
}
|
||||
|
||||
void sendMotors() {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
ledcWrite(motorPins[i], getDutyCycle(motors[i]));
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
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);
|
||||
} else { // duty cycle mode
|
||||
return round(value * ((1 << pwmResolution) - 1));
|
||||
}
|
||||
}
|
||||
|
||||
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]));
|
||||
}
|
||||
|
||||
bool motorsActive() {
|
||||
|
||||
@@ -6,22 +6,20 @@
|
||||
#include <Preferences.h>
|
||||
#include "util.h"
|
||||
|
||||
extern int channelZero[16];
|
||||
extern int channelMax[16];
|
||||
extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
|
||||
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
|
||||
const char *name; // max length is 15 (Preferences key limit)
|
||||
bool integer;
|
||||
union { float *f; int *i; }; // pointer to the variable
|
||||
union { float *f; int *i; }; // pointer to variable
|
||||
float cache; // what's stored in flash
|
||||
void (*callback)(); // called after parameter change
|
||||
Parameter(const char *name, float *variable, void (*callback)() = nullptr) : name(name), integer(false), f(variable), callback(callback) {};
|
||||
Parameter(const char *name, int *variable, void (*callback)() = nullptr) : name(name), integer(true), i(variable), callback(callback) {};
|
||||
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; };
|
||||
};
|
||||
@@ -50,9 +48,6 @@ 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},
|
||||
@@ -63,20 +58,9 @@ 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], setupMotors},
|
||||
{"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT], setupMotors},
|
||||
{"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT], setupMotors},
|
||||
{"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT], setupMotors},
|
||||
{"MOT_PWM_FREQ", &pwmFrequency, setupMotors},
|
||||
{"MOT_PWM_RES", &pwmResolution, setupMotors},
|
||||
{"MOT_PWM_STOP", &pwmStop},
|
||||
{"MOT_PWM_MIN", &pwmMin},
|
||||
{"MOT_PWM_MAX", &pwmMax},
|
||||
// rc
|
||||
{"RC_ZERO_0", &channelZero[0]},
|
||||
{"RC_ZERO_1", &channelZero[1]},
|
||||
@@ -107,14 +91,10 @@ Parameter parameters[] = {
|
||||
{"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() {
|
||||
print("Setup parameters\n");
|
||||
storage.begin("flix");
|
||||
storage.begin("flix", false);
|
||||
// Read parameters from storage
|
||||
for (auto ¶meter : parameters) {
|
||||
if (!storage.isKey(parameter.name)) {
|
||||
@@ -141,7 +121,7 @@ float getParameter(int index) {
|
||||
|
||||
float getParameter(const char *name) {
|
||||
for (auto ¶meter : parameters) {
|
||||
if (strcasecmp(parameter.name, name) == 0) {
|
||||
if (strcmp(parameter.name, name) == 0) {
|
||||
return parameter.getValue();
|
||||
}
|
||||
}
|
||||
@@ -150,10 +130,9 @@ float getParameter(const char *name) {
|
||||
|
||||
bool setParameter(const char *name, const float value) {
|
||||
for (auto ¶meter : parameters) {
|
||||
if (strcasecmp(parameter.name, name) == 0) {
|
||||
if (strcmp(parameter.name, name) == 0) {
|
||||
if (parameter.integer && !isfinite(value)) return false; // can't set integer to NaN or Inf
|
||||
parameter.setValue(value);
|
||||
if (parameter.callback) parameter.callback();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -167,7 +146,8 @@ void syncParameters() {
|
||||
|
||||
for (auto ¶meter : parameters) {
|
||||
if (parameter.getValue() == parameter.cache) continue; // no change
|
||||
if (isnan(parameter.getValue()) && isnan(parameter.cache)) continue; // both are NAN
|
||||
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
|
||||
|
||||
35
flix/rc.ino
@@ -9,14 +9,15 @@
|
||||
SBUS rc(Serial2);
|
||||
|
||||
uint16_t channels[16]; // raw rc channels
|
||||
int channelZero[16]; // calibration zero values
|
||||
int channelMax[16]; // calibration max values
|
||||
float channelZero[16]; // calibration zero values
|
||||
float channelMax[16]; // calibration max values
|
||||
|
||||
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
||||
float controlMode = NAN;
|
||||
float controlTime = NAN; // time of the last controls update
|
||||
float controlMode = NAN; //
|
||||
float controlTime; // time of the last controls update (0 when no RC)
|
||||
|
||||
int rollChannel = -1, pitchChannel = -1, throttleChannel = -1, yawChannel = -1, modeChannel = -1; // channel mapping
|
||||
// Channels mapping (using float to store in parameters):
|
||||
float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;
|
||||
|
||||
void setupRC() {
|
||||
print("Setup RC\n");
|
||||
@@ -40,11 +41,11 @@ void normalizeRC() {
|
||||
controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);
|
||||
}
|
||||
// Update control values
|
||||
controlRoll = rollChannel < 0 ? 0 : controls[rollChannel];
|
||||
controlPitch = pitchChannel < 0 ? 0 : controls[pitchChannel];
|
||||
controlYaw = yawChannel < 0 ? 0 : controls[yawChannel];
|
||||
controlThrottle = throttleChannel < 0 ? 0 : controls[throttleChannel];
|
||||
controlMode = modeChannel < 0 ? NAN : controls[modeChannel]; // mode control is ineffective if not mapped
|
||||
controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;
|
||||
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
|
||||
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
|
||||
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
|
||||
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
|
||||
}
|
||||
|
||||
void calibrateRC() {
|
||||
@@ -63,7 +64,7 @@ void calibrateRC() {
|
||||
printRCCalibration();
|
||||
}
|
||||
|
||||
void calibrateRCChannel(int *channel, uint16_t in[16], uint16_t out[16], const char *str) {
|
||||
void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str) {
|
||||
print("%s", str);
|
||||
pause(3);
|
||||
for (int i = 0; i < 30; i++) readRC(); // try update 30 times max
|
||||
@@ -84,15 +85,15 @@ void calibrateRCChannel(int *channel, uint16_t in[16], uint16_t out[16], const c
|
||||
channelZero[ch] = in[ch];
|
||||
channelMax[ch] = out[ch];
|
||||
} else {
|
||||
*channel = -1;
|
||||
*channel = NAN;
|
||||
}
|
||||
}
|
||||
|
||||
void printRCCalibration() {
|
||||
print("Control Ch Zero Max\n");
|
||||
print("Roll %-7d%-7d%-7d\n", rollChannel, rollChannel < 0 ? 0 : channelZero[rollChannel], rollChannel < 0 ? 0 : channelMax[rollChannel]);
|
||||
print("Pitch %-7d%-7d%-7d\n", pitchChannel, pitchChannel < 0 ? 0 : channelZero[pitchChannel], pitchChannel < 0 ? 0 : channelMax[pitchChannel]);
|
||||
print("Yaw %-7d%-7d%-7d\n", yawChannel, yawChannel < 0 ? 0 : channelZero[yawChannel], yawChannel < 0 ? 0 : channelMax[yawChannel]);
|
||||
print("Throttle %-7d%-7d%-7d\n", throttleChannel, throttleChannel < 0 ? 0 : channelZero[throttleChannel], throttleChannel < 0 ? 0 : channelMax[throttleChannel]);
|
||||
print("Mode %-7d%-7d%-7d\n", modeChannel, modeChannel < 0 ? 0 : channelZero[modeChannel], modeChannel < 0 ? 0 : channelMax[modeChannel]);
|
||||
print("Roll %-7g%-7g%-7g\n", rollChannel, rollChannel >= 0 ? channelZero[(int)rollChannel] : NAN, rollChannel >= 0 ? channelMax[(int)rollChannel] : NAN);
|
||||
print("Pitch %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN);
|
||||
print("Yaw %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN);
|
||||
print("Throttle %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN);
|
||||
print("Mode %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);
|
||||
}
|
||||
|
||||
@@ -3,12 +3,12 @@
|
||||
|
||||
// Fail-safe functions
|
||||
|
||||
#define RC_LOSS_TIMEOUT 1
|
||||
#define DESCEND_TIME 10
|
||||
|
||||
extern float controlTime;
|
||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw;
|
||||
|
||||
float rcLossTimeout = 1;
|
||||
float descendTime = 10;
|
||||
|
||||
void failsafe() {
|
||||
rcLossFailsafe();
|
||||
autoFailsafe();
|
||||
@@ -16,8 +16,9 @@ void failsafe() {
|
||||
|
||||
// RC loss failsafe
|
||||
void rcLossFailsafe() {
|
||||
if (controlTime == 0) return; // no RC at all
|
||||
if (!armed) return;
|
||||
if (t - controlTime > rcLossTimeout) {
|
||||
if (t - controlTime > RC_LOSS_TIMEOUT) {
|
||||
descend();
|
||||
}
|
||||
}
|
||||
@@ -26,7 +27,7 @@ void rcLossFailsafe() {
|
||||
void descend() {
|
||||
mode = AUTO;
|
||||
attitudeTarget = Quaternion();
|
||||
thrustTarget -= dt / descendTime;
|
||||
thrustTarget -= dt / DESCEND_TIME;
|
||||
if (thrustTarget < 0) {
|
||||
thrustTarget = 0;
|
||||
armed = false;
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
||||
// Repository: https://github.com/okalachev/flix
|
||||
|
||||
// Wi-Fi communication
|
||||
// Wi-Fi connectivity
|
||||
|
||||
#include <WiFi.h>
|
||||
#include <WiFiAP.h>
|
||||
|
||||
@@ -165,7 +165,6 @@ void delay(uint32_t ms) {
|
||||
|
||||
bool ledcAttach(uint8_t pin, uint32_t freq, uint8_t resolution) { return true; }
|
||||
bool ledcWrite(uint8_t pin, uint32_t duty) { return true; }
|
||||
uint32_t ledcChangeFrequency(uint8_t pin, uint32_t freq, uint8_t resolution) { return freq; }
|
||||
|
||||
unsigned long __micros;
|
||||
unsigned long __resetTime = 0;
|
||||
|
||||
@@ -9,7 +9,6 @@
|
||||
#include "quaternion.h"
|
||||
#include "Arduino.h"
|
||||
#include "wifi.h"
|
||||
#include "lpf.h"
|
||||
|
||||
extern float t, dt;
|
||||
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
||||
@@ -20,7 +19,6 @@ extern float motors[4];
|
||||
|
||||
Vector gyro, acc, imuRotation;
|
||||
Vector accBias, gyroBias, accScale(1, 1, 1);
|
||||
LowPassFilter<Vector> gyroBiasFilter(0);
|
||||
|
||||
// declarations
|
||||
void step();
|
||||
@@ -34,7 +32,6 @@ void controlRates();
|
||||
void controlTorque();
|
||||
const char* getModeName();
|
||||
void sendMotors();
|
||||
int getDutyCycle(float value);
|
||||
bool motorsActive();
|
||||
void testMotor(int n);
|
||||
void print(const char* format, ...);
|
||||
@@ -43,7 +40,7 @@ void doCommand(String str, bool echo);
|
||||
void handleInput();
|
||||
void normalizeRC();
|
||||
void calibrateRC();
|
||||
void calibrateRCChannel(int *channel, uint16_t zero[16], uint16_t max[16], const char *str);
|
||||
void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str);
|
||||
void printRCCalibration();
|
||||
void printLogHeader();
|
||||
void printLogData();
|
||||
|
||||
@@ -11,10 +11,9 @@
|
||||
#include <sys/poll.h>
|
||||
#include <gazebo/gazebo.hh>
|
||||
|
||||
int wifiMode = 1; // mock
|
||||
int udpLocalPort = 14580;
|
||||
int udpRemotePort = 14550;
|
||||
const char *udpRemoteIP = "255.255.255.255";
|
||||
#define WIFI_UDP_PORT 14580
|
||||
#define WIFI_UDP_REMOTE_PORT 14550
|
||||
#define WIFI_UDP_REMOTE_ADDR "255.255.255.255"
|
||||
|
||||
int wifiSocket;
|
||||
|
||||
@@ -23,22 +22,22 @@ void setupWiFi() {
|
||||
sockaddr_in addr; // local address
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_addr.s_addr = INADDR_ANY;
|
||||
addr.sin_port = htons(udpLocalPort);
|
||||
addr.sin_port = htons(WIFI_UDP_PORT);
|
||||
if (bind(wifiSocket, (sockaddr *)&addr, sizeof(addr))) {
|
||||
gzerr << "Failed to bind WiFi UDP socket on port " << udpLocalPort << std::endl;
|
||||
gzerr << "Failed to bind WiFi UDP socket on port " << WIFI_UDP_PORT << std::endl;
|
||||
return;
|
||||
}
|
||||
int broadcast = 1;
|
||||
setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
|
||||
gzmsg << "WiFi UDP socket initialized on port " << udpLocalPort << " (remote port " << udpRemotePort << ")" << std::endl;
|
||||
gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT << " (remote port " << WIFI_UDP_REMOTE_PORT << ")" << 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(udpRemoteIP);
|
||||
addr.sin_port = htons(udpRemotePort);
|
||||
addr.sin_addr.s_addr = inet_addr(WIFI_UDP_REMOTE_ADDR);
|
||||
addr.sin_port = htons(WIFI_UDP_REMOTE_PORT);
|
||||
sendto(wifiSocket, buf, len, 0, (sockaddr *)&addr, sizeof(addr));
|
||||
}
|
||||
|
||||
|
||||
@@ -13,7 +13,7 @@ lines = []
|
||||
|
||||
print('Downloading log...')
|
||||
count = 0
|
||||
dev.write('log dump\n'.encode())
|
||||
dev.write('log\n'.encode())
|
||||
while True:
|
||||
line = dev.readline()
|
||||
if not line:
|
||||
|
||||
@@ -43,7 +43,6 @@ records = [record for record in records if record[0] != 0]
|
||||
|
||||
print(f'Received records: {len(records)}')
|
||||
|
||||
os.makedirs(f'{DIR}/log', exist_ok=True)
|
||||
log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
|
||||
log.write(header.encode() + b'\n')
|
||||
for record in records:
|
||||
|
||||
@@ -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,3 +277,7 @@ 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.
|
||||
|
||||
@@ -138,7 +138,7 @@ class Flix:
|
||||
while True:
|
||||
try:
|
||||
msg: Optional[mavlink.MAVLink_message] = self.connection.recv_match(blocking=True)
|
||||
if msg is None or msg.get_srcSystem() != self.system_id:
|
||||
if msg is None:
|
||||
continue
|
||||
self._connected()
|
||||
msg_dict = msg.to_dict()
|
||||
|
||||