/****************************************************************************
* 名 称：key_action(void)
* 功 能：键盘处理函数
* 入口参数：无
* 出口参数：无
* 说 明：
* 调用方法：无
****************************************************************************/
void key_action(void)
{
if(i==24)
{
i=0;
}
else
{
i++;
point=&pic_shoot_name[i];
point_2=&pic_shoot_name_2[i];
printf("字符串为: %c\n",pic_shoot_name[i]);
printf("指针指向元素: %c\n",*point);
printf("字符串2为: %c\n",pic_shoot_name_2[i]);
printf("指针2指向元素: %c\n",*point_2);
LED2(ON);
Screen_shot(0, 0, 400,240 , point);
LED2(OFF);
LED3(ON);
Camera_shot(0, 0, 320,240 , point_2);
LED3(OFF);
}
}
/**
* @brief ������
* @param ��
* @retval ��
*/
int main(void)
{
/* ���� led */
LED_GPIO_Config();
/* ����exti�ж� */
EXTI_Key_Config();
/* ���ô���Ϊ�ж�ģʽ */
USART1_Config();
printf("\r\n Ұ��ISO-MINI�����壬˯��ʵ�� \r\n");
while(1)
{
/* wait interrupt */
LED1( ON ); // ��
Delay(0xFFFFF);
LED1( OFF ); // ��
LED2( ON ); // ��
Delay(0xFFFFF);
LED2( OFF ); // ��
LED3( ON ); // ��
Delay(0xFFFFF);
LED3( OFF ); // ��
__WFI(); //����˯��ģʽ���ȴ��жϻ���
}
}
static void process_recv_html(char * body)
{
char * find = strstr(body,"}");
if (find == NULL)
{
printf("ERROR\n");
return ;
}
find = find -1 ;
if (find[0] == '1' )
{
LED1(0);
LED2(0);
LED3(0);
printf("ON\n");
}
if (find[0] == '0' )
{
LED1(1);
LED2(1);
LED3(1); //PB8 为继电器控制管脚
printf("OFF\n");
}
//
}
/*----------------------------------------------------------------*/
void LED_Test(void)
{
/* LED 端口初始化 */
LED_GPIO_Config();
/* 方法2，使用固件库控制IO */
while (1)
{
LED1( ON ); // 亮
LED_Delay(0x0FFFFF);
LED1( OFF ); // 灭
LED2( ON ); // 亮
LED_Delay(0x0FFFFF);
LED2( OFF ); // 灭
LED3( ON ); // 亮
LED_Delay(0x0FFFFF);
LED3( OFF ); // 灭
}
}
void Task_LED(void *p_arg)
{
(void)p_arg; //'p_arg'没有用到，防止编译器警告
while(1)
{
LED1( ON );
OSTimeDlyHMSM(0,0,0,500);
LED1( OFF );
LED2( ON );
OSTimeDlyHMSM(0,0,0,500);
LED2( OFF );
LED3( ON );
OSTimeDlyHMSM(0,0,0,500);
LED3( OFF );
LED4( ON );
OSTimeDlyHMSM(0,0,0,500);
LED4( OFF );
}
}
// IO 线中断，中断口为PC13
void EXTI15_10_IRQHandler(void)
{
if(EXTI_GetITStatus(EXTI_Line13) != RESET) //确保是否产生了EXTI Line中断
{
/* 刚从停机唤醒，由于时钟未配置正确，
此printf语句的内容不能正常发送出去 */
printf("\r\n 进入中断 \r\n");
SYSCLKConfig_STOP(); //停机唤醒后需要启动HSE
LED1( ON ); LED2( ON ); LED3( ON ); //点亮所有LED一段时间指示停机唤醒
Delay(0xFFFFFF);
LED1( OFF ); LED2( OFF ); LED3( OFF );
/*由于前面已经重新启动了HSE，
所以本printf语句能正常发出 */
printf("\r\n 退出中断 \r\n");
EXTI_ClearITPendingBit(EXTI_Line13); //清除中断标志位
}
}
int main(void)
{
EXTI_PE5_Init();
KEY_Init();
LED_Init();
LED1(ON);
LED2(ON);
LED3(ON);
while(1)
{
;
}
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
LED_GPIO_Config();
SysTick_Init();
LED1(ON);
LED3(ON);
while (1)
{
LED2(ON);
Delay_us(50000);
LED2(OFF);
Delay_us(50000);
}
}
void UDP_Test(void)
{
struct udp_pcb *UdpPcb;
struct ip_addr ipaddr;
struct pbuf *p;
p = pbuf_alloc(PBUF_RAW,sizeof(UDPData),PBUF_RAM);
if(p==NULL)
return;
p->payload=(void *)UDPData;
UdpPcb=udp_new(); /* 建立UDP 通信的控制块(pcb) */
udp_bind(UdpPcb,IP_ADDR_ANY,8080); /* 绑定本地IP 地址 */
IP4_ADDR(&ipaddr,192, 168, 10, 100);
udp_sendto(UdpPcb,p,&ipaddr,8080); /* 将收到的数据再发送出去 */
udp_recv(UdpPcb,UDP_Receive,NULL); /* 设置数据接收时的回调函数 */
LED1(1);
LED2(1);
LED3(1);
}
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);
}
}
