110 lines
4.7 KiB
C++
110 lines
4.7 KiB
C++
#include "Tasks/Task.h"
|
||
#include <cmath>
|
||
#include <vector>
|
||
|
||
// Данная функция выполняется только один раз при запуске программы
|
||
// (при подаче питания на плату, перепрошивке, или нажатии кнопки reset)
|
||
|
||
// Настройки
|
||
constexpr float Ki = 0.03f;
|
||
constexpr float target_height = 65.f;
|
||
constexpr float waypoint_radius = 1.5f;
|
||
constexpr float pitch_constraint = 20.f;
|
||
constexpr float yaw_constraint = 20.f;
|
||
|
||
struct WayPoint {
|
||
float x;
|
||
float z;
|
||
};
|
||
|
||
std::vector<WayPoint> wayPoints = {
|
||
WayPoint{.x = -480.f, .z = 85.f},
|
||
WayPoint{.x = -30.f, .z = 500.f},
|
||
WayPoint{.x = 150.f, .z = 330.f},
|
||
WayPoint{.x = 150.f, .z = 5.f},
|
||
};
|
||
|
||
void Task_solution::setup(HardwareSerial *a_Serial) {
|
||
debug_serial = a_Serial;
|
||
_Point_Index = 0;
|
||
}
|
||
|
||
float distance(const float x1, const float y1, const float x2, const float y2) {
|
||
return static_cast<float>(sqrt(pow((x2 - x1), 2) + pow((y2 - y1), 2)));
|
||
}
|
||
|
||
bool isEqual(const float a, const float b, const float accuracy) {
|
||
return abs(a - b) <= accuracy;
|
||
}
|
||
|
||
/*
|
||
### УЧАСТНИКАМ ОЛИМПИАДЫ - Основная функция отправки пакета значений крена и тангажа (град) ####
|
||
Данная функция осуществляет циклический обмен данных с симулятором с частотой 100 Гц.
|
||
*/
|
||
SignalBody Task_solution::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);
|
||
const float target_x = wayPoints[_Point_Index].x;
|
||
const float target_y = wayPoints[_Point_Index].z;
|
||
const auto target_psi = static_cast<float>(
|
||
-degrees(atan2(target_y - a_telemetry.Z, target_x - a_telemetry.L))
|
||
);
|
||
|
||
|
||
_ans.Gamma_direct = GammaReg(a_telemetry.Psi, target_psi);
|
||
_ans.Tang_direct = HeightReg(a_telemetry.H, a_telemetry.Vy1, target_height);
|
||
|
||
const float dist_to_target = distance(a_telemetry.L, a_telemetry.Z, target_x, target_y);
|
||
if (dist_to_target <= waypoint_radius) {
|
||
// Проверяем расстояние до цели (радиус 5 метров)
|
||
if (_Point_Index < wayPoints.size() - 1) {
|
||
_Point_Index++;
|
||
}
|
||
}
|
||
|
||
// Отправляем команды на симулятор
|
||
// БЛА будет пытаться выдерживать заданные углы крена и тангажа
|
||
return _ans;
|
||
}
|
||
|
||
float roll_err_last = 0.f;
|
||
float integral_err = 0.05f;
|
||
|
||
float Task_solution::GammaReg(float Psi, float Psi_dir) {
|
||
float roll_err = AngDefines(Psi - Psi_dir);
|
||
integral_err += roll_err * 10.f;
|
||
integral_err = constrain(integral_err, -50.f, 50.f);
|
||
float gamma_cmd = 2.0f * roll_err + (Ki * integral_err) + 35.f * (roll_err - roll_err_last);
|
||
roll_err_last = roll_err;
|
||
gamma_cmd = constrain(gamma_cmd, -yaw_constraint, yaw_constraint);
|
||
return gamma_cmd;
|
||
}
|
||
|
||
float Task_solution::HeightReg(float Yg, float Vy, float Hz) {
|
||
constexpr float K_H = 0.56f;
|
||
constexpr float K_vy_1 = 7.7591f;
|
||
constexpr float K_vy_2 = 9.8941f;
|
||
const float kh1 = constrain(K_H * (Hz - Yg), -7.5f, 7.5f);
|
||
const float Pitch_direct = constrain((kh1 * K_vy_1 - Vy * K_vy_2), -pitch_constraint, pitch_constraint);
|
||
return Pitch_direct;
|
||
}
|