121 lines
6.1 KiB
C++
121 lines
6.1 KiB
C++
#include "Tasks/Task3.h"
|
||
|
||
/*#################### Участникам олимпиады ################*/
|
||
/*
|
||
Наименование функции:
|
||
Task3_in_the_loop()
|
||
Назначение функции: Данная функция используется для обмена данными в режиме ТКПА, → симулятора
|
||
Входные данные приходящие с симулятора:
|
||
|
||
a_Elevator_zad - заданное значение руля высоты, град.
|
||
a_Rudder_zad - заданное значение руля направления, град.
|
||
a_Elerons_zad - заданное значения элеронов, град.
|
||
a_Thrust_zad - заданное значения тяги двигателя, Н.
|
||
|
||
a_Pitch_deg - текущее значение угла тангажа, град
|
||
a_Gamma_deg - текущее значение угла крена, град
|
||
|
||
Выходные данные идущие на симулятор:
|
||
|
||
Структура Task2_PWM содержащая следующие поля данных:
|
||
|
||
Elevator_PWM - RC сигнал, руль высоты
|
||
Rudder_PWM - RC сигнал, руль направления
|
||
Elerons_PWM - RC сигнал, элероны
|
||
Engine_RPM - RC сигнал, двигатель
|
||
|
||
Примечание: delay() в данной функции вызывать бесполезно, так как обмен с симулятором синхронный.
|
||
|
||
define SIM - включить когда отлаживаете ТОЛЬКО на симуляторе, когда переходите на стенд то необходимо закомментировать
|
||
|
||
*/
|
||
// Закомментировать только когда отлаживаете на стенде.
|
||
#define SIM
|
||
|
||
Task3_PWM Task3_Solution::Task3_in_the_loop(const float & a_Elevator_zad,
|
||
const float & a_Rudder_zad,
|
||
const float & a_Elerons_zad,
|
||
const float & a_Thrust_zad,
|
||
const float & a_Pitch_deg,
|
||
const float & a_Gamma_deg) {
|
||
// Структура которая содержит заданный RC сигнал
|
||
Task3_PWM _ans;
|
||
// Текущие углы тангажа и крена с самолета (работают только при подключении к стенду)
|
||
double IMS_Pitch_deg = mav_orient.Pitch; // Угол тангажа,градусы
|
||
double IMS_Roll_deg = mav_orient.Roll; // Угол крена,градусы
|
||
// Текущие углы тангажа и крена поступающие с симулятора
|
||
double SIM_Pitch_deg = a_Pitch_deg;
|
||
double SIM_Roll_deg = a_Gamma_deg;
|
||
|
||
// Функция прямого контроля положения органов управления
|
||
_ans = full_control_mode(a_Elevator_zad, a_Rudder_zad, a_Elerons_zad);
|
||
|
||
// Функция стабилизации, в зависимости от текущих углов приходящих с симулятора
|
||
// _ans = stab_mode(SIM_Pitch_deg, SIM_Roll_deg);
|
||
|
||
#ifndef SIM
|
||
// Функция стабилизации, в зависимости от текущих углов приходящих с стенда
|
||
_ans = stab_mode(IMS_Pitch_deg, IMS_Roll_deg);
|
||
// Подаем структуру сигналов на Ardupilot (Участникам не трогать)
|
||
if ((millis() - t_last_control) > control_interval) {
|
||
mav_rc_override(_ans);
|
||
}
|
||
#endif
|
||
// Возвращаем RC сигнал на симулятор
|
||
return _ans;
|
||
}
|
||
|
||
// Функция прямого контроля положения органов управления
|
||
Task3_PWM Task3_Solution::full_control_mode(const float & a_Elevator_zad,
|
||
const float & a_Rudder_zad,
|
||
const float & a_Elerons_zad) {
|
||
// Структура которая содержит заданный RC сигнал
|
||
Task3_PWM _ans;
|
||
_ans.Elerons_PWM = map(a_Elerons_zad, ELERONS_DEG_MIN, ELERONS_DEG_MAX, ALERON_PWM_MIN, ALERON_PWM_MAX);
|
||
_ans.Elevator_PWM = map(a_Elevator_zad, ELEVATOR_DEG_MIN, ELEVATOR_DEG_MAX, ELEVATOR_PWM_MIN, ELEVATOR_PWM_MAX);
|
||
_ans.Rudder_PWM = map(a_Rudder_zad, RUDDER_DEG_MIN, RUDDER_DEG_MAX, RUDDER_PWM_MIN, RUDDER_PWM_MAX);
|
||
return _ans;
|
||
}
|
||
|
||
// Функция стабилизации, в зависимости от текущих углов
|
||
Task3_PWM Task3_Solution::stab_mode(double Pitch, double Roll) {
|
||
// Структура которая содержит заданный RC сигнал
|
||
Task3_PWM _ans;
|
||
double Roll_zad = -1.3 * Roll;
|
||
double Pitch_zad = 1.2 * Pitch;
|
||
double Head_zad = 0.1 * Roll;
|
||
|
||
// эталонка
|
||
_ans.Elerons_PWM = map(Roll_zad, ELERONS_DEG_MIN, ELERONS_DEG_MAX, ALERON_PWM_MIN, ALERON_PWM_MAX);
|
||
_ans.Rudder_PWM = map(Head_zad, RUDDER_DEG_MIN, RUDDER_DEG_MAX, RUDDER_PWM_MIN, RUDDER_PWM_MAX);
|
||
_ans.Elevator_PWM = map(Pitch_zad, ELEVATOR_DEG_MIN, ELEVATOR_DEG_MAX, ELEVATOR_PWM_MIN, ELEVATOR_PWM_MAX);
|
||
return _ans;
|
||
}
|
||
|
||
void Task3_Solution::init(HardwareSerial * a_mav_serial) {
|
||
_mav_serial = a_mav_serial;
|
||
// Запускаем обмен с Matek
|
||
init_base(_mav_serial);
|
||
}
|
||
|
||
void Task3_Solution::init_via_debug(HardwareSerial * a_mav_serial, HardwareSerial * a_debug) {
|
||
_mav_serial = a_mav_serial;
|
||
_debug = a_debug;
|
||
#ifndef SIM
|
||
// Запускаем обмен с Ardupilot
|
||
_debug -> println("Matek initialization start...");
|
||
init_base(_mav_serial);
|
||
#endif
|
||
}
|
||
|
||
void Task3_Solution::mav_rc_override(Task3_PWM a_pwm) {
|
||
mavlink_message_t msg;
|
||
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
||
|
||
mavlink_msg_rc_channels_override_pack(0xFF, 0xBE, & msg, 1, 1, roll, pitch, throttle, yaw, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
|
||
mavlink_msg_rc_channels_override_pack(0xFF, 0xBE, & msg, (uint8_t) 1, (uint8_t) 1, (uint16_t) a_pwm.Elerons_PWM, (uint16_t) a_pwm.Elevator_PWM, (uint16_t)(1100), (uint16_t) a_pwm.Rudder_PWM, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX);
|
||
|
||
uint16_t len = mavlink_msg_to_send_buffer(buf, & msg);
|
||
_mav_serial -> write(buf, len);
|
||
t_last_control = millis();
|
||
} |