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