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

238
points_2022.cpp
View File

@@ -2,32 +2,33 @@
// Данная функция выполняется только один раз при запуске программы // Данная функция выполняется только один раз при запуске программы
// (при подаче питания на плату, перепрошивке, или нажатии кнопки reset) // (при подаче питания на плату, перепрошивке, или нажатии кнопки reset)
void Task4_solution::init(HardwareSerial * a_Serial) { void Task4_solution::init(HardwareSerial * a_Serial) {
debug_serial = a_Serial; debug_serial = a_Serial;
// Примеры вывода в отладочную консоль // Примеры вывода в отладочную консоль
printtoDebugSerial("Hello team!"); printtoDebugSerial("Hello team!");
printtoDebugSerial(String(3.1415926)); printtoDebugSerial(String(3.1415926));
// Путевые точки в произвольном порядке // Путевые точки в произвольном порядке
_PointsArray[0] = FlyPoints(0, 250, 70); _PointsArray[0] = FlyPoints(0, 250, 70);
_PointsArray[1] = FlyPoints(0, 500, 90); _PointsArray[1] = FlyPoints(0, 500, 90);
_PointsArray[2] = FlyPoints(250, 500, 70); _PointsArray[2] = FlyPoints(250, 500, 70);
_PointsArray[3] = FlyPoints(500, 500, 70); _PointsArray[3] = FlyPoints(500, 500, 70);
_PointsArray[4] = FlyPoints(250, 250, 90); _PointsArray[4] = FlyPoints(250, 250, 90);
_PointsArray[5] = FlyPoints(250, 250, 80); _PointsArray[5] = FlyPoints(250, 250, 80);
_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[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, → SignalBody Task4_solution::Task8_in_the_loop(Skywalker2015PacketTelemetry a_telemetry) {
a_telemetry) { /* a_telemetry - структура данных, полученная с симулятора. Она содержит
/* a_telemetry - структура данных, полученная с симулятора. Она содержит , →
, → текущие параметры БЛА
текущие параметры БЛА a_telemetry.L - координата Х (север) [м]
a_telemetry.L - координата Х (север) [м] a_telemetry.Z - координата Z (восток) [м]
a_telemetry.Z - координата Z (восток) [м] a_telemetry.H - координата Y (высота) [м]
a_telemetry.H - координата Y (высота) [м] a_telemetry.Psi - угол рысканья [град]
a_telemetry.Psi - угол рысканья [град] a_telemetry.Gam - Угол крена [град]
a_telemetry.Gam - Угол крена [град] a_telemetry.Tan - Угол тангажа [град]
a_telemetry.Tan - Угол тангажа [град] a_telemetry.V - Скорость полета БЛА [м/с]
a_telemetry.V - Скорость полета БЛА [м/с] a_telemetry.Vx1 - Продольная скорость [м/с]
a_telemetry.Vx1 - Продольная скорость [м/с] a_telemetry.Vz1 - Поперечная скорость [м/с]
a_telemetry.Vz1 - Поперечная скорость [м/с] a_telemetry.Vy1 - Вертикальная скорость [м/с]
a_telemetry.Vy1 - Вертикальная скорость [м/с] a_telemetry.wx - Угловая скорость вокруг продольной оси [1/с]
a_telemetry.wx - Угловая скорость вокруг продольной оси [1/с] a_telemetry.wy - Угловая скорость вокруг вертикальной оси [1/с]
a_telemetry.wy - Угловая скорость вокруг вертикальной оси [1/с] a_telemetry.wz - Угловая скорость вокруг поперечной оси [1/с]
a_telemetry.wz - Угловая скорость вокруг поперечной оси [1/с] */
*/ // РЕКОМЕНДУЕМЫЙ АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ 4
// РЕКОМЕНДУЕМЫЙ АЛГОРИТМ РЕШЕНИЯ ЗАДАЧИ 4 //
// SignalBody _ans; // Структура которая отсылается на симулятор
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);
_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; // заданный угол тангажа
_ans.Tang_direct = PointsFlyTan(a_telemetry.H, a_telemetry.Vy1, , →
_Point_Index); // Отправляем команды на симулятор
// Рассчитать требуемый угол тангажа (для регулирования высоты) // БЛА будет пытаться выдерживать заданные углы крена и тангажа
//_ans.Gamma_direct = 0; // заданный угол крена return _ans;
//_ans.Tang_direct = 0; // заданный угол тангажа
} }
// Отправляем команды на симулятор
// БЛА будет пытаться выдерживать заданные углы крена и тангажа
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;
float roll_err = AngDefines(Psi - Psi_dir); float roll_err = AngDefines(Psi - Psi_dir);
float gamma_cmd = 2.0 * roll_err + 35 * (roll_err - roll_err_last); float gamma_cmd = 2.0 * roll_err + 35 * (roll_err - roll_err_last);
roll_err_last = roll_err; roll_err_last = roll_err;
gamma_cmd = constrain(gamma_cmd, -30, 30); gamma_cmd = constrain(gamma_cmd, -30, 30);
return gamma_cmd; return gamma_cmd;
} }
float Task4_solution::HeightReg(float Yg, float Vy, float Hz) { float Task4_solution::HeightReg(float Yg, float Vy, float Hz) {
float K_H = 0.56; float K_H = 0.56;
float K_vy_1 = 7.7591; float K_vy_1 = 7.7591;
float K_vy_2 = 9.8941; float K_vy_2 = 9.8941;
float kh1 = constrain(K_H * (Hz - Yg), -7.5, 7.5); 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); 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, → float Task4_solution::PointsFlyGam(const size_t & a_Point_index, float _Xt, float _Yt, float Psi) {
_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);
} }
float Task4_solution::PointsFlyTan(float H, float Vy, float Task4_solution::PointsFlyTan(float H, float Vy,
const size_t & a_Point_index) { const size_t & a_Point_index) {
float H_z = _PointsArray[_Point_Index].Height; float H_z = _PointsArray[_Point_Index].Height;
return HeightReg(H, Vy, H_z); return HeightReg(H, Vy, H_z);
} }
size_t Task4_solution::GetNowPointIndex(float X, float Z, float H) { size_t Task4_solution::GetNowPointIndex(float X, float Z, float H) {
size_t max_index = sizeof(_PointsArray) / sizeof(_PointsArray[0]); size_t max_index = sizeof(_PointsArray) / sizeof(_PointsArray[0]);
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;
float Yg_cmd = _PointsArray[_Point_Index].Height; float Yg_cmd = _PointsArray[_Point_Index].Height;
float delX = Xg_cmd - X; float delX = Xg_cmd - X;
float delY = Zg_cmd - Z; float delY = Zg_cmd - Z;
float delZ = Yg_cmd - H; float delZ = Yg_cmd - H;
float Size = sqrtf(pow(delX, 2) + pow(delY, 2) + pow(delZ, 2)); float Size = sqrtf(pow(delX, 2) + pow(delY, 2) + pow(delZ, 2));
// Calculate crosstrack error // Calculate crosstrack error
float x_og = 0.0; float x_og = 0.0;
float z_og = 0.0; float z_og = 0.0;
if (_Point_Index > 0) { if (_Point_Index > 0) {
x_og = _PointsArray[_Point_Index - 1].North; x_og = _PointsArray[_Point_Index - 1].North;
z_og = _PointsArray[_Point_Index - 1].East; z_og = _PointsArray[_Point_Index - 1].East;
} else { } else {
x_og = _PointsArray[max_index - 1].North; x_og = _PointsArray[max_index - 1].North;
z_og = _PointsArray[max_index - 1].East; z_og = _PointsArray[max_index - 1].East;
} }
float relx = X - x_og; float relx = X - x_og;
float rely = Z - z_og; float rely = Z - z_og;
float delx = Xg_cmd - x_og; float delx = Xg_cmd - x_og;
float dely = Zg_cmd - z_og; float dely = Zg_cmd - z_og;
float U = (relx * delx + rely * dely) / (delx * delx + dely * dely); float U = (relx * delx + rely * dely) / (delx * delx + dely * dely);
float cte = (rely * delx - relx * dely) / (delx * delx + dely * dely); float cte = (rely * delx - relx * dely) / (delx * delx + dely * dely);
if (Size < 7.0) { if (Size < 7.0) {
_Point_Index++; _Point_Index++;
} }
if (_Point_Index >= max_index) { if (_Point_Index >= max_index) {
_Point_Index = 0; _Point_Index = 0;
} }
return _Point_Index; return _Point_Index;
} }