119 lines
4.5 KiB
Markdown
119 lines
4.5 KiB
Markdown
## Принятые термины
|
||
`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;
|
||
}
|
||
``` |