Обновить points_2022.cpp

points_2022.cpp

@@ -1,12 +1,11 @@
 #include "Tasks/Task4.h"
- // Данная функция выполняется только один раз при запуске программы
-// (при подаче питания на плату, перепрошивке, или нажатии кнопки reset)
-void Task4_solution::init(HardwareSerial * a_Serial) {
+
+// Данная функция выполняется только один раз при запуске программы
+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);
@@ -16,7 +15,6 @@ void Task4_solution::init(HardwareSerial * a_Serial) {
     _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);
@@ -29,51 +27,15 @@ void Task4_solution::init(HardwareSerial * a_Serial) {
     _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; // заданный угол тангажа
 
-    // Отправляем команды на симулятор
-    // БЛА будет пытаться выдерживать заданные углы крена и тангажа
+SignalBody Task4_solution::Task8_in_the_loop(Skywalker2015PacketTelemetry a_telemetry) {
+    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);
     return _ans;
 }
+
 float roll_err_last = 0;
 float Task4_solution::GammaReg(float Psi, float Psi_dir) {
     float Kpsi = 1.6;
@@ -83,6 +45,7 @@ float Task4_solution::GammaReg(float Psi, float Psi_dir) {
     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;
@@ -91,31 +54,35 @@ float Task4_solution::HeightReg(float Yg, float Vy, float Hz) {
     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 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 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) {
@@ -125,12 +92,14 @@ size_t Task4_solution::GetNowPointIndex(float X, float Z, float H) {
         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++;
     }