[Plane] add omega stab (#104)

* Подключил либу PIReg

Добавил обработчик if для регулятора по угловой скорости

* Формирование управляющего сигнала

* Реализована стабилизация крена

* Добавлен режим стабилизации

Арминг на отдельном свитче
обычное управление нижнее положение тумблера А
стабилизация - среднее положение тумблера А
Отправка данных в любом режиме в юарт длина сообщения 100

* Добавил ФНЧ для ПВД

* Поменял лог и его скорость

ТЕперь отправляет getTick и весь лог шлется каждые 50 миллисекунд

* Исправил формат логирования

Исправил разделители для показаний гироскопа

* Минорные фиксы либы ПИ регуля

Убрал лишний инклюд
Добавил имена переменных в прототипы функций

* Минорные фиксы

Исправил 0 и 1 на flase и true

* Минорные фиксы либы ПИ рега

Объединил объявление и инициализацию переменной

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();

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

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

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

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 */