bas/UAViant API Reference.md

Принятые термины

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 и т. п. вещи)

Регуляровка курса

Регулируем тангаж

#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;  
}