Загрузить файлы в «/»

2025-04-01 12:25:51 +07:00
parent 9bcaa46f87
commit 380c42a5f6
1 changed files with 119 additions and 0 deletions
--- a/Reference.md
+++ b/Reference.md
@@ -0,0 +1,119 @@
+## Принятые термины
+`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;  
+}
+```