国产精品乱码久久久久,裸戏床戏精品吻胸视频,亚洲精品中文一区二区三区

#include "sys.h"
#include "lib.h"
#include "myiic.h"
#include "mpu6050.h"
#include "ano.h"
#include "config.h"
#include "inertial_sensor.h"
#include "ahrs_dcm.h"
#include "ahrs_quat.h"
#include "attitude_control.h"
#include "rcin.h"
#include "motor.h"
//====================================================
//PPM
data uint16_t ppm_start[8];
data uint16_t ppm_value_temp[8];
data uint16_t ppm_value[8] = {2750,2750,1500,2750,1500,1500};
data uint8_t ppm_counter = 0;
bit ppm_ok = 0;
//====================================================
//
inertial_sensor_t ins;
ahrs_quat_t ahrs_quat;
attitude_control_t atti_ctrl;
rcin_t rcin_roll,rcin_pitch,rcin_thr,rcin_yaw,rcin_5,rcin_6;
motor_t apmotor;
//======================================================
uint8_t t1,t2;
void fast_loop(void);
void arm_check_loop(void);
void rc_loop(void);
//手動模式
void test_run()
{
atti_ctrl.rate_bf_target.x = rcin_roll.control_in;
atti_ctrl.rate_bf_target.y = rcin_pitch.control_in;
atti_ctrl.rate_bf_target.z = rcin_yaw.control_in*3;
apmotor.throttle_out = rcin_thr.control_in;
}
void setup()
{
inertial_sensor_init();
ahrs_quat_init();
attitude_control_init();
rcin_init();
motor_init();
}
void main()
{
gpio_init();
uart_init(115200);
timer1_init(); //10ms無中斷
IIC_Init();
mpu6050_init();
pwm_init();
pca_init();
timer0_init(); //2ms中斷
setup();
while(1)
{
while(!TF1);TF1=0; //100Hz
fast_loop();
if(ppm_ok) //50Hz
{
rc_loop(); //217us
}
if(++t1 == 10) //10Hz
{
t1 = 0;
arm_check_loop(); //解鎖檢測
}
// ANO_DT_Send_Sensor(ahrs_quat.omega.z*100,apmotor.yaw_out,ahrs_quat.omega.x*100,apmotor.roll_pwm,0,0,0,0,0);
// ANO_DT_Send_Status(rcin_roll.control_in,rcin_pitch.control_in,rcin_yaw.control_in,rcin_thr.control_in,0,0);
// ANO_DT_Send_Status(ahrs_quat.roll_sensor,ahrs_quat.pitch_sensor,ahrs_quat.yaw_sensor,0,0,0);
// ANO_DT_Send_Sensor(acceladc[0],acceladc[1],acceladc[2],gyroadc[0],gyroadc[1],gyroadc[2],0,0,0);
// ANO_DT_Send_Sensor(rcin_thr.radio_in,rcin_thr.control_in,apmotor.throttle_pwm,0,0,0,0,0,0);
ANO_DT_Send_Sensor(ins.gyro.x*100,ins.gyro.y*100,ins.gyro.z*100,0,0,0,0,0,0);
// ANO_DT_Send_MotoPWM(apmotor.motor_out[0],apmotor.motor_out[1],apmotor.motor_out[2],apmotor.motor_out[3],0,0,0,0);
}
}
void fast_loop()
{
ahrs_quat_update(); //更新姿態(tài)
rate_control_run(); //內環(huán)控制0.823ms->motor.roll_out,pitch_out,yaw_out
motor_output(); //動力輸出
stabilize_run(); //外環(huán)控制
}
void rc_loop()
{
if(ppm_ok)
{
rcin_set_control(&rcin_roll,ppm_value[0]); // -> radio_in -> control_in
rcin_set_control(&rcin_pitch,ppm_value[1]);
rcin_set_control(&rcin_thr,ppm_value[2]);
rcin_set_control(&rcin_yaw,ppm_value[3]);
ppm_ok = 0;
}
}
void arm_check_loop()
{
static int16_t arming_counter = 0;
int16_t tmp;
if (rcin_thr.control_in > 0) //油門非0，直接退出
{
arming_counter = 0;
return;
}
tmp = rcin_yaw.control_in; //方向通道值
if (tmp > 4000) //contol_in對radio_in做了映射，最大是4500
{
if (arming_counter < ARM_DELAY)
{
arming_counter++;
}
if (arming_counter == ARM_DELAY && !apmotor.armed) //檢測到解鎖動作2s
{
init_arm_motors();
arming_counter = 0; //解鎖成功
}
}
else if (tmp < -4000)//上鎖
{
if (arming_counter < DISARM_DELAY)
{
arming_counter++;
}
if (arming_counter == DISARM_DELAY && apmotor.armed)
{
init_disarm_motors();//上鎖
arming_counter = 0;
}
}
else//非解鎖或上鎖
{
arming_counter = 0;
}
}
#define CNT_PER_US 2.5
#define CNT_600US 600*CNT_PER_US //1500
#define CNT_1100US 1100*CNT_PER_US //2750
#define CNT_1500US 1600*CNT_PER_US //4000
#define CNT_2500US 2500*CNT_PER_US //2000+500
void PCA_isr() interrupt ISR_PCA using 1
{
if (CCF1)
{
CCF1 = 0;
if(PPM_Pin) //上升沿
{
CH = CL = 0;
}
else //下降沿
{
uint16_t value;
value = (CCAP1H<<8) + CCAP1L;
if(value > CNT_2500US)
{
ppm_value[0] = ppm_value_temp[0];
ppm_value[1] = ppm_value_temp[1];
ppm_value[2] = ppm_value_temp[2];
ppm_value[3] = ppm_value_temp[3];
……………………
…………限于本文篇幅余下代碼請從51黑下載附件…………

復制代碼

欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136