//解锁
rt_err_t disarm()
{
Motor_Set(0, 0, 0, 0);
if (!armed)
return RT_EOK;
armed = RT_FALSE;
rt_kprintf("disarmed.\n");
excute_task("wait");
return RT_EOK;
}
//看门狗线程
void watchdog_entry(void *parameter)
{
while (1)
{
if (rt_sem_take(&watchdog, RT_TICK_PER_SECOND) != RT_EOK)
{
rt_kprintf("watchdog timeout!!!!!.\n");
while (1)
{
Motor_Set(0, 0, 0, 0);
disarm();
rt_thread_suspend(&control_thread);
}
}
if (armed && (abs(ahrs.degree_pitch) > 45.0f || abs(ahrs.degree_roll) > 45.0f))
{
Motor_Set(0, 0, 0, 0);
disarm();
rt_kprintf("degree out of range.\n");
}
}
}
void panic_ir(unsigned char val) {
filtered_ir[panic_sensor] = (val+filtered_ir[panic_sensor]*9)/10;
if (filtered_ir[panic_sensor] < 100 && filtered_sonar < 1750000) {
if (usemotors) Motor_Set(127,127);
ADC_Background_Read(&avoid_ir);
} else {
ADC_Single_Background_Read(panic_sensor,&panic_ir);
}
LED_Set(LED_2,c++);
if (c==0) UARTprintf("ir==>%d sonar==>%d sensor==>%d c=>%d\n",filtered_ir[panic_sensor],filtered_sonar,panic_sensor,c);
}
//光流定点模式
rt_err_t loiter_mode(u8 height)
{
const u16 basic_thought = 450;
if (check_safe(SAFE_ADNS3080))
return RT_EIO;
if (pwm.throttle > 0.3f && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40)
{
loiter(pos_X, pos_y, yaw_exp);
althold(height);
motor_hupdate(basic_thought);
}
else
Motor_Set(60, 60, 60, 60);
return RT_EOK;
}
//自稳模式
rt_err_t stable_mode(u8 var)
{
if (pwm.throttle > 0.05f && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40)
{
yaw_exp += pwm.yaw * 0.5f;
if (yaw_exp > 360.0f)
yaw_exp -= 360.0f;
if (yaw_exp < 0.0f)
yaw_exp += 360.0f;
stable(pwm.pitch * 30.0f, pwm.roll * 30.0f, yaw_exp);
motor_update(pwm.throttle * 1000);
}
else
Motor_Set(60, 60, 60, 60);
return RT_EOK;
}
//定高模式
rt_err_t althold_mode(u8 height)
{
const u16 basic_thought = 530;
if (pwm.throttle > 0.3f && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40)
{
yaw_exp += pwm.yaw * 0.5f;
if (yaw_exp > 360.0f)
yaw_exp -= 360.0f;
if (yaw_exp < 0.0f)
yaw_exp += 360.0f;
stable(pwm.pitch * 30.0f, pwm.roll * 30.0f, yaw_exp);
althold(height);
motor_hupdate(basic_thought);
}
else
Motor_Set(60, 60, 60, 60);
return RT_EOK;
}
void avoid_ir(unsigned char * adc) {
signed char offset;
for(c=0; c<7; c++)
filtered_ir[c] = (adc[c]+filtered_ir[c]*9)/10;
offset =
(filtered_ir[IR_FRONT_LEFT]- filtered_ir[IR_FRONT_RIGHT])/100 +
(filtered_ir[IR_LONG_LEFT] - filtered_ir[IR_LONG_RIGHT] )/50;
LED_Set(LED_0,offset+1);
LED_Set(LED_1,1-offset);
if (offset > 0)
Motor_Set(127,127-offset);
else
Motor_Set(127+offset,127);
if (filtered_ir[IR_FRONT] > 100 || Sonar_Value > 1750000) {
if (usemotors) {
if (filtered_ir[IR_FRONT_LEFT] > filtered_ir[IR_FRONT_RIGHT])
Motor_Set(-127,127);
else
Motor_Set(127,-127);
}
ADC_Single_Background_Read(panic_sensor=IR_FRONT,&panic_ir);
} else if (filtered_ir[IR_FRONT_LEFT] > 100) {
if (usemotors) Motor_Set(-127,127);
ADC_Single_Background_Read(panic_sensor=IR_FRONT_LEFT,&panic_ir);
} else if (filtered_ir[IR_FRONT_RIGHT] > 100) {
if (usemotors) Motor_Set(127,-127);
ADC_Single_Background_Read(panic_sensor=IR_FRONT_RIGHT,&panic_ir);
} else {
ADC_Background_Read(&avoid_ir);
}
LED_Set(LED_3,d++);
if (d==0) UARTprintf("off-->%d sonar-->%d d->%d\n",offset,filtered_sonar,d);
}
int main(void) {
LockoutProtection();
InitializeMCU();
//init uart
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
UARTStdioInit(0);
LED_Init();
Jumper_Init();
ADC_Init();
Sonar_Init();
usemotors = Jumper_Value & 0x8;
if ((Jumper_Value & 0x7) == 0x1) {
if (usemotors) {
Motor_Init(false,true);
Motor_Set(127,127);
}
avoid_sonar(0);
avoid_ir(filtered_ir);
for (;;);
}
if ((Jumper_Value & 0x7) == 0x2) {
Travel_Init(usemotors);
Travel_Go(FULL_SPEED);
for (;;);
}
if ((Jumper_Value & 0x7) == 0x3) {
if (usemotors) {
Motor_Init(false,true);
Motor_Set(127,127);
}
for (;;);
}
//if no jumpers are set, enter debug mode
Encoder_Init(true,false);
for (;;c++) {
ADC_Background_Read(0);
Sonar_Background_Read(0);
Encoder_Background_Read(0);
Jumper_Read();
UARTprintf("ADC[%3d %3d %3d %3d %3d %3d %3d %3d] S[%7d] E[%3d %3d] J[%1x] c:%d\n",
ADC_Values[0],ADC_Values[1],ADC_Values[2],ADC_Values[3],ADC_Values[4],ADC_Values[5],ADC_Values[6],ADC_Values[7],
Sonar_Value,
Encoder_Values[0],Encoder_Values[1],
Jumper_Value,
c
);
LED_Set(LED_0,c);
LED_Set(LED_1,c+64);
LED_Set(LED_2,c+128);
LED_Set(LED_3,c+192);
WaitUS(20000);
}
}
rt_err_t wait_mode(u8 var)
{
Motor_Set(0, 0, 0, 0);
return RT_EOK;
}
//断言失败时关闭电机
void assert_protect(const char *c1, const char *c2, rt_size_t size)
{
Motor_Set(0, 0, 0, 0);
disarm();
}
//硬件错误时关闭电机
rt_err_t hardfalt_protect(void *stack)
{
Motor_Set(0, 0, 0, 0);
disarm();
return RT_EOK;
}
//控制线程循环
void control_thread_entry(void *parameter)
{
LED2(5);
wait_dmp();
rt_kprintf("start control\n");
excute_task("wait");
while (1)
{
LED2(armed * 3);
receive_pwm(&pwm); //接受PWM信号
//若遥控器关闭则锁定
if (pwm.switch1 < 2 && pwm.switch1 != -1 && pwm.throttle < 0.05f && pwm.throttle >= 0.0f)
{
arm(SAFE_PWM);
}
/*
符合解锁条件则解锁
1.遥控器开关处于2档或3档 或者 两根遥控器屏蔽跳线接地
2.任务安全需求满足
*/
if ((((pwm.switch1 == 2 || pwm.switch1 == -1) && (GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_3) == Bit_RESET || GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_4) == Bit_RESET)) || check_safe(current_task->depend)))
{
disarm();
}
//得到dmp数据且解锁则执行任务
if (get_dmp() && armed)
{
//若地面站离线 遥控器在线则手动切换模式
if (check_safe(SAFE_PWM) == RT_EOK && check_safe(SAFE_TFCR) != RT_EOK)
{
if (current_task->id != 2 && pwm.switch1 == 1)
{
excute_task("stable");
}
if (current_task->id != 3 && pwm.switch1 == 0 && pwm.switch2 == 0)
{
excute_task("althold");
}
if (current_task->id != 4 && pwm.switch1 == 0 && pwm.switch2 == 1)
{
pos_X = ahrs.x;
pos_y = ahrs.y;
excute_task("loiter");
}
}
//执行任务循环
if (current_task->func(current_task->var) != RT_EOK)
disarm();
}
//若锁定则电机关闭
if (!armed)
Motor_Set(0, 0, 0, 0);
rt_sem_release(&watchdog);
rt_thread_delay(2);
}
}
/* Model step function */
void test_udp_step(void)
{
/* local block i/o variables */
real32_T rtb_ARDrone_Acc_Y;
real32_T rtb_ARDrone_Acc_Z;
real32_T rtb_ARDrone_Acc_X;
real32_T rtb_ARDrone_Magneto_X;
int32_T rtb_UDPM_Receive_Int32;
int32_T rtb_UDPM_Receive_Int32_h;
int32_T rtb_UDPM_Receive_Int32_n;
int32_T rtb_UDPM_Receive_Int32_a;
int32_T rtb_UDPM_Receive_Int32_i;
int32_T rtb_UDPM_Receive_Int32_l;
int32_T rtb_UDPM_Receive_Int32_ac;
int32_T rtb_DataTypeConversion2;
int32_T rtb_DataTypeConversion3;
int32_T rtb_DataTypeConversion12;
int32_T rtb_DataTypeConversion11;
int32_T rtb_DataTypeConversion9;
int32_T rtb_DataTypeConversion1;
int16_T rtb_Height;
int8_T rtb_DataTypeConversion3_h;
real32_T tmp;
/* S-Function (UDPM_Receive_Int32): '<S10>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2);
/* DataTypeConversion: '<S4>/Data Type Conversion' */
rtb_DataTypeConversion3_h = (int8_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_LED): '<S4>/ARDrone_LED' */
LED_Set( (int8_T)rtb_DataTypeConversion3_h, (uint8_T)test_udp_P.ARDrone_LED_p1);
/* S-Function (UDPM_Receive_Int32): '<S12>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_d);
/* DataTypeConversion: '<S4>/Data Type Conversion1' */
rtb_DataTypeConversion3_h = (int8_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_LED): '<S4>/ARDrone_LED1' */
LED_Set( (int8_T)rtb_DataTypeConversion3_h, (uint8_T)
test_udp_P.ARDrone_LED1_p1);
/* S-Function (UDPM_Receive_Int32): '<S13>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_f);
/* DataTypeConversion: '<S4>/Data Type Conversion2' */
rtb_DataTypeConversion3_h = (int8_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_LED): '<S4>/ARDrone_LED2' */
LED_Set( (int8_T)rtb_DataTypeConversion3_h, (uint8_T)
test_udp_P.ARDrone_LED2_p1);
/* S-Function (UDPM_Receive_Int32): '<S14>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_p);
/* DataTypeConversion: '<S4>/Data Type Conversion3' */
rtb_DataTypeConversion3_h = (int8_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_LED): '<S4>/ARDrone_LED3' */
LED_Set( (int8_T)rtb_DataTypeConversion3_h, (uint8_T)
test_udp_P.ARDrone_LED3_p1);
/* S-Function (UDPM_Receive_Int32): '<S7>/UDPM_Receive_Int32' */
rtb_DataTypeConversion1 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_j);
/* DataTypeConversion: '<S6>/Data Type Conversion' */
rtb_ARDrone_Acc_X = (real32_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_Motor): '<S6>/ARDrone_Motor' */
Motor_Set( (real32_T)rtb_ARDrone_Acc_X, (uint8_T)test_udp_P.ARDrone_Motor_p1);
/* DataTypeConversion: '<S6>/Data Type Conversion1' */
rtb_ARDrone_Acc_X = (real32_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_Motor): '<S6>/ARDrone_Motor1' */
Motor_Set( (real32_T)rtb_ARDrone_Acc_X, (uint8_T)test_udp_P.ARDrone_Motor1_p1);
/* DataTypeConversion: '<S6>/Data Type Conversion2' */
rtb_ARDrone_Acc_X = (real32_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_Motor): '<S6>/ARDrone_Motor2' */
Motor_Set( (real32_T)rtb_ARDrone_Acc_X, (uint8_T)test_udp_P.ARDrone_Motor2_p1);
/* DataTypeConversion: '<S6>/Data Type Conversion3' */
rtb_ARDrone_Acc_X = (real32_T)rtb_DataTypeConversion1;
/* S-Function (ARDrone_Motor): '<S6>/ARDrone_Motor3' */
Motor_Set( (real32_T)rtb_ARDrone_Acc_X, (uint8_T)test_udp_P.ARDrone_Motor3_p1);
/* S-Function (ARDrone_Acc_X): '<S1>/ARDrone_Acc_X' */
rtb_ARDrone_Acc_X = Accelero_Get_X();
/* S-Function (ARDrone_Acc_Y): '<S1>/ARDrone_Acc_Y' */
rtb_ARDrone_Acc_Y = Accelero_Get_Y();
/* S-Function (ARDrone_Acc_Z): '<S1>/ARDrone_Acc_Z' */
rtb_ARDrone_Acc_Z = Accelero_Get_Z();
/* S-Function (ARDrone_Gyro_X): '<S3>/ARDrone_Gyro_X' */
rtb_ARDrone_Magneto_X = Gyro_Get_X();
/* DataTypeConversion: '<Root>/Data Type Conversion1' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion1 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion1' */
/* S-Function (ARDrone_Gyro_Y): '<S3>/ARDrone_Gyro_Y' */
rtb_ARDrone_Magneto_X = Gyro_Get_Y();
/* DataTypeConversion: '<Root>/Data Type Conversion2' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion2 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion2' */
/* S-Function (ARDrone_Gyro_Z): '<S3>/ARDrone_Gyro_Z' */
rtb_ARDrone_Magneto_X = Gyro_Get_Z();
/* DataTypeConversion: '<Root>/Data Type Conversion3' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion3 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion3' */
/* DataTypeConversion: '<Root>/Data Type Conversion4' */
tmp = (real32_T)floor(rtb_ARDrone_Acc_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion12 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion4' */
/* DataTypeConversion: '<Root>/Data Type Conversion5' */
tmp = (real32_T)floor(rtb_ARDrone_Acc_Y);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion11 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion5' */
/* DataTypeConversion: '<Root>/Data Type Conversion6' */
tmp = (real32_T)floor(rtb_ARDrone_Acc_Z);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion9 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion6' */
/* S-Function (UDPM_Send_Int32): '<S7>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion11, (int8_T)
test_udp_P.UDPM_Send_Int32_p2);
/* S-Function (UDPM_Receive_Int32): '<S8>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32 = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_m);
/* S-Function (UDPM_Send_Int32): '<S8>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion9, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_j);
/* S-Function (UDPM_Send_Int32): '<S10>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion1, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_h);
/* S-Function (UDPM_Send_Int32): '<S12>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion2, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_p);
/* S-Function (UDPM_Send_Int32): '<S13>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion3, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_i);
/* S-Function (UDPM_Send_Int32): '<S14>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion12, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_js);
/* S-Function (UDPM_Receive_Int32): '<S9>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_h = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_g);
/* S-Function (UDPM_Send_Int32): '<S9>/UDPM_Send_Int32' incorporates:
* S-Function (ARDrone_Temperature): '<S2>/ARDrone_Temperature'
*/
udp_send_int32( (int32_T)Barometer_Get_Temperature(), (int8_T)
test_udp_P.UDPM_Send_Int32_p2_k);
/* S-Function (UDPM_Receive_Int32): '<S18>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_n = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_c);
/* S-Function (UDPM_Send_Int32): '<S18>/UDPM_Send_Int32' incorporates:
* S-Function (ARDrone_Pressure): '<S2>/ARDrone_Pressure'
*/
udp_send_int32( (int32_T)Barometer_Get_Pressure(), (int8_T)
test_udp_P.UDPM_Send_Int32_p2_o);
/* S-Function (ARDrone_Magneto_Z): '<S5>/ARDrone_Magneto_Z' */
rtb_ARDrone_Magneto_X = Magneto_Get_Z();
/* DataTypeConversion: '<Root>/Data Type Conversion10' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion9 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion10' */
/* S-Function (ARDrone_Magneto_Y): '<S5>/ARDrone_Magneto_Y' */
rtb_ARDrone_Magneto_X = Magneto_Get_Y();
/* DataTypeConversion: '<Root>/Data Type Conversion11' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion11 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion11' */
/* S-Function (ARDrone_Magneto_X): '<S5>/ARDrone_Magneto_X' */
rtb_ARDrone_Magneto_X = Magneto_Get_X();
/* DataTypeConversion: '<Root>/Data Type Conversion12' */
tmp = (real32_T)floor(rtb_ARDrone_Magneto_X);
if (rtIsNaNF(tmp) || rtIsInfF(tmp)) {
tmp = 0.0F;
} else {
tmp = (real32_T)fmod(tmp, 4.2949673E+9F);
}
rtb_DataTypeConversion12 = tmp < 0.0F ? -(int32_T)(uint32_T)-tmp : (int32_T)
(uint32_T)tmp;
/* End of DataTypeConversion: '<Root>/Data Type Conversion12' */
/* S-Function (UDPM_Receive_Int32): '<S11>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_a = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_a);
/* S-Function (UDPM_Send_Int32): '<S11>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion12, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_oz);
/* S-Function (UDPM_Receive_Int32): '<S15>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_i = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_gb);
/* S-Function (UDPM_Send_Int32): '<S15>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion11, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_n);
/* S-Function (UDPM_Receive_Int32): '<S16>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_l = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_dn);
/* S-Function (UDPM_Send_Int32): '<S16>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion9, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_d);
/* S-Function (ARDrone_Height): '<Root>/Height' */
rtb_Height = Height_Get();
/* DataTypeConversion: '<Root>/Data Type Conversion9' */
rtb_DataTypeConversion9 = rtb_Height;
/* S-Function (UDPM_Receive_Int32): '<S17>/UDPM_Receive_Int32' */
rtb_UDPM_Receive_Int32_ac = udp_recv_int32( (int8_T)
test_udp_P.UDPM_Receive_Int32_p2_pv);
/* S-Function (UDPM_Send_Int32): '<S17>/UDPM_Send_Int32' */
udp_send_int32( (int32_T)rtb_DataTypeConversion9, (int8_T)
test_udp_P.UDPM_Send_Int32_p2_kb);
}
void control_thread_entry(void* parameter)
{
float yaw_inc = 0;
float yaw_exp = 0;
u8 i;
u8 take_off = 0;
p_rate_pid.expect = 0;
r_rate_pid.expect = 0;
y_rate_pid.expect = 0;
p_angle_pid.expect = 0;
r_angle_pid.expect = 0;
LED2(5);
for (i = 0;i < 15;i++)
{
u8 j;
for (j = 0;j < 200;j++)
{
get_dmp();
rt_thread_delay(2);
}
}
rt_kprintf("start control\n");
while (1)
{
LED2(pwmcon * 3);
if (pwmcon)
{
if (PWM3_Time <= settings.th_max&&PWM3_Time >= settings.th_min)
throttle = (PWM3_Time - settings.th_min) * 1000 / (settings.th_max - settings.th_min);
else
throttle = 0;
if (PWM1_Time <= settings.roll_max&&PWM1_Time >= settings.roll_min)
{
roll = MoveAve_WMA(PWM1_Time, roll_ctl, 16) - settings.roll_mid;
if (roll > 5)
PID_SetTarget(&r_angle_pid, -roll / (float)(settings.roll_max - settings.roll_mid)*45.0f);
else if (roll < -5)
PID_SetTarget(&r_angle_pid, -roll
/ (float)(settings.roll_mid - settings.roll_min)*45.0f);
else
PID_SetTarget(&r_angle_pid, 0);
}
if (PWM2_Time <= settings.pitch_max&&PWM2_Time >= settings.pitch_min)
{
pitch = MoveAve_WMA(PWM2_Time, pitch_ctl, 16) - settings.pitch_mid;
if (pitch > 5)
PID_SetTarget(&p_angle_pid, -pitch
/ (float)(settings.pitch_max - settings.pitch_mid)*30.0f);
else if (pitch < -5)
PID_SetTarget(&p_angle_pid, -pitch
/ (float)(settings.pitch_mid - settings.pitch_min)*30.0f);
else
PID_SetTarget(&p_angle_pid, 0);
}
if (PWM4_Time <= settings.yaw_max&&PWM4_Time >= settings.yaw_min)
{
yaw_inc = MoveAve_WMA(PWM4_Time, yaw_ctl, 16) - settings.yaw_mid;
if (has_hmc5883)
{
if (yaw_inc > 5)
yaw_exp -= yaw_inc / (float)(settings.yaw_max - settings.yaw_mid)*0.5f;
else if (yaw_inc < -5)
yaw_exp -= yaw_inc / (float)(settings.yaw_mid - settings.yaw_min)*0.5f;
if (yaw_exp > 360.0f)yaw_exp -= 360.0f;
else if (yaw_exp < 0.0f)yaw_exp += 360.0f;
PID_SetTarget(&y_angle_pid, 0);
}
else
{
if (yaw > 5)
PID_SetTarget(&y_rate_pid, -yaw_inc
/ (float)(settings.yaw_max - settings.yaw_mid)*100.0f);
else if (yaw < -5)
PID_SetTarget(&y_rate_pid, -yaw_inc
/ (float)(settings.yaw_mid - settings.yaw_min)*100.0f);
else
PID_SetTarget(&y_rate_pid, 0);
}
}
if (!balence)
Motor_Set(throttle, throttle, throttle, throttle);
}
else if (PWM3_Time > settings.th_min&&PWM3_Time < settings.th_min + 40 &&
PWM5_Time < 1700 && PWM5_Time>500)
{
//set pwm middle
if (!pwmcon)
{
settings.roll_mid = PWM1_Time;
settings.pitch_mid = PWM2_Time;
settings.yaw_mid = PWM4_Time;
}
pwmcon = 1;
balence = 1;
p_rate_pid.iv = 0;
r_rate_pid.iv = 0;
y_rate_pid.iv = 0;
take_off = 1;
yaw_exp = ahrs.degree_yaw;
}
if (get_dmp() && balence)
{
rt_uint32_t dump;
if (throttle > 60 && abs(ahrs.degree_pitch) < 40 && abs(ahrs.degree_roll) < 40)
{
if (PWM5_Time < 1200 && PWM5_Time>800 && sonar_state)
{
if (rt_event_recv(&ahrs_event, AHRS_EVENT_SONAR, RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_NO, &dump) == RT_EOK)
{
PID_SetTarget(&h_pid, 60.0);
PID_xUpdate(&h_pid, sonar_h);
h_pid.out = RangeValue(h_pid.out, -200, 200);
}
LED4(4);
throttle = 500;
if (has_adns3080&& PWM7_Time > 1500)
{
if (!poscon)
{
poscon = 1;
pos_X = opx;
pos_y = opy;
}
if (rt_event_recv(&ahrs_event, AHRS_EVENT_ADNS3080, RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_NO, &dump) == RT_EOK)
{
PID_SetTarget(&x_d_pid, pos_X);
PID_SetTarget(&y_d_pid, pos_y);
PID_xUpdate(&x_d_pid, opx);
PID_xUpdate(&y_d_pid, opy);
PID_SetTarget(&x_v_pid, -RangeValue(x_d_pid.out, -10, 10));
PID_SetTarget(&y_v_pid, -RangeValue(y_d_pid.out, -10, 10));
PID_xUpdate(&x_v_pid, optc_dx);
PID_xUpdate(&y_v_pid, optc_dy);
}
LED4(2);
PID_SetTarget(&r_angle_pid, -RangeValue(x_v_pid.out, -10, 10));
PID_SetTarget(&p_angle_pid, +RangeValue(y_v_pid.out, -10, 10));
}
else
poscon = 0;
}
else
{
h_pid.out = 0;
LED4(0);
}
PID_xUpdate(&p_angle_pid, ahrs.degree_pitch);
PID_SetTarget(&p_rate_pid, -RangeValue(p_angle_pid.out, -80, 80));
PID_xUpdate(&p_rate_pid, ahrs.gryo_pitch);
PID_xUpdate(&r_angle_pid, ahrs.degree_roll);
PID_SetTarget(&r_rate_pid, -RangeValue(r_angle_pid.out, -80, 80));
PID_xUpdate(&r_rate_pid, ahrs.gryo_roll);
if (has_hmc5883)
{
if (rt_event_recv(&ahrs_event, AHRS_EVENT_HMC5883, RT_EVENT_FLAG_AND | RT_EVENT_FLAG_CLEAR, RT_WAITING_NO, &dump) == RT_EOK)
{
yaw = ahrs.degree_yaw - yaw_exp;
if (yaw > 180.0f)yaw -= 360.0f;
if (yaw < -180.0f)yaw += 360.0f;
PID_xUpdate(&y_angle_pid, yaw);
PID_SetTarget(&y_rate_pid, -RangeValue(y_angle_pid.out, -100, 100));
}
}
PID_xUpdate(&y_rate_pid, ahrs.gryo_yaw);
Motor_Set1(throttle - p_rate_pid.out - r_rate_pid.out + y_rate_pid.out - h_pid.out);
Motor_Set2(throttle - p_rate_pid.out + r_rate_pid.out - y_rate_pid.out - h_pid.out);
Motor_Set3(throttle + p_rate_pid.out - r_rate_pid.out - y_rate_pid.out - h_pid.out);
Motor_Set4(throttle + p_rate_pid.out + r_rate_pid.out + y_rate_pid.out - h_pid.out);
}
else
Motor_Set(60, 60, 60, 60);
}
else
LED4(0);
if (PWM5_Time > 1700)
{
Motor_Set(0, 0, 0, 0);
p_rate_pid.iv = 0;
r_rate_pid.iv = 0;
y_rate_pid.iv = 0;
balence = 0;
pwmcon = 0;
}
if (dmp_retry > 200)
{
Motor_Set(0, 0, 0, 0);
balence = 0;
pwmcon = 0;
LED3(2);
break;
}
if (en_out_pwm)
{
rt_kprintf("\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",
PWM1_Time, PWM2_Time, PWM3_Time, PWM4_Time,
PWM5_Time, PWM6_Time, PWM7_Time, PWM8_Time);
// rt_kprintf("\t%d\t%d\t%d\t%d\t%d\t%d\n",
// Motor1,Motor2,Motor3,
// Motor4,Motor5,Motor6);
}
rt_thread_delay(2);
}
}
