Add log dir creation before log writing

So the control command is above the PID controllers.

README.md

@@ -21,15 +21,13 @@
 * Dedicated for education and research.
 * Made from general-purpose components.
 * Simple and clean source code in Arduino (<2k lines firmware).
+* Connectivity using Wi-Fi and MAVLink protocol.
 * Control using USB gamepad, remote control or smartphone.
-* Wi-Fi and MAVLink support.
 * Wireless command line interface and analyzing.
 * Precise simulation with Gazebo.
-* Python library.
+* Python library for scripting and automatic flights.
 * Textbook on flight control theory and practice ([in development](https://quadcopter.dev)).
-* *Position control (using external camera) and autonomous flights¹*.
+* *Position control (planned)*.
-
-*¹ — planned.*
 
 ## It actually flies
 

docs/book/gyro.md

@@ -87,13 +87,13 @@ Flix поддерживает следующие модели IMU:
 #include <FlixPeriph.h>
 #include <SPI.h>
 
-MPU9250 IMU(SPI);
+MPU9250 imu(SPI);
 
 void setup() {
 	Serial.begin(115200);
-	bool success = IMU.begin();
+	bool success = imu.begin();
 	if (!success) {
-		Serial.println("Failed to initialize IMU");
+		Serial.println("Failed to initialize the IMU");
 	}
 }
 ```
@@ -108,21 +108,21 @@ void setup() {
 #include <FlixPeriph.h>
 #include <SPI.h>
 
-MPU9250 IMU(SPI);
+MPU9250 imu(SPI);
 
 void setup() {
 	Serial.begin(115200);
-	bool success = IMU.begin();
+	bool success = imu.begin();
 	if (!success) {
-		Serial.println("Failed to initialize IMU");
+		Serial.println("Failed to initialize the IMU");
 	}
 }
 
 void loop() {
-	IMU.waitForData();
+	imu.waitForData();
 
 	float gx, gy, gz;
-	IMU.getGyro(gx, gy, gz);
+	imu.getGyro(gx, gy, gz);
 
 	Serial.printf("gx:%f gy:%f gz:%f\n", gx, gy, gz);
 	delay(50); // замедление вывода
@@ -135,36 +135,36 @@ void loop() {
 
 ## Конфигурация гироскопа
 
-В коде Flix настройка IMU происходит в функции `configureIMU`. В этой функции настраиваются три основных параметра гироскопа: диапазон измерений, частота сэмплов и частота LPF-фильтра.
+В коде Flix настройка IMU происходит в функции `configureIMU`. В этой функции настраиваются три основных параметра гироскопа: диапазон измерений, частота сэмплирования и частота LPF-фильтра.
 
-### Частота сэмплов
+### Частота сэмплирования
 
-Большинство IMU могут обновлять данные с разной частотой. В полетных контроллерах обычно используется частота обновления от 500 Гц до 8 кГц. Чем выше частота сэмплов, тем выше точность управления полетом, но и больше нагрузка на микроконтроллер.
+Большинство IMU могут обновлять данные с разной частотой. В полетных контроллерах обычно используется частота обновления от 500 Гц до 8 кГц. Чем выше частота, тем выше точность управления полетом, но и тем больше нагрузка на микроконтроллер.
 
-Частота сэмплов устанавливается методом `setSampleRate()`. В Flix используется частота 1 кГц:
+Частота сэмплирования устанавливается методом `setSampleRate()`. В Flix используется частота 1 кГц:
 
 ```cpp
 IMU.setRate(IMU.RATE_1KHZ_APPROX);
 ```
 
-Поскольку не все поддерживаемые IMU могут работать строго на частоте 1 кГц, в библиотеке FlixPeriph существует возможность приближенной настройки частоты сэмплов. Например, у IMU ICM-20948 при такой настройке реальная частота сэмплирования будет равна 1125 Гц.
+Поскольку не все поддерживаемые IMU могут работать строго на частоте 1 кГц, в библиотеке FlixPeriph существует возможность приближенной настройки частоты сэмплирования. Например, у IMU ICM-20948 при такой настройке реальная частота сэмплирования будет равна 1125 Гц.
 
 Другие доступные для установки в библиотеке FlixPeriph частоты сэмплирования:
 
-* `RATE_MIN` — минимальная частота сэмплов для конкретного IMU.
+* `RATE_MIN` — минимальная частота для конкретного IMU.
 * `RATE_50HZ_APPROX` — значение, близкое к 50 Гц.
 * `RATE_1KHZ_APPROX` — значение, близкое к 1 кГц.
 * `RATE_8KHZ_APPROX` — значение, близкое к 8 кГц.
-* `RATE_MAX` — максимальная частота сэмплов для конкретного IMU.
+* `RATE_MAX` — максимальная частота для конкретного IMU.
 
 #### Диапазон измерений
 
-Большинство MEMS-гироскопов поддерживают несколько диапазонов измерений угловой скорости. Главное преимущество выбора меньшего диапазона — бо́льшая чувствительность. В полетных контроллерах обычно выбирается максимальный диапазон измерений от –2000 до 2000 градусов в секунду, чтобы обеспечить возможность динамичных маневров.
+Большинство MEMS-гироскопов поддерживают несколько диапазонов измерений угловой скорости. Главное преимущество выбора меньшего диапазона — бо́льшая чувствительность. В полетных контроллерах обычно выбирается максимальный диапазон измерений от –2000 до 2000 градусов в секунду, чтобы обеспечить возможность быстрых маневров.
 
 В библиотеке FlixPeriph диапазон измерений гироскопа устанавливается методом `setGyroRange()`:
 
 ```cpp
-IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS);
+imu.setGyroRange(imu.GYRO_RANGE_2000DPS);
 ```
 
 ### LPF-фильтр
@@ -172,7 +172,7 @@ IMU.setGyroRange(IMU.GYRO_RANGE_2000DPS);
 IMU InvenSense могут фильтровать измерения на аппаратном уровне при помощи фильтра нижних частот (LPF). Flix реализует собственный фильтр для гироскопа, чтобы иметь больше гибкости при поддержке разных IMU. Поэтому для встроенного LPF устанавливается максимальная частота среза:
 
 ```cpp
-IMU.setDLPF(IMU.DLPF_MAX);
+imu.setDLPF(imu.DLPF_MAX);
 ```
 
 ## Калибровка гироскопа
@@ -181,7 +181,7 @@ IMU.setDLPF(IMU.DLPF_MAX);
 
 \\[ gyro_{xyz}=rates_{xyz}+bias_{xyz}+noise \\]
 
-Для качественной работы подсистемы оценки ориентации и управления дроном необходимо оценить *bias* гироскопа и учесть его в вычислениях. Для этого при запуске программы производится калибровка гироскопа, которая реализована в функции `calibrateGyro()`. Эта функция считывает данные с гироскопа в состоянии покоя 1000 раз и усредняет их. Полученные значения считаются *bias* гироскопа и в дальнейшем вычитаются из измерений.
+Для точной работы подсистемы оценки ориентации и управления дроном необходимо оценить *bias* гироскопа и учесть его в вычислениях. Для этого при запуске программы производится калибровка гироскопа, которая реализована в функции `calibrateGyro()`. Эта функция считывает данные с гироскопа в состоянии покоя 1000 раз и усредняет их. Полученные значения считаются *bias* гироскопа и в дальнейшем вычитаются из измерений.
 
 Программа для вывода данных с гироскопа с калибровкой:
 
@@ -189,23 +189,23 @@ IMU.setDLPF(IMU.DLPF_MAX);
 #include <FlixPeriph.h>
 #include <SPI.h>
 
-MPU9250 IMU(SPI);
+MPU9250 imu(SPI);
 
 float gyroBiasX, gyroBiasY, gyroBiasZ; // bias гироскопа
 
 void setup() {
 	Serial.begin(115200);
-	bool success = IMU.begin();
+	bool success = imu.begin();
 	if (!success) {
-		Serial.println("Failed to initialize IMU");
+		Serial.println("Failed to initialize the IMU");
 	}
 	calibrateGyro();
 }
 
 void loop() {
 	float gx, gy, gz;
-	IMU.waitForData();
+	imu.waitForData();
-	IMU.getGyro(gx, gy, gz);
+	imu.getGyro(gx, gy, gz);
 
 	// Устранение bias гироскопа
 	gx -= gyroBiasX;
@@ -226,9 +226,9 @@ void calibrateGyro() {
 
 	// Получение 1000 измерений гироскопа
 	for (int i = 0; i < samples; i++) {
-		IMU.waitForData();
+		imu.waitForData();
 		float gx, gy, gz;
-		IMU.getGyro(gx, gy, gz);
+		imu.getGyro(gx, gy, gz);
 		gyroBiasX += gx;
 		gyroBiasY += gy;
 		gyroBiasZ += gz;

docs/firmware.md

@@ -42,7 +42,7 @@ Pilot inputs are interpreted in `interpretControls()`, and then converted to the
 
 * `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*.
+* `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 motor thrust target, range [0, 1].
 

docs/img/flix.svg

@@ -1,4 +1,4 @@
-<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 718.86 362.46">
+<svg xmlns="http://www.w3.org/2000/svg" viewBox="0 0 734.86 378.46">
   <defs>
     <style>
       .a {
@@ -23,16 +23,16 @@
   </defs>
   <g>
     <g>
-      <line class="a" x1="443.12" y1="283.88" x2="641.43" y2="84.58"/>
+      <line class="a" x1="448.78" y1="294.23" x2="648.77" y2="93.24"/>
-      <line class="a" x1="444.12" y1="85.58" x2="642.42" y2="284.87"/>
+      <line class="a" x1="449.78" y1="94.24" x2="649.77" y2="295.23"/>
-      <circle class="b" cx="542.77" cy="184.23" r="41.15"/>
+      <circle class="b" cx="549.27" cy="193.73" r="41.5"/>
-      <circle class="c" cx="443.69" cy="85.08" r="77.29"/>
+      <circle class="c" cx="449.35" cy="93.74" r="77.95"/>
-      <circle class="c" cx="641.55" cy="84.87" r="77.29"/>
+      <circle class="c" cx="648.89" cy="93.53" r="77.95"/>
-      <circle class="c" cx="640.56" cy="284.16" r="77.29"/>
+      <circle class="c" cx="647.89" cy="294.51" r="77.95"/>
-      <circle class="c" cx="443.25" cy="284.16" r="77.29"/>
+      <circle class="c" cx="448.9" cy="294.51" r="77.95"/>
     </g>
-    <path class="d" d="M0,153.93q0-17.85,22.36-17.85h4.81V92.57Q27.17.48,123.23,0h1q36.87,0,47.31,7.48L173,9.41l1,2.41V42q0,12.32-5.82,12.31-2.43,0-7.4-4.64t-14.19-9a48.63,48.63,0,0,0-21.22-4.41q-12,0-19.17,3.5a18.85,18.85,0,0,0-9.82,10.62,67.35,67.35,0,0,0-3.52,12.06,82.52,82.52,0,0,0-.85,13.39v60.32h27.28q10.86,0,16.05,5.43a17.52,17.52,0,0,1,5.19,12.42,22.5,22.5,0,0,1-1.21,7.36q-1.22,3.51-6.64,7.24t-14.36,3.74H93.64V336.82a56,56,0,0,1-.61,9.65,37.8,37.8,0,0,1-2.56,7.6,11.83,11.83,0,0,1-6.94,6.4q-5,1.93-13.51,1.93H49.08q-10.47,0-15.7-4.71t-5.73-8.44a77.31,77.31,0,0,1-.48-9.53V172.27H21.4Q0,172.27,0,153.93Z"/>
+    <path class="d" d="M8,161.93q0-17.85,22.36-17.85h4.81V100.57Q35.17,8.49,131.23,8h1q36.87,0,47.31,7.48L181,17.41l1,2.41V50q0,12.32-5.82,12.31-2.43,0-7.4-4.64t-14.19-9a48.63,48.63,0,0,0-21.22-4.41q-12,0-19.17,3.5a18.85,18.85,0,0,0-9.82,10.62,67.35,67.35,0,0,0-3.52,12.06,82.52,82.52,0,0,0-.85,13.39v60.32h27.28q10.86,0,16.05,5.43a17.52,17.52,0,0,1,5.19,12.42,22.5,22.5,0,0,1-1.21,7.36q-1.22,3.51-6.64,7.24t-14.36,3.74H101.64V344.82a56,56,0,0,1-.61,9.65,37.8,37.8,0,0,1-2.56,7.6,11.83,11.83,0,0,1-6.94,6.4q-5,1.93-13.51,1.93H57.08q-10.47,0-15.7-4.71t-5.73-8.44a77.31,77.31,0,0,1-.48-9.53V180.27H29.4Q8,180.27,8,161.93Z"/>
-    <path class="d" d="M193.21,340.2V29q0-23.16,20.86-23.4h22.81q22.8,0,22.8,22.44V338.27a68.92,68.92,0,0,1-.49,9.41,22.59,22.59,0,0,1-2.42,7.12,11.48,11.48,0,0,1-6.67,5.43,47.78,47.78,0,0,1-12.74,2.17H217q-11.4,0-17.58-4.47T193.21,340.2Z"/>
+    <path class="d" d="M201.21,348.2V37q0-23.16,20.86-23.4h22.81q22.8,0,22.8,22.44V346.27a68.92,68.92,0,0,1-.49,9.41,22.59,22.59,0,0,1-2.42,7.12,11.48,11.48,0,0,1-6.67,5.43,47.78,47.78,0,0,1-12.74,2.17H225q-11.4,0-17.58-4.47T201.21,348.2Z"/>
-    <path class="d" d="M276.9,54.08V29.47a40.39,40.39,0,0,1,1.7-12.91,11.36,11.36,0,0,1,6.18-7,25.27,25.27,0,0,1,6.68-2.3c1.45-.15,4-.4,7.76-.72h25.23q10.43,0,16.73,4.7t6.31,18.22V55.05a27.94,27.94,0,0,1-.85,7.11,23,23,0,0,1-2.06,5.43,20,20,0,0,1-2.91,4,10,10,0,0,1-3.52,2.54c-1.21.48-2.54,1-4,1.44a11.53,11.53,0,0,1-3.4.73,13.71,13.71,0,0,0-3.15.48H299.7q-10.43,0-16.61-4.7T276.9,54.08Zm1.94,284.71V159.28q0-22.2,21.83-22.2h20.38q10.92,0,16.62,4t6.67,8.45a60.47,60.47,0,0,1,1,12.18V341.2q0,22.2-21.83,22.2H300.67q-10.43,0-15.64-4.95t-5.7-8.81A90.93,90.93,0,0,1,278.84,338.79Z"/>
+    <path class="d" d="M284.9,61.08V36.47a40.39,40.39,0,0,1,1.7-12.91,11.36,11.36,0,0,1,6.18-7,25.27,25.27,0,0,1,6.68-2.3c1.45-.15,4-.4,7.76-.72h25.23q10.43,0,16.73,4.7t6.31,18.22V62.05a27.94,27.94,0,0,1-.85,7.11,23,23,0,0,1-2.06,5.43,20,20,0,0,1-2.91,4,10,10,0,0,1-3.52,2.54c-1.21.48-2.54,1-4,1.44a11.53,11.53,0,0,1-3.4.73,13.71,13.71,0,0,0-3.15.48H307.7q-10.43,0-16.61-4.7T284.9,61.08Zm1.94,284.71V166.28q0-22.2,21.83-22.2h20.38q10.92,0,16.62,4t6.67,8.45a60.47,60.47,0,0,1,1,12.18V348.2q0,22.2-21.83,22.2H308.67q-10.43,0-15.64-4.95t-5.7-8.81A90.93,90.93,0,0,1,286.84,345.79Z"/>
   </g>
 </svg>

docs/usage.md

@@ -143,9 +143,9 @@ Before flight you need to calibrate the accelerometer:
    * The `accel` and `gyro` fields should change as you move the drone.
    * The `landed` field should be `1` when the drone is still on the ground and `0` when you lift it up.
 
-2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Attitude indicator in QGroundControl is shown below:
+2. Check the attitude estimation: connect to the drone using QGroundControl, rotate the drone in different orientations and check if the attitude estimation shown in QGroundControl is correct. Compare your attitude indicator (in the *large vertical* mode) to the video:
 
-   <img src="img/qgc-attitude.png" height="200">
+    <a href="https://youtu.be/yVRN23-GISU"><img width=300 src="https://i3.ytimg.com/vi/yVRN23-GISU/maxresdefault.jpg"></a>
 
 3. Perform motor tests in the console. Use the following commands **— remove the propellers before running the tests!**
 

flix/control.ino

@@ -38,6 +38,12 @@ const int RAW = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
 int mode = STAB;
 bool armed = false;
 
+Quaternion attitudeTarget;
+Vector ratesTarget;
+Vector ratesExtra; // feedforward rates
+Vector torqueTarget;
+float thrustTarget;
+
 PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
 PID pitchRatePID(PITCHRATE_P, PITCHRATE_I, PITCHRATE_D, PITCHRATE_I_LIM, RATES_D_LPF_ALPHA);
 PID yawRatePID(YAWRATE_P, YAWRATE_I, YAWRATE_D);
@@ -47,12 +53,6 @@ PID yawPID(YAW_P, 0, 0);
 Vector maxRate(ROLLRATE_MAX, PITCHRATE_MAX, YAWRATE_MAX);
 float tiltMax = TILT_MAX;
 
-Quaternion attitudeTarget;
-Vector ratesTarget;
-Vector ratesExtra; // feedforward rates
-Vector torqueTarget;
-float thrustTarget;
-
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;
 

tools/log.py

@@ -43,6 +43,7 @@ records = [record for record in records if record[0] != 0]
 
 print(f'Received records: {len(records)}')
 
+os.makedirs(f'{DIR}/log', exist_ok=True)
 log = open(f'{DIR}/log/{datetime.datetime.now().isoformat()}.csv', 'wb')
 log.write(header.encode() + b'\n')
 for record in records: