#include "RockingBar.h"
#include "Filtering.h"

//static uint32_t RockingBar_ADC_Value[ROCKING_BAR_CH][FILLTERING_NUM];
float RockingBar_Value[ROCKING_BAR_CH];
SqQuecu RockingBar_ADC_Value_0;
SqQuecu RockingBar_ADC_Value_1;
SqQuecu RockingBar_ADC_Value_2;
SqQuecu *P[ROCKING_BAR_CH]={&RockingBar_ADC_Value_0,&RockingBar_ADC_Value_1};
/*函数RockingBar_ADC_GetValue
*	功能：获取ADC转换的值存在数组中
*/
void RockingBarInit(void){
	InitQuecu(&RockingBar_ADC_Value_0);
	InitQuecu(&RockingBar_ADC_Value_1);
}


void RockingBar_ADC_GetValue(void){
	uint32_t sum=0;
	uint8_t num=0;
	uint32_t adcv=0;

	for(int i=0;i<ROCKING_BAR_CH;i++){
		HAL_ADC_Start(&hadc1);												//启动ADC转换
		HAL_ADC_PollForConversion(&hadc1,10);								//等待ADC转换完成
		if(HAL_IS_BIT_SET(HAL_ADC_GetState(&hadc1),HAL_ADC_STATE_REG_EOC))
			{
				adcv = HAL_ADC_GetValue(&hadc1);
				if(enQueue(P[i],adcv)==false)deQueue(P[i]);
				enQueue(P[i],adcv);
			}
			
		}
	HAL_ADC_Stop(&hadc1);
	
	for(int i=0;i<ROCKING_BAR_CH;i++){
		sum=0;
		num=0;
		for(int n=0;n<P[i]->size;n++){
			num+=1;
			sum += P[i]->data[n];
		}
		RockingBar_Value[i]=sum/num;
	}
}

void RockingBar_GetValue(void){
	RockingBar_ADC_GetValue();
	RockingBar_Value[0] = THRUST_MIN + ((THRUST_MAX-THRUST_MIN)/(4096))*(RockingBar_Value[0]-0);
	RockingBar_Value[1] = YAW_MIN + ((YAW_MAX-YAW_MIN)/(4096))*(RockingBar_Value[1]-0);
}