## Принятые термины
`Psi` (фи) - угол рысканья
`Gam` (гамма) - угол крена
`Tan`  - угол тангажа

## Структура `Skywalker2015PacketTelemetry`
Входные данные с симулятора

| Поле  | Описание                                 | Ед. измерения |
| ----- | ---------------------------------------- | ------------- |
| `L`   | Координата Х (север)                     | м             |
| `Z`   | Координата Z (восток)                    | м             |
| `H`   | Координата Y (высота)                    | м             |
| `Psi` | Угол рысканья                            | градусы       |
| `Gam` | Угол крена                               | градусы       |
| `Tan` | Угол тангажа                             | градусы       |
| `V`   | Скорость полета БПЛА                     | м/с           |
| `Vx1` | Продольная скорость                      | м/с           |
| `Vy1` | Поперечная скорость                      | м/c           |
| `Vz1` | Вертикальная скорость                    | м/c           |
| `wx`  | Угловая скорость вокруг продольной оси   | 1°/с          |
| `wy`  | Угловая скорость вокруг вертикальной оси | 1°/с          |
| `wz`  | Угловая скорость вокруг поперечной оси   | 1°/с          |

## Структура `SignalBody`
Меняется от задания к заданию

| Поле           | Описание             | Ед. измерения |
| -------------- | -------------------- | ------------- |
| `Gamma_direct` | Целевой угол крена   | м             |
| `Tang_direct`  | Целевой угол тангажа | м             |

## Комментарий SIM
```
#define SIM
```
Иногда используется для отладки на симуляторе, необходимо отключить при переходе на стэнд.

## Вывод в консоль
```
void printtoDebugSerial(String s)
```
Используется для вывода в консоль. Необходимо приводить аргумент к строке.

## Вычитание углов
```
float AngDefines(float angleDelta)
```
Принимает разность углов и нормализует её (обрабатывает переход через 360 и т. п. вещи)


## Регуляровка курса
Регулируем тангаж 
```cpp
#include <iostream>  
#include <cmath>  
  
template <typename T>  
constexpr T constrain(T value, T low, T high) {  
   if (value < low) {  
       return low;  
   } else if (value > high) {  
       return high;  
   } else {  
       return value;  
   }  
}  
  
/**  
* Нормализует угловую ошибку в диапазон [-180°, 180°).  
* @param error Угловая ошибка в градусах.  
* @return Нормализованная ошибка.  
*/  
float AngDefines(float error) {  
   error = fmod(error, 360.0f);  
  
   if (error < 0) {  
       error += 360.0f;  
   }  
  
   if (error > 180.0f) {  
       error -= 360.0f;  
   }  
  
   return error;  
}  
  
// Reference: GammaReg, page 141  
// Description: Keep yaw angle target  
class YawRegulator final {  
public:  
       YawRegulator(float P = 2.f, float D = 35.f) {  
               _p = P;  
               _d = D;  
       }  
  
       float Calc(float yaw, float target_yaw) {  
               float roll_err = AngDefines(yaw - target_yaw);  
               float roll_cmd = _p * roll_err + _d * (roll_err - _roll_err_last);  
               _roll_err_last = roll_err;  
               roll_cmd = constrain(roll_cmd, -30.f, 30.f);  
               return roll_cmd;  
       }  
  
private:  
       float _roll_err_last = 0.f;  
       float _p = 0.f;  
       float _d = 0.f;  
};  
  
  
int main() {  
       YawRegulator yawRegulator;  
       // Не забывайте про инверсию угла
       std::cout << yawRegulator.Calc(-30.f, 15.f) << std::endl;  
  
       return EXIT_SUCCESS;  
}
```