mirror of
https://github.com/okalachev/flix.git
synced 2026-01-11 13:36:43 +00:00
Compare commits
6 Commits
1d99d255d0
...
v1.2
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
f4e58a652a | ||
|
|
6c46328da1 | ||
|
|
c8e5e08b03 | ||
|
|
a5e3dfcf69 | ||
|
|
d6e8be0c05 | ||
|
|
68d16855df |
@@ -65,5 +65,6 @@
|
|||||||
"PX4"
|
"PX4"
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
"MD045": false
|
"MD045": false,
|
||||||
|
"MD060": false
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,6 +1,9 @@
|
|||||||
# Flix
|
<!-- markdownlint-disable MD041 -->
|
||||||
|
|
||||||
**Flix** (*flight + X*) — open source ESP32-based quadcopter made from scratch.
|
<p align="center">
|
||||||
|
<img src="docs/img/flix.svg" width=180 alt="Flix logo"><br>
|
||||||
|
<b>Flix</b> (<i>flight + X</i>) — open source ESP32-based quadcopter made from scratch.
|
||||||
|
</p>
|
||||||
|
|
||||||
<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 thrust target, range [0, 1].
|
* `thrustTarget` *(float)* — collective motor 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,6 +62,11 @@ 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).
|
||||||
|
|||||||
38
docs/img/flix.svg
Normal file
38
docs/img/flix.svg
Normal file
@@ -0,0 +1,38 @@
|
|||||||
|
<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 718.86 362.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="443.12" y1="283.88" x2="641.43" y2="84.58"/>
|
||||||
|
<line class="a" x1="444.12" y1="85.58" x2="642.42" y2="284.87"/>
|
||||||
|
<circle class="b" cx="542.77" cy="184.23" r="41.15"/>
|
||||||
|
<circle class="c" cx="443.69" cy="85.08" r="77.29"/>
|
||||||
|
<circle class="c" cx="641.55" cy="84.87" r="77.29"/>
|
||||||
|
<circle class="c" cx="640.56" cy="284.16" r="77.29"/>
|
||||||
|
<circle class="c" cx="443.25" cy="284.16" r="77.29"/>
|
||||||
|
</g>
|
||||||
|
<path class="d" d="M0,153.93q0-17.85,22.36-17.85h4.81V92.57Q27.17.48,123.23,0h1q36.87,0,47.31,7.48L173,9.41l1,2.41V42q0,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.74H93.64V336.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.93H49.08q-10.47,0-15.7-4.71t-5.73-8.44a77.31,77.31,0,0,1-.48-9.53V172.27H21.4Q0,172.27,0,153.93Z"/>
|
||||||
|
<path class="d" d="M193.21,340.2V29q0-23.16,20.86-23.4h22.81q22.8,0,22.8,22.44V338.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.17H217q-11.4,0-17.58-4.47T193.21,340.2Z"/>
|
||||||
|
<path class="d" d="M276.9,54.08V29.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.22V55.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.48H299.7q-10.43,0-16.61-4.7T276.9,54.08Zm1.94,284.71V159.28q0-22.2,21.83-22.2h20.38q10.92,0,16.62,4t6.67,8.45a60.47,60.47,0,0,1,1,12.18V341.2q0,22.2-21.83,22.2H300.67q-10.43,0-15.64-4.95t-5.7-8.81A90.93,90.93,0,0,1,278.84,338.79Z"/>
|
||||||
|
</g>
|
||||||
|
</svg>
|
||||||
|
After Width: | Height: | Size: 2.2 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#firmware).
|
* **Check ESP32 core is installed**. Check if the version matches the one used in the [tutorial](usage.md#building-the-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, make sure `rotateIMU` function is defined correctly in `imu.ino` file.
|
* **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 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).
|
||||||
|
|||||||
@@ -80,7 +80,7 @@ QGroundControl is a ground control station software that can be used to monitor
|
|||||||
1. Install mobile or desktop version of [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html).
|
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,7 +96,8 @@ 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.
|
||||||
@@ -107,11 +108,13 @@ The drone is configured using parameters. To access and modify them, go to the Q
|
|||||||
|
|
||||||
<img src="img/parameters.png" width="400">
|
<img src="img/parameters.png" width="400">
|
||||||
|
|
||||||
|
You can also work with parameters using `p` command in the console.
|
||||||
|
|
||||||
### Define IMU orientation
|
### 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`.
|
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 side with the components:
|
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">
|
<img src="img/imu-axes.png" width="200">
|
||||||
|
|
||||||
@@ -119,10 +122,10 @@ Use the following table to set the parameters for common IMU orientations:
|
|||||||
|
|
||||||
|Orientation|Parameters|Orientation|Parameters|
|
|Orientation|Parameters|Orientation|Parameters|
|
||||||
|:-:|-|-|-|
|
|:-:|-|-|-|
|
||||||
|<img src="img/imu-rot-1.png" width="200">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0 |<img src="img/imu-rot-5.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 0|
|
|<img src="img/imu-rot-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="200">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|<img src="img/imu-rot-6.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|
|
|<img src="img/imu-rot-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="200">|`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|<img src="img/imu-rot-7.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 3.142|
|
|<img src="img/imu-rot-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="200"><br><b style="text-align:center">☑️ Default</b>|<br>`IMU_ROT_ROLL` = 0<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = -1.571|<img src="img/imu-rot-8.png" width="200">|`IMU_ROT_ROLL` = 3.142<br>`IMU_ROT_PITCH` = 0<br>`IMU_ROT_YAW` = 1.571|
|
|<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
|
||||||
|
|
||||||
@@ -151,6 +154,10 @@ 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.
|
||||||
|
|
||||||
@@ -158,7 +165,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 smartphone
|
### Control with a 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.
|
||||||
@@ -170,7 +177,7 @@ There are several ways to control the drone's flight: using **smartphone** (Wi-F
|
|||||||
> [!TIP]
|
> [!TIP]
|
||||||
> Decrease `CTL_TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive.
|
> Decrease `CTL_TILT_MAX` parameter when flying using the smartphone to make the controls less sensitive.
|
||||||
|
|
||||||
### Control with remote control
|
### Control with a 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:
|
||||||
|
|
||||||
@@ -178,7 +185,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 USB remote control
|
### Control with a 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.
|
||||||
|
|
||||||
@@ -236,8 +243,6 @@ 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.
|
||||||
|
|
||||||
<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:
|
||||||
|
|||||||
@@ -16,7 +16,7 @@ Author: [goldarte](https://t.me/goldarte).<br>
|
|||||||
|
|
||||||
## School 548 course
|
## School 548 course
|
||||||
|
|
||||||
Special quadcopter design and engineering course took place in october-november 2025 in School 548, Moscow. Course included UAV control theory, electronics, and practical drone assembly and setup using the Flix project.
|
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">
|
<img height=200 src="img/user/school548/1.jpg"> <img height=200 src="img/user/school548/2.jpg"> <img height=200 src="img/user/school548/3.jpg">
|
||||||
|
|
||||||
@@ -25,7 +25,7 @@ STL files and other materials: see [here](https://drive.google.com/drive/folders
|
|||||||
### Selected works
|
### Selected works
|
||||||
|
|
||||||
Author: [KiraFlux](https://t.me/@kiraflux_0XC0000005).<br>
|
Author: [KiraFlux](https://t.me/@kiraflux_0XC0000005).<br>
|
||||||
Description: **custom ESPNOW remote control** is implemented, firmware modified to support ESPNOW protocol.<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>
|
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>
|
Modified Flix firmware: https://github.com/KiraFlux/flix/tree/klyax.<br>
|
||||||
Remote control project: https://github.com/KiraFlux/ESP32-DJC.<br>
|
Remote control project: https://github.com/KiraFlux/ESP32-DJC.<br>
|
||||||
|
|||||||
@@ -11,7 +11,7 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO
|
|||||||
extern const int RAW, ACRO, STAB, AUTO;
|
extern const int RAW, 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 controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
|
extern float controlTime;
|
||||||
extern int mode;
|
extern int mode;
|
||||||
extern bool armed;
|
extern bool armed;
|
||||||
|
|
||||||
@@ -132,6 +132,7 @@ void doCommand(String str, bool echo = false) {
|
|||||||
}
|
}
|
||||||
print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n",
|
print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n",
|
||||||
controlRoll, controlPitch, controlYaw, controlThrottle, controlMode);
|
controlRoll, controlPitch, controlYaw, controlThrottle, controlMode);
|
||||||
|
print("time: %.1f\n", controlTime);
|
||||||
print("mode: %s\n", getModeName());
|
print("mode: %s\n", getModeName());
|
||||||
print("armed: %d\n", armed);
|
print("armed: %d\n", armed);
|
||||||
} else if (command == "wifi") {
|
} else if (command == "wifi") {
|
||||||
|
|||||||
@@ -8,6 +8,10 @@
|
|||||||
#include "lpf.h"
|
#include "lpf.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
|
Vector rates; // estimated angular rates, rad/s
|
||||||
|
Quaternion attitude; // estimated attitude
|
||||||
|
bool landed;
|
||||||
|
|
||||||
float accWeight = 0.003;
|
float accWeight = 0.003;
|
||||||
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
|
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
|
||||||
|
|
||||||
|
|||||||
@@ -9,16 +9,13 @@
|
|||||||
|
|
||||||
#define WIFI_ENABLED 1
|
#define WIFI_ENABLED 1
|
||||||
|
|
||||||
float t = NAN; // current step time, s
|
extern float t, dt;
|
||||||
float dt; // time delta from previous step, s
|
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
||||||
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
extern Vector gyro, acc;
|
||||||
float controlMode = NAN;
|
extern Vector rates;
|
||||||
Vector gyro; // gyroscope data
|
extern Quaternion attitude;
|
||||||
Vector acc; // accelerometer data, m/s/s
|
extern bool landed;
|
||||||
Vector rates; // filtered angular rates, rad/s
|
extern float motors[4];
|
||||||
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);
|
||||||
|
|||||||
@@ -12,9 +12,12 @@
|
|||||||
MPU9250 imu(SPI);
|
MPU9250 imu(SPI);
|
||||||
Vector imuRotation(0, 0, -PI / 2); // imu orientation as Euler angles
|
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");
|
||||||
@@ -44,7 +47,6 @@ void readIMU() {
|
|||||||
gyro = Quaternion::rotateVector(gyro, rotation.inversed());
|
gyro = Quaternion::rotateVector(gyro, rotation.inversed());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
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
|
||||||
|
|||||||
@@ -12,12 +12,11 @@
|
|||||||
#define MAVLINK_RATE_SLOW 1
|
#define MAVLINK_RATE_SLOW 1
|
||||||
#define MAVLINK_RATE_FAST 10
|
#define MAVLINK_RATE_FAST 10
|
||||||
|
|
||||||
|
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();
|
||||||
|
|||||||
@@ -17,7 +17,8 @@
|
|||||||
#define PWM_MIN 0
|
#define PWM_MIN 0
|
||||||
#define PWM_MAX 1000000 / PWM_FREQUENCY
|
#define PWM_MAX 1000000 / PWM_FREQUENCY
|
||||||
|
|
||||||
// Motors array indexes:
|
float motors[4]; // normalized motor thrusts in range [0..1]
|
||||||
|
|
||||||
const int MOTOR_REAR_LEFT = 0;
|
const int MOTOR_REAR_LEFT = 0;
|
||||||
const int MOTOR_REAR_RIGHT = 1;
|
const int MOTOR_REAR_RIGHT = 1;
|
||||||
const int MOTOR_FRONT_RIGHT = 2;
|
const int MOTOR_FRONT_RIGHT = 2;
|
||||||
|
|||||||
17
flix/rc.ino
17
flix/rc.ino
@@ -6,13 +6,16 @@
|
|||||||
#include <SBUS.h>
|
#include <SBUS.h>
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
SBUS rc(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
|
SBUS rc(Serial2);
|
||||||
|
|
||||||
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;
|
||||||
|
|
||||||
@@ -38,11 +41,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] : NAN;
|
controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;
|
||||||
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
|
controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;
|
||||||
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
|
controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;
|
||||||
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
|
controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;
|
||||||
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
|
controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set
|
||||||
}
|
}
|
||||||
|
|
||||||
void calibrateRC() {
|
void calibrateRC() {
|
||||||
|
|||||||
@@ -3,6 +3,8 @@
|
|||||||
|
|
||||||
// Time related functions
|
// Time related functions
|
||||||
|
|
||||||
|
float t = NAN; // current time, s
|
||||||
|
float dt; // time delta with the previous step, s
|
||||||
float loopRate; // Hz
|
float loopRate; // Hz
|
||||||
|
|
||||||
void step() {
|
void step() {
|
||||||
|
|||||||
@@ -35,7 +35,6 @@ 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);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -12,16 +12,15 @@
|
|||||||
|
|
||||||
#define WIFI_ENABLED 1
|
#define WIFI_ENABLED 1
|
||||||
|
|
||||||
float t = NAN;
|
extern float t, dt;
|
||||||
float dt;
|
extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;
|
||||||
float motors[4];
|
extern Vector rates;
|
||||||
float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
|
extern Quaternion attitude;
|
||||||
float controlMode = NAN;
|
extern bool landed;
|
||||||
Vector acc;
|
extern float motors[4];
|
||||||
Vector gyro;
|
|
||||||
Vector rates;
|
Vector gyro, acc, imuRotation;
|
||||||
Quaternion attitude;
|
Vector accBias, gyroBias, accScale(1, 1, 1);
|
||||||
bool landed;
|
|
||||||
|
|
||||||
// declarations
|
// declarations
|
||||||
void step();
|
void step();
|
||||||
@@ -74,4 +73,3 @@ 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() {};
|
void printWiFiInfo() {};
|
||||||
Vector accBias, gyroBias, accScale(1, 1, 1);
|
|
||||||
|
|||||||
@@ -95,7 +95,7 @@ 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 sends text to the console|Text|
|
|`print`|The drone prints 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*)|
|
||||||
@@ -112,7 +112,7 @@ Full list of events:
|
|||||||
> [!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 piece of data from the drone, and do not mean the value has changed.
|
||||||
|
|
||||||
### Common methods
|
### Basic 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:
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user