110 lines
4.0 KiB
C++
110 lines
4.0 KiB
C++
#include "Tasks/Task4.h"
|
|
|
|
// Данная функция выполняется только один раз при запуске программы
|
|
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);
|
|
_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);
|
|
}
|
|
|
|
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;
|
|
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 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);
|
|
}
|
|
|
|
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));
|
|
|
|
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;
|
|
} |