gogacoder/bas
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Обновить points_2022.cpp

Browse Source
This commit is contained in:
Goga Coder
2025-04-04 12:47:22 +07:00
parent 32b5ee3cde
commit 141a4b28aa
1 changed files with 117 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
points_2022.cpp
View File

@@ -16,7 +16,7 @@ void Task4_solution::init(HardwareSerial * a_Serial) {
_PointsArray[6] = FlyPoints(500, 0, 70); _PointsArray[6] = FlyPoints(500, 0, 70);
_PointsArray[7] = FlyPoints(500, -250, 90); _PointsArray[7] = FlyPoints(500, -250, 90);
_PointsArray[8] = FlyPoints(250, -250, 70); _PointsArray[8] = FlyPoints(250, -250, 70);
}
_PointsArray[9] = FlyPoints(0, -250, 90); _PointsArray[9] = FlyPoints(0, -250, 90);
_PointsArray[10] = FlyPoints(0, -250, 80); _PointsArray[10] = FlyPoints(0, -250, 80);
_PointsArray[11] = FlyPoints(0, -500, 70); _PointsArray[11] = FlyPoints(0, -500, 70);
@@ -28,6 +28,7 @@ _PointsArray[16] = FlyPoints(-250, 500, 80);
_PointsArray[17] = FlyPoints(-250, 250, 80); _PointsArray[17] = FlyPoints(-250, 250, 80);
_PointsArray[18] = FlyPoints(0, 0, 90); _PointsArray[18] = FlyPoints(0, 0, 90);
_PointsArray[19] = FlyPoints(0, 0, 80); _PointsArray[19] = FlyPoints(0, 0, 80);
}
/* /*
### УЧАСТНИКАМ ОЛИМПИАДЫ - Основная функция отправки пакета значений крена и ### УЧАСТНИКАМ ОЛИМПИАДЫ - Основная функция отправки пакета значений крена и
, → , →
@@ -36,8 +37,7 @@ _PointsArray[19] = FlyPoints(0, 0, 80);
, → , →
Гц. Гц.
*/ */
SignalBody Task4_solution::Task8_in_the_loop(Skywalker2015PacketTelemetry, → SignalBody Task4_solution::Task8_in_the_loop(Skywalker2015PacketTelemetry a_telemetry) {
a_telemetry) {
/* a_telemetry - структура данных, полученная с симулятора. Она содержит /* a_telemetry - структура данных, полученная с симулятора. Она содержит
, → , →
текущие параметры БЛА текущие параметры БЛА
@@ -58,25 +58,22 @@ SignalBody Task4_solution::Task8_in_the_loop(Skywalker2015PacketTelemetry, →
// РЕКОМЕНДУЕМЫЙ АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ 4 // РЕКОМЕНДУЕМЫЙ АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ 4
// //
SignalBody _ans; // Структура которая отсылается на симулятор SignalBody _ans; // Структура которая отсылается на симулятор
// Проверить близость БЛА к текущей путевой точке. Изменить путевую точку при // Проверить близость БЛА к текущей путевой точке. Изменить путевую точку при необходимости
, →
необходимости
_Point_Index = GetNowPointIndex(a_telemetry.L, a_telemetry.Z, a_telemetry.H); _Point_Index = GetNowPointIndex(a_telemetry.L, a_telemetry.Z, a_telemetry.H);
// Через координаты путевой точки и текущего местоположения БЛА найти, направление на путевую точку(пеленг) // Через координаты путевой точки и текущего местоположения БЛА найти, направление на путевую точку(пеленг)
_ans.Gamma_direct = PointsFlyGam(_Point_Index, a_telemetry.L, a_telemetry.Z, , → _ans.Gamma_direct = PointsFlyGam(_Point_Index, a_telemetry.L, a_telemetry.Z, a_telemetry.Psi);
a_telemetry.Psi);
// Рассчитать необходимое изменение угла курса // Рассчитать необходимое изменение угла курса
// Рассчитать требуемый угол крена (для регулирования курса) // Рассчитать требуемый угол крена (для регулирования курса)
// На основе текущей высоты БЛА и заданной высоты путевой точки рассчитать, необходимое изменение высоты // На основе текущей высоты БЛА и заданной высоты путевой точки рассчитать, необходимое изменение высоты
_ans.Tang_direct = PointsFlyTan(a_telemetry.H, a_telemetry.Vy1, , → _ans.Tang_direct = PointsFlyTan(a_telemetry.H, a_telemetry.Vy1, _Point_Index);
_Point_Index);
// Рассчитать требуемый угол тангажа (для регулирования высоты) // Рассчитать требуемый угол тангажа (для регулирования высоты)
//_ans.Gamma_direct = 0; // заданный угол крена //_ans.Gamma_direct = 0; // заданный угол крена
//_ans.Tang_direct = 0; // заданный угол тангажа //_ans.Tang_direct = 0; // заданный угол тангажа
}
// Отправляем команды на симулятор // Отправляем команды на симулятор
// БЛА будет пытаться выдерживать заданные углы крена и тангажа // БЛА будет пытаться выдерживать заданные углы крена и тангажа
return _ans; return _ans;
}
float roll_err_last = 0; float roll_err_last = 0;
float Task4_solution::GammaReg(float Psi, float Psi_dir) { float Task4_solution::GammaReg(float Psi, float Psi_dir) {
float Kpsi = 1.6; float Kpsi = 1.6;
@@ -94,13 +91,12 @@ float Task4_solution::HeightReg(float Yg, float Vy, float Hz) {
float _Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -20, 20); float _Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -20, 20);
return _Pitch_direct; return _Pitch_direct;
} }
float Task4_solution::ToPointXY(float _Xt, float _Yt, float a_Xg, float a_Zg, , → float Task4_solution::ToPointXY(float _Xt, float _Yt, float a_Xg, float a_Zg, float Psi) {
float Psi) {}
float Psi_cmd = -atan2(a_Zg - _Yt, a_Xg - _Xt); float Psi_cmd = -atan2(a_Zg - _Yt, a_Xg - _Xt);
Psi_cmd *= 57.3; Psi_cmd *= 57.3;
return GammaReg(Psi, Psi_cmd); 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 Xg_cmd = _PointsArray[_Point_Index].North;
float Zg_cmd = _PointsArray[_Point_Index].East; float Zg_cmd = _PointsArray[_Point_Index].East;
return ToPointXY(_Xt, _Yt, Xg_cmd, Zg_cmd, Psi); return ToPointXY(_Xt, _Yt, Xg_cmd, Zg_cmd, Psi);