+Прошивка Flix это обычный скетч Arduino, реализованный в однопоточном стиле. Код инициализации находится в функции `setup()`, а главный цикл — в функции `loop()`. Скетч состоит из нескольких файлов, каждый из которых отвечает за определенную подсистему.
-Главный цикл работает на частоте 1000 Гц. Передача данных между подсистемами происходит через глобальные переменные:
+
+
+Главный цикл `loop()` работает на частоте 1000 Гц. Передача данных между подсистемами происходит через глобальные переменные:
* `t` *(float)* — текущее время шага, *с*.
* `dt` *(float)* — дельта времени между текущим и предыдущим шагами, *с*.
@@ -15,18 +17,34 @@
## Исходные файлы
-Исходные файлы прошивки находятся в директории `flix`. Ключевые файлы:
+Исходные файлы прошивки находятся в директории `flix`. Основные файлы:
-* [`flix.ino`](https://github.com/okalachev/flix/blob/canonical/flix/flix.ino) — основной входной файл, скетч Arduino. Включает определение глобальных переменных и главный цикл.
-* [`imu.ino`](https://github.com/okalachev/flix/blob/canonical/flix/imu.ino) — чтение данных с датчика IMU (гироскоп и акселерометр), калибровка IMU.
-* [`rc.ino`](https://github.com/okalachev/flix/blob/canonical/flix/rc.ino) — чтение данных с RC-приемника, калибровка RC.
-* [`mavlink.ino`](https://github.com/okalachev/flix/blob/canonical/flix/mavlink.ino) — взаимодействие с QGroundControl через MAVLink.
-* [`estimate.ino`](https://github.com/okalachev/flix/blob/canonical/flix/estimate.ino) — оценка ориентации дрона, комплементарный фильтр.
-* [`control.ino`](https://github.com/okalachev/flix/blob/canonical/flix/control.ino) — управление ориентацией и угловыми скоростями дрона, трехмерный двухуровневый каскадный PID-регулятор.
-* [`motors.ino`](https://github.com/okalachev/flix/blob/canonical/flix/motors.ino) — управление выходными сигналами на моторы через ШИМ.
+* [`flix.ino`](https://github.com/okalachev/flix/blob/master/flix/flix.ino) — основной файл Arduino-скетча. Определяет некоторые глобальные переменные и главный цикл.
+* [`imu.ino`](https://github.com/okalachev/flix/blob/master/flix/imu.ino) — чтение данных с датчика IMU (гироскоп и акселерометр), калибровка IMU.
+* [`rc.ino`](https://github.com/okalachev/flix/blob/master/flix/rc.ino) — чтение данных с RC-приемника, калибровка RC.
+* [`estimate.ino`](https://github.com/okalachev/flix/blob/master/flix/estimate.ino) — оценка ориентации дрона, комплементарный фильтр.
+* [`control.ino`](https://github.com/okalachev/flix/blob/master/flix/control.ino) — подсистема управления, трехмерный двухуровневый каскадный ПИД-регулятор.
+* [`motors.ino`](https://github.com/okalachev/flix/blob/master/flix/motors.ino) — выход PWM на моторы.
+* [`mavlink.ino`](https://github.com/okalachev/flix/blob/master/flix/mavlink.ino) — взаимодействие с QGroundControl или [pyflix](https://github.com/okalachev/flix/tree/master/tools/pyflix) через протокол MAVLink.
-Вспомогательные файлы включают:
+Вспомогательные файлы:
-* [`vector.h`](https://github.com/okalachev/flix/blob/canonical/flix/vector.h), [`quaternion.h`](https://github.com/okalachev/flix/blob/canonical/flix/quaternion.h) — реализация библиотек векторов и кватернионов.
-* [`pid.h`](https://github.com/okalachev/flix/blob/canonical/flix/pid.h) — реализация общего ПИД-регулятора.
-* [`lpf.h`](https://github.com/okalachev/flix/blob/canonical/flix/lpf.h) — реализация общего фильтра нижних частот.
+* [`vector.h`](https://github.com/okalachev/flix/blob/master/flix/vector.h), [`quaternion.h`](https://github.com/okalachev/flix/blob/master/flix/quaternion.h) — библиотеки векторов и кватернионов.
+* [`pid.h`](https://github.com/okalachev/flix/blob/master/flix/pid.h) — ПИД-регулятор.
+* [`lpf.h`](https://github.com/okalachev/flix/blob/master/flix/lpf.h) — фильтр нижних частот.
+
+### Подсистема управления
+
+Состояние органов управления обрабатывается в функции `interpretControls()` и преобразуется в *команду управления*, которая включает следующее:
+
+* `attitudeTarget` *(Quaternion)* — целевая ориентация дрона.
+* `ratesTarget` *(Vector)* — целевые угловые скорости, *рад/с*.
+* `ratesExtra` *(Vector)* — дополнительные (feed-forward) угловые скорости, для управления рысканием в режиме STAB, *рад/с*.
+* `torqueTarget` *(Vector)* — целевой крутящий момент, диапазон [-1, 1].
+* `thrustTarget` *(float)* — целевая общая тяга, диапазон [0, 1].
+
+Команда управления обрабатывается в функциях `controlAttitude()`, `controlRates()`, `controlTorque()`. Если значение одной из переменных установлено в `NAN`, то соответствующая функция пропускается.
+
+
+
+Состояние *armed* хранится в переменной `armed`, а текущий режим — в переменной `mode`.
diff --git a/docs/firmware.md b/docs/firmware.md
index 8aad0af..beeff66 100644
--- a/docs/firmware.md
+++ b/docs/firmware.md
@@ -1,38 +1,55 @@
# Firmware overview
-The firmware is a regular Arduino sketch, and follows the classic Arduino one-threaded design. The initialization code is in the `setup()` function, and the main loop is in the `loop()` function. The sketch includes multiple files, each responsible for a specific part of the system.
+The firmware is a regular Arduino sketch, and it follows the classic Arduino one-threaded design. The initialization code is in the `setup()` function, and the main loop is in the `loop()` function. The sketch includes several files, each responsible for a specific subsystem.
## Dataflow
-
+
-The main loop is running at 1000 Hz. All the dataflow is happening through global variables (for simplicity):
+The main loop is running at 1000 Hz. All the dataflow goes through global variables (for simplicity):
-* `t` *(double)* — current step time, *s*.
+* `t` *(double)* — current step time, *s*.
* `dt` *(float)* — time delta between the current and previous steps, *s*.
* `gyro` *(Vector)* — data from the gyroscope, *rad/s*.
* `acc` *(Vector)* — acceleration data from the accelerometer, *m/s2*.
* `rates` *(Vector)* — filtered angular rates, *rad/s*.
* `attitude` *(Quaternion)* — estimated attitude (orientation) of drone.
-* `controlRoll`, `controlPitch`, ... *(float[])* — pilot's control inputs, range [-1, 1].
-* `motors` *(float[])* — motor outputs, range [0, 1].
+* `controlRoll`, `controlPitch`, ... *(float[])* — pilot control inputs, range [-1, 1].
+* `motors` *(float[])* — motor outputs, range [0, 1].
## Source files
-Firmware source files are located in `flix` directory. The key files are:
+Firmware source files are located in `flix` directory. The core files are:
-* [`flix.ino`](../flix/flix.ino) — main entry point, Arduino sketch. Includes global variables definition and the main loop.
+* [`flix.ino`](../flix/flix.ino) — Arduino sketch main file, entry point.Includes some global variable definitions and the main loop.
* [`imu.ino`](../flix/imu.ino) — reading data from the IMU sensor (gyroscope and accelerometer), IMU calibration.
* [`rc.ino`](../flix/rc.ino) — reading data from the RC receiver, RC calibration.
-* [`estimate.ino`](../flix/estimate.ino) — drone's attitude estimation, complementary filter.
-* [`control.ino`](../flix/control.ino) — drone's attitude and rates control, three-dimensional two-level cascade PID controller.
-* [`motors.ino`](../flix/motors.ino) — PWM motor outputs control.
+* [`estimate.ino`](../flix/estimate.ino) — attitude estimation, complementary filter.
+* [`control.ino`](../flix/control.ino) — control subsystem, three-dimensional two-level cascade PID controller.
+* [`motors.ino`](../flix/motors.ino) — PWM motor output control.
+* [`mavlink.ino`](../flix/mavlink.ino) — interaction with QGroundControl or [pyflix](../tools/pyflix) via MAVLink protocol.
-Utility files include:
+Utility files:
-* [`vector.h`](../flix/vector.h), [`quaternion.h`](../flix/quaternion.h) — vector and quaternion libraries implementation.
-* [`pid.h`](../flix/pid.h) — generic PID controller implementation.
-* [`lpf.h`](../flix/lpf.h) — generic low-pass filter implementation.
+* [`vector.h`](../flix/vector.h), [`quaternion.h`](../flix/quaternion.h) — vector and quaternion libraries.
+* [`pid.h`](../flix/pid.h) — generic PID controller.
+* [`lpf.h`](../flix/lpf.h) — generic low-pass filter.
+
+### Control subsystem
+
+Pilot inputs are interpreted in `interpretControls()`, and then converted to the *control command*, which consists of the following:
+
+* `attitudeTarget` *(Quaternion)* — target attitude of the drone.
+* `ratesTarget` *(Vector)* — target angular rates, *rad/s*.
+* `ratesExtra` *(Vector)* — additional (feed-forward) angular rates , used for yaw rate control in STAB mode, *rad/s*.
+* `torqueTarget` *(Vector)* — target torque, range [-1, 1].
+* `thrustTarget` *(float)* — collective thrust target, range [0, 1].
+
+Control command is processed in `controlAttitude()`, `controlRates()`, `controlTorque()` functions. Each function may be skipped if the corresponding target is set to `NAN`.
+
+
+
+Armed state is stored in `armed` variable, and current mode is stored in `mode` variable.
## Building
diff --git a/docs/img/control.svg b/docs/img/control.svg
new file mode 100644
index 0000000..79abe53
--- /dev/null
+++ b/docs/img/control.svg
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/docs/img/dataflow.svg b/docs/img/dataflow.svg
index 5413a39..a2c545a 100644
--- a/docs/img/dataflow.svg
+++ b/docs/img/dataflow.svg
@@ -1,330 +1,4 @@
-
-
-
-
+
+
+
+
\ No newline at end of file
diff --git a/flix/cli.ino b/flix/cli.ino
index c022ceb..c7e6eaa 100644
--- a/flix/cli.ino
+++ b/flix/cli.ino
@@ -12,7 +12,7 @@ extern const int ACRO, STAB;
extern float loopRate, dt;
extern double t;
extern uint16_t channels[16];
-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
extern int mode;
const char* motd =
@@ -118,8 +118,8 @@ void doCommand(String str, bool echo = false) {
for (int i = 0; i < 16; i++) {
print("%u ", channels[i]);
}
- print("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",
- controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);
+ print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n",
+ controlRoll, controlPitch, controlYaw, controlThrottle, controlMode);
print("mode: %s\n", getModeName());
} else if (command == "mot") {
print("front-right %g front-left %g rear-right %g rear-left %g\n",