52 lines
1.4 KiB
C
52 lines
1.4 KiB
C
#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);
|
||
} |