void LED2_control(char *cmd,char state)
{
if(state)
LED2(ON);//非0表示LED2开启
else
LED2(OFF);
rt_kprintf("LD2控制线程发送邮件:%s",cmd);
rt_mb_send(&dfs_mb,(rt_uint32_t)cmd);//发送邮件
}
/**
* @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 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)
{
;
}
}
/*
* 函数名：main
* 描述 : "主机"的主函数
* 输入 ：无
* 输出 : 无
*/
int main(void)
{
LED_GPIO_Config();
TIM2_Config();
USART2_Config(115200);
LED1(OFF);
LED2(OFF);
Get_ChipID();
printf("USART2 Test OK!!\r\n");
printf("Chip ID is 0x%08X 0x%08X 0x%08X.\r\n",
ChipUniqueID[0],ChipUniqueID[1],ChipUniqueID[2]);
printf("Chip Size is %d KBytes.\r\n",(*(__IO uint32_t *)(0X1FFF7A22))&0xFFFF);
while(1)
{
}
}
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);
}
/**
* The task for generating button events, also does terminal and ping sending
*/
static void vButtonTask( int8_t *pvParameters )
{
uint8_t event;
uint8_t buttons = 0;
uint8_t s1_count = 0;
uint8_t s2_count = 0;
int16_t byte;
uint8_t i;
uint8_t channel;
pvParameters;
N710_BUTTON_INIT();
vTaskDelay( 200 / portTICK_RATE_MS );
stack_event = stack_service_init(NULL); /* Open socket for stack status message */
channel = mac_current_channel();
while (1)
{
if(gw_assoc_state == 0 && scan_active == 0)
{
LED1_OFF();
scan_active=1;
scan_start = xTaskGetTickCount();
gw_discover();
}
/* Sleep for 10 ms or received from UART */
byte = debug_read_blocking(10 / portTICK_RATE_MS);
if (byte != -1)
{
switch(byte)
{
case '1': /*send a button 1 event*/
event = 1;
xQueueSend( button_events, ( void * ) &event, (portTickType) 0 );
break;
case '2': /*send a button 2 event*/
event = 2;
xQueueSend( button_events, ( void * ) &event, (portTickType) 0 );
break;
case '\r':
debug("\r\n");
break;
case 'm':
{
sockaddr_t mac;
rf_mac_get(&mac);
debug("MAC: ");
debug_address(&mac);
debug("\r\n");
}
break;
case 'p':
if(ping_active == 0)
{
echo_result.count=0;
if(ping(NULL, &echo_result) == pdTRUE) /* Broadcast */
{
ping_start = xTaskGetTickCount();
ping_active = 2;
debug("Ping\r\n");
}
else
debug("No buffer.\r\n");
}
break;
case 'u':
if(ping_active == 0)
{
echo_result.count=0;
if(udp_echo(NULL, &echo_result) == pdTRUE)
{
ping_start = xTaskGetTickCount();
ping_active = 1;
debug("udp echo_req()\r\n");
}
else
debug("No buffer.\r\n");
}
break;
/*case 'C':
if (channel < 26) channel++;
channel++;
case 'c':
if (channel > 11) channel--;
mac_set_channel(channel);
debug("Channel: ");
debug_int(channel);
debug("\r\n");
break;*/
default:
debug_put(byte);
}
}
/* Read button (S1 and S2) status */
if (N710_BUTTON_1 == 1)
{
if (s1_count > 5)
{
event = 1;
if (s1_count >= 100) event |= 0x80; /*long keypress*/
xQueueSend( button_events, ( void * ) &event, (portTickType) 0 );
}
s1_count = 0;
}
else
{
if (s1_count < 100)
{
s1_count++;
}
}
if (N710_BUTTON_2 == 1)
{
if (s2_count > 5)
{
event = 2;
if (s2_count >= 100) event |= 0x80; /*long keypress*/
xQueueSend( button_events, ( void * ) &event, (portTickType) 0 );
}
s2_count = 0;
}
else
{
if (s2_count < 100)
{
s2_count++;
}
}
/*LED blinkers*/
if (led1_count)
{
led1_count--;
LED1_ON();
}
else
{
LED1_OFF();
}
if (led2_count)
{
led2_count--;
LED2_ON();
}
else
{
LED2_OFF();
}
buttons = 0;
if (LED1())
{
buttons |= 1;
}
if (LED2())
{
buttons |= 2;
}
ssi_sensor_update(3, (uint32_t) buttons);
/* ping response handling */
if ((xTaskGetTickCount() - ping_start)*portTICK_RATE_MS > 1000 && ping_active)
{
debug("Ping timeout.\r\n");
stop_ping();
if(echo_result.count)
{
debug("Response: \r\n");
for(i=0; i<echo_result.count; i++)
{
debug_address(&(echo_result.result[i].src));
debug(" ");
debug_int(echo_result.result[i].rssi);
debug(" dbm, ");
debug_int(echo_result.result[i].time);
debug(" ms\r\n");
vTaskDelay(4);
}
echo_result.count=0;
}
else
{
debug("No response.\r\n");
}
ping_active = 0;
}
/* stack events */
if(stack_event)
{
buffer_t *buffer = waiting_stack_event(10);
if(buffer)
{
switch (parse_event_message(buffer))
{
case BROKEN_LINK:
buffer->dst_sa.port = datasensor_address.port;
if(memcmp(&datasensor_address.address, &(buffer->dst_sa.address), 8) == 0)
{
gw_assoc_state = 0;
datasensor_address.addr_type = ADDR_NONE;
LED1_OFF();
}
break;
case ROUTER_DISCOVER_RESPONSE:
scan_active=0;
if(gw_assoc_state==0)
{
memcpy(datasensor_address.address, buffer->src_sa.address, 8);
datasensor_address.addr_type = buffer->src_sa.addr_type ;
gw_assoc_state=1;
LED1_ON();
}
break;
default:
break;
}
if(buffer)
{
socket_buffer_free(buffer);
buffer = 0;
}
}
} /*end stack events*/
if (scan_active == 1 && (xTaskGetTickCount() - scan_start)*portTICK_RATE_MS > 1000)
{
mac_gw_discover();
scan_start = xTaskGetTickCount();
}
} /*end main loop*/
}
//控制线程循环
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);
}
}
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);
}
}
