Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Add quaternion and vector chapter to book
Oleg Kalachev98fc0cf5b494
diff --git a/docs/book.css b/docs/book.cssindex 7612e77..f724c32 100644--- a/docs/book.css+++ b/docs/book.css@@ -53,6 +53,12 @@ footer a.telegram, footer a.github {border: 1px solid #c9c9c9;}+@media (max-width: 600px) {+ .MathJax_Display {+ overflow-x: auto;+ }+}+.firmware {position: relative;margin: 20px 0;diff --git a/docs/book.toml b/docs/book.tomlindex d5c505b..de44bce 100644--- a/docs/book.toml+++ b/docs/book.toml@@ -10,7 +10,7 @@ description = "Учебник по разработке полетного коbuild-dir = "build"[output.html]-additional-css = ["book.css", "zoom.css"]+additional-css = ["book.css", "zoom.css", "rotation.css"]additional-js = ["zoom.js", "js.js"]edit-url-template = "https://github.com/okalachev/flix/blob/master/docs/{path}?plain=1"mathjax-support = truediff --git a/docs/book/SUMMARY.md b/docs/book/SUMMARY.mdindex e0280ab..249913e 100644--- a/docs/book/SUMMARY.md+++ b/docs/book/SUMMARY.md@@ -11,6 +11,7 @@* [Светодиод]()* [Моторы]()* [Радиоуправление]()+* [Вектор, кватернион](geometry.md)* [Гироскоп](gyro.md)* [Акселерометр]()* [Оценка состояния]()diff --git a/docs/book/geometry.md b/docs/book/geometry.mdnew file mode 100644index 0000000..bb45721--- /dev/null+++ b/docs/book/geometry.md@@ -0,0 +1,309 @@+# Вектор, кватернион++В алгоритме управления квадрокоптером широко применяются геометрические (и алгебраические) объекты, такие как **векторы** и **кватернионы**. Они позволяют упростить математические вычисления и улучшить читаемость кода. В этой главе мы рассмотрим именно те геометрические объекты, которые используются в алгоритме управления квадрокоптером Flix, причем акцент будет сделан на практических аспектах их использования.++## Система координат++### Оси координат++Для работы с объектами в трехмерном пространстве необходимо определить *систему координат*. Как известно, система координат задается тремя взаимно перпендикулярными осями, которые обозначаются как *X*, *Y* и *Z*. Порядок обозначения этих осей зависит от того, какую систему координат мы выбрали — *левую* или *правую*:++|Левая система координат|Правая система координат|+|-----------------------|------------------------|+|<img src="img/left-axes.svg" alt="Левая система координат" width="200">|<img src="img/right-axes.svg" alt="Правая система координат" width="200">|++В Flix для всех математических расчетов используется **правая система координат**, что является стандартом в робототехнике и авиации.++Также необходимо выбрать направление осей — в Flix они выбраны в соответствии со стандартом [REP-103](https://www.ros.org/reps/rep-0103.html). Для величин, заданных в подвижной системе координат, связанной с корпусом дрона, применяется порядок <abbr title="Forward Left Up">FLU</abbr>:++* ось X — направлена **вперед**;+* ось Y — направлена **влево**;+* ось Z — направлена **вверх**.++Для величин, заданных в *мировой* системе координат (относительно фиксированной точки в пространстве) — <abbr title="East North Up">ENU</abbr>:++* ось X — направлена на **восток** (условный);+* ось Y — направлена на **север** (условный);+* ось Z — направлена **вверх**.++> [!NOTE]+> Для системы ENU важно только взаимное направление осей. Если доступен магнитометр, то используются реальные восток и север, но если нет — то произвольно выбранные.++Углы и угловые скорости определяются в соответствии с правилами математики: значения увеличиваются против часовой стрелки, если смотреть в сторону начала координат. Общий вид системы координат:++<img src="img/axes-rotation.svg" alt="Система координат" width="200">++> [!TIP]+> Оси координат <i>X</i>, <i>Y</i> и <i>Z</i> часто обозначаются красными, зелеными и синими цветами соответственно. Запомнить это можно с помощью сокращения <abbr title="Red Green Blue">RGB</abbr>.++## Вектор++<div class="firmware">+ <strong>Файл прошивки:</strong>+ <a href="https://github.com/okalachev/flix/blob/master/flix/vector.h"><code>vector.h</code></a>.<br>+</div>++**Вектор** — простой геометрический объект, который содержит три значения, соответствующие координатам *X*, *Y* и *Z*. Эти значения называются *компонентами вектора*. Вектор может описывать точку в пространстве, направление или ось вращения, скорость, ускорение, угловые скорости и другие физические величины. В Flix векторы задаются объектами `Vector` из библиотеки `vector.h`:++```cpp+Vector v(1, 2, 3);+v.x = 5;+v.y = 10;+v.z = 15;+```++> [!TIP]+> Не следует путать геометрический вектор — <code>vector</code> и динамический массив в стандартной библиотеке C++ — <code>std::vector</code>.++В прошивке в виде векторов представлены, например:++* `acc` — истинное ускорение с акселерометра.+* `gyro` — угловые скорости с гироскопа.+* `rates` — рассчитанная угловая скорость дрона.+* `accBias`, `accScale`, `gyroBias` — параметры калибровки IMU.++### Операции с векторами++**Длина вектора** рассчитывается при помощи теоремы Пифагора; в прошивке используется метод `norm()`:++```cpp+Vector v(3, 4, 5);+float length = v.norm(); // 7.071+```++Любой вектор можно привести к **единичному вектору** (сохранить направление, но сделать длину равной 1) при помощи метода `normalize()`:++```cpp+Vector v(3, 4, 5);+v.normalize(); // 0.424, 0.566, 0.707+```++**Сложение и вычитание** векторов реализуется через простое покомпонентное сложение и вычитание. Геометрически сумма векторов представляет собой вектор, который соединяет начало первого вектора с концом второго. Разность векторов представляет собой вектор, который соединяет конец первого вектора с концом второго. Это удобно для расчета относительных позиций, суммарных скоростей и решения других задач. В коде эти операции интуитивно понятны:++```cpp+Vector a(1, 2, 3);+Vector b(4, 5, 6);+Vector sum = a + b; // 5, 7, 9+Vector diff = a - b; // -3, -3, -3+```++Операция **умножения на число** `n` увеличивает (или уменьшает) длину вектора в `n` раз (сохраняя направление):++```cpp+Vector a(1, 2, 3);+Vector b = a * 2; // 2, 4, 6+```++В некоторых случаях полезна операция **покомпонентного умножения** (или деления) векторов. Например, для применения коэффициентов калибровки к данным с IMU. В разных библиотеках эта операция обозначается по разному, но в библиотеке `vector.h` используется простые знаки `*` и `/`:++```cpp+acc = acc / accScale;+```++**Угол между векторами** можно найти при помощи статического метода `Vector::angleBetween()`:++```cpp+Vector a(1, 0, 0);+Vector b(0, 1, 0);+float angle = Vector::angleBetween(a, b); // 1.57 (90 градусов)+```++#### Скалярное произведение++Скалярное произведение векторов (*dot product*) — это произведение длин двух векторов на косинус угла между ними. В математике оно обозначается знаком `·` или слитным написанием векторов. Интуитивно, результат скалярного произведения показывает, насколько два вектора *сонаправлены*.++В Flix используется статический метод `Vector::dot()`:++```cpp+Vector a(1, 2, 3);+Vector b(4, 5, 6);+float dotProduct = Vector::dot(a, b); // 32+```++Операция скалярного произведения может помочь, например, при расчете проекции одного вектора на другой.++### Векторное произведение++Векторное произведение (*cross product*) позволяет найти вектор, перпендикулярный двум другим векторам. В математике оно обозначается знаком `×`, а в прошивке — статический метод `Vector::cross()`:++```cpp+Vector a(1, 2, 3);+Vector b(4, 5, 6);+Vector crossProduct = Vector::cross(a, b); // -3, 6, -3+```++## Кватернион++### Ориентация в трехмерном пространстве++В отличие от позиции и скорости, у ориентации в трехмерном пространстве нет универсального для всех случаев способа представления. В зависимости от задачи ориентация может быть представлена в виде углов Эйлера, матрицы поворота, вектора вращения или кватерниона. Рассмотрим используемые в полетной прошивке способы представления ориентации.++### Углы Эйлера++**Углы Эйлера** — *крен*, *тангаж* и *рыскание* — это наиболее «естественный» для человека способ представления ориентации. Они описывают последовательные вращения объекта вокруг трех осей координат.++В прошивке углы Эйлера сохраняются в обычный объект `Vector` (хоть и, геометрически говоря, не являются вектором):++* Угол по крену (*roll*) — `vector.x`.+* Угол по тангажу (*pitch*) — `vector.y`.+* Угол по рысканию (*yaw*) — `vector.z`.++Особенности углов Эйлера:++1. Углы Эйлера зависят от порядка применения вращений, то есть существует 6 типов углов Эйлера. Порядок вращений, принятый в Flix (и в роботехнике в целом) — рыскание, тангаж, крен (ZYX).+2. Для некоторых ориентаций углы Эйлера «вырождаются». Так, если объект «смотрит» строго вниз, то угол по рысканию и угол по крену становятся неразличимыми. Эта ситуация называется *gimbal lock* — потеря одной степени свободы.++Ввиду этих особенности для углов Эйлера не существует общих формул для самых базовых задач с ориентациями, таких как применение одного вращения (ориентации) к другому, расчет разницы между ориентациями и подобных. Поэтому в основном углы Эйлера применяются в пользовательском интерфейсе, но редко используются в математических расчетах.++> [!IMPORTANT]+> Для углов Эйлера не существует общих формул для самых базовых операций с ориентациями.++### Axis-angle++Помимо углов Эйлера, любую ориентацию в трехмерном пространстве можно представить в виде вращения вокруг некоторой оси на некоторый угол. В геометрии это доказывается, как **теорема вращения Эйлера**. В таком представлении ориентация задается двумя величинами:++* **Ось вращения** (*axis*) — единичный вектор, определяющий ось вращения.+* **Угол поворота** (*angle* или *θ*) — угол, на который нужно повернуть объект вокруг этой оси.++В Flix ось вращения задается объектом `Vector`, а угол поворота — числом типа `float` в радианах:++```cpp+// Вращение на 45 градусов вокруг оси (1, 2, 3)+Vector axis(1, 2, 3);+float angle = radians(45);+```++Этот способ более удобен для расчетов, чем углы Эйлера, но все еще не является оптимальным.++### Вектор вращения++Если умножить вектор *axis* на угол поворота *θ*, то получится **вектор вращения** (*rotation vector*). Этот вектор играет важную роль в алгоритмах управления ориентацией летательного аппарата.++Вектор вращения обладает замечательным свойством: если угловые скорости объекта (в собственной системе координат) в каждый момент времени совпадают с компонентами этого вектора, то за единичное время объект придет к заданной этим вектором ориентации. Это свойство позволяет использовать вектор вращения для управления ориентацией объекта посредством управления угловыми скоростями.++> [!IMPORTANT]+> Чтобы за единичное время прийти к заданной ориентации, собственные угловые скорости объекта должны быть равны компонентам вектора вращения.++Вектора вращения в Flix представляются в виде объектов `Vector`:++```cpp+// Вращение на 45 градусов вокруг оси (1, 2, 3)+Vector rotation = radians(45) * Vector(1, 2, 3);+```++### Кватернион++<div class="firmware">+ <strong>Файл прошивки:</strong>+ <a href="https://github.com/okalachev/flix/blob/master/flix/quaternion.h"><code>quaternion.h</code></a>.<br>+</div>++Вектор вращения удобен, но для математических расчетов еще удобнее использовать **кватернион**. В Flix кватернионы представляются объектами `Quaternion` из библиотеки `quaternion.h`. Кватернион состоит из четырех значений: *w*, *x*, *y*, *z* и рассчитывается из вектора оси вращения (*axis*) и угла поворота (*θ*) по следующей формуле:++\\[ q = \left( \begin{array}{c} w \\\\ x \\\\ y \\\\ z \end{array} \right) = \left( \begin{array}{c} \cos\left(\frac{\theta}{2}\right) \\\\ axis\_x \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_y \cdot \sin\left(\frac{\theta}{2}\right) \\\\ axis\_z \cdot \sin\left(\frac{\theta}{2}\right) \end{array} \right) \\]++На практике оказывается, что **именно такое представление наиболее удобно для математических расчетов**.++Проиллюстрируем кватернион и остальные описанные выше способы представления ориентации с помощью интерактивной визуализации. Изменяйте угол поворота *θ* с помощью ползунка (ось вращения константна) и изучите, как меняется ориентация объекта, вектор вращения и кватернион:++<div id="rotation-diagram" class="diagram">+ <p>+ <label class="angle" for="angle-range"></label>+ <input type="range" name="angle" id="angle-range" min="0" max="360" value="0" step="1">+ </p>+ <p class="axis"></p>+ <p class="rotation-vector"></p>+ <p class="quaternion"></p>+ <p class="euler"></p>+</div>++<script type="importmap">+{+ "imports": {+ "three": "https://cdn.jsdelivr.net/npm/[email protected]/build/three.module.js",+ "three/addons/": "https://cdn.jsdelivr.net/npm/[email protected]/examples/jsm/"+ }+}+</script>+<script type="module" src="js/rotation.js"></script>++> [!IMPORTANT]+> В контексте управляющих алгоритмов кватернион — это оптимизированный для расчетов аналог вектора вращения.++Кватернион это наиболее часто используемый способ представления ориентации в алгоритмах. Кроме этого, у кватерниона есть большое значение в теории чисел и алгебре, как у расширения понятия комплексного числа, но рассмотрение этого аспекта выходит за рамки описания работы с вращениями с практической точки зрения.++В прошивке в виде кватернионов представлены, например:++* `attitude` — текущая ориентация квадрокоптера.+* `attitudeTarget` — целевая ориентация квадрокоптера.++### Операции с кватернионами++Кватернион создается напрямую из четырех его компонент:++```cpp+// Кватернион, представляющий нулевую (исходную) ориентацию+Quaternion q(1, 0, 0, 0);+```++Кватернион можно создать из оси вращения и угла поворота, вектора вращения или углов Эйлера:++```cpp+Quaternion q1 = Quaternion::fromAxisAngle(axis, angle);+Quaternion q2 = Quaternion::fromRotationVector(rotation);+Quaternion q3 = Quaternion::fromEuler(Vector(roll, pitch, yaw));+```++И наоборот:++```cpp+q1.toAxisAngle(axis, angle);+Vector rotation = q2.toRotationVector();+Vector euler = q3.toEuler();+```++Возможно рассчитать вращение между двумя обычными векторами:++```cpp+Quaternion q = Quaternion::fromBetweenVectors(v1, v2); // в виде кватерниона+Vector rotation = Vector::rotationVectorBetween(v1, v2); // в виде вектора вращения+```++Шорткаты для работы с вращением по рысканию (удобно для алгоритмов управления полетом):++```cpp+float yaw = q.getYaw();+q.setYaw(yaw);+```++#### Применения вращений++Чтобы применить вращение, выраженное в кватернионе, к другому кватерниону, в математике используется операция **умножения кватернионов**. При использовании этой операции, необходимо учитывать, что она не является коммутативной, то есть порядок операндов имеет значение. Формула умножения кватернионов выглядит так:++\\[ q_1 \times q_2 = \left( \begin{array}{c} w_1 \\\\ x_1 \\\\ y_1 \\\\ z_1 \end{array} \right) \times \left( \begin{array}{c} w_2 \\\\ x_2 \\\\ y_2 \\\\ z_2 \end{array} \right) = \left( \begin{array}{c} w_1 w_2 - x_1 x_2 - y_1 y_2 - z_1 z_2 \\\\ w_1 x_2 + x_1 w_2 + y_1 z_2 - z_1 y_2 \\\\ w_1 y_2 - x_1 z_2 + y_1 w_2 + z_1 x_2 \\\\ w_1 z_2 + x_1 y_2 - y_1 x_2 + z_1 w_2 \end{array} \right) \\]++В библиотеке `quaternion.h` для этой операции используется статический метод `Quaternion::rotate()`:++```cpp+// Композиция вращений q1 и q2+Quaternion result = Quaternion::rotate(q1, q2);+```++Также полезной является операция применения вращения к вектору, которая делается похожим образом:++```cpp+// Вращение вектора v кватернионом q+Vector result = Quaternion::rotateVector(v, q);+```++Для расчета разницы между двумя ориентациями используется метод `Quaternion::between()`:++```cpp+// Расчет вращения от q1 к q2+Quaternion q = Quaternion::between(q1, q2);+```++## Дополнительные материалы++* [Интерактивный учебник по кватернионам](https://eater.net/quaternions).+* [Визуализация вращения вектора с помощью кватернионов](https://quaternions.online).diff --git a/docs/book/js/rotation.js b/docs/book/js/rotation.jsnew file mode 100644index 0000000..a092b2c--- /dev/null+++ b/docs/book/js/rotation.js@@ -0,0 +1,262 @@+import * as THREE from 'three';+import { SVGRenderer, SVGObject } from 'three/addons/renderers/SVGRenderer.js';+import { OrbitControls } from 'three/addons/controls/OrbitControls.js';++const diagramEl = document.getElementById('rotation-diagram');++const scene = new THREE.Scene();+scene.background = new THREE.Color(0xffffff);++const camera = new THREE.OrthographicCamera();++camera.position.set(9, 26, 20);+camera.up.set(0, 0, 1);+camera.lookAt(0, 0, 0);++const renderer = new SVGRenderer();+diagramEl.prepend(renderer.domElement);++const controls = new OrbitControls(camera, renderer.domElement);+controls.enableZoom = false;++const LINE_WIDTH = 4;++function createLabel(text, x, y, z, min = false) {+ const label = document.createElementNS('http://www.w3.org/2000/svg', 'text');+ label.setAttribute('class', 'label' + (min ? ' min' : ''));+ label.textContent = text;+ label.setAttribute('y', -15);+ const object = new SVGObject(label);+ object.position.x = x;+ object.position.y = y;+ object.position.z = z;+ return object;+}++function createLine(x1, y1, z1, x2, y2, z2, color) {+ const geometry = new THREE.BufferGeometry().setFromPoints([+ new THREE.Vector3(x1, y1, z1),+ new THREE.Vector3(x2, y2, z2)+ ]);+ const material = new THREE.LineBasicMaterial({ color: color, linewidth: LINE_WIDTH, transparent: true, opacity: 0.8 });+ const line = new THREE.Line(geometry, material);+ scene.add(line);+ return line;+}++function changeLine(line, x1, y1, z1, x2, y2, z2) {+ line.geometry.setFromPoints([new THREE.Vector3(x1, y1, z1), new THREE.Vector3(x2, y2, z2)]);+ return line;+}++function createVector(x1, y1, z1, x2, y2, z2, color, label = '') {+ const HEAD_LENGTH = 1;+ const HEAD_WIDTH = 0.2;++ const group = new THREE.Group();+ const direction = new THREE.Vector3(x2 - x1, y2 - y1, z2 - z1).normalize();+ const norm = new THREE.Vector3(x2 - x1, y2 - y1, z2 - z1).length();+ let end = new THREE.Vector3(x2, y2, z2);++ if (norm > HEAD_LENGTH) {+ end = new THREE.Vector3(x2 - direction.x * HEAD_LENGTH / 2, y2 - direction.y * HEAD_LENGTH / 2, z2 - direction.z * HEAD_LENGTH / 2);+ }++ // create line+ const geometry = new THREE.BufferGeometry().setFromPoints([new THREE.Vector3(x1, y1, z1), end]);+ const material = new THREE.LineBasicMaterial({ color: color, linewidth: LINE_WIDTH, transparent: true, opacity: 0.8 });+ const line = new THREE.Line(geometry, material);+ group.add(line);++ if (norm > HEAD_LENGTH) {+ // Create arrow+ const arrowGeometry = new THREE.ConeGeometry(HEAD_WIDTH, HEAD_LENGTH, 16);+ const arrowMaterial = new THREE.MeshBasicMaterial({ color: color });+ const arrow = new THREE.Mesh(arrowGeometry, arrowMaterial);+ arrow.position.set(x2 - direction.x * HEAD_LENGTH / 2, y2 - direction.y * HEAD_LENGTH / 2, z2 - direction.z * HEAD_LENGTH / 2);+ arrow.lookAt(new THREE.Vector3(x1, y1, z1));+ arrow.rotateX(-Math.PI / 2);+ group.add(arrow);+ }++ // create label+ if (label) group.add(createLabel(label, x2, y2, z2));+ scene.add(group);+ return group;+}++function changeVector(vector, x1, y1, z1, x2, y2, z2, color, label = '') {+ vector.removeFromParent();+ return createVector(x1, y1, z1, x2, y2, z2, color, label);+}++function createDrone(x, y, z) {+ const group = new THREE.Group();++ // Fuselage and wing triangle (main body)+ const fuselageGeometry = new THREE.BufferGeometry();+ const fuselageVertices = new Float32Array([+ 1, 0, 0,+ -1, 0.6, 0,+ -1, -0.6, 0+ ]);+ fuselageGeometry.setAttribute('position', new THREE.BufferAttribute(fuselageVertices, 3));+ const fuselageMaterial = new THREE.MeshBasicMaterial({ color: 0xb3b3b3, side: THREE.DoubleSide, transparent: true, opacity: 0.8 });+ const fuselage = new THREE.Mesh(fuselageGeometry, fuselageMaterial);+ group.add(fuselage);++ // Tail triangle+ const tailGeometry = new THREE.BufferGeometry();+ const tailVertices = new Float32Array([+ -0.2, 0, 0,+ -1, 0, 0,+ -1, 0, 0.5,+ ]);+ tailGeometry.setAttribute('position', new THREE.BufferAttribute(tailVertices, 3));+ const tailMaterial = new THREE.MeshBasicMaterial({ color: 0xd80100, side: THREE.DoubleSide, transparent: true, opacity: 0.9 });+ const tail = new THREE.Mesh(tailGeometry, tailMaterial);+ group.add(tail);++ group.position.set(x, y, z);+ group.scale.set(2, 2, 2);+ scene.add(group);+ return group;+}++// Create axes+const AXES_LENGTH = 10;+createVector(0, 0, 0, AXES_LENGTH, 0, 0, 0xd80100, 'x');+createVector(0, 0, 0, 0, AXES_LENGTH, 0, 0x0076ba, 'y');+createVector(0, 0, 0, 0, 0, AXES_LENGTH, 0x57ed00, 'z');++// Rotation values+const rotationAxisSrc = new THREE.Vector3(2, 1, 3);+let rotationAngle = 0;+let rotationAxis = rotationAxisSrc.clone().normalize();+let rotationVector = new THREE.Vector3(rotationAxis.x * rotationAngle, rotationAxis.y * rotationAngle, rotationAxis.z * rotationAngle);++let rotationVectorObj = createVector(0, 0, 0, rotationVector.x, rotationVector.y, rotationVector.z, 0xff9900);+let axisObj = createLine(0, 0, 0, rotationAxis.x * AXES_LENGTH, rotationAxis.y * AXES_LENGTH, rotationAxis.z * AXES_LENGTH, 0xe8e8e8);++const drone = createDrone(0, 0, 0);++// UI+const angleInput = diagramEl.querySelector('input[name=angle]');+const rotationVectorEl = diagramEl.querySelector('.rotation-vector');+const angleEl = diagramEl.querySelector('.angle');+const quaternionEl = diagramEl.querySelector('.quaternion');+const eulerEl = diagramEl.querySelector('.euler');+diagramEl.querySelector('.axis').innerHTML = `<b style='color:#b6b6b6'>Ось вращения:</b> (${rotationAxisSrc.x}, ${rotationAxisSrc.y}, ${rotationAxisSrc.z}) ∥ (${rotationAxis.x.toFixed(1)}, ${rotationAxis.y.toFixed(1)}, ${rotationAxis.z.toFixed(1)})`;++function updateScene() {+ rotationAngle = parseFloat(angleInput.value) * Math.PI / 180;+ rotationVector.set(rotationAxis.x * rotationAngle, rotationAxis.y * rotationAngle, rotationAxis.z * rotationAngle);+ rotationVectorObj = changeVector(rotationVectorObj, 0, 0, 0, rotationVector.x, rotationVector.y, rotationVector.z, 0xff9900);++ // rotate drone+ drone.rotation.set(0, 0, 0);+ drone.rotateOnAxis(rotationAxis, rotationAngle);++ // update labels+ angleEl.innerHTML = `<b>Угол вращения:</b> ${parseFloat(angleInput.value).toFixed(0)}° = ${(rotationAngle).toFixed(2)} рад`;+ rotationVectorEl.innerHTML = `<b style='color:#e49a44'>Вектор вращения:</b> (${rotationVector.x.toFixed(1)}, ${rotationVector.y.toFixed(1)}, ${rotationVector.z.toFixed(1)}) рад`;++ let quaternion = new THREE.Quaternion();+ quaternion.setFromAxisAngle(rotationAxis, rotationAngle);++ quaternionEl.innerHTML = `<b>Кватернион:</b>+ <math xmlns="http://www.w3.org/1998/Math/MathML">+ <mrow>+ <mo>(</mo>+ <mrow>+ <mi>cos</mi>+ <mo>(</mo>+ <mfrac>+ <mi>${rotationAngle.toFixed(2)}</mi>+ <mn>2</mn>+ </mfrac>+ <mo>)</mo>+ </mrow>+ <mo>, </mo>+ <mrow>+ <mi>${rotationAxis.x.toFixed(1)}</mi>+ <mo>·</mo>+ <mi>sin</mi>+ <mo>(</mo>+ <mfrac>+ <mi>${rotationAngle.toFixed(2)}</mi>+ <mn>2</mn>+ </mfrac>+ <mo>)</mo>+ </mrow>+ <mo>, </mo>+ <mrow>+ <mi>${rotationAxis.y.toFixed(1)}</mi>+ <mo>·</mo>+ <mi>sin</mi>+ <mo>(</mo>+ <mfrac>+ <mi>${rotationAngle.toFixed(2)}</mi>+ <mn>2</mn>+ </mfrac>+ <mo>)</mo>+ </mrow>+ <mo>,</mo>+ <mrow>+ <mi>${rotationAxis.z.toFixed(1)}</mi>+ <mo>·</mo>+ <mi>sin</mi>+ <mo>(</mo>+ <mfrac>+ <mi>${rotationAngle.toFixed(2)}</mi>+ <mn>2</mn>+ </mfrac>+ <mo>)</mo>+ </mrow>+ <mo>)</mo>+ </mrow>+ </math>+ = (${quaternion.w.toFixed(1)}, ${(quaternion.x).toFixed(1)}, ${(quaternion.y).toFixed(1)}, ${(quaternion.z).toFixed(1)})`;++ eulerEl.innerHTML = `<b>Углы Эйлера:</b> крен ${(drone.rotation.x * 180 / Math.PI).toFixed(0)}°,+ тангаж ${(drone.rotation.y * 180 / Math.PI).toFixed(0)}°, рыскание ${(drone.rotation.z * 180 / Math.PI).toFixed(0)}°`;+}++function updateCamera() {+ const RANGE = 8;+ const VERT_SHIFT = 2;+ const HOR_SHIFT = -2;+ const width = renderer.domElement.clientWidth;+ const height = renderer.domElement.clientHeight;+ const ratio = width / height;+ if (ratio > 1) {+ camera.left = -RANGE * ratio;+ camera.right = RANGE * ratio;+ camera.top = RANGE + VERT_SHIFT;+ camera.bottom = -RANGE + VERT_SHIFT;+ } else {+ camera.left = -RANGE + HOR_SHIFT;+ camera.right = RANGE + HOR_SHIFT;+ camera.top = RANGE / ratio + VERT_SHIFT;+ camera.bottom = -RANGE / ratio + VERT_SHIFT;+ }+ camera.updateProjectionMatrix();+ renderer.setSize(width, height);+}++function update() {+ // requestAnimationFrame(update);+ updateCamera();+ updateScene();+ controls.update();+ renderer.render(scene, camera);+}+update();++window.addEventListener('resize', update);+angleInput.addEventListener('input', update);+angleInput.addEventListener('change', update);+diagramEl.addEventListener('mousemove', update);+diagramEl.addEventListener('touchmove', update);+diagramEl.addEventListener('scroll', update);+diagramEl.addEventListener('wheel', update);diff --git a/docs/img/axes-rotation.svg b/docs/img/axes-rotation.svgnew file mode 100644index 0000000..d53bb59--- /dev/null+++ b/docs/img/axes-rotation.svg@@ -0,0 +1,94 @@+<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 533 646.91">+ <defs>+ <style>+ .a {+ font-size: 50px;+ font-family: Tahoma;+ }++ .b {+ opacity: 0.8;+ }++ .c, .e, .g, .i {+ fill: none;+ }++ .c {+ stroke: #0076ba;+ }++ .c, .e, .g {+ stroke-linejoin: bevel;+ stroke-width: 13px;+ }++ .d {+ fill: #0076ba;+ }++ .e {+ stroke: #d80100;+ }++ .f {+ fill: #d80100;+ }++ .g {+ stroke: #57ed00;+ }++ .h {+ fill: #57ed00;+ }++ .i {+ stroke: #000;+ stroke-miterlimit: 10;+ stroke-width: 10px;+ }+ </style>+ </defs>+ <g>+ <text class="a" transform="translate(58.62 636.12)">x</text>+ <text class="a" transform="translate(505.06 562.18)">y</text>+ <text class="a" transform="translate(370.06 43.18)">z</text>+ <g class="b">+ <g>+ <line class="c" x1="347" y1="420.2" x2="347" y2="61.78"/>+ <polygon class="d" points="370.34 68.61 347 28.2 323.66 68.61 370.34 68.61"/>+ </g>+ </g>+ <g class="b">+ <g>+ <line class="e" x1="347" y1="420.2" x2="29.31" y2="597.81"/>+ <polygon class="f" points="23.89 574.11 0 614.2 46.66 614.84 23.89 574.11"/>+ </g>+ </g>+ <g class="b">+ <g>+ <line class="g" x1="347" y1="420.2" x2="503.22" y2="501.67"/>+ <polygon class="h" points="486.38 519.2 533 517.2 507.96 477.82 486.38 519.2"/>+ </g>+ </g>+ <g class="b">+ <g>+ <path class="i" d="M103.19,617.68a52.66,52.66,0,1,0-55.51-89.19"/>+ <polygon points="41.63 516.97 34.76 541.97 59.85 535.42 41.63 516.97"/>+ </g>+ </g>+ <g class="b">+ <g>+ <path class="i" d="M295.58,87.51a52.66,52.66,0,1,0,103.78,16.31"/>+ <polygon points="412.03 106.78 397.6 85.24 386.16 108.51 412.03 106.78"/>+ </g>+ </g>+ <g class="b">+ <g>+ <path class="i" d="M505,452.58a52.66,52.66,0,1,0-76,72.53"/>+ <polygon points="418.96 533.38 444.84 535 433.31 511.78 418.96 533.38"/>+ </g>+ </g>+ </g>+</svg>diff --git a/docs/img/left-axes.svg b/docs/img/left-axes.svgnew file mode 100644index 0000000..1ecf4e7--- /dev/null+++ b/docs/img/left-axes.svg@@ -0,0 +1,67 @@+<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 533 646.68">+ <defs>+ <style>+ .a {+ font-size: 50px;+ font-family: Tahoma;+ }++ .b {+ opacity: 0.8;+ }++ .c, .e, .g {+ fill: none;+ stroke-linejoin: bevel;+ stroke-width: 13px;+ }++ .c {+ stroke: #0076ba;+ }++ .d {+ fill: #0076ba;+ }++ .e {+ stroke: #57ed00;+ }++ .f {+ fill: #57ed00;+ }++ .g {+ stroke: #d80100;+ }++ .h {+ fill: #d80100;+ }+ </style>+ </defs>+ <g>+ <text class="a" transform="translate(500.62 556.12)">x</text>+ <text class="a" transform="translate(370.06 43.18)">z</text>+ <g class="b">+ <g>+ <line class="c" x1="347" y1="420.2" x2="347" y2="61.78"/>+ <polygon class="d" points="370.34 68.61 347 28.2 323.66 68.61 370.34 68.61"/>+ </g>+ </g>+ <g class="b">+ <g>+ <line class="e" x1="347" y1="420.2" x2="29.31" y2="597.81"/>+ <polygon class="f" points="23.89 574.11 0 614.2 46.66 614.84 23.89 574.11"/>+ </g>+ </g>+ <g class="b">+ <g>+ <line class="g" x1="347" y1="420.2" x2="503.22" y2="501.67"/>+ <polygon class="h" points="486.38 519.2 533 517.2 507.96 477.82 486.38 519.2"/>+ </g>+ </g>+ <text class="a" transform="translate(58.06 635.89)">y</text>+ </g>+</svg>diff --git a/docs/img/right-axes.svg b/docs/img/right-axes.svgnew file mode 100644index 0000000..ef26c6d--- /dev/null+++ b/docs/img/right-axes.svg@@ -0,0 +1,67 @@+<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 533 646.91">+ <defs>+ <style>+ .a {+ opacity: 0.8;+ }++ .b, .d, .f {+ fill: none;+ stroke-linejoin: bevel;+ stroke-width: 13px;+ }++ .b {+ stroke: #57ed00;+ }++ .c {+ fill: #57ed00;+ }++ .d {+ stroke: #d80100;+ }++ .e {+ fill: #d80100;+ }++ .f {+ stroke: #0076ba;+ }++ .g {+ fill: #0076ba;+ }++ .h {+ font-size: 50px;+ font-family: Tahoma;+ }+ </style>+ </defs>+ <g>+ <g class="a">+ <g>+ <line class="b" x1="347" y1="420.2" x2="503.22" y2="501.67"/>+ <polygon class="c" points="486.38 519.2 533 517.2 507.96 477.82 486.38 519.2"/>+ </g>+ </g>+ <g class="a">+ <g>+ <line class="d" x1="347" y1="420.2" x2="29.31" y2="597.81"/>+ <polygon class="e" points="23.89 574.11 0 614.2 46.66 614.84 23.89 574.11"/>+ </g>+ </g>+ <g class="a">+ <g>+ <line class="f" x1="347" y1="420.2" x2="347" y2="61.78"/>+ <polygon class="g" points="370.34 68.61 347 28.2 323.66 68.61 370.34 68.61"/>+ </g>+ </g>+ <text class="h" transform="translate(58.62 636.12)">x</text>+ <text class="h" transform="translate(505.06 562.18)">y</text>+ <text class="h" transform="translate(370.06 43.18)">z</text>+ </g>+</svg>diff --git a/docs/rotation.css b/docs/rotation.cssnew file mode 100644index 0000000..0a0c3db--- /dev/null+++ b/docs/rotation.css@@ -0,0 +1,29 @@+.diagram svg {+ display: block;+ width: 100%;+ height: 400px;+}+.diagram .label {+ font-family: Arial, sans-serif;+ font-size: 20px;+ pointer-events: none;+ color: black;+ opacity: 0.8;+ user-select: none;+}+.diagram label {+ display: block;+}+@media (min-width: 800px) {+ .diagram b {+ width: 200px;+ display: inline-block;+ }+}+.diagram p.quaternion {+ overflow-x: auto;+}+.diagram input {+ text-align: center;+ width: 100%;+}