From cb788441ad4488686122ed8abb77edd407420f35 Mon Sep 17 00:00:00 2001 From: Goga Coder Date: Fri, 4 Apr 2025 13:00:58 +0700 Subject: [PATCH] =?UTF-8?q?=D0=9E=D0=B1=D0=BD=D0=BE=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 | 73 ++++++++++++++----------------------------------- 1 file changed, 21 insertions(+), 52 deletions(-) diff --git a/points_2022.cpp b/points_2022.cpp index 0a4baa0..e2d65ee 100644 --- a/points_2022.cpp +++ b/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++; }