diff --git a/points_2022.cpp b/points_2022.cpp index cb2270d..0a4baa0 100644 --- a/points_2022.cpp +++ b/points_2022.cpp @@ -2,32 +2,33 @@ // Данная функция выполняется только один раз при запуске программы // (при подаче питания на плату, перепрошивке, или нажатии кнопки 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); + 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); } -_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); /* ### УЧАСТНИКАМ ОЛИМПИАДЫ - Основная функция отправки пакета значений крена и , → @@ -36,110 +37,105 @@ _PointsArray[19] = FlyPoints(0, 0, 80); , → Гц. */ -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) { + /* 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; } -// Отправляем команды на симулятор -// БЛА будет пытаться выдерживать заданные углы крена и тангажа -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 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 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::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); + 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; + 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