From 39d5b8d1629da5fd3639482a9a2d4273c7d83149 Mon Sep 17 00:00:00 2001
From: Goga Coder <me@gogacoder.com>
Date: Fri, 4 Apr 2025 12:40:28 +0700
Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=B8=D1=82?=
 =?UTF-8?q?=D1=8C=20points=5F2022.cpp?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 points_2022.cpp | 149 ++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 149 insertions(+)
 create mode 100644 points_2022.cpp

diff --git a/points_2022.cpp b/points_2022.cpp
new file mode 100644
index 0000000..b86866c
--- /dev/null
+++ b/points_2022.cpp
@@ -0,0 +1,149 @@
+#include "Tasks/Task4.h"
+ // Данная функция выполняется только один раз при запуске программы
+// (при подаче питания на плату, перепрошивке, или нажатии кнопки reset)
+void Task4_solution::init(HardwareSerial * a_Serial) {
+  debug_serial = a_Serial;
+  // Примеры вывода в отладочную консоль
+  printtoDebugSerial("Hello team!");
+  printtoDebugSerial(String(3.1415926));
+  // Путевые точки в произвольном порядке
+  _PointsArray[0] = FlyPoints(0, 250, 70);
+  _PointsArray[1] = FlyPoints(0, 500, 90);
+  _PointsArray[2] = FlyPoints(250, 500, 70);
+  _PointsArray[3] = FlyPoints(500, 500, 70);
+  _PointsArray[4] = FlyPoints(250, 250, 90);
+  _PointsArray[5] = FlyPoints(250, 250, 80);
+  _PointsArray[6] = FlyPoints(500, 0, 70);
+  _PointsArray[7] = FlyPoints(500, -250, 90);
+  _PointsArray[8] = FlyPoints(250, -250, 70);
+}
+_PointsArray[9] = FlyPoints(0, -250, 90);
+_PointsArray[10] = FlyPoints(0, -250, 80);
+_PointsArray[11] = FlyPoints(0, -500, 70);
+_PointsArray[12] = FlyPoints(-250, -500, 80);
+_PointsArray[13] = FlyPoints(-250, -250, 70);
+_PointsArray[14] = FlyPoints(-500, 0, 90);
+_PointsArray[15] = FlyPoints(-500, 250, 80);
+_PointsArray[16] = FlyPoints(-250, 500, 80);
+_PointsArray[17] = FlyPoints(-250, 250, 80);
+_PointsArray[18] = FlyPoints(0, 0, 90);
+_PointsArray[19] = FlyPoints(0, 0, 80);
+/*
+### УЧАСТНИКАМ ОЛИМПИАДЫ - Основная функция отправки пакета значений крена и
+, →
+ тангажа (град) ####
+Данная функция осуществляет циклический обмен данных с симулятором с частотой 100
+, →
+ Гц.
+*/
+SignalBody Task4_solution::Task8_in_the_loop(Skywalker2015PacketTelemetry, →
+  a_telemetry) {
+  /* a_telemetry - структура данных, полученная с симулятора. Она содержит
+  , →
+   текущие параметры БЛА
+  a_telemetry.L - координата Х (север) [м]
+  a_telemetry.Z - координата Z (восток) [м]
+  a_telemetry.H - координата Y (высота) [м]
+  a_telemetry.Psi - угол рысканья [град]
+  a_telemetry.Gam - Угол крена [град]
+  a_telemetry.Tan - Угол тангажа [град]
+  a_telemetry.V - Скорость полета БЛА [м/с]
+  a_telemetry.Vx1 - Продольная скорость [м/с]
+  a_telemetry.Vz1 - Поперечная скорость [м/с]
+  a_telemetry.Vy1 - Вертикальная скорость [м/с]
+  a_telemetry.wx - Угловая скорость вокруг продольной оси [1/с]
+  a_telemetry.wy - Угловая скорость вокруг вертикальной оси [1/с]
+  a_telemetry.wz - Угловая скорость вокруг поперечной оси [1/с]
+  */
+  // РЕКОМЕНДУЕМЫЙ АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ 4
+  //
+  SignalBody _ans; // Структура которая отсылается на симулятор
+  // Проверить близость БЛА к текущей путевой точке. Изменить путевую точку при
+  , →
+  необходимости
+  _Point_Index = GetNowPointIndex(a_telemetry.L, a_telemetry.Z, a_telemetry.H);
+  // Через координаты путевой точки и текущего местоположения БЛА найти
+  , →
+  направление на путевую точку(пеленг)
+  _ans.Gamma_direct = PointsFlyGam(_Point_Index, a_telemetry.L, a_telemetry.Z, , →
+    a_telemetry.Psi);
+  // Рассчитать необходимое изменение угла курса
+  // Рассчитать требуемый угол крена (для регулирования курса)
+  // На основе текущей высоты БЛА и заданной высоты путевой точки рассчитать
+  , →
+  необходимое изменение высоты
+  _ans.Tang_direct = PointsFlyTan(a_telemetry.H, a_telemetry.Vy1, , →
+    _Point_Index);
+  // Рассчитать требуемый угол тангажа (для регулирования высоты)
+  //_ans.Gamma_direct = 0; // заданный угол крена
+  //_ans.Tang_direct = 0; // заданный угол тангажа
+}
+// Отправляем команды на симулятор
+// БЛА будет пытаться выдерживать заданные углы крена и тангажа
+return _ans;
+float roll_err_last = 0;
+float Task4_solution::GammaReg(float Psi, float Psi_dir) {
+  float Kpsi = 1.6;
+  float roll_err = AngDefines(Psi - Psi_dir);
+  float gamma_cmd = 2.0 * roll_err + 35 * (roll_err - roll_err_last);
+  roll_err_last = roll_err;
+  gamma_cmd = constrain(gamma_cmd, -30, 30);
+  return gamma_cmd;
+}
+float Task4_solution::HeightReg(float Yg, float Vy, float Hz) {
+  float K_H = 0.56;
+  float K_vy_1 = 7.7591;
+  float K_vy_2 = 9.8941;
+  float kh1 = constrain(K_H * (Hz - Yg), -7.5, 7.5);
+  float _Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -20, 20);
+  return _Pitch_direct;
+}
+float Task4_solution::ToPointXY(float _Xt, float _Yt, float a_Xg, float a_Zg, , →
+  float Psi) {}
+float Psi_cmd = -atan2(a_Zg - _Yt, a_Xg - _Xt);
+Psi_cmd *= 57.3;
+return GammaReg(Psi, Psi_cmd);
+float Task4_solution::PointsFlyGam(const size_t & a_Point_index, float _Xt, float, →
+  _Yt, float Psi) {
+  float Xg_cmd = _PointsArray[_Point_Index].North;
+  float Zg_cmd = _PointsArray[_Point_Index].East;
+  return ToPointXY(_Xt, _Yt, Xg_cmd, Zg_cmd, Psi);
+}
+float Task4_solution::PointsFlyTan(float H, float Vy,
+  const size_t & a_Point_index) {
+  float H_z = _PointsArray[_Point_Index].Height;
+  return HeightReg(H, Vy, H_z);
+}
+size_t Task4_solution::GetNowPointIndex(float X, float Z, float H) {
+  size_t max_index = sizeof(_PointsArray) / sizeof(_PointsArray[0]);
+  float Xg_cmd = _PointsArray[_Point_Index].North;
+  float Zg_cmd = _PointsArray[_Point_Index].East;
+  float Yg_cmd = _PointsArray[_Point_Index].Height;
+  float delX = Xg_cmd - X;
+  float delY = Zg_cmd - Z;
+  float delZ = Yg_cmd - H;
+  float Size = sqrtf(pow(delX, 2) + pow(delY, 2) + pow(delZ, 2));
+  // Calculate crosstrack error
+  float x_og = 0.0;
+  float z_og = 0.0;
+  if (_Point_Index > 0) {
+    x_og = _PointsArray[_Point_Index - 1].North;
+    z_og = _PointsArray[_Point_Index - 1].East;
+  } else {
+    x_og = _PointsArray[max_index - 1].North;
+    z_og = _PointsArray[max_index - 1].East;
+  }
+  float relx = X - x_og;
+  float rely = Z - z_og;
+  float delx = Xg_cmd - x_og;
+  float dely = Zg_cmd - z_og;
+  float U = (relx * delx + rely * dely) / (delx * delx + dely * dely);
+  float cte = (rely * delx - relx * dely) / (delx * delx + dely * dely);
+  if (Size < 7.0) {
+    _Point_Index++;
+  }
+  if (_Point_Index >= max_index) {
+    _Point_Index = 0;
+  }
+  return _Point_Index;
+}
\ No newline at end of file