mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 01:29:33 +00:00
Merge branch 'master' into run-sim
This commit is contained in:
Oleg Kalachev
commit
d0eb2dd9ba
@ -9,3 +9,7 @@ charset = utf-8
|
||||
indent_style = tab
|
||||
tab_width = 4
|
||||
trim_trailing_whitespace = true
|
||||
|
||||
[{*.yml,*.yaml,CMakeLists.txt}]
|
||||
indent_style = space
|
||||
indent_size = 2
|
||||
|
||||
2
.github/workflows/build.yml
vendored
2
.github/workflows/build.yml
vendored
@ -12,7 +12,7 @@ jobs:
|
||||
steps:
|
||||
- uses: actions/checkout@v3
|
||||
- name: Install Arduino CLI
|
||||
uses: arduino/setup-arduino-cli@v1.1.1
|
||||
run: curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=/usr/local/bin sh
|
||||
- name: Build firmware
|
||||
run: make
|
||||
|
||||
|
||||
2
.gitignore
vendored
2
.gitignore
vendored
@ -1,5 +1,5 @@
|
||||
*.hex
|
||||
*.elf
|
||||
gazebo/build/
|
||||
build/
|
||||
tools/log/
|
||||
.dependencies
|
||||
|
||||
3
Makefile
3
Makefile
@ -15,8 +15,7 @@ dependencies .dependencies:
|
||||
arduino-cli core update-index --config-file arduino-cli.yaml
|
||||
arduino-cli core install esp32:esp32@2.0.11 --config-file arduino-cli.yaml
|
||||
arduino-cli lib update-index
|
||||
arduino-cli lib install "Bolder Flight Systems SBUS"@1.0.1
|
||||
arduino-cli lib install "Bolder Flight Systems MPU9250"@1.0.2
|
||||
arduino-cli lib install "FlixPeriph"
|
||||
arduino-cli lib install "MAVLink"@2.0.1
|
||||
touch .dependencies
|
||||
|
||||
|
||||
12
README.md
12
README.md
@ -38,7 +38,7 @@ Simulation in Gazebo using a plugin that runs original Arduino code is implement
|
||||
|
||||
<img src="docs/img/schematics.svg" width=800 alt="Flix schematics">
|
||||
|
||||
You can also check a user contributed [variant of complete circuit diagram](https://github.com/okalachev/flix/issues/3#issue-2066079898) of the drone.
|
||||
You can also check a user contributed [variant of complete circuit diagram](https://miro.com/app/board/uXjVN-dTjoo=/) of the drone.
|
||||
|
||||
*\* — SBUS inverter is not needed as ESP32 supports [software pin inversion](https://github.com/bolderflight/sbus#inverted-serial).*
|
||||
|
||||
@ -49,7 +49,7 @@ You can also check a user contributed [variant of complete circuit diagram](http
|
||||
|ESP32 Mini|Microcontroller board|<img src="docs/img/esp32.jpg" width=100>|1|
|
||||
|GY-91|IMU+LDO+barometer board|<img src="docs/img/gy-91.jpg" width=100>|1|
|
||||
|K100|Quadcopter frame|<img src="docs/img/frame.jpg" width=100>|1|
|
||||
|8520 3.7V brushed motor|Motor|<img src="docs/img/motor.jpeg" width=100>|4|
|
||||
|8520 3.7V brushed motor (**shaft 0.8mm!**)|Motor|<img src="docs/img/motor.jpeg" width=100>|4|
|
||||
|Hubsan 55 mm| Propeller|<img src="docs/img/prop.jpg" width=100>|4|
|
||||
|2.7A 1S Dual Way Micro Brush ESC|Motor ESC|<img src="docs/img/esc.jpg" width=100>|4|
|
||||
|KINGKONG TINY X8|RC transmitter|<img src="docs/img/tx.jpg" width=100>|1|
|
||||
@ -57,11 +57,13 @@ You can also check a user contributed [variant of complete circuit diagram](http
|
||||
||~~SBUS inverter~~*|<img src="docs/img/inv.jpg" width=100>|~~1~~|
|
||||
|3.7 Li-Po 850 MaH 60C|Battery|||
|
||||
||Battery charger|<img src="docs/img/charger.jpg" width=100>|1|
|
||||
||Wires, connectors, tape, ...||
|
||||
||3D-printed frame parts||
|
||||
||Wires, connectors, tape, ...|||
|
||||
||3D-printed frame parts|||
|
||||
|
||||
*\* — not needed as ESP32 supports [software pin inversion](https://github.com/bolderflight/sbus#inverted-serial).*
|
||||
|
||||
## Telegram-channel
|
||||
## Materials
|
||||
|
||||
Subscribe to Telegram-channel on developing the drone and the flight controller (in Russian): https://t.me/opensourcequadcopter.
|
||||
|
||||
Detailed article on Habr.com about the development of the drone (in Russian): https://habr.com/ru/articles/814127/.
|
||||
|
||||
@ -14,7 +14,7 @@ cd flix
|
||||
1. Install Arduino CLI:
|
||||
|
||||
```bash
|
||||
curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/local/bin sh
|
||||
curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=/usr/local/bin sh
|
||||
```
|
||||
|
||||
2. Install Gazebo 11:
|
||||
@ -36,7 +36,13 @@ cd flix
|
||||
sudo apt-get update && sudo apt-get install build-essential libsdl2-dev
|
||||
```
|
||||
|
||||
4. Run the simulation:
|
||||
4. Add your user to the `input` group to enable joystick support (you need to re-login after this command):
|
||||
|
||||
```bash
|
||||
sudo usermod -a -G input $USER
|
||||
```
|
||||
|
||||
5. Run the simulation:
|
||||
|
||||
```bash
|
||||
make simulator
|
||||
@ -59,6 +65,13 @@ cd flix
|
||||
brew install sdl2
|
||||
```
|
||||
|
||||
Set up your Gazebo environment variables:
|
||||
|
||||
```bash
|
||||
echo "source /opt/homebrew/share/gazebo/setup.sh" >> ~/.zshrc
|
||||
source ~/.zshrc
|
||||
```
|
||||
|
||||
3. Run the simulation:
|
||||
|
||||
```bash
|
||||
@ -67,7 +80,7 @@ cd flix
|
||||
|
||||
### Flight
|
||||
|
||||
Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* setting enabled) to control the drone.
|
||||
Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* setting enabled) to control the drone. *Auto-Center Throttle* setting **should be disabled**.
|
||||
|
||||
## Firmware
|
||||
|
||||
@ -75,9 +88,8 @@ Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* set
|
||||
|
||||
1. Install [Arduino IDE](https://www.arduino.cc/en/software) (version 2 is recommended).
|
||||
2. Install ESP32 core using [Boards Manager](https://docs.arduino.cc/learn/starting-guide/cores).
|
||||
3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library) (**versions are significant**):
|
||||
* `Bolder Flight Systems SBUS`, version 1.0.1.
|
||||
* `Bolder Flight Systems MPU9250`, version 1.0.2.
|
||||
3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library):
|
||||
* `FlixPeriph`.
|
||||
* `MAVLink`, version 2.0.1.
|
||||
4. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).
|
||||
5. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.
|
||||
|
||||
@ -8,9 +8,10 @@ The main loop is running at 1000 Hz. All the dataflow is happening through globa
|
||||
|
||||
* `t` *(float)* — current step time, *s*.
|
||||
* `dt` *(float)* — time delta between the current and previous steps, *s*.
|
||||
* `rates` *(Vector)* — angular rates from the gyroscope, *rad/s*.
|
||||
* `gyro` *(Vector)* — data from the gyroscope, *rad/s*.
|
||||
* `acc` *(Vector)* — acceleration data from the accelerometer, *m/s<sup>2</sup>*.
|
||||
* `attitude` *(Quaternion)* — current estimated attitude (orientation) of drone.
|
||||
* `rates` *(Vector)* — filtered angular rates, *rad/s*.
|
||||
* `attitude` *(Quaternion)* — estimated attitude (orientation) of drone.
|
||||
* `controls` *(float[])* — user control inputs from the RC, normalized to [-1, 1] range.
|
||||
* `motors` *(float[])* — motor outputs, normalized to [-1, 1] range; reverse rotation is possible.
|
||||
|
||||
|
||||
@ -2,181 +2,19 @@
|
||||
<!-- Created with Inkscape (http://www.inkscape.org/) -->
|
||||
|
||||
<svg
|
||||
width="613.59802mm"
|
||||
height="267.24701mm"
|
||||
viewBox="0 -10 613.59802 267.247"
|
||||
version="1.1"
|
||||
id="svg1"
|
||||
width="2431.925"
|
||||
height="1044.4771"
|
||||
viewBox="0 0 2431.9251 1044.4771"
|
||||
sodipodi:docname="flix-primakova-22.svg"
|
||||
inkscape:version="1.3 (0e150ed, 2023-07-21)"
|
||||
xml:space="preserve"
|
||||
inkscape:version="1.3.2 (091e20e, 2023-11-25)"
|
||||
sodipodi:docname="dataflow.svg"
|
||||
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
|
||||
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
|
||||
xmlns:xlink="http://www.w3.org/1999/xlink"
|
||||
xmlns="http://www.w3.org/2000/svg"
|
||||
xmlns:svg="http://www.w3.org/2000/svg">
|
||||
<defs
|
||||
id="defs1">
|
||||
<color-profile
|
||||
inkscape:label="sRGB IEC61966-2.1"
|
||||
name="sRGB-IEC61966-2.1"
|
||||
xlink:href="data:application/vnd.iccprofile;base64,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"
|
||||
id="color-profile2" />
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath2">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path2" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath4">
|
||||
<path
|
||||
d="m 745.9206,375.0079 h 404.1921 V 493.7148 H 745.9206 Z"
|
||||
transform="matrix(1,0,0,-1,-874.5547,410.75391)"
|
||||
id="path4" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath6">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path6" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath7">
|
||||
<path
|
||||
d="m 734.9959,651.142 h 426.0414 V 769.8489 H 734.9959 Z"
|
||||
transform="matrix(1,0,0,-1,-786.22464,686.88802)"
|
||||
id="path7" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath9">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path9" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath10">
|
||||
<path
|
||||
d="m 67.84839,771.7229 h 404.1921 V 890.4298 H 67.84839 Z"
|
||||
transform="matrix(1,0,0,-1,-172.21011,807.46902)"
|
||||
id="path10" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath12">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path12" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath13">
|
||||
<path
|
||||
d="m 1452.804,771.7229 h 375.3804 V 890.4298 H 1452.804 Z"
|
||||
transform="matrix(1,0,0,-1,-1500.8361,807.46902)"
|
||||
id="path13" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath15">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path15" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath16">
|
||||
<path
|
||||
d="m 1452.804,226.7937 h 375.3804 V 345.5006 H 1452.804 Z"
|
||||
transform="matrix(1,0,0,-1,-1499.533,262.53983)"
|
||||
id="path16" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath18">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-451.65591,778.64052)"
|
||||
id="path18" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath20">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path20" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath21">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(0.98150515,-0.1914358,-0.1914358,-0.98150515,-353.6359,868.10017)"
|
||||
id="path21" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath23">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(0.89875521,0.43845065,0.43845065,-0.89875521,-1406.8193,-156.93149)"
|
||||
id="path23" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath25">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(0.98480768,0.17364817,0.17364817,-0.98480768,-1344.8142,527.62156)"
|
||||
id="path25" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath27">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-1132.0071,771.72292)"
|
||||
id="path27" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath30">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-1142.931,782.32692)"
|
||||
id="path30" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath33">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-1656.51,771.72572)"
|
||||
id="path33" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath35">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-1703.37,542.87351)"
|
||||
id="path35" />
|
||||
</clipPath>
|
||||
<clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath37">
|
||||
<path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-270.40501,771.68402)"
|
||||
id="path37" />
|
||||
</clipPath>
|
||||
</defs>
|
||||
<sodipodi:namedview
|
||||
xmlns:svg="http://www.w3.org/2000/svg"><sodipodi:namedview
|
||||
id="namedview1"
|
||||
pagecolor="#ffffff"
|
||||
bordercolor="#666666"
|
||||
@ -185,279 +23,308 @@
|
||||
inkscape:pageopacity="0.0"
|
||||
inkscape:pagecheckerboard="0"
|
||||
inkscape:deskcolor="#d1d1d1"
|
||||
showgrid="false"
|
||||
showguides="false"
|
||||
inkscape:zoom="0.31949673"
|
||||
inkscape:cx="1292.658"
|
||||
inkscape:cy="316.12217"
|
||||
inkscape:document-units="mm"
|
||||
inkscape:zoom="0.34862039"
|
||||
inkscape:cx="1219.091"
|
||||
inkscape:cy="608.1113"
|
||||
inkscape:window-width="1496"
|
||||
inkscape:window-height="905"
|
||||
inkscape:window-x="0"
|
||||
inkscape:window-y="34"
|
||||
inkscape:window-maximized="1"
|
||||
inkscape:current-layer="g1">
|
||||
<inkscape:page
|
||||
inkscape:current-layer="svg1"><inkscape:page
|
||||
x="0"
|
||||
y="0"
|
||||
inkscape:label="1"
|
||||
id="page1"
|
||||
width="2431.925"
|
||||
height="1044.4771"
|
||||
width="613.59802"
|
||||
height="267.24701"
|
||||
id="page2"
|
||||
margin="0"
|
||||
bleed="0" />
|
||||
</sodipodi:namedview>
|
||||
<g
|
||||
bleed="0" /><inkscape:page
|
||||
x="-30.32262"
|
||||
y="-66.876167"
|
||||
width="677.33331"
|
||||
height="381"
|
||||
id="page3"
|
||||
margin="0"
|
||||
bleed="0" /></sodipodi:namedview><defs
|
||||
id="defs1"><color-profile
|
||||
inkscape:label="sRGB IEC61966-2.1"
|
||||
name="sRGB-IEC61966-2.1"
|
||||
xlink:href="data:application/vnd.iccprofile;base64,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"
|
||||
id="color-profile1" /><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath2"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path2" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath4"><path
|
||||
d="m 745.9206,375.0079 h 404.1921 V 493.7148 H 745.9206 Z"
|
||||
transform="matrix(1,0,0,-1,-874.5547,410.75391)"
|
||||
id="path4" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath6"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path6" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath7"><path
|
||||
d="m 734.9959,651.142 h 426.0414 V 769.8489 H 734.9959 Z"
|
||||
transform="matrix(1,0,0,-1,-786.22464,686.88802)"
|
||||
id="path7" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath9"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path9" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath10"><path
|
||||
d="m 67.84839,771.7229 h 404.1921 V 890.4298 H 67.84839 Z"
|
||||
transform="matrix(1,0,0,-1,-172.21011,807.46902)"
|
||||
id="path10" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath12"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path12" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath13"><path
|
||||
d="m 1452.804,771.7229 h 375.3804 V 890.4298 H 1452.804 Z"
|
||||
transform="matrix(1,0,0,-1,-1500.8361,807.46902)"
|
||||
id="path13" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath15"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path15" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath16"><path
|
||||
d="m 1452.804,226.7937 h 375.3804 V 345.5006 H 1452.804 Z"
|
||||
transform="matrix(1,0,0,-1,-1499.533,262.53983)"
|
||||
id="path16" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath18"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-451.65591,778.64052)"
|
||||
id="path18" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath20"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
id="path20" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath21"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(0.98150515,-0.1914358,-0.1914358,-0.98150515,-353.6359,868.10017)"
|
||||
id="path21" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath23"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(0.89875521,0.43845065,0.43845065,-0.89875521,-1406.8193,-156.93149)"
|
||||
id="path23" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath25"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(0.98480768,0.17364817,0.17364817,-0.98480768,-1293.1181,526.43111)"
|
||||
id="path25" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath27"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-1132.0071,771.72292)"
|
||||
id="path27" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath30"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-1142.931,782.32692)"
|
||||
id="path30" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath33"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-1656.51,771.72572)"
|
||||
id="path33" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath35"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-1703.37,542.87351)"
|
||||
id="path35" /></clipPath><clipPath
|
||||
clipPathUnits="userSpaceOnUse"
|
||||
id="clipPath37"><path
|
||||
d="M 0,0 H 1920 V 1080 H 0 Z"
|
||||
transform="matrix(1,0,0,-1,-270.40501,771.68402)"
|
||||
id="path37" /></clipPath></defs><g
|
||||
inkscape:label="Layer 1"
|
||||
inkscape:groupmode="layer"
|
||||
id="layer1"
|
||||
transform="translate(-250.32262,-66.876165)" /><g
|
||||
id="g1"
|
||||
inkscape:groupmode="layer"
|
||||
inkscape:label="1"
|
||||
transform="translate(-59.605,-197.76013)">
|
||||
<rect
|
||||
style="font-variation-settings:normal;opacity:0.468447;vector-effect:none;fill:#ffffff;fill-opacity:1;fill-rule:evenodd;stroke:none;stroke-width:1;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-dashoffset:0;stroke-opacity:1;-inkscape-stroke:none;stop-color:#000000;stop-opacity:1"
|
||||
id="rect38"
|
||||
width="2431.925"
|
||||
height="1044.4771"
|
||||
x="59.605"
|
||||
y="197.76013"
|
||||
inkscape:label="rect38" />
|
||||
<g
|
||||
id="g2">
|
||||
<path
|
||||
transform="matrix(0.26458333,0,0,0.26458334,-30.322612,-66.876165)"><g
|
||||
id="g2"><path
|
||||
id="path3"
|
||||
d="m 786.6761,493.7148 h 322.6809 c 6.647,0 10.635,0 13.293,-1.11 3.833,-1.3949 6.852,-4.4141 8.247,-8.2468 1.11,-2.6586 1.11,-6.6466 1.11,-13.2932 v -73.4069 c 0,-6.6466 0,-10.6346 -1.11,-13.2932 -1.395,-3.8327 -4.414,-6.8518 -8.247,-8.2468 -2.658,-1.11 -6.646,-1.11 -13.293,-1.11 H 786.6761 c -6.6467,0 -10.6346,0 -13.2933,1.11 -3.8327,1.395 -6.8518,4.4141 -8.2468,8.2468 -1.1099,2.6586 -1.1099,6.6466 -1.1099,13.2932 v 73.4069 c 0,6.6466 0,10.6346 1.1099,13.2932 1.395,3.8327 4.4141,6.8519 8.2468,8.2468 2.6587,1.11 6.6466,1.11 13.2933,1.11 z"
|
||||
style="fill:#0076ba;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" />
|
||||
</g>
|
||||
<g
|
||||
id="g3">
|
||||
<text
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" /></g><g
|
||||
id="g3"><text
|
||||
id="text3"
|
||||
xml:space="preserve"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,1166.0729,892.32813)"
|
||||
clip-path="url(#clipPath4)"><tspan
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma,Arial;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 21.33 49.02 67.169998 80.879997 114.33"
|
||||
y="0"
|
||||
sodipodi:role="line"
|
||||
id="tspan3">rc.ino</tspan></text>
|
||||
</g>
|
||||
<g
|
||||
id="g4">
|
||||
<path
|
||||
id="tspan3">rc.ino</tspan></text></g><g
|
||||
id="g4"><path
|
||||
id="path5"
|
||||
d="m 775.7514,769.8489 h 344.5306 c 6.646,0 10.634,0 13.293,-1.11 3.833,-1.395 6.852,-4.4141 8.247,-8.2468 1.11,-2.6586 1.11,-6.6466 1.11,-13.2932 V 673.792 c 0,-6.6466 0,-10.6346 -1.11,-13.2932 -1.395,-3.8327 -4.414,-6.8519 -8.247,-8.2468 -2.659,-1.11 -6.647,-1.11 -13.293,-1.11 H 775.7514 c -6.6466,0 -10.6346,0 -13.2933,1.11 -3.8326,1.3949 -6.8518,4.4141 -8.2468,8.2468 -1.1099,2.6586 -1.1099,6.6466 -1.1099,13.2932 v 73.4069 c 0,6.6466 0,10.6346 1.1099,13.2932 1.395,3.8327 4.4142,6.8518 8.2468,8.2468 2.6587,1.11 6.6467,1.11 13.2933,1.11 z"
|
||||
style="fill:#0076ba;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)"
|
||||
clip-path="url(#clipPath6)" />
|
||||
</g>
|
||||
<g
|
||||
id="g6">
|
||||
<text
|
||||
clip-path="url(#clipPath6)" /></g><g
|
||||
id="g6"><text
|
||||
id="text6"
|
||||
xml:space="preserve"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,1048.2995,524.14933)"
|
||||
clip-path="url(#clipPath7)"><tspan
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma,Arial;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 31.584 58.368 78.431999 92.136002 142.5 173.964 194.028 225.612 243.756 257.45999 290.90399"
|
||||
y="0"
|
||||
sodipodi:role="line"
|
||||
id="tspan6">estimate.ino</tspan></text>
|
||||
</g>
|
||||
<g
|
||||
id="g7">
|
||||
<path
|
||||
id="tspan6">estimate.ino</tspan></text></g><g
|
||||
id="g7"><path
|
||||
id="path8"
|
||||
d="M 108.6039,890.4298 H 431.285 c 6.6466,0 10.6346,0 13.2932,-1.1099 3.8327,-1.395 6.8519,-4.4142 8.2468,-8.2468 1.11,-2.6587 1.11,-6.6467 1.11,-13.2933 V 794.373 c 0,-6.6467 0,-10.6346 -1.11,-13.2933 -1.3949,-3.8327 -4.4141,-6.8518 -8.2468,-8.2468 -2.6586,-1.11 -6.6466,-1.11 -13.2932,-1.11 H 108.6039 c -6.6467,0 -10.63463,0 -13.29329,1.11 -3.83267,1.395 -6.85182,4.4141 -8.2468,8.2468 -1.10995,2.6587 -1.10995,6.6466 -1.10995,13.2933 v 73.4068 c 0,6.6466 0,10.6346 1.10995,13.2933 1.39498,3.8326 4.41413,6.8518 8.2468,8.2468 2.65866,1.1099 6.64659,1.1099 13.29329,1.1099 z"
|
||||
style="fill:#0076ba;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)"
|
||||
clip-path="url(#clipPath9)" />
|
||||
</g>
|
||||
<g
|
||||
id="g9">
|
||||
<text
|
||||
clip-path="url(#clipPath9)" /></g><g
|
||||
id="g9"><text
|
||||
id="text9"
|
||||
xml:space="preserve"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,229.61347,363.37467)"
|
||||
clip-path="url(#clipPath10)"><tspan
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma,Arial;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 13.71 64.080002 97.529999 115.68 129.39 162.84"
|
||||
y="0"
|
||||
sodipodi:role="line"
|
||||
id="tspan9">imu.ino</tspan></text>
|
||||
</g>
|
||||
<g
|
||||
id="g10">
|
||||
<path
|
||||
id="tspan9">imu.ino</tspan></text></g><g
|
||||
id="g10"><path
|
||||
id="path11"
|
||||
d="m 1493.56,890.4298 h 293.869 c 6.647,0 10.635,0 13.294,-1.1099 3.832,-1.395 6.851,-4.4142 8.246,-8.2468 1.11,-2.6587 1.11,-6.6467 1.11,-13.2933 V 794.373 c 0,-6.6467 0,-10.6346 -1.11,-13.2933 -1.395,-3.8327 -4.414,-6.8518 -8.246,-8.2468 -2.659,-1.11 -6.647,-1.11 -13.294,-1.11 H 1493.56 c -6.647,0 -10.635,0 -13.293,1.11 -3.833,1.395 -6.852,4.4141 -8.247,8.2468 -1.11,2.6587 -1.11,6.6466 -1.11,13.2933 v 73.4068 c 0,6.6466 0,10.6346 1.11,13.2933 1.395,3.8326 4.414,6.8518 8.247,8.2468 2.658,1.1099 6.646,1.1099 13.293,1.1099 z"
|
||||
style="fill:#0076ba;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)"
|
||||
clip-path="url(#clipPath12)" />
|
||||
</g>
|
||||
<g
|
||||
id="g12">
|
||||
<text
|
||||
clip-path="url(#clipPath12)" /></g><g
|
||||
id="g12"><text
|
||||
id="text12"
|
||||
xml:space="preserve"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,2001.1147,363.37467)"
|
||||
clip-path="url(#clipPath13)"><tspan
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma,Arial;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 27.684 60.264 93.720001 113.79 135.12 167.7 181.41 199.57201 213.282 246.73801"
|
||||
y="0"
|
||||
sodipodi:role="line"
|
||||
id="tspan12">control.ino</tspan></text>
|
||||
</g>
|
||||
<g
|
||||
id="g13">
|
||||
<path
|
||||
id="tspan12">control.ino</tspan></text></g><g
|
||||
id="g13"><path
|
||||
id="path14"
|
||||
d="m 1493.56,345.5006 h 293.869 c 6.647,0 10.635,0 13.294,-1.1099 3.832,-1.395 6.851,-4.4142 8.246,-8.2468 1.11,-2.6587 1.11,-6.6467 1.11,-13.2933 v -73.4068 c 0,-6.6467 0,-10.6346 -1.11,-13.2933 -1.395,-3.8327 -4.414,-6.8518 -8.246,-8.2468 -2.659,-1.11 -6.647,-1.11 -13.294,-1.11 H 1493.56 c -6.647,0 -10.635,0 -13.293,1.11 -3.833,1.395 -6.852,4.4141 -8.247,8.2468 -1.11,2.6587 -1.11,6.6466 -1.11,13.2933 v 73.4068 c 0,6.6466 0,10.6346 1.11,13.2933 1.395,3.8326 4.414,6.8518 8.247,8.2468 2.658,1.1099 6.646,1.1099 13.293,1.1099 z"
|
||||
style="fill:#0076ba;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)"
|
||||
clip-path="url(#clipPath15)" />
|
||||
</g>
|
||||
<g
|
||||
id="g15">
|
||||
<text
|
||||
clip-path="url(#clipPath15)" /></g><g
|
||||
id="g15"><text
|
||||
id="text15"
|
||||
xml:space="preserve"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,1999.3773,1089.9469)"
|
||||
clip-path="url(#clipPath16)"><tspan
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma,Arial;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
style="font-variant:normal;font-weight:normal;font-size:60px;font-family:Tahoma;writing-mode:lr-tb;fill:#ffffff;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 50.388 82.968002 103.038 135.618 157.242 184.02 202.18201 215.892 249.34801"
|
||||
y="0"
|
||||
sodipodi:role="line"
|
||||
id="tspan15">motors.ino</tspan></text>
|
||||
</g>
|
||||
<g
|
||||
id="g16">
|
||||
<path
|
||||
id="tspan15">motors.ino</tspan></text></g><g
|
||||
id="g16"><path
|
||||
id="path17"
|
||||
d="m 0,0 c 89.24774,21.64997 181.7839,38.35706 277.6084,50.12129 l 2.9802,0.35217"
|
||||
style="fill:none;stroke:#d5d5d5;stroke-width:6;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,602.20787,401.81267)"
|
||||
clip-path="url(#clipPath18)" />
|
||||
<path
|
||||
clip-path="url(#clipPath18)" /><path
|
||||
id="path19"
|
||||
d="m 727.8571,716.602 25.2424,9.1006 -22.426,14.7336 z"
|
||||
style="fill:#d5d5d5;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)"
|
||||
clip-path="url(#clipPath20)" />
|
||||
</g>
|
||||
<g
|
||||
id="g20" />
|
||||
<g
|
||||
id="g21">
|
||||
<text
|
||||
clip-path="url(#clipPath20)" /></g><g
|
||||
id="g20" /><g
|
||||
id="g21"><text
|
||||
id="text21"
|
||||
xml:space="preserve"
|
||||
transform="matrix(1.3086735,0.25524773,-0.25524773,1.3086735,684.37453,394.20507)"
|
||||
clip-path="url(#clipPath21)"><tspan
|
||||
style="font-variant:normal;font-weight:normal;font-size:40px;font-family:Tahoma,Arial;writing-mode:lr-tb;fill:#000000;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 13.732 34.743999 48.116001 69.167999 87.019997 99.112 111.604 132.616 150.948"
|
||||
style="font-variant:normal;font-weight:normal;font-size:40px;font-family:Tahoma;writing-mode:lr-tb;fill:#000000;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 22.108 42.028 56.248001 77.400002 89.508003 102.008 123.004 141.34399"
|
||||
y="0"
|
||||
sodipodi:role="line"
|
||||
id="tspan21">rates, acc</tspan></text>
|
||||
</g>
|
||||
<g
|
||||
id="g22" />
|
||||
<g
|
||||
id="g23">
|
||||
<text
|
||||
id="tspan21">gyro, acc</tspan></text></g><g
|
||||
id="g22" /><g
|
||||
id="g23"><text
|
||||
id="text23"
|
||||
xml:space="preserve"
|
||||
transform="matrix(1.1983404,-0.58460093,0.58460093,1.1983404,1777.5907,805.62947)"
|
||||
clip-path="url(#clipPath23)"><tspan
|
||||
style="font-variant:normal;font-weight:normal;font-size:40px;font-family:Tahoma,Arial;writing-mode:lr-tb;fill:#000000;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
style="font-variant:normal;font-weight:normal;font-size:40px;font-family:Tahoma;writing-mode:lr-tb;fill:#000000;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 18.455999 40.175999 62.48 75.860001 90.080002 111.8 120.94 138.79201 154.104 175.94 197.776"
|
||||
y="0"
|
||||
sodipodi:role="line"
|
||||
id="tspan23">controls[16]</tspan></text>
|
||||
</g>
|
||||
<g
|
||||
id="g24" />
|
||||
<g
|
||||
id="g25">
|
||||
<text
|
||||
id="tspan23">controls[16]</tspan></text></g><g
|
||||
id="g24" /><g
|
||||
id="g25"><text
|
||||
id="text25"
|
||||
xml:space="preserve"
|
||||
transform="matrix(1.3130771,-0.23153093,0.23153093,1.3130771,1643.684,435.82613)"
|
||||
transform="matrix(1.3130771,-0.23153093,0.23153093,1.3130771,1576.0787,449.35853)"
|
||||
clip-path="url(#clipPath25)"><tspan
|
||||
style="font-variant:normal;font-weight:normal;font-size:40px;font-family:Tahoma,Arial;writing-mode:lr-tb;fill:#000000;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 20.996 34.071999 47.467999 56.703999 70.099998 92.416 114.532"
|
||||
style="font-variant:normal;font-weight:normal;font-size:40px;font-family:Tahoma;writing-mode:lr-tb;fill:#000000;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 13.732 34.743999 48.116001 69.167999 87.019997 99.112 111.604 132.616 145.668 159.03999 168.252 181.62399 203.916 226.008"
|
||||
y="0"
|
||||
sodipodi:role="line"
|
||||
id="tspan25">attitude</tspan></text>
|
||||
</g>
|
||||
<g
|
||||
id="g26">
|
||||
<path
|
||||
id="tspan25">rates, attitude</tspan></text></g><g
|
||||
id="g26"><path
|
||||
id="path26"
|
||||
d="M 0,306.3767 C 237.7238,253.1408 395.5189,158.083 473.3852,21.20323 l 1.4097,-2.65533"
|
||||
style="fill:none;stroke:#d5d5d5;stroke-width:6;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,1509.3427,411.03613)"
|
||||
clip-path="url(#clipPath27)" />
|
||||
<path
|
||||
clip-path="url(#clipPath27)" /><path
|
||||
id="path28"
|
||||
d="m 1615.994,744.8981 0.656,26.8248 -21.853,-15.5703 z"
|
||||
style="fill:#d5d5d5;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" />
|
||||
</g>
|
||||
<g
|
||||
id="g28">
|
||||
<path
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" /></g><g
|
||||
id="g28"><path
|
||||
id="path29"
|
||||
d="M 0,60.29815 C 99.1961,49.5746 200.9904,31.41576 305.383,5.821648 l 2.914,-0.728473"
|
||||
style="fill:none;stroke:#d5d5d5;stroke-width:6;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,1523.908,396.89747)"
|
||||
clip-path="url(#clipPath30)" />
|
||||
<path
|
||||
clip-path="url(#clipPath30)" /><path
|
||||
id="path31"
|
||||
d="m 1451.228,764.8644 20.373,17.4625 -26.194,5.821 z"
|
||||
style="fill:#d5d5d5;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" />
|
||||
</g>
|
||||
<g
|
||||
id="g31">
|
||||
<path
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" /></g><g
|
||||
id="g31"><path
|
||||
id="path32"
|
||||
d="M 5.061111,0 C 39.5121,116.8397 39.6576,251.1261 5.497595,402.859 l -0.68786,2.9234"
|
||||
style="fill:none;stroke:#d5d5d5;stroke-width:6;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,2208.68,411.0324)"
|
||||
clip-path="url(#clipPath33)" />
|
||||
<path
|
||||
clip-path="url(#clipPath33)" /><path
|
||||
id="path34"
|
||||
d="m 1650.326,371.6119 6.184,-26.1104 17.178,20.6136 z"
|
||||
style="fill:#d5d5d5;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" />
|
||||
</g>
|
||||
<g
|
||||
id="g34" />
|
||||
<g
|
||||
id="g35">
|
||||
<text
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" /></g><g
|
||||
id="g34" /><g
|
||||
id="g35"><text
|
||||
id="text35"
|
||||
xml:space="preserve"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,2271.16,716.16867)"
|
||||
clip-path="url(#clipPath35)"><tspan
|
||||
style="font-variant:normal;font-weight:normal;font-size:40px;font-family:Tahoma,Arial;writing-mode:lr-tb;fill:#000000;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
style="font-variant:normal;font-weight:normal;font-size:40px;font-family:Tahoma;writing-mode:lr-tb;fill:#000000;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
x="0 33.591999 55.312 68.692001 90.412003 104.828"
|
||||
y="0"
|
||||
sodipodi:role="line"
|
||||
id="tspan35">motors</tspan></text>
|
||||
</g>
|
||||
<g
|
||||
id="g36">
|
||||
<path
|
||||
id="tspan35">motors</tspan></text></g><g
|
||||
id="g36"><path
|
||||
id="path36"
|
||||
d="M 1200.4,523.3549 C 431.1994,677.8194 31.45986,511.3581 1.181784,23.97107 L 1.034053,20.97453"
|
||||
style="fill:none;stroke:#ff9300;stroke-width:6;stroke-linecap:butt;stroke-linejoin:miter;stroke-miterlimit:4;stroke-dasharray:12, 12;stroke-dashoffset:0;stroke-opacity:1"
|
||||
transform="matrix(1.3333333,0,0,1.3333333,360.54,411.088)"
|
||||
clip-path="url(#clipPath37)" />
|
||||
<path
|
||||
clip-path="url(#clipPath37)" /><path
|
||||
id="path38"
|
||||
d="m 283.5722,748.304 -13.1672,23.38 -10.8036,-24.5618 z"
|
||||
style="fill:#ff9300;fill-opacity:1;fill-rule:nonzero;stroke:none"
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" />
|
||||
</g>
|
||||
</g>
|
||||
</svg>
|
||||
transform="matrix(1.3333333,0,0,-1.3333333,0,1440)" /></g></g></svg>
|
||||
|
||||
|
Before Width: | Height: | Size: 23 KiB After Width: | Height: | Size: 22 KiB |
@ -30,6 +30,7 @@ const char* motd =
|
||||
"cr - calibrate RC\n"
|
||||
"cg - calibrate gyro\n"
|
||||
"ca - calibrate accel\n"
|
||||
"mfr, mfl, mrr, mrl - test appropriate motor\n"
|
||||
"fullmot <n> - full motor test\n"
|
||||
"reset - reset drone's state\n";
|
||||
|
||||
@ -72,6 +73,7 @@ void doCommand(String& command, String& value) {
|
||||
Serial.printf("gyro: %f %f %f\n", rates.x, rates.y, rates.z);
|
||||
Serial.printf("acc: %f %f %f\n", acc.x, acc.y, acc.z);
|
||||
printIMUCal();
|
||||
Serial.printf("frequency: %f\n", loopFreq);
|
||||
} else if (command == "rc") {
|
||||
Serial.printf("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",
|
||||
channels[RC_CHANNEL_THROTTLE], channels[RC_CHANNEL_YAW], channels[RC_CHANNEL_PITCH],
|
||||
|
||||
@ -32,10 +32,9 @@
|
||||
#define YAWRATE_MAX radians(360)
|
||||
#define MAX_TILT radians(30)
|
||||
|
||||
#define RATES_LFP_ALPHA 0.8 // cutoff frequency ~ 250 Hz
|
||||
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
||||
|
||||
enum { MANUAL, ACRO, STAB } mode = STAB;
|
||||
enum { MANUAL, ACRO, STAB, USER } mode = STAB;
|
||||
enum { YAW, YAW_RATE } yawMode = YAW;
|
||||
bool armed = false;
|
||||
|
||||
@ -46,8 +45,6 @@ PID rollPID(ROLL_P, ROLL_I, ROLL_D);
|
||||
PID pitchPID(PITCH_P, PITCH_I, PITCH_D);
|
||||
PID yawPID(YAW_P, 0, 0);
|
||||
|
||||
LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
|
||||
|
||||
Quaternion attitudeTarget;
|
||||
Vector ratesTarget;
|
||||
Vector torqueTarget;
|
||||
@ -68,6 +65,8 @@ void control() {
|
||||
}
|
||||
|
||||
void interpretRC() {
|
||||
armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_ARMED] >= 0.5;
|
||||
|
||||
// NOTE: put ACRO or MANUAL modes there if you want to use them
|
||||
if (controls[RC_CHANNEL_MODE] < 0.25) {
|
||||
mode = STAB;
|
||||
@ -77,7 +76,6 @@ void interpretRC() {
|
||||
mode = STAB;
|
||||
}
|
||||
|
||||
armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_ARMED] >= 0.5;
|
||||
thrustTarget = controls[RC_CHANNEL_THROTTLE];
|
||||
|
||||
if (mode == ACRO) {
|
||||
@ -89,10 +87,10 @@ void interpretRC() {
|
||||
} else if (mode == STAB) {
|
||||
yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;
|
||||
|
||||
attitudeTarget = Quaternion::fromEulerZYX(
|
||||
attitudeTarget = Quaternion::fromEulerZYX(Vector(
|
||||
controls[RC_CHANNEL_ROLL] * MAX_TILT,
|
||||
-controls[RC_CHANNEL_PITCH] * MAX_TILT,
|
||||
attitudeTarget.getYaw());
|
||||
attitudeTarget.getYaw()));
|
||||
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
|
||||
|
||||
} else if (mode == MANUAL) {
|
||||
@ -137,8 +135,7 @@ void controlRate() {
|
||||
return;
|
||||
}
|
||||
|
||||
Vector ratesFiltered = ratesFilter.update(rates);
|
||||
Vector error = ratesTarget - ratesFiltered;
|
||||
Vector error = ratesTarget - rates;
|
||||
|
||||
// Calculate desired torque, where 0 - no torque, 1 - maximum possible torque
|
||||
torqueTarget.x = rollRatePID.update(error.x, dt);
|
||||
@ -152,10 +149,10 @@ void controlTorque() {
|
||||
return;
|
||||
}
|
||||
|
||||
motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
|
||||
motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;
|
||||
motors[MOTOR_REAR_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;
|
||||
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;
|
||||
motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;
|
||||
motors[MOTOR_FRONT_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;
|
||||
motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z;
|
||||
motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z;
|
||||
|
||||
motors[0] = constrain(motors[0], 0, 1);
|
||||
motors[1] = constrain(motors[1], 0, 1);
|
||||
@ -172,6 +169,7 @@ const char* getModeName() {
|
||||
case MANUAL: return "MANUAL";
|
||||
case ACRO: return "ACRO";
|
||||
case STAB: return "STAB";
|
||||
case USER: return "USER";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
@ -5,9 +5,13 @@
|
||||
|
||||
#include "quaternion.h"
|
||||
#include "vector.h"
|
||||
#include "lpf.h"
|
||||
|
||||
#define ONE_G 9.807f
|
||||
#define WEIGHT_ACC 0.5f
|
||||
#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
||||
|
||||
LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
|
||||
|
||||
void estimate() {
|
||||
applyGyro();
|
||||
@ -16,7 +20,10 @@ void estimate() {
|
||||
}
|
||||
|
||||
void applyGyro() {
|
||||
// applying gyro
|
||||
// filter gyro to get angular rates
|
||||
rates = ratesFilter.update(gyro);
|
||||
|
||||
// apply rates to attitude
|
||||
attitude *= Quaternion::fromAngularRates(rates * dt);
|
||||
attitude.normalize();
|
||||
}
|
||||
|
||||
@ -26,10 +26,11 @@
|
||||
float t = NAN; // current step time, s
|
||||
float dt; // time delta from previous step, s
|
||||
float loopFreq; // loop frequency, Hz
|
||||
uint16_t channels[16]; // raw rc channels
|
||||
int16_t channels[16]; // raw rc channels
|
||||
float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)
|
||||
Vector rates; // angular rates, rad/s
|
||||
Vector gyro; // gyroscope data
|
||||
Vector acc; // accelerometer data, m/s/s
|
||||
Vector rates; // filtered angular rates, rad/s
|
||||
Quaternion attitude; // estimated attitude
|
||||
float motors[4]; // normalized motors thrust in range [-1..1]
|
||||
|
||||
@ -51,8 +52,7 @@ void setup() {
|
||||
}
|
||||
|
||||
void loop() {
|
||||
if (!readIMU()) return;
|
||||
|
||||
readIMU();
|
||||
step();
|
||||
readRC();
|
||||
estimate();
|
||||
|
||||
145
flix/imu.ino
145
flix/imu.ino
@ -6,87 +6,116 @@
|
||||
#include <SPI.h>
|
||||
#include <MPU9250.h>
|
||||
|
||||
#define IMU_CS_PIN 4 // chip-select pin for IMU SPI connection
|
||||
#define LOAD_GYRO_CAL false
|
||||
#define ONE_G 9.80665
|
||||
|
||||
MPU9250 IMU(SPI, IMU_CS_PIN);
|
||||
// NOTE: use 'ca' command to calibrate the accelerometer and put the values here
|
||||
Vector accBias(0, 0, 0);
|
||||
Vector accScale(1, 1, 1);
|
||||
|
||||
MPU9250 IMU(SPI);
|
||||
Vector gyroBias;
|
||||
|
||||
void setupIMU() {
|
||||
Serial.println("Setup IMU, stand still");
|
||||
|
||||
auto status = IMU.begin();
|
||||
if (status < 0) {
|
||||
Serial.println("Setup IMU");
|
||||
bool status = IMU.begin();
|
||||
if (!status) {
|
||||
while (true) {
|
||||
Serial.printf("IMU begin error: %d\n", status);
|
||||
Serial.println("IMU begin error");
|
||||
delay(1000);
|
||||
}
|
||||
}
|
||||
|
||||
if (LOAD_GYRO_CAL) loadGyroCal();
|
||||
loadAccelCal();
|
||||
|
||||
IMU.setSrd(0); // set sample rate to 1000 Hz
|
||||
// NOTE: very important, without the above the rate would be terrible 50 Hz
|
||||
calibrateGyro();
|
||||
}
|
||||
|
||||
bool readIMU() {
|
||||
if (IMU.readSensor() < 0) {
|
||||
Serial.println("IMU read error");
|
||||
return false;
|
||||
}
|
||||
void configureIMU() {
|
||||
IMU.setAccelRange(IMU.ACCEL_RANGE_4G);
|
||||
IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS);
|
||||
IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ);
|
||||
IMU.setSrd(0); // set sample rate to 1000 Hz
|
||||
}
|
||||
|
||||
auto lastRates = rates;
|
||||
|
||||
rates.x = IMU.getGyroX_rads();
|
||||
rates.y = IMU.getGyroY_rads();
|
||||
rates.z = IMU.getGyroZ_rads();
|
||||
acc.x = IMU.getAccelX_mss();
|
||||
acc.y = IMU.getAccelY_mss();
|
||||
acc.z = IMU.getAccelZ_mss();
|
||||
|
||||
return rates != lastRates;
|
||||
void readIMU() {
|
||||
IMU.waitForData();
|
||||
IMU.getGyro(gyro.x, gyro.y, gyro.z);
|
||||
IMU.getAccel(acc.x, acc.y, acc.z);
|
||||
// apply scale and bias
|
||||
acc = (acc - accBias) / accScale;
|
||||
gyro = gyro - gyroBias;
|
||||
}
|
||||
|
||||
void calibrateGyro() {
|
||||
const int samples = 1000;
|
||||
Serial.println("Calibrating gyro, stand still");
|
||||
int status = IMU.calibrateGyro();
|
||||
Serial.printf("Calibration status: %d\n", status);
|
||||
IMU.setSrd(0);
|
||||
IMU.setGyroRange(IMU.GYRO_RANGE_250DPS); // the most sensitive mode
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
void calibrateAccel() {
|
||||
Serial.println("Cal accel: place level"); delay(3000);
|
||||
IMU.calibrateAccel();
|
||||
Serial.println("Cal accel: place nose up"); delay(3000);
|
||||
IMU.calibrateAccel();
|
||||
Serial.println("Cal accel: place nose down"); delay(3000);
|
||||
IMU.calibrateAccel();
|
||||
Serial.println("Cal accel: place on right side"); delay(3000);
|
||||
IMU.calibrateAccel();
|
||||
Serial.println("Cal accel: place on left side"); delay(3000);
|
||||
IMU.calibrateAccel();
|
||||
Serial.println("Cal accel: upside down"); delay(300);
|
||||
IMU.calibrateAccel();
|
||||
Serial.println("Calibrating accelerometer");
|
||||
IMU.setAccelRange(IMU.ACCEL_RANGE_2G); // the most sensitive mode
|
||||
IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_20HZ);
|
||||
IMU.setSrd(19);
|
||||
|
||||
Serial.setTimeout(60000);
|
||||
Serial.print("Place level [enter] "); Serial.readStringUntil('\n');
|
||||
calibrateAccelOnce();
|
||||
Serial.print("Place nose up [enter] "); Serial.readStringUntil('\n');
|
||||
calibrateAccelOnce();
|
||||
Serial.print("Place nose down [enter] "); Serial.readStringUntil('\n');
|
||||
calibrateAccelOnce();
|
||||
Serial.print("Place on right side [enter] "); Serial.readStringUntil('\n');
|
||||
calibrateAccelOnce();
|
||||
Serial.print("Place on left side [enter] "); Serial.readStringUntil('\n');
|
||||
calibrateAccelOnce();
|
||||
Serial.print("Place upside down [enter] "); Serial.readStringUntil('\n');
|
||||
calibrateAccelOnce();
|
||||
|
||||
printIMUCal();
|
||||
configureIMU();
|
||||
}
|
||||
|
||||
void loadAccelCal() {
|
||||
// NOTE: this should be changed to the actual values
|
||||
IMU.setAccelCalX(-0.0048542023, 1.0008112192);
|
||||
IMU.setAccelCalY(0.0521845818, 0.9985780716);
|
||||
IMU.setAccelCalZ(0.5754694939, 1.0045746565);
|
||||
}
|
||||
void calibrateAccelOnce() {
|
||||
const int samples = 100;
|
||||
static Vector accMax(-INFINITY, -INFINITY, -INFINITY);
|
||||
static Vector accMin(INFINITY, INFINITY, INFINITY);
|
||||
|
||||
void loadGyroCal() {
|
||||
// NOTE: this should be changed to the actual values
|
||||
IMU.setGyroBiasX_rads(-0.0185128022);
|
||||
IMU.setGyroBiasY_rads(-0.0262369743);
|
||||
IMU.setGyroBiasZ_rads(0.0163032326);
|
||||
// Compute the average of the accelerometer readings
|
||||
acc = Vector(0, 0, 0);
|
||||
for (int i = 0; i < samples; i++) {
|
||||
IMU.waitForData();
|
||||
Vector sample;
|
||||
IMU.getAccel(sample.x, sample.y, sample.z);
|
||||
acc = acc + sample;
|
||||
}
|
||||
acc = acc / samples;
|
||||
|
||||
// Update the maximum and minimum values
|
||||
if (acc.x > accMax.x) accMax.x = acc.x;
|
||||
if (acc.y > accMax.y) accMax.y = acc.y;
|
||||
if (acc.z > accMax.z) accMax.z = acc.z;
|
||||
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;
|
||||
Serial.printf("acc %f %f %f\n", acc.x, acc.y, acc.z);
|
||||
Serial.printf("max %f %f %f\n", accMax.x, accMax.y, accMax.z);
|
||||
Serial.printf("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;
|
||||
}
|
||||
|
||||
void printIMUCal() {
|
||||
Serial.printf("gyro bias: %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads());
|
||||
Serial.printf("accel bias: %f %f %f\n", IMU.getAccelBiasX_mss(), IMU.getAccelBiasY_mss(), IMU.getAccelBiasZ_mss());
|
||||
Serial.printf("accel scale: %f %f %f\n", IMU.getAccelScaleFactorX(), IMU.getAccelScaleFactorY(), IMU.getAccelScaleFactorZ());
|
||||
Serial.printf("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);
|
||||
Serial.printf("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);
|
||||
Serial.printf("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z);
|
||||
}
|
||||
|
||||
@ -44,6 +44,7 @@ void dumpLog() {
|
||||
Serial.printf("t,rates.x,rates.y,rates.z,ratesTarget.x,ratesTarget.y,ratesTarget.z,"
|
||||
"attitude.x,attitude.y,attitude.z,attitudeTarget.x,attitudeTarget.y,attitudeTarget.z,thrustTarget\n");
|
||||
for (int i = 0; i < LOG_SIZE; i++) {
|
||||
if (logBuffer[i][0] == 0) continue; // skip empty records
|
||||
for (int j = 0; j < LOG_COLUMNS - 1; j++) {
|
||||
Serial.printf("%f,", logBuffer[i][j]);
|
||||
}
|
||||
|
||||
@ -13,8 +13,7 @@ public:
|
||||
|
||||
LowPassFilter(float alpha): alpha(alpha) {};
|
||||
|
||||
T update(const T input)
|
||||
{
|
||||
T update(const T input) {
|
||||
if (alpha == 1) { // filter disabled
|
||||
return input;
|
||||
}
|
||||
@ -26,13 +25,11 @@ public:
|
||||
return output = output * (1 - alpha) + input * alpha;
|
||||
}
|
||||
|
||||
void setCutOffFrequency(float cutOffFreq, float dt)
|
||||
{
|
||||
void setCutOffFrequency(float cutOffFreq, float dt) {
|
||||
alpha = 1 - exp(-2 * PI * cutOffFreq * dt);
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
void reset() {
|
||||
initialized = false;
|
||||
}
|
||||
|
||||
|
||||
@ -55,7 +55,7 @@ void sendMavlink() {
|
||||
|
||||
mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
|
||||
acc.x * 1000, acc.y * 1000, acc.z * 1000,
|
||||
rates.x * 1000, rates.y * 1000, rates.z * 1000,
|
||||
gyro.x * 1000, gyro.y * 1000, gyro.z * 1000,
|
||||
0, 0, 0, 0);
|
||||
sendMessage(&msg);
|
||||
}
|
||||
|
||||
@ -9,16 +9,13 @@
|
||||
#define MOTOR_1_PIN 13
|
||||
#define MOTOR_2_PIN 14
|
||||
#define MOTOR_3_PIN 15
|
||||
|
||||
#define PWM_FREQUENCY 200
|
||||
#define PWM_RESOLUTION 8
|
||||
|
||||
#define PWM_NEUTRAL 1500
|
||||
|
||||
const uint16_t pwmMin[] = {1600, 1600, 1600, 1600};
|
||||
const uint16_t pwmMax[] = {2300, 2300, 2300, 2300};
|
||||
const uint16_t pwmReverseMin[] = {1390, 1440, 1440, 1440};
|
||||
const uint16_t pwmReverseMax[] = {1100, 1100, 1100, 1100};
|
||||
#define PWM_MIN 1600
|
||||
#define PWM_MAX 2300
|
||||
#define PWM_REVERSE_MIN 1400
|
||||
#define PWM_REVERSE_MAX 700
|
||||
|
||||
void setupMotors() {
|
||||
Serial.println("Setup Motors");
|
||||
@ -43,9 +40,9 @@ uint16_t getPWM(float val, int n) {
|
||||
if (val == 0) {
|
||||
return PWM_NEUTRAL;
|
||||
} else if (val > 0) {
|
||||
return mapff(val, 0, 1, pwmMin[n], pwmMax[n]);
|
||||
return mapff(val, 0, 1, PWM_MIN, PWM_MAX);
|
||||
} else {
|
||||
return mapff(val, 0, -1, pwmReverseMin[n], pwmReverseMax[n]);
|
||||
return mapff(val, 0, -1, PWM_REVERSE_MIN, PWM_REVERSE_MAX);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -21,8 +21,7 @@ public:
|
||||
|
||||
PID(float p, float i, float d, float windup = 0, float dAlpha = 1) : p(p), i(i), d(d), windup(windup), lpf(dAlpha) {};
|
||||
|
||||
float update(float error, float dt)
|
||||
{
|
||||
float update(float error, float dt) {
|
||||
integral += error * dt;
|
||||
|
||||
if (isfinite(prevError) && dt > 0) {
|
||||
@ -38,8 +37,7 @@ public:
|
||||
return p * error + constrain(i * integral, -windup, windup) + d * derivative; // PID
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
void reset() {
|
||||
prevError = NAN;
|
||||
integral = 0;
|
||||
derivative = 0;
|
||||
|
||||
@ -15,8 +15,7 @@ public:
|
||||
|
||||
Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {};
|
||||
|
||||
static Quaternion fromAxisAngle(float a, float b, float c, float angle)
|
||||
{
|
||||
static Quaternion fromAxisAngle(float a, float b, float c, float angle) {
|
||||
float halfAngle = angle * 0.5;
|
||||
float sin2 = sin(halfAngle);
|
||||
float cos2 = cos(halfAngle);
|
||||
@ -24,22 +23,20 @@ public:
|
||||
return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm);
|
||||
}
|
||||
|
||||
static Quaternion fromAngularRates(const Vector& rates)
|
||||
{
|
||||
static Quaternion fromAngularRates(const Vector& rates) {
|
||||
if (rates.zero()) {
|
||||
return Quaternion();
|
||||
}
|
||||
return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());
|
||||
}
|
||||
|
||||
static Quaternion fromEulerZYX(float x, float y, float z)
|
||||
{
|
||||
float cx = cos(x / 2);
|
||||
float cy = cos(y / 2);
|
||||
float cz = cos(z / 2);
|
||||
float sx = sin(x / 2);
|
||||
float sy = sin(y / 2);
|
||||
float sz = sin(z / 2);
|
||||
static Quaternion fromEulerZYX(const Vector& euler) {
|
||||
float cx = cos(euler.x / 2);
|
||||
float cy = cos(euler.y / 2);
|
||||
float cz = cos(euler.z / 2);
|
||||
float sx = sin(euler.x / 2);
|
||||
float sy = sin(euler.y / 2);
|
||||
float sz = sin(euler.z / 2);
|
||||
|
||||
return Quaternion(
|
||||
cx * cy * cz + sx * sy * sz,
|
||||
@ -48,8 +45,7 @@ public:
|
||||
cx * cy * sz - sx * sy * cz);
|
||||
}
|
||||
|
||||
static Quaternion fromBetweenVectors(Vector u, Vector v)
|
||||
{
|
||||
static Quaternion fromBetweenVectors(Vector u, Vector v) {
|
||||
float dot = u.x * v.x + u.y * v.y + u.z * v.z;
|
||||
float w1 = u.y * v.z - u.z * v.y;
|
||||
float w2 = u.z * v.x - u.x * v.z;
|
||||
@ -64,33 +60,46 @@ public:
|
||||
return ret;
|
||||
}
|
||||
|
||||
void toAxisAngle(float& a, float& b, float& c, float& angle)
|
||||
{
|
||||
void toAxisAngle(float& a, float& b, float& c, float& angle) {
|
||||
angle = acos(w) * 2;
|
||||
a = x / sin(angle / 2);
|
||||
b = y / sin(angle / 2);
|
||||
c = z / sin(angle / 2);
|
||||
}
|
||||
|
||||
Vector toEulerZYX() const
|
||||
{
|
||||
return Vector(
|
||||
atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)),
|
||||
asin(2 * (w * y - z * x)),
|
||||
atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)));
|
||||
Vector toEulerZYX() const {
|
||||
// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L87
|
||||
Vector euler;
|
||||
float sqx = x * x;
|
||||
float sqy = y * y;
|
||||
float sqz = z * z;
|
||||
float sqw = w * w;
|
||||
// Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
|
||||
float sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw); /* normalization added from urdfom_headers */
|
||||
if (sarg <= -0.99999) {
|
||||
euler.x = 0;
|
||||
euler.y = -0.5 * PI;
|
||||
euler.z = -2 * atan2(y, x);
|
||||
} else if (sarg >= 0.99999) {
|
||||
euler.x = 0;
|
||||
euler.y = 0.5 * PI;
|
||||
euler.z = 2 * atan2(y, x);
|
||||
} else {
|
||||
euler.x = atan2(2 * (y * z + w * x), sqw - sqx - sqy + sqz);
|
||||
euler.y = asin(sarg);
|
||||
euler.z = atan2(2 * (x * y + w * z), sqw + sqx - sqy - sqz);
|
||||
}
|
||||
return euler;
|
||||
}
|
||||
|
||||
float getYaw() const
|
||||
{
|
||||
float getYaw() const {
|
||||
// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L122
|
||||
float yaw;
|
||||
float sqx = x * x;
|
||||
float sqy = y * y;
|
||||
float sqz = z * z;
|
||||
float sqw = w * w;
|
||||
|
||||
double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);
|
||||
|
||||
if (sarg <= -0.99999) {
|
||||
yaw = -2 * atan2(y, x);
|
||||
} else if (sarg >= 0.99999) {
|
||||
@ -101,15 +110,14 @@ public:
|
||||
return yaw;
|
||||
}
|
||||
|
||||
void setYaw(float yaw)
|
||||
{
|
||||
void setYaw(float yaw) {
|
||||
// TODO: optimize?
|
||||
Vector euler = toEulerZYX();
|
||||
(*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw);
|
||||
euler.z = yaw;
|
||||
(*this) = Quaternion::fromEulerZYX(euler);
|
||||
}
|
||||
|
||||
Quaternion& operator *= (const Quaternion& q)
|
||||
{
|
||||
Quaternion& operator *= (const Quaternion& q) {
|
||||
Quaternion ret(
|
||||
w * q.w - x * q.x - y * q.y - z * q.z,
|
||||
w * q.x + x * q.w + y * q.z - z * q.y,
|
||||
@ -118,8 +126,7 @@ public:
|
||||
return (*this = ret);
|
||||
}
|
||||
|
||||
Quaternion operator * (const Quaternion& q)
|
||||
{
|
||||
Quaternion operator * (const Quaternion& q) {
|
||||
return Quaternion(
|
||||
w * q.w - x * q.x - y * q.y - z * q.z,
|
||||
w * q.x + x * q.w + y * q.z - z * q.y,
|
||||
@ -127,8 +134,7 @@ public:
|
||||
w * q.z + z * q.w + x * q.y - y * q.x);
|
||||
}
|
||||
|
||||
Quaternion inversed() const
|
||||
{
|
||||
Quaternion inversed() const {
|
||||
float normSqInv = 1 / (w * w + x * x + y * y + z * z);
|
||||
return Quaternion(
|
||||
w * normSqInv,
|
||||
@ -137,13 +143,11 @@ public:
|
||||
-z * normSqInv);
|
||||
}
|
||||
|
||||
float norm() const
|
||||
{
|
||||
float norm() const {
|
||||
return sqrt(w * w + x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
void normalize()
|
||||
{
|
||||
void normalize() {
|
||||
float n = norm();
|
||||
w /= n;
|
||||
x /= n;
|
||||
@ -151,27 +155,24 @@ public:
|
||||
z /= n;
|
||||
}
|
||||
|
||||
Vector conjugate(const Vector& v)
|
||||
{
|
||||
Vector conjugate(const Vector& v) {
|
||||
Quaternion qv(0, v.x, v.y, v.z);
|
||||
Quaternion res = (*this) * qv * inversed();
|
||||
return Vector(res.x, res.y, res.z);
|
||||
}
|
||||
|
||||
Vector conjugateInversed(const Vector& v)
|
||||
{
|
||||
Vector conjugateInversed(const Vector& v) {
|
||||
Quaternion qv(0, v.x, v.y, v.z);
|
||||
Quaternion res = inversed() * qv * (*this);
|
||||
return Vector(res.x, res.y, res.z);
|
||||
}
|
||||
|
||||
inline Vector rotate(const Vector& v)
|
||||
{
|
||||
// Rotate vector by quaternion
|
||||
inline Vector rotate(const Vector& v) {
|
||||
return conjugateInversed(v);
|
||||
}
|
||||
|
||||
inline bool finite() const
|
||||
{
|
||||
inline bool finite() const {
|
||||
return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
|
||||
}
|
||||
|
||||
|
||||
12
flix/rc.ino
12
flix/rc.ino
@ -5,9 +5,7 @@
|
||||
|
||||
#include <SBUS.h>
|
||||
|
||||
#define INVERT_SERIAL true // false if external SBUS invertor is used
|
||||
|
||||
// NOTE: use `cr` command and replace with the actual values
|
||||
// NOTE: use 'cr' command to calibrate the RC and put the values here
|
||||
int channelNeutral[] = {995, 883, 200, 972, 512, 512};
|
||||
int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472};
|
||||
|
||||
@ -16,14 +14,12 @@ SBUS RC(Serial2);
|
||||
void setupRC() {
|
||||
Serial.println("Setup RC");
|
||||
RC.begin();
|
||||
Serial2.setRxInvert(INVERT_SERIAL);
|
||||
}
|
||||
|
||||
void readRC() {
|
||||
bool failSafe, lostFrame;
|
||||
if (RC.read(channels, &failSafe, &lostFrame)) {
|
||||
if (failSafe) { return; } // TODO:
|
||||
if (lostFrame) { return; }
|
||||
if (RC.read()) {
|
||||
SBUSData data = RC.data();
|
||||
memcpy(channels, data.ch, sizeof(channels)); // copy channels data
|
||||
normalizeRC();
|
||||
}
|
||||
}
|
||||
|
||||
@ -13,79 +13,79 @@ public:
|
||||
|
||||
Vector(float x, float y, float z): x(x), y(y), z(z) {};
|
||||
|
||||
float norm() const
|
||||
{
|
||||
float norm() const {
|
||||
return sqrt(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
bool zero() const
|
||||
{
|
||||
bool zero() const {
|
||||
return x == 0 && y == 0 && z == 0;
|
||||
}
|
||||
|
||||
void normalize()
|
||||
{
|
||||
void normalize() {
|
||||
float n = norm();
|
||||
x /= n;
|
||||
y /= n;
|
||||
z /= n;
|
||||
}
|
||||
|
||||
Vector operator * (const float b) const
|
||||
{
|
||||
Vector operator * (const float b) const {
|
||||
return Vector(x * b, y * b, z * b);
|
||||
}
|
||||
|
||||
Vector operator / (const float b) const
|
||||
{
|
||||
Vector operator / (const float b) const {
|
||||
return Vector(x / b, y / b, z / b);
|
||||
}
|
||||
|
||||
Vector operator + (const Vector& b) const
|
||||
{
|
||||
Vector operator + (const Vector& b) const {
|
||||
return Vector(x + b.x, y + b.y, z + b.z);
|
||||
}
|
||||
|
||||
Vector operator - (const Vector& b) const
|
||||
{
|
||||
Vector operator - (const Vector& b) const {
|
||||
return Vector(x - b.x, y - b.y, z - b.z);
|
||||
}
|
||||
|
||||
inline bool operator == (const Vector& b) const
|
||||
{
|
||||
// Element-wise multiplication
|
||||
Vector operator * (const Vector& b) const {
|
||||
return Vector(x * b.x, y * b.y, z * b.z);
|
||||
}
|
||||
|
||||
// Element-wise division
|
||||
Vector operator / (const Vector& b) const {
|
||||
return Vector(x / b.x, y / b.y, z / b.z);
|
||||
}
|
||||
|
||||
inline bool operator == (const Vector& b) const {
|
||||
return x == b.x && y == b.y && z == b.z;
|
||||
}
|
||||
|
||||
inline bool operator != (const Vector& b) const
|
||||
{
|
||||
inline bool operator != (const Vector& b) const {
|
||||
return !(*this == b);
|
||||
}
|
||||
|
||||
inline bool finite() const
|
||||
{
|
||||
inline bool finite() const {
|
||||
return isfinite(x) && isfinite(y) && isfinite(z);
|
||||
}
|
||||
|
||||
static float dot(const Vector& a, const Vector& b)
|
||||
{
|
||||
static float dot(const Vector& a, const Vector& b) {
|
||||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||||
}
|
||||
|
||||
static Vector cross(const Vector& a, const Vector& b)
|
||||
{
|
||||
static Vector cross(const Vector& a, const Vector& b) {
|
||||
return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
|
||||
}
|
||||
|
||||
static float angleBetweenVectors(const Vector& a, const Vector& b)
|
||||
{
|
||||
static float angleBetweenVectors(const Vector& a, const Vector& b) {
|
||||
return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1));
|
||||
}
|
||||
|
||||
static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v)
|
||||
{
|
||||
Vector direction = cross(u, v);
|
||||
static Vector angularRatesBetweenVectors(const Vector& a, const Vector& b) {
|
||||
Vector direction = cross(a, b);
|
||||
if (direction.zero()) {
|
||||
// vectors are opposite, return any perpendicular vector
|
||||
return cross(a, Vector(1, 0, 0));
|
||||
}
|
||||
direction.normalize();
|
||||
float angle = angleBetweenVectors(u, v);
|
||||
float angle = angleBetweenVectors(a, b);
|
||||
return direction * angle;
|
||||
}
|
||||
|
||||
|
||||
@ -5,11 +5,20 @@
|
||||
|
||||
#include "joystick.h"
|
||||
|
||||
struct SBUSData {
|
||||
int16_t ch[16];
|
||||
};
|
||||
|
||||
class SBUS {
|
||||
public:
|
||||
SBUS(HardwareSerial& bus) {};
|
||||
SBUS(HardwareSerial& bus, const bool inv = true) {};
|
||||
void begin() {};
|
||||
bool read(int16_t* channels, bool* failsafe, bool* lostFrame) { // NOTE: on the hardware channels is uint16_t
|
||||
return joystickGet();
|
||||
bool read() { return joystickGet(); };
|
||||
SBUSData data() {
|
||||
SBUSData data;
|
||||
for (uint8_t i = 0; i < 16; i++) {
|
||||
data.ch[i] = channels[i];
|
||||
}
|
||||
return data;
|
||||
};
|
||||
};
|
||||
|
||||
@ -23,9 +23,10 @@ float t = NAN;
|
||||
float dt;
|
||||
float loopFreq;
|
||||
float motors[4];
|
||||
int16_t channels[16]; // raw rc channels WARNING: unsigned on hardware
|
||||
int16_t channels[16]; // raw rc channels
|
||||
float controls[RC_CHANNELS];
|
||||
Vector acc;
|
||||
Vector gyro;
|
||||
Vector rates;
|
||||
Quaternion attitude;
|
||||
|
||||
|
||||
@ -8,15 +8,15 @@
|
||||
#include <iostream>
|
||||
|
||||
// simulation calibration overrides, NOTE: use `cr` command and replace with the actual values
|
||||
const int channelNeutralOverride[] = {-258, -258, -27349, 0, 0, 1};
|
||||
const int channelMaxOverride[] = {27090, 27090, 27090, 27090, 0, 1};
|
||||
const int channelNeutralOverride[] = {-258, -258, -27349, 0, -27349, 0};
|
||||
const int channelMaxOverride[] = {27090, 27090, 27090, 27090, -5676, 1};
|
||||
|
||||
#define RC_CHANNEL_ROLL 0
|
||||
#define RC_CHANNEL_PITCH 1
|
||||
#define RC_CHANNEL_THROTTLE 2
|
||||
#define RC_CHANNEL_YAW 3
|
||||
#define RC_CHANNEL_ARMED 4
|
||||
#define RC_CHANNEL_MODE 5
|
||||
#define RC_CHANNEL_ARMED 5
|
||||
#define RC_CHANNEL_MODE 4
|
||||
|
||||
SDL_Joystick *joystick;
|
||||
bool joystickInitialized = false, warnShown = false;
|
||||
@ -49,10 +49,8 @@ bool joystickGet() {
|
||||
|
||||
SDL_JoystickUpdate();
|
||||
|
||||
for (uint8_t i = 0; i < 4; i++) {
|
||||
for (uint8_t i = 0; i < 8; i++) {
|
||||
channels[i] = SDL_JoystickGetAxis(joystick, i);
|
||||
}
|
||||
channels[RC_CHANNEL_MODE] = SDL_JoystickGetButton(joystick, 0) ? 1 : 0;
|
||||
controls[RC_CHANNEL_MODE] = channels[RC_CHANNEL_MODE];
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -64,7 +64,7 @@ public:
|
||||
step();
|
||||
|
||||
// read imu
|
||||
rates = flu2frd(imu->AngularVelocity());
|
||||
gyro = flu2frd(imu->AngularVelocity());
|
||||
acc = this->accFilter.update(flu2frd(imu->LinearAcceleration()));
|
||||
|
||||
// read rc
|
||||
@ -92,10 +92,10 @@ public:
|
||||
const double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78
|
||||
|
||||
const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05; // imitating motors asymmetry
|
||||
float mfl = scale0 * maxThrust * abs(motors[MOTOR_FRONT_LEFT]);
|
||||
float mfr = scale1 * maxThrust * abs(motors[MOTOR_FRONT_RIGHT]);
|
||||
float mrl = scale2 * maxThrust * abs(motors[MOTOR_REAR_LEFT]);
|
||||
float mrr = scale3 * maxThrust * abs(motors[MOTOR_REAR_RIGHT]);
|
||||
float mfl = scale0 * maxThrust * motors[MOTOR_FRONT_LEFT];
|
||||
float mfr = scale1 * maxThrust * motors[MOTOR_FRONT_RIGHT];
|
||||
float mrl = scale2 * maxThrust * motors[MOTOR_REAR_LEFT];
|
||||
float mrr = scale3 * maxThrust * motors[MOTOR_REAR_RIGHT];
|
||||
|
||||
body->AddLinkForce(Vector3d(0.0, 0.0, mfl), Vector3d(dist, dist, 0.0));
|
||||
body->AddLinkForce(Vector3d(0.0, 0.0, mfr), Vector3d(dist, -dist, 0.0));
|
||||
|
||||
@ -25,7 +25,7 @@ void setupWiFi() {
|
||||
bind(wifiSocket, (sockaddr *)&addr, sizeof(addr));
|
||||
int broadcast = 1;
|
||||
setsockopt(wifiSocket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); // enable broadcast
|
||||
gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT_LOCAL << std::endl;
|
||||
gzmsg << "WiFi UDP socket initialized on port " << WIFI_UDP_PORT_LOCAL << " (remote port " << WIFI_UDP_PORT_REMOTE << ")" << std::endl;
|
||||
}
|
||||
|
||||
void sendWiFi(const uint8_t *buf, int len) {
|
||||
|
||||
@ -9,8 +9,7 @@ PORT = os.environ['PORT']
|
||||
DIR = os.path.dirname(os.path.realpath(__file__))
|
||||
|
||||
dev = serial.Serial(port=PORT, baudrate=115200, timeout=0.5)
|
||||
|
||||
log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
|
||||
lines = []
|
||||
|
||||
print('Downloading log...')
|
||||
count = 0
|
||||
@ -19,8 +18,14 @@ while True:
|
||||
line = dev.readline()
|
||||
if not line:
|
||||
break
|
||||
log.write(line)
|
||||
lines.append(line)
|
||||
count += 1
|
||||
print(f'\r{count} lines', end='')
|
||||
|
||||
# sort by timestamp
|
||||
header = lines.pop(0)
|
||||
lines.sort(key=lambda line: float(line.split(b',')[0]))
|
||||
|
||||
log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
|
||||
log.writelines([header] + lines)
|
||||
print(f'\nWritten {os.path.relpath(log.name, os.curdir)}')
|
||||
|
||||
2
tools/requirements.txt
Normal file
2
tools/requirements.txt
Normal file
@ -0,0 +1,2 @@
|
||||
docopt
|
||||
matplotlib
|
||||
Loading…
x
Reference in New Issue
Block a user