24 Commits

Author SHA1 Message Date
Oleg Kalachev
f2dec78b16 Fix docker sim build 2025-04-24 19:21:23 +03:00
Oleg Kalachev
31ac562f67 Fix docker sim build again 2025-04-24 19:20:01 +03:00
Oleg Kalachev
feb686727b Fix Docker sim build 2025-04-24 18:47:51 +03:00
Oleg Kalachev
1975ed30b4 Make job using Ubuntu 22.02 2025-04-24 18:45:25 +03:00
Oleg Kalachev
1859fca150 Update APT before installing libsdl2-dev 2025-04-24 18:38:08 +03:00
Oleg Kalachev
42d844287e Fix docker run 2025-04-24 18:23:14 +03:00
Oleg Kalachev
226da71719 Run sim build in docker using container directive 2025-04-24 18:21:58 +03:00
Oleg Kalachev
405777dc46 Use ubuntu-22.04 for sim build, add job with building with docker 2025-04-24 18:20:44 +03:00
Oleg Kalachev
962757f46e Update user builds illustration in readme 2025-04-23 20:10:29 +03:00
Oleg Kalachev
f03dec4fae Update demo video 2025-04-22 11:27:29 +03:00
Oleg Kalachev
fe98a5bf97 Minor code simplifications 2025-04-13 01:42:47 +03:00
Oleg Kalachev
253f2fe3dd Update MAVLink-Arduino to 2.0.16 2025-04-11 07:01:54 +03:00
Oleg Kalachev
94dc566643 Show landed state in imu command output 2025-03-29 16:19:23 +03:00
Oleg Kalachev
547f5087ef Pass landed state to mavlink
Using EXTENDED_SYS_STATE message
2025-03-29 16:14:37 +03:00
Oleg Kalachev
66a43ab246 Continuous gyro bias estimation (#17)
Estimate gyro bias continuously instead of calibrating the gyro at startup.
2025-03-29 12:21:40 +03:00
Oleg Kalachev
117ae42d1b Add Wi-Fi password to build tutorial 2025-03-29 12:02:59 +03:00
Oleg Kalachev
3a61dca102 Simplify and improve acc calibration command output 2025-03-29 01:05:55 +03:00
Oleg Kalachev
a8fe1324c3 Minor readme update 2025-03-28 20:50:23 +03:00
Oleg Kalachev
fc0b805cc2 Add cryptokobans's build to user projects 2025-03-28 18:23:09 +03:00
Oleg Kalachev
d68222953d Simplify user builds article layout: remove tables
Tables make photos squeezed in phones
2025-03-27 18:56:35 +03:00
Oleg Kalachev
bca1312b46 Remove twxs.cmake from the list of recommended extensions 2025-03-14 03:24:30 +03:00
Oleg Kalachev
d5148d12a1 Minor code style fix 2025-03-14 03:03:27 +03:00
Oleg Kalachev
208e50aa15 Encode if the mode in stabilized in heartbeat message 2025-03-14 03:02:43 +03:00
Oleg Kalachev
0a87ccf435 Some minor readme updates 2025-03-01 00:27:55 +03:00
19 changed files with 81 additions and 134 deletions

View File

@@ -46,7 +46,7 @@ jobs:
run: python3 tools/check_c_cpp_properties.py
build_simulator:
runs-on: ubuntu-20.04
runs-on: ubuntu-22.04
steps:
- name: Install Arduino CLI
uses: arduino/setup-arduino-cli@v1.1.1
@@ -63,6 +63,26 @@ jobs:
path: gazebo/build/*.so
retention-days: 1
build_simulator_docker:
runs-on: ubuntu-latest
container:
image: ubuntu:20.04
steps:
- name: Install Arduino CLI
uses: arduino/setup-arduino-cli@v1.1.1
- uses: actions/checkout@v4
- name: Install Gazebo
run: curl -sSL http://get.gazebosim.org | sh
- name: Install SDL2
run: apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install build-essential libsdl2-dev -y
- name: Build simulator
run: make build_simulator
- uses: actions/upload-artifact@v4
with:
name: gazebo-plugin-binary
path: gazebo/build/*.so
retention-days: 1
build_simulator_macos:
runs-on: macos-latest
if: github.event_name == 'workflow_dispatch'

View File

@@ -2,7 +2,6 @@
// See https://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
"recommendations": [
"ms-vscode.cpptools",
"twxs.cmake",
"ms-vscode.cmake-tools",
"ms-python.python"
],

View File

@@ -16,7 +16,7 @@ dependencies .dependencies:
arduino-cli core install esp32:esp32@3.1.0 --config-file arduino-cli.yaml
arduino-cli lib update-index
arduino-cli lib install "FlixPeriph"
arduino-cli lib install "MAVLink"@2.0.12
arduino-cli lib install "MAVLink"@2.0.16
touch .dependencies
gazebo/build cmake: gazebo/CMakeLists.txt

View File

@@ -32,17 +32,17 @@
## It actually flies
See detailed demo video (for version 0): https://youtu.be/8GzzIQ3C6DQ.
See detailed demo video: https://youtu.be/hT46CZ1CgC4.
<a href="https://youtu.be/hT46CZ1CgC4"><img width=500 src="https://i3.ytimg.com/vi/hT46CZ1CgC4/maxresdefault.jpg"></a>
Version 0 demo video: https://youtu.be/8GzzIQ3C6DQ.
<a href="https://youtu.be/8GzzIQ3C6DQ"><img width=500 src="https://i3.ytimg.com/vi/8GzzIQ3C6DQ/maxresdefault.jpg"></a>
Version 1 test flight: https://t.me/opensourcequadcopter/42.
<a href="https://t.me/opensourcequadcopter/42"><img width=500 src="docs/img/flight-video.jpg"></a>
See the [user builds gallery](docs/user.md).
<img src="docs/img/user/user.jpg" width=400>
<a href="docs/user.md"><img src="docs/img/user/user.jpg" width=400></a>
## Simulation
@@ -57,7 +57,7 @@ See [instructions on running the simulation](docs/build.md).
|Type|Part|Image|Quantity|
|-|-|:-:|:-:|
|Microcontroller board|ESP32 Mini|<img src="docs/img/esp32.jpg" width=100>|1|
|IMU (and barometer²) board|GY91 (or other MPU9250/MPU6500 board), ICM20948³|<img src="docs/img/gy-91.jpg" width=90 align=center><img src="docs/img/icm-20948.jpg" width=100>|1|
|IMU (and barometer²) board|GY91, MPU-9265 (or other MPU9250/MPU6500 board), ICM20948³|<img src="docs/img/gy-91.jpg" width=90 align=center><img src="docs/img/icm-20948.jpg" width=100>|1|
|Motor|8520 3.7V brushed motor (shaft 0.8mm).<br>Motor with exact 3.7V voltage is needed, not ranged working voltage (3.7V — 6V).|<img src="docs/img/motor.jpeg" width=100>|4|
|Propeller|Hubsan 55 mm|<img src="docs/img/prop.jpg" width=100>|4|
|MOSFET (transistor)|100N03A or [analog](https://t.me/opensourcequadcopter/33)|<img src="docs/img/100n03a.jpg" width=100>|4|
@@ -70,7 +70,7 @@ See [instructions on running the simulation](docs/build.md).
|Frame bottom part|3D printed⁴:<br>[`flix-frame-1.1.stl`](docs/assets/flix-frame-1.1.stl) [`flix-frame-1.1.step`](docs/assets/flix-frame-1.1.step)|<img src="docs/img/frame1.jpg" width=100>|1|
|Frame top part|3D printed:<br>[`esp32-holder.stl`](docs/assets/esp32-holder.stl) [`esp32-holder.step`](docs/assets/esp32-holder.step)|<img src="docs/img/esp32-holder.jpg" width=100>|1|
|Washer for IMU board mounting|3D printed:<br>[`washer-m3.stl`](docs/assets/washer-m3.stl) [`washer-m3.step`](docs/assets/washer-m3.step)|<img src="docs/img/washer-m3.jpg" width=100>|2|
|*RC transmitter (optional)*|*KINGKONG TINY X8 or other⁵*|<img src="docs/img/tx.jpg" width=100>|1|
|*RC transmitter (optional)*|*KINGKONG TINY X8 (warning: lacks USB support) or other⁵*|<img src="docs/img/tx.jpg" width=100>|1|
|*RC receiver (optional)*|*DF500 or other⁵*|<img src="docs/img/rx.jpg" width=100>|1|
|Wires|28 AWG recommended|<img src="docs/img/wire-28awg.jpg" width=100>||
|Tape, double-sided tape||||

View File

@@ -109,7 +109,7 @@ The latest version of Ubuntu supported by Gazebo 11 simulator is 20.04. If you h
3. Install ESP32 core, version 3.1.0 (version 2.x is not supported). 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.12.
* `MAVLink`, version 2.0.16.
5. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).
6. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.
7. 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.
@@ -160,7 +160,7 @@ Before flight you need to calibrate the accelerometer:
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.
3. Connect your smartphone to the appeared `flix` Wi-Fi network.
3. Connect your smartphone to the appeared `flix` Wi-Fi network (password: `flixwifi`).
4. Open QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
5. Go to the settings and enable *Virtual Joystick*. *Auto-Center Throttle* setting **should be disabled**.
6. Use the virtual joystick to fly the drone!
@@ -180,7 +180,7 @@ If your drone doesn't have RC receiver installed, you can use USB remote control
1. Install [QGroundControl](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/getting_started/download_and_install.html) app on your computer.
2. Connect your USB remote control to the computer.
3. Power up the drone.
4. Connect your computer to the appeared `flix` Wi-Fi network.
4. Connect your computer to the appeared `flix` Wi-Fi network (password: `flixwifi`).
5. Launch QGroundControl app. It should connect and begin showing the drone's telemetry automatically.
6. Go the the QGroundControl menu ⇒ *Vehicle Setup**Joystick*. Calibrate you USB remote control there.
7. Use the USB remote control to fly the drone!

Binary file not shown.

After

Width:  |  Height:  |  Size: 35 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 36 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 43 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 54 KiB

After

Width:  |  Height:  |  Size: 87 KiB

View File

@@ -16,7 +16,6 @@ Do the following:
* **Make sure correct IMU model is chosen**. If using ICM-20948 board, change `MPU9250` to `ICM20948` everywhere in the `imu.ino` file.
* **Check if the CLI is working**. Perform `help` command in Serial Monitor. You should see the list of available commands. You can also access the CLI using QGroundControl (*Vehicle Setup* ⇒ *Analyze Tools**MAVLink Console*).
* **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**.
* **Make sure you're not moving the drone several seconds after the power on**. The drone calibrates its gyroscope on the start so it should stay still for a while.
* **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).

View File

@@ -4,6 +4,17 @@ This page contains user-built drones based on the Flix project. Publish your pro
---
Author: [@cryptokobans](https://t.me/cryptokobans).<br>
Features: ESP32-C3 SuperMini board, INA226 power monitor, IRLZ44N MOSFETs, MPU-6500 IMU.
**Flight video:**
<a href="https://drive.google.com/file/d/1-4ciDsj8slTEaxxRl1-QAkx0cUDkb8iy/view?usp=sharing"><img height=300 src="img/user/cryptokobans/video.jpg"></a>
<img src="img/user/cryptokobans/1.jpg" height=150> <img src="img/user/cryptokobans/2.jpg" height=150>
---
Author: [@jeka_chex](https://t.me/jeka_chex).<br>
Features: custom frame, FPV camera, 3-blade 31 mm propellers.<br>
Motor drivers: AON7410 MOSFET + capacitors.<br>
@@ -17,27 +28,14 @@ Custom frame files: https://drive.google.com/drive/folders/1QCIc-_YYFxJN4cMhVLjL
<a href="https://drive.google.com/file/d/1RSU6VWs9omsge4hGloH5NQqnxvLyhMKB/view?usp=sharing"><img height=300 src="img/user/jeka_chex/video-fpv.jpg"></a>
<table>
<tr>
<td><img src="img/user/jeka_chex/1.jpg" height=150></td>
<td><img src="img/user/jeka_chex/2.jpg" height=150></td>
<td><img src="img/user/jeka_chex/3.jpg" height=150></td>
<td><img src="img/user/jeka_chex/4.jpg" height=150></td>
<td><img src="img/user/jeka_chex/5.jpg" height=150></td>
</tr>
</table>
<img src="img/user/jeka_chex/1.jpg" height=150> <img src="img/user/jeka_chex/2.jpg" height=150> <img src="img/user/jeka_chex/3.jpg" height=150> <img src="img/user/jeka_chex/4.jpg" height=150> <img src="img/user/jeka_chex/5.jpg" height=150>
---
Author: [@fisheyeu](https://t.me/fisheyeu).<br>
[Video](https://drive.google.com/file/d/1IT4eMmWPZpmaZR_qsIRmNJ52hYkFB_0q/view?usp=share_link).
<table>
<tr>
<td><img src="img/user/fisheyeu/1.jpg" height=300></td>
<td><img src="img/user/fisheyeu/2.jpg" height=300></td>
</tr>
</table>
<img src="img/user/fisheyeu/1.jpg" height=300> <img src="img/user/fisheyeu/2.jpg" height=300>
---
@@ -46,13 +44,7 @@ Custom propellers guard 3D-model: https://drive.google.com/file/d/1TKnzwvrZYzYuR
Features: ESP32-C3 microcontroller is used.<br>
[Video](https://drive.google.com/file/d/1B0NMcsk0fgwUgNr9XuLOdR2yYCuaj008/view?usp=share_link).
<table>
<tr>
<td><img src="img/user/p_kabakov/1.jpg" width=150></td>
<td><img src="img/user/p_kabakov/2.jpg" width=150></td>
<td><img src="img/user/p_kabakov/3.jpg" width=150></td>
</tr>
</table>
<img src="img/user/p_kabakov/1.jpg" width=150> <img src="img/user/p_kabakov/2.jpg" width=150> <img src="img/user/p_kabakov/3.jpg" width=150>
**Custom Wi-Fi RC control:**
@@ -65,12 +57,7 @@ See source and description (in Russian): https://github.com/pavelkabakov/flix/tr
Author: [@yi_lun](https://t.me/yi_lun).<br>
[Video](https://drive.google.com/file/d/1TkSuvHQ_0qQPFUpY5XjJzmhnpX_07cTg/view?usp=share_link).
<table>
<tr>
<td><img src="img/user/yi_lun/1.jpg" width=300></td>
<td><img src="img/user/yi_lun/2.jpg" width=300></td>
</tr>
</table>
<img src="img/user/yi_lun/1.jpg" width=300> <img src="img/user/yi_lun/2.jpg" width=300>
---
@@ -81,12 +68,7 @@ Schematics: https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=34587646121795
<a href="https://www.youtube.com/watch?v=wi4w_hOmKcQ"><img width=500 src="img/user/peter_ukhov-2/video.jpg"></a>
<table>
<tr>
<td><img src="img/user/peter_ukhov-2/1.jpg" width=300></td>
<td><img src="img/user/peter_ukhov-2/2.jpg" width=300></td>
</tr>
</table>
<img src="img/user/peter_ukhov-2/1.jpg" width=300> <img src="img/user/peter_ukhov-2/2.jpg" width=300>
---
@@ -95,15 +77,7 @@ Files for 3D printing of the custom frame: https://drive.google.com/file/d/1tkNm
<a href="https://t.me/opensourcequadcopter/61"><img width=500 src="img/user/alexey_karakash/video.jpg"></a>
<table>
<tr>
<td><img src="img/user/alexey_karakash/1.jpg" height=150></td>
<td><img src="img/user/alexey_karakash/2.jpg" height=150></td>
<td><img src="img/user/alexey_karakash/3.jpg" height=150></td>
<td><img src="img/user/alexey_karakash/4.jpg" height=150></td>
<td><img src="img/user/alexey_karakash/5.jpg" height=150></td>
</tr>
</table>
<img src="img/user/alexey_karakash/1.jpg" height=150> <img src="img/user/alexey_karakash/2.jpg" height=150> <img src="img/user/alexey_karakash/3.jpg" height=150> <img src="img/user/alexey_karakash/4.jpg" height=150> <img src="img/user/alexey_karakash/5.jpg" height=150>
---
@@ -111,13 +85,7 @@ Author: [@rudpa](https://t.me/rudpa).<br>
<a href="https://t.me/opensourcequadcopter/46"><img width=500 src="img/user/rudpa/video.jpg"></a>
<table>
<tr>
<td><img src="img/user/rudpa/1.jpg" height=150></td>
<td><img src="img/user/rudpa/2.jpg" height=150></td>
<td><img src="img/user/rudpa/3.jpg" height=150></td>
</tr>
</table>
<img src="img/user/rudpa/1.jpg" height=150> <img src="img/user/rudpa/2.jpg" height=150> <img src="img/user/rudpa/3.jpg" height=150>
---
@@ -126,10 +94,4 @@ Schematics: https://miro.com/app/board/uXjVN-dTjoo=/?moveToWidget=34587646123382
<a href="https://t.me/opensourcequadcopter/24"><img width=500 src="img/user/peter_ukhov/video.jpg"></a>
<table>
<tr>
<td><img src="img/user/peter_ukhov/1.jpg" height=150></td>
<td><img src="img/user/peter_ukhov/2.jpg" height=150></td>
<td><img src="img/user/peter_ukhov/3.jpg" height=150></td>
</tr>
</table>
<img src="img/user/peter_ukhov/1.jpg" height=150> <img src="img/user/peter_ukhov/2.jpg" height=150> <img src="img/user/peter_ukhov/3.jpg" height=150>

View File

@@ -34,7 +34,6 @@ const char* motd =
"mot - show motor output\n"
"log - dump in-RAM log\n"
"cr - calibrate RC\n"
"cg - calibrate gyro\n"
"ca - calibrate accel\n"
"mfr, mfl, mrr, mrl - test motor (remove props)\n"
"reset - reset drone's state\n"
@@ -109,6 +108,7 @@ void doCommand(String str, bool echo = false) {
print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
printIMUCal();
print("rate: %f\n", loopRate);
print("landed: %d\n", landed);
} else if (command == "rc") {
print("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
channels[throttleChannel], channels[yawChannel], channels[pitchChannel],
@@ -124,8 +124,6 @@ void doCommand(String str, bool echo = false) {
dumpLog();
} else if (command == "cr") {
calibrateRC();
} else if (command == "cg") {
calibrateGyro();
} else if (command == "ca") {
calibrateAccel();
} else if (command == "mfr") {

View File

@@ -29,7 +29,7 @@ void applyGyro() {
void applyAcc() {
// test should we apply accelerometer gravity correction
float accNorm = acc.norm();
bool landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
if (!landed) return;

View File

@@ -18,6 +18,7 @@ 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 [-1..1]
void setup() {

View File

@@ -5,6 +5,7 @@
#include <SPI.h>
#include <MPU9250.h>
#include "lpf.h"
#include "util.h"
MPU9250 IMU(SPI);
@@ -17,8 +18,6 @@ void setupIMU() {
print("Setup IMU\n");
IMU.begin();
configureIMU();
delay(500); // wait a bit before calibrating
calibrateGyro();
}
void configureIMU() {
@@ -32,6 +31,7 @@ void readIMU() {
IMU.waitForData();
IMU.getGyro(gyro.x, gyro.y, gyro.z);
IMU.getAccel(acc.x, acc.y, acc.z);
calibrateGyroOnce();
// apply scale and bias
acc = (acc - accBias) / accScale;
gyro = gyro - gyroBias;
@@ -47,21 +47,13 @@ void rotateIMU(Vector& data) {
// Axes orientation for various boards: https://github.com/okalachev/flixperiph#imu-axes-orientation
}
void calibrateGyro() {
const int samples = 1000;
print("Calibrating gyro, stand still\n");
IMU.setGyroRange(IMU.GYRO_RANGE_250DPS); // the most sensitive mode
void calibrateGyroOnce() {
static float landedTime = 0;
landedTime = landed ? landedTime + dt : 0;
if (landedTime < 2) return; // calibrate only if definitely stationary
gyroBias = Vector(0, 0, 0);
for (int i = 0; i < samples; i++) {
IMU.waitForData();
IMU.getGyro(gyro.x, gyro.y, gyro.z);
gyroBias = gyroBias + gyro;
}
gyroBias = gyroBias / samples;
printIMUCal();
configureIMU();
static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
gyroBias = gyroCalibrationFilter.update(gyro);
}
void calibrateAccel() {
@@ -88,6 +80,7 @@ void calibrateAccel() {
calibrateAccelOnce();
printIMUCal();
print("✓ Calibration done!\n");
configureIMU();
}
@@ -113,9 +106,6 @@ void calibrateAccelOnce() {
if (acc.x < accMin.x) accMin.x = acc.x;
if (acc.y < accMin.y) accMin.y = acc.y;
if (acc.z < accMin.z) accMin.z = acc.z;
print("acc %f %f %f\n", acc.x, acc.y, acc.z);
print("max %f %f %f\n", accMax.x, accMax.y, accMax.z);
print("min %f %f %f\n", accMin.x, accMin.y, accMin.z);
// Compute scale and bias
accScale = (accMax - accMin) / 2 / ONE_G;
accBias = (accMax + accMin) / 2;

View File

@@ -33,19 +33,23 @@ void sendMavlink() {
if (t - lastSlow >= PERIOD_SLOW) {
lastSlow = t;
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR,
MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0),
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed * MAV_MODE_FLAG_SAFETY_ARMED) | ((mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED),
0, MAV_STATE_STANDBY);
sendMessage(&msg);
mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
sendMessage(&msg);
}
if (t - lastFast >= PERIOD_FAST) {
lastFast = t;
const float zeroQuat[] = {0, 0, 0, 0};
Quaternion attitudeFRD = fluToFrd(attitude); // MAVLink uses FRD coordinate system
Quaternion att = fluToFrd(attitude); // MAVLink uses FRD coordinate system
mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.z, rates.x, rates.y, rates.z, zeroQuat);
time, att.w, att.x, att.y, att.z, rates.x, rates.y, rates.z, zeroQuat);
sendMessage(&msg);
mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlsTime * 1000, 0,
@@ -88,7 +92,7 @@ void receiveMavlink() {
}
void handleMavlink(const void *_msg) {
const mavlink_message_t &msg = *(mavlink_message_t *)_msg;
const mavlink_message_t& msg = *(mavlink_message_t *)_msg;
if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
mavlink_manual_control_t m;

View File

@@ -48,9 +48,6 @@ Parameter parameters[] = {
{"ACC_SCALE_X", &accScale.x},
{"ACC_SCALE_Y", &accScale.y},
{"ACC_SCALE_Z", &accScale.z},
{"GYRO_BIAS_X", &gyroBias.x},
{"GYRO_BIAS_Y", &gyroBias.y},
{"GYRO_BIAS_Z", &gyroBias.z},
// rc
{"RC_NEUTRAL_0", &channelNeutral[0]},
{"RC_NEUTRAL_1", &channelNeutral[1]},

View File

@@ -21,6 +21,7 @@ Vector acc;
Vector gyro;
Vector rates;
Quaternion attitude;
bool landed;
// declarations
void step();

View File

@@ -1,6 +1,7 @@
<?xml version="1.0"?>
<sdf version="1.5">
<model name="flix">
<plugin name="flix" filename="libflix.so"/>
<link name="body">
<inertial>
<mass>0.065</mass>
@@ -23,38 +24,14 @@
<update_rate>1000</update_rate>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<stddev>0.00174533</stddev><!-- 0.1 degrees per second -->
</noise>
</x>
<y>
<noise type="gaussian">
<stddev>0.00174533</stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<stddev>0.00174533</stddev>
</noise>
</z>
<x><noise type="gaussian"><stddev>0.00174533</stddev></noise></x><!-- 0.1 degrees per second -->
<y><noise type="gaussian"><stddev>0.00174533</stddev></noise></y>
<z><noise type="gaussian"><stddev>0.00174533</stddev></noise></z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<stddev>0.0784</stddev><!-- 8 mg -->
</noise>
</x>
<y>
<noise type="gaussian">
<stddev>0.0784</stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<stddev>0.0784</stddev>
</noise>
</z>
<x><noise type="gaussian"><stddev>0.0784</stddev></noise></x><!-- 8 mg -->
<y><noise type="gaussian"><stddev>0.0784</stddev></noise></y>
<z><noise type="gaussian"><stddev>0.0784</stddev></noise></z>
</linear_acceleration>
</imu>
</sensor>
@@ -90,6 +67,5 @@
<material><ambient>1 1 1 0.5</ambient><diffuse>1 1 1 0.5</diffuse></material>
</visual>
</link>
<plugin name="flix" filename="libflix.so"/>
</model>
</sdf>