From f0c539831a2fc30212f1017a9ea5ec619af0ed8d Mon Sep 17 00:00:00 2001 From: NoMonsters <33801216+NoMonsters@users.noreply.github.com> Date: Wed, 28 Oct 2020 08:42:11 +0300 Subject: [PATCH] [Plane] add omega stab (#104) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Подключил либу PIReg Добавил обработчик if для регулятора по угловой скорости * Формирование управляющего сигнала * Реализована стабилизация крена * Добавлен режим стабилизации Арминг на отдельном свитче обычное управление нижнее положение тумблера А стабилизация - среднее положение тумблера А Отправка данных в любом режиме в юарт длина сообщения 100 * Добавил ФНЧ для ПВД * Поменял лог и его скорость ТЕперь отправляет getTick и весь лог шлется каждые 50 миллисекунд * Исправил формат логирования Исправил разделители для показаний гироскопа * Минорные фиксы либы ПИ регуля Убрал лишний инклюд Добавил имена переменных в прототипы функций * Минорные фиксы Исправил 0 и 1 на flase и true * Минорные фиксы либы ПИ рега Объединил объявление и инициализацию переменной --- Libraries/AirSpeed/MPXV7002.cpp | 10 +++- Libraries/AirSpeed/MPXV7002.h | 2 + Libraries/PIReg/PIReg.cpp | 37 +++++++++++++ Libraries/PIReg/PIReg.h | 22 ++++++++ Plane/Core/Src/main.cpp | 95 +++++++++++++++++++++++++++------ 5 files changed, 147 insertions(+), 19 deletions(-) create mode 100644 Libraries/PIReg/PIReg.cpp create mode 100644 Libraries/PIReg/PIReg.h diff --git a/Libraries/AirSpeed/MPXV7002.cpp b/Libraries/AirSpeed/MPXV7002.cpp index 7ac3fc1..fb042ed 100644 --- a/Libraries/AirSpeed/MPXV7002.cpp +++ b/Libraries/AirSpeed/MPXV7002.cpp @@ -7,13 +7,14 @@ MPXV7002::MPXV7002(ADC_HandleTypeDef hadc) rho = 1.225; // kg/m^3 k_adc_to_pressure = 0.001221; b_adc_to_pressure = -2.5; + this->adc_raw_prev = 0; } void MPXV7002::convertADC(void) { HAL_ADC_Start(&this->hadc); HAL_ADC_PollForConversion(&this->hadc,100); - + this->adc_raw = HAL_ADC_GetValue(&this->hadc); HAL_ADC_Stop(&this->hadc); @@ -42,7 +43,12 @@ void MPXV7002::calcAirSpeed(void) calcPressure(); this->airSpeed = sqrt(2*abs(this->pressure)/rho); } - +uint32_t MPXV7002::getFilteredADC(void) +{ + convertADC(); + this->adc_raw_prev = (1 - 0.1) *this->adc_raw_prev + 0.1*(this->adc_raw); + return this->adc_raw_prev; +} double MPXV7002::getAirSpeed(void) { calcAirSpeed(); diff --git a/Libraries/AirSpeed/MPXV7002.h b/Libraries/AirSpeed/MPXV7002.h index 72d4d35..cf24020 100644 --- a/Libraries/AirSpeed/MPXV7002.h +++ b/Libraries/AirSpeed/MPXV7002.h @@ -8,6 +8,7 @@ class MPXV7002 ADC_HandleTypeDef hadc; //pointer to ADC uint32_t adc_raw; + uint32_t adc_raw_prev; double pressure; double airSpeed; @@ -29,5 +30,6 @@ class MPXV7002 uint32_t getRawData(void); double getPressure(void); double getAirSpeed(void); + uint32_t getFilteredADC(void); }; #endif diff --git a/Libraries/PIReg/PIReg.cpp b/Libraries/PIReg/PIReg.cpp new file mode 100644 index 0000000..8ce9f5b --- /dev/null +++ b/Libraries/PIReg/PIReg.cpp @@ -0,0 +1,37 @@ +#include "PIReg.h" +#include "math.h" + +PIReg::PIReg(double k_int, double k_pr, int dt) +{ + this->k_int = k_int; + this->k_pr = k_pr; + this->dt = dt; + + this->integral = 0; + this->error = 0; + this->output = 0; +} + +void PIReg::setError(double error) +{ + this->error = error; +} + +void PIReg::calcOutput(void) +{ + double proportional = this->k_pr*this->error; + + this->integral += this->error*this->k_int; + + this->output = proportional+this->integral; +} + +double PIReg::getOutput(void) +{ + return this->output; +} + +void PIReg::integralReset(void) +{ + this->integral = 0; +} diff --git a/Libraries/PIReg/PIReg.h b/Libraries/PIReg/PIReg.h new file mode 100644 index 0000000..f3106c8 --- /dev/null +++ b/Libraries/PIReg/PIReg.h @@ -0,0 +1,22 @@ +#ifndef _PIREG_LIB_H_ +#define _PIREG_LIB_H_ + +class PIReg +{ +private: + double k_int; + double k_pr; + double dt; + double error; + double integral; + double output; + +public: + PIReg(double k_int, double k_pr, int dt); //constructor + void setError(double error); + void calcOutput(void); + double getOutput(void); + void integralReset(void); + +}; +#endif diff --git a/Plane/Core/Src/main.cpp b/Plane/Core/Src/main.cpp index db95d6a..9555d2e 100644 --- a/Plane/Core/Src/main.cpp +++ b/Plane/Core/Src/main.cpp @@ -29,6 +29,7 @@ #include "Baro/MS5611.h" #include "AirSpeed/MPXV7002.h" #include "Gyro/bno055.h" +#include "PIReg/PIReg.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -70,6 +71,7 @@ extern RcChannel thr_rc, elev_rc, ail_rc, rud_rc, switch_rc, slider_rc; uint32_t manage_UART_counter = 0; uint32_t manage_vertical_speed_counter = 0; +uint32_t manage_omega_counter = 0; const uint32_t every_second = 10000; const uint32_t every_millisecond = 10; @@ -99,32 +101,43 @@ void time_manager(TIM_HandleTypeDef *htim) { manage_UART_counter++; manage_vertical_speed_counter++; + manage_omega_counter++; } uint8_t Armed(Beeper* beeper) { static uint8_t arm_flag = 0; static uint8_t enter_once = 0; - if(switch_rc.matchMidValue() && (enter_once == 0)) + if(slider_rc.matchMaxValue() && (enter_once == false)) { arm_flag = 1; beeper->longBeep(); enter_once = 1; } - if((switch_rc.matchMinValue() || switch_rc.matchMaxValue()) && (enter_once == 1)) + if(slider_rc.matchMinValue() && (enter_once == true)) { - if(switch_rc.matchMaxValue()) - { - arm_flag = 0; - enter_once = 0; - return arm_flag; - } arm_flag = 0; beeper->longBeep(); enter_once = 0; } return arm_flag; } - +uint8_t Stab(PIReg* reg) +{ + static uint8_t stab_flag = 0; + static uint8_t enter_once = 0; + if(switch_rc.matchMidValue() && (enter_once == false)) + { + stab_flag = 1; + enter_once = 1; + } + if((switch_rc.matchMinValue() || switch_rc.matchMaxValue()) && (enter_once == true)) + { + stab_flag = 0; + reg->integralReset(); + enter_once = 0; + } + return stab_flag; +} /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ @@ -220,6 +233,14 @@ int main(void) bno055.setOperationModeNDOF(); //--------------------------------------------------------- + //------------------Regulators INIT------------------------ + + int k_int_omega_x = 1.5; + int k_pr_omega_x = 0.1; + PIReg omega_x_PI_reg(k_int_omega_x,k_pr_omega_x,0.01); + + //--------------------------------------------------------- + /* USER CODE END 2 */ /* Infinite loop */ @@ -235,30 +256,69 @@ int main(void) rud_servo.setPositionMicroSeconds(rud_rc.getPulseWidth()); altitude = ms5611.getRawAltitude(); - voltageAirSpeed = mpxv7002.getRawData(); + voltageAirSpeed = mpxv7002.getFilteredADC(); v = bno055.getVectorGyroscopeRemap(); //отправка данных: - if(manage_UART_counter >= (100*every_millisecond)) + if(manage_UART_counter >= (50*every_millisecond)) { - //HAL_UART_Transmit(&huart2, (uint8_t*)str, sprintf(str, "alt=%d air_speed=%d ", (int) (altitude * 100), (int) (voltageAirSpeed)), 1000); - sprintf(str, "%d %d %d\n", (int) v.x*10, (int) v.y*10, (int) v.z*10); - HAL_UART_Transmit(&huart2, (uint8_t*)str, sizeof(str), 1000); + sprintf(str, "t=%d;mode=arm;gyr=%d-%d-%d;alt=%d;air=%d", HAL_GetTick(), (int) v.x*10, (int) v.y*10, (int) v.z*10, (int) (altitude*100), voltageAirSpeed); + HAL_UART_Transmit(&huart2, (uint8_t*)str, 100, 1000); manage_UART_counter = 0; } if(manage_vertical_speed_counter >= (10*every_millisecond)) { - //HAL_UART_Transmit(&huart2, (uint8_t*)str, sprintf(str, "Hello\n"), 1000); manage_vertical_speed_counter = 0; } - } + while(Stab(&omega_x_PI_reg)) + { + thr_servo.setPositionMicroSeconds(thr_rc.getPulseWidth()); + elev_servo.setPositionMicroSeconds(elev_rc.getPulseWidthDif()); + ail_servo_1.setPositionMicroSeconds((int)(1500+0.4*omega_x_PI_reg.getOutput())); + ail_servo_2.setPositionMicroSeconds((int)(1500+0.4*omega_x_PI_reg.getOutput())); + rud_servo.setPositionMicroSeconds(rud_rc.getPulseWidth()); + + altitude = ms5611.getRawAltitude(); + voltageAirSpeed = mpxv7002.getFilteredADC(); + v = bno055.getVectorGyroscopeRemap(); + + //отправка данных: + if(manage_UART_counter >= (50*every_millisecond)) + { + sprintf(str, "t=%d;mode=stab;gyr=%d-%d-%d;alt=%d;air=%d", HAL_GetTick(), (int) v.x*10, (int) v.y*10, (int) v.z*10, (int) (altitude*100), voltageAirSpeed); HAL_UART_Transmit(&huart2, (uint8_t*)str, 100, 1000); + manage_UART_counter = 0; + } + if(manage_vertical_speed_counter >= (10*every_millisecond)) + { + manage_vertical_speed_counter = 0; + } + if(manage_omega_counter >= (10*every_millisecond)) + { + omega_x_PI_reg.setError(0.0-v.x); + omega_x_PI_reg.calcOutput(); + manage_omega_counter = 0; + } + } + } elev_servo.setPositionMicroSeconds(elev_rc.getPulseWidthDif()); ail_servo_1.setPositionMicroSeconds(ail_rc.getPulseWidthDif()); ail_servo_2.setPositionMicroSeconds(ail_rc.getPulseWidthDif()); rud_servo.setPositionMicroSeconds(rud_rc.getPulseWidth()); thr_servo.setPositionMicroSeconds(thr_rc.getChannelMinWidth()); + altitude = ms5611.getRawAltitude(); + voltageAirSpeed = mpxv7002.getFilteredADC(); + v = bno055.getVectorGyroscopeRemap(); + + if(manage_UART_counter >= (50*every_millisecond)) + { + sprintf(str, "t=%d;mode=darm;gyr=%d-%d-%d;alt=%d;air=%d", HAL_GetTick(), (int) v.x*10, (int) v.y*10, (int) v.z*10, (int) (altitude*100), voltageAirSpeed); + HAL_UART_Transmit(&huart2, (uint8_t*)str, 100, 1000); + manage_UART_counter = 0; + } + + #ifdef SERVO_DEBUG_UART HAL_UART_Transmit(&huart2, (uint8_t*)str, sprintf(str, "%d ", thr_rc.getPulseWidth()), 1000); HAL_UART_Transmit(&huart2, (uint8_t*)str, sprintf(str, "%d ", elev_rc.getPulseWidth()), 1000); @@ -269,7 +329,8 @@ int main(void) HAL_Delay(500); #endif #ifdef SENSOR_DEBUG_UART - HAL_UART_Transmit(&huart2, (uint8_t*)str, sprintf(str, "alt=%d air_speed=%d\n", (int) (altitude * 100), (int) (voltageAirSpeed)), 1000); + sprintf(str, "gyr=%d, %d, %d, alt=%d, air=%d\n", (int) v.x*10, (int) v.y*10, (int) v.z*10, (int) (altitude*100), voltageAirSpeed); + HAL_UART_Transmit(&huart2, (uint8_t*)str, 100, 1000); HAL_Delay(500); #endif /* USER CODE END WHILE */