void sonar_thread_entry(void* parameter)
{
rt_kprintf("start sonar\n");
while(1)
{
GPIO_SetBits(GPIOE,GPIO_Pin_7);
rt_thread_delay(RT_TICK_PER_SECOND*10/1000);
GPIO_ResetBits(GPIOE,GPIO_Pin_7);
rt_thread_delay(RT_TICK_PER_SECOND*50/1000);
sonar_h=Moving_Median(PWM8_Time/58.0f*ahrs.g_z,sonar_avr,SAMPLE_COUNT);
ahrs.height_v=HV_A*(ahrs.height_v+ahrs.height_acc*0.06f)+(1.0f-HV_A)*((sonar_h-ahrs.height)/0.06f);
ahrs.height=sonar_h;
h=(u16)sonar_h;
sonar_state=sonar_h>3.0f&&sonar_h<150.0f;
ahrs_state.sonar=!sonar_state;
rt_event_send(&ahrs_event,AHRS_EVENT_SONAR);
}
}
void Ultra_Ctrl(float T,float thr)
{
float ultra_sp_tmp,ultra_dis_tmp;
exp_height_speed = ULTRA_SPEED *my_deathzoom_2(thr - 500,50)/200.0f; //+-ULTRA_SPEEDmm / s
exp_height_speed = LIMIT(exp_height_speed ,-ULTRA_SPEED,ULTRA_SPEED);
if( exp_height > ULTRA_MAX_HEIGHT )
{
if( exp_height_speed > 0 )
{
exp_height_speed = 0;
}
}
else if( exp_height < 20 )
{
if( exp_height_speed < 0 )
{
exp_height_speed = 0;
}
}
exp_height += exp_height_speed *T;
if( thr < 100 )
{
exp_height += ( 1 / ( 1 + 1 / ( 0.2f *3.14f *T ) ) ) *( -exp_height);
}
ultra_sp_tmp = Moving_Median(0,5,ultra_delta/T); //ultra_delta/T;
if( ultra_dis_tmp < 2000 )
{
if( ABS(ultra_sp_tmp) < 100 )
{
ultra_speed += ( 1 / ( 1 + 1 / ( 4 *3.14f *T ) ) ) * ( (float)(ultra_sp_tmp) - ultra_speed );
}
else
{
ultra_speed += ( 1 / ( 1 + 1 / ( 1.0f *3.14f *T ) ) ) * ( (float)(ultra_sp_tmp) - ultra_speed );
}
}
ultra_dis_tmp = Moving_Median(1,5,ultra_distance);
if( ultra_dis_tmp < 2000 )
{
if( ABS(ultra_dis_tmp - ultra_dis_lpf) < 100 )
{
ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 4.0f *3.14f *T ) ) ) *(ultra_dis_tmp - ultra_dis_lpf) ;
}
else if( ABS(ultra_dis_tmp - ultra_dis_lpf) < 200 )
{
ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 2.2f *3.14f *T ) ) ) *(ultra_dis_tmp- ultra_dis_lpf) ;
}
else if( ABS(ultra_dis_tmp - ultra_dis_lpf) < 400 )
{
ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 1.2f *3.14f *T ) ) ) *(ultra_dis_tmp- ultra_dis_lpf) ;
}
else
{
ultra_dis_lpf += ( 1 / ( 1 + 1 / ( 0.6f *3.14f *T ) ) ) *(ultra_dis_tmp- ultra_dis_lpf) ;
}
}
else
{
}
ultra_ctrl.err = ( ultra_pid.kp*(exp_height - ultra_dis_lpf) );
ultra_ctrl.err_i += ultra_pid.ki *ultra_ctrl.err *T;
ultra_ctrl.err_i = LIMIT(ultra_ctrl.err_i,-Thr_Weight *ULTRA_INT,Thr_Weight *ULTRA_INT);
ultra_ctrl.err_d = ultra_pid.kd *( 0.6f *(-wz_speed*T) + 0.4f *(ultra_ctrl.err - ultra_ctrl.err_old) );
ultra_ctrl.pid_out = ultra_ctrl.err + ultra_ctrl.err_i + ultra_ctrl.err_d;
ultra_ctrl.pid_out = LIMIT(ultra_ctrl.pid_out,-500,500);
ultra_ctrl_out = ultra_ctrl.pid_out;
ultra_ctrl.err_old = ultra_ctrl.err;
}
void NRF_DataAnl(void)// NRF 解码函数
{
int16_t RC_GET_TEMP[4];
float RC_GETR[4][MID_RC_GET];
u8 temp_key[7];
u8 temp;
u8 i=0,j,sum = 0;
for( i=0;i<31;i++)
sum += NRF24L01_RXDATA[i];
if(!(sum==NRF24L01_RXDATA[31]))
{i=0;
return;
}
if(!(NRF24L01_RXDATA[0]==0x8A))
{
i=0;
return;
}
if(NRF24L01_RXDATA[1]==0x8A)
{
data_rate++;
loss_nrf=0;
RC_GET_TEMP[0]= (vs16)(NRF24L01_RXDATA[3]<<8)|NRF24L01_RXDATA[4];
Rc_Get.THROTTLE= Moving_Median(25,3,(vs16)(NRF24L01_RXDATA[3]<<8)|NRF24L01_RXDATA[4]);
Rc_Get.YAW = Moving_Median(26,3,(vs16)(NRF24L01_RXDATA[5]<<8)|NRF24L01_RXDATA[6]);
Rc_Get.PITCH = Moving_Median(27,3,(vs16)(NRF24L01_RXDATA[7]<<8)|NRF24L01_RXDATA[8]);
Rc_Get.ROLL = Moving_Median(28,3,(vs16)(NRF24L01_RXDATA[9]<<8)|NRF24L01_RXDATA[10]);
RC_GET_TEMP[1]= (vs16)((NRF24L01_RXDATA[5]<<8)|NRF24L01_RXDATA[6])/3+1000;
RC_GET_TEMP[2]= (vs16)((NRF24L01_RXDATA[7]<<8)|NRF24L01_RXDATA[8])/3+1000;
RC_GET_TEMP[3]= (vs16)((NRF24L01_RXDATA[9]<<8)|NRF24L01_RXDATA[10])/3+1000;
RX_CH[AUX1r]=Rc_Get.AUX1 = (vs16)(NRF24L01_RXDATA[11]<<8)|NRF24L01_RXDATA[12];
RX_CH[AUX4r]=Rc_Get.AUX4 = (vs16)(NRF24L01_RXDATA[13]<<8)|NRF24L01_RXDATA[14];
RX_CH[AUX3r]=Rc_Get.AUX3 = (vs16)(NRF24L01_RXDATA[15]<<8)|NRF24L01_RXDATA[16];
RX_CH[AUX2r]=Rc_Get.AUX2 = (vs16)(NRF24L01_RXDATA[17]<<8)|NRF24L01_RXDATA[18];
Rc_Get.AUX5 = (vs16)(NRF24L01_RXDATA[19]<<8)|NRF24L01_RXDATA[20];
ctrl_angle_offset.x =(float)(Rc_Get.AUX1-500)/1000.*MAX_FIX_ANGLE*2;
ctrl_angle_offset.y =(float)(Rc_Get.AUX2-500)/1000.*MAX_FIX_ANGLE*2;
if(fabs(ctrl_angle_offset.x )<0.2) {ctrl_angle_offset.x =0;}
if(fabs(ctrl_angle_offset.y )<0.2) {ctrl_angle_offset.y =0;}
RX_CH[THRr]= Rc_Get.THROTTLE-RX_CH_FIX[THRr] ;
RX_CH[ROLr]= my_deathzoom_rc(Rc_Get.ROLL-RX_CH_FIX[ROLr],100) ;
RX_CH[PITr]= my_deathzoom_rc(Rc_Get.PITCH-RX_CH_FIX[PITr],100) ;
KEY_SEL[0]=(NRF24L01_RXDATA[21])&0x01;
KEY_SEL[1]=(NRF24L01_RXDATA[21]>>1)&0x01;
KEY_SEL[2]=(NRF24L01_RXDATA[21]>>2)&0x01;
KEY_SEL[3]=(NRF24L01_RXDATA[21]>>3)&0x01;
KEY[0]=(NRF24L01_RXDATA[22])&0x01;
KEY[1]=(NRF24L01_RXDATA[22]>>1)&0x01;
KEY[2]=(NRF24L01_RXDATA[22]>>2)&0x01;
KEY[3]=(NRF24L01_RXDATA[22]>>3)&0x01;
KEY[4]=(NRF24L01_RXDATA[22]>>4)&0x01;
KEY[5]=(NRF24L01_RXDATA[22]>>5)&0x01;
KEY[6]=(NRF24L01_RXDATA[22]>>6)&0x01;
KEY[7]=(NRF24L01_RXDATA[22]>>7)&0x01;
ypr_sb[0]=(int)(((NRF24L01_RXDATA[23]<<8)|NRF24L01_RXDATA[24]))/100.;
ypr_sb[1]=(int)(((NRF24L01_RXDATA[25]<<8)|NRF24L01_RXDATA[26]))/100.;
ypr_sb[2]=(int)(((NRF24L01_RXDATA[27]<<8)|NRF24L01_RXDATA[28]))/100.;
for(j=0;j<3;j++)
if(ypr_sb[j]>360)
ypr_sb[j]-=655.35;
if(!mode.yaw_imu_control)
RX_CH[YAWr]= Rc_Get.YAW-RX_CH_FIX[YAWr] ;
else{
if(fabs( ypr_sb[2])>32&&fabs(ypr_sb[1])<15)
RX_CH[YAWr]= limit_mine(ypr_sb[2]*10,500) +1500;
else
RX_CH[YAWr]=1500;
}
if(mode.dj_lock){
if(fabs( ypr_sb[1]-17)>40&&fabs(ypr_sb[2])<15)
{dj_angle_set+= (ypr_sb[1]-17)*0.008;
dj_angle_set=LIMIT(dj_angle_set,-12,12);
}
}
else
dj_angle_set=0;
//-------------------------------------------------------------------------------------------------------------------------
}
