int lpc_emac_hw_init(void)
{
rt_event_init(&tx_event, "tx_event", RT_IPC_FLAG_FIFO);
rt_sem_init(&sem_lock, "eth_lock", 1, RT_IPC_FLAG_FIFO);
/* set autonegotiation mode */
lpc_emac_device.phy_mode = EMAC_PHY_AUTO;
// OUI 00-60-37 NXP Semiconductors
lpc_emac_device.dev_addr[0] = 0x00;
lpc_emac_device.dev_addr[1] = 0x60;
lpc_emac_device.dev_addr[2] = 0x37;
/* set mac address: (only for test) */
lpc_emac_device.dev_addr[3] = 0x12;
lpc_emac_device.dev_addr[4] = 0x34;
lpc_emac_device.dev_addr[5] = 0x56;
lpc_emac_device.parent.parent.init = lpc_emac_init;
lpc_emac_device.parent.parent.open = lpc_emac_open;
lpc_emac_device.parent.parent.close = lpc_emac_close;
lpc_emac_device.parent.parent.read = lpc_emac_read;
lpc_emac_device.parent.parent.write = lpc_emac_write;
lpc_emac_device.parent.parent.control = lpc_emac_control;
lpc_emac_device.parent.parent.user_data = RT_NULL;
lpc_emac_device.parent.eth_rx = lpc_emac_rx;
lpc_emac_device.parent.eth_tx = lpc_emac_tx;
eth_device_init(&(lpc_emac_device.parent), "e0");
return 0;
}
void rt_can_thread_entry(void *parameter)
{
struct rt_can_msg msg;
struct can_app_struct *canpara = (struct can_app_struct *) parameter;
rt_device_t candev;
rt_uint32_t e;
candev = rt_device_find(canpara->name);
RT_ASSERT(candev);
rt_event_init(&canpara->event, canpara->name, RT_IPC_FLAG_FIFO);
rt_device_open(candev, (RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_RX | RT_DEVICE_FLAG_INT_TX));
rt_device_control(candev, RT_CAN_CMD_SET_FILTER, canpara->filter);
while (1)
{
if (
rt_event_recv(&canpara->event,
((1 << canpara->filter->items[0].hdr) |
(1 << canpara->filter->items[1].hdr) |
(1 << canpara->filter->items[2].hdr) |
(1 << canpara->filter->items[3].hdr)),
canpara->eventopt,
RT_WAITING_FOREVER, &e) != RT_EOK
)
{
continue;
}
if (e & (1 << canpara->filter->items[0].hdr))
{
msg.hdr = canpara->filter->items[0].hdr;
while (rt_device_read(candev, 0, &msg, sizeof(msg)) == sizeof(msg))
{
rt_device_write(candev, 0, &msg, sizeof(msg));
}
}
if (e & (1 << canpara->filter->items[1].hdr))
{
msg.hdr = canpara->filter->items[1].hdr;
while (rt_device_read(candev, 0, &msg, sizeof(msg)) == sizeof(msg))
{
rt_device_write(candev, 0, &msg, sizeof(msg));
}
}
if (e & (1 << canpara->filter->items[2].hdr))
{
msg.hdr = canpara->filter->items[2].hdr;
while (rt_device_read(candev, 0, &msg, sizeof(msg)) == sizeof(msg))
{
rt_device_write(candev, 0, &msg, sizeof(msg));
}
}
if (e & (1 << canpara->filter->items[3].hdr))
{
msg.hdr = canpara->filter->items[3].hdr;
while (rt_device_read(candev, 0, &msg, sizeof(msg)) == sizeof(msg))
{
rt_device_write(candev, 0, &msg, sizeof(msg));
}
}
}
}
void
jffs2_start_garbage_collect_thread(struct jffs2_sb_info *c)
{
struct super_block *sb=OFNI_BS_2SFFJ(c);
cyg_mtab_entry *mte;
int result;
RT_ASSERT(c);
//RT_ASSERT(!sb->s_gc_thread_handle);
mte=(cyg_dir *) sb->s_root;
RT_ASSERT(mte);
rt_event_init(&sb->s_gc_thread_flags, "gc_event", RT_IPC_FLAG_FIFO);
rt_mutex_init(&sb->s_lock, "gc_mutex", RT_IPC_FLAG_FIFO);
// rt_mutex_init(&mte->fs->syncmode, "fs_lock", RT_IPC_FLAG_FIFO);
D1(printk("jffs2_start_garbage_collect_thread\n"));
/* Start the thread. Doesn't matter if it fails -- it's only an
* optimisation anyway */
result = rt_thread_init(&sb->s_gc_thread,
"jffs2_gc_thread",
jffs2_garbage_collect_thread,
(void *)c,
(void*)sb->s_gc_thread_stack,
sizeof(sb->s_gc_thread_stack),
CYGNUM_JFFS2_GC_THREAD_PRIORITY,
CYGNUM_JFFS2_GC_THREAD_TICKS
);
if (result != RT_EOK) {
rt_thread_startup(&sb->s_gc_thread);
/* how to deal with the following filed? */
/* sb->s_gc_thread_handle; */
}
}
void rt_task4_thread_entry(void* parameter)
{
extern void start_transmit(void);
extern void stop_transmit(void);
extern uint8_t check_parameter(void);
// extern uint8_t self_check(void);
// extern void cpu_usage_init();
rt_uint32_t ev;
if((GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_3) == 0))
{
coil.usRegCoilBuf |= M(0);
}
else
{
coil.usRegCoilBuf &= ~M(0);
}
rt_event_init(&key_event, "key_event", RT_IPC_FLAG_FIFO );
while(1)
{
if( rt_event_recv( &key_event, FOOT_PUPD | KEY_PUPD, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, RT_WAITING_FOREVER, &ev ) == RT_EOK )
{
rt_thread_delay(6);
if( (ev & FOOT_PUPD) && (coil.usRegCoilBuf & M(0)) &&(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == 0)&& \
(check_parameter()) && (coil.usRegCoilBuf & M(7)))
{
start_transmit();
}
else
{
stop_transmit();
}
if(ev & KEY_PUPD)
{
if((GPIO_ReadInputDataBit(GPIOD, GPIO_Pin_3) == 0))
{
coil.usRegCoilBuf |= M(0);
}
else
{
coil.usRegCoilBuf &= ~M(0);
}
}
}
rt_event_recv( &key_event, FOOT_PUPD | KEY_PUPD, RT_EVENT_FLAG_OR | RT_EVENT_FLAG_CLEAR, 2, &ev );
rt_thread_delay(20);
}
}
int rt_application_init()
{
rt_thread_t thread_h;
rt_err_t result;
/* init system data cfg table */
init_syscfgdata_tbl();
rt_sem_init(&print_sem, "prtsem", 1, RT_IPC_FLAG_PRIO);
#if RT_USING_FILESYSTEM
rt_sem_init(&spiflash_sem, "sfsem", 1, RT_IPC_FLAG_PRIO);
#endif
#if (ADE7880_SPIFLASH_SHARE_SPI || ADE7880_SI4432_SHARE_SPI)
rt_sem_init(&spi_sem_share_by_ade7880, "spi3sem", 1, RT_IPC_FLAG_PRIO);
#endif
#if RT_USING_ADE7880 || RT_USING_SI4432_MAC
rt_event_init(& isr_event_set, "isr_eve", RT_IPC_FLAG_PRIO);
#endif
#if (RT_USING_RS485_BUS && !TEST_485)
init_sys_485();
#endif
RT_APPS_INIT_DEBUG(("func:%s(), will init rt_sys_misc_entry\n", __FUNCTION__));
/* init led thread */
result = rt_thread_init(&misc_thread, "misc", rt_sys_misc_entry,
RT_NULL, (rt_uint8_t*)&misc_stack[0], sizeof(misc_stack), 0x19, 5);
if (result == RT_EOK)
rt_thread_startup(&misc_thread);
#if RT_USING_ADE7880 || RT_USING_SI4432_MAC
thread_h = rt_thread_create("isr_e", rt_isr_event_entry, RT_NULL, 512, ISR_EVENT_THREAD_PRIORITY, 10);
if (thread_h != RT_NULL)
rt_thread_startup(thread_h);
#endif
#if (RT_THREAD_PRIORITY_MAX == 32)
thread_h = rt_thread_create("init", rt_init_thread_entry, RT_NULL, 2048, 8, 20);
#else
thread_h = rt_thread_create("init", rt_init_thread_entry, RT_NULL, 2048, 80, 20);
#endif
if (thread_h != RT_NULL)
rt_thread_startup(thread_h);
RT_APPS_INIT_DEBUG(("func:%s(), base thread initial over\n", __FUNCTION__));
return 0;
}
int rt_application_init()
{
rt_err_t result;
/******************初始化usart2 留出uart2作为WIFI接口并*******************/
uart2_config(115200);
/******************usart2初始化结束***************/
/******************初始化信号量和邮箱**************/
rt_mb_init(&mb,"mb",&mailpool[0],sizeof(mailpool)/4,RT_IPC_FLAG_FIFO);//按照先入先出的原则，申请了一个可以存放32封邮件的邮箱
rt_mb_init(&dfs_mb,"dfs_mb",&dfs_mailpool[0],sizeof(dfs_mailpool)/4,RT_IPC_FLAG_FIFO);
result = rt_event_init(&event, "event", RT_IPC_FLAG_FIFO); //初始化事件机制 用于mp3播放
if(result != RT_EOK)
{
rt_kprintf("init event failed.\r\n");
return-1;
}
/***************************************************/
/*初始化LED1 时间统计线程*/
result = rt_thread_init(&led1_thread,"led1",led1_thread_entry,RT_NULL,&led1_stack[0],sizeof(led1_stack),7,3);
if (result == RT_EOK)
{
rt_thread_startup(&led1_thread);
}
/*初始化LED3 红外收发线程*/
result = rt_thread_init(&led3_thread, "led3",led3_thread_entry,RT_NULL,&led3_stack[0],sizeof(led3_stack), 5,3 );
/*如果初始化LED1线程成功*/
if (result == RT_EOK)
{ /*启动LED1线程*/
rt_thread_startup(&led3_thread);
}
/***************初始化DFS线程***************************/
result=rt_thread_init(&dfs_thread, "dfs",dfs_thread_entry, RT_NULL,&dfs_stack[0], sizeof(dfs_stack),6,20);
if (result==RT_EOK)
{
rt_thread_startup(&dfs_thread);
}
/***************初始化LD3320播放线程***************************/
result=rt_thread_init(&LD_thread,"ld_t",LD_thread_entry,RT_NULL,&LD_stack[0], sizeof(LD_stack), 4,50);
if (result==RT_EOK)
{
rt_thread_startup(&LD_thread);
}
return 0;
}
int cc1101_init()
{
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
rt_bool_t status = RT_FALSE;
rt_event_init(&cc1101_event, "cc1101_event", RT_IPC_FLAG_FIFO );
spi_init();
cc1101_hw_init();
/* cc1101 int init
* */
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
GPIO_Init(GPIOC, &GPIO_InitStructure);
#if HW
GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource4);
/* Configure the SPI interrupt priority */
NVIC_InitStructure.NVIC_IRQChannel = EXTI4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
EXTI_InitStructure.EXTI_Line = EXTI_Line4;
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTI_InitStructure);
/* Clear DM9000A EXTI line pending bit */
EXTI_ClearITPendingBit(EXTI_Line4);
#endif
return RT_TRUE;
}
void lpc17xx_emac_hw_init(void)
{
rt_event_init(&tx_event, "tx_event", RT_IPC_FLAG_FIFO);
rt_sem_init(&sem_lock, "eth_lock", 1, RT_IPC_FLAG_FIFO);
/* set autonegotiation mode */
lpc17xx_emac_device.phy_mode = EMAC_PHY_AUTO;
// OUI 00-60-37 NXP Semiconductors
lpc17xx_emac_device.dev_addr[0] = 0x00;
lpc17xx_emac_device.dev_addr[1] = 0x60;
lpc17xx_emac_device.dev_addr[2] = 0x37;
/* set mac address: (only for test) */
#if 1
{
unsigned int tmpmac = devguid[0];
int i = 0 ;
for(i = 1 ; i < 4 ;i++){
tmpmac ^= devguid[i];
}
rt_memcpy(lpc17xx_emac_device.dev_addr+3,&tmpmac,3);
}
#else
lpc17xx_emac_device.dev_addr[3] = 0x12;
lpc17xx_emac_device.dev_addr[4] = 0x34;
lpc17xx_emac_device.dev_addr[5] = 0x56;
#endif
lpc17xx_emac_device.parent.parent.init = lpc17xx_emac_init;
lpc17xx_emac_device.parent.parent.open = lpc17xx_emac_open;
lpc17xx_emac_device.parent.parent.close = lpc17xx_emac_close;
lpc17xx_emac_device.parent.parent.read = lpc17xx_emac_read;
lpc17xx_emac_device.parent.parent.write = lpc17xx_emac_write;
lpc17xx_emac_device.parent.parent.control = lpc17xx_emac_control;
lpc17xx_emac_device.parent.parent.user_data = RT_NULL;
lpc17xx_emac_device.parent.eth_rx = lpc17xx_emac_rx;
lpc17xx_emac_device.parent.eth_tx = lpc17xx_emac_tx;
eth_device_init(&(lpc17xx_emac_device.parent), "e0");
}
/*GPRS端口初始化，打开设备，注册回调函数*/
rt_bool_t sysconfig(void)
{
gprs_device = rt_device_find("uart3");
if (gprs_device != RT_NULL)
{
rt_kprintf("\r\n GPRS port initialized!\r\n");
GPRSPortConfig();
/* 设置回调函数及打开设备*/
rt_device_set_rx_indicate(gprs_device, uart_input);
rt_device_open(gprs_device, RT_DEVICE_OFLAG_RDWR);
}
else
{
rt_kprintf("\r\n GPRS port not find !\r\n");
return RT_FALSE;
}
/*GPRS串口打开后，初始化串口接收事件*/
rt_event_init(&rev_event, "rev_ev", RT_IPC_FLAG_FIFO);
return RT_TRUE;
}
void I2C1_INIT()
{
GPIO_InitTypeDef GPIO_InitStructure;
I2C_InitTypeDef I2C_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
if( i2c1_init_flag == 0 )
{
/* Enable the I2C clock */
RCC_APB1PeriphClockCmd(I2C1_CLK, ENABLE);
/* GPIOB clock enable */
RCC_AHB1PeriphClockCmd(I2C1_GPIO_CLK, ENABLE);
/* Enable the DMA1 clock */
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
//Reset GPIO
GPIO_InitStructure.GPIO_Pin = I2C1_SDA_PIN | I2C1_SCL_PIN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(I2C1_GPIO_PORT, &GPIO_InitStructure);
/* Connect PXx to I2C_SCL*/
GPIO_PinAFConfig(I2C1_GPIO_PORT, I2C1_SDA_SOURCE, GPIO_AF_I2C1);
/* Connect PXx to I2C_SDA*/
GPIO_PinAFConfig(I2C1_GPIO_PORT, I2C1_SCL_SOURCE, GPIO_AF_I2C1);
/* Enable I2C1 reset state */
RCC_APB1PeriphResetCmd(I2C1_CLK, ENABLE);
/* Release I2C1 from reset state */
RCC_APB1PeriphResetCmd(I2C1_CLK, DISABLE);
I2C_DeInit(I2C1);
I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;
I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;
I2C_InitStructure.I2C_OwnAddress1 = OwnAddress1;
I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_ClockSpeed = ClockSpeed;
I2C_Init(I2C1, &I2C_InitStructure);
I2C_Cmd(I2C1, ENABLE);
/* Configure and enable I2C1 event interrupt -------------------------------*/
NVIC_InitStructure.NVIC_IRQChannel = I2C1_EV_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* Configure and enable I2C1 DMA interrupt -------------------------------*/
NVIC_InitStructure.NVIC_IRQChannel = I2C1_DMA_TX_IRQn;
NVIC_Init(&NVIC_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = I2C1_DMA_RX_IRQn;
NVIC_Init(&NVIC_InitStructure);
/* Configure and enable I2C1 error interrupt -------------------------------*/
NVIC_InitStructure.NVIC_IRQChannel = I2C1_ER_IRQn;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_Init(&NVIC_InitStructure);
/* I2C1 TX DMA Channel configuration */
DMA_Cmd(I2C1_DMA_CHANNEL_TX, DISABLE);
DMA_DeInit(I2C1_DMA_CHANNEL_TX);
I2CDMA_InitStructure.DMA_Channel = DMA_Channel_1;
I2CDMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)I2C1_DR_Address;
I2CDMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)0; /* This parameter will be configured durig communication */
I2CDMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory; /* This parameter will be configured durig communication */
I2CDMA_InitStructure.DMA_BufferSize = 0xFFFF; /* This parameter will be configured durig communication */
I2CDMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
I2CDMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
I2CDMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
I2CDMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
I2CDMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
I2CDMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
//I2CDMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
I2CDMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
I2CDMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
I2CDMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
I2CDMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_Init(I2C1_DMA_CHANNEL_TX, &I2CDMA_InitStructure);
/* I2C1 RX DMA Channel configuration */
DMA_Cmd(I2C1_DMA_CHANNEL_RX, DISABLE);
DMA_DeInit(I2C1_DMA_CHANNEL_RX);
DMA_Init(I2C1_DMA_CHANNEL_RX, &I2CDMA_InitStructure);
//I2C_AcknowledgePolling(I2C1, 0x70);
rt_event_init(&i2c_event, "i2c_event", RT_IPC_FLAG_FIFO );
i2c_mux = rt_mutex_create("i2c_mux", RT_IPC_FLAG_FIFO );
i2c1_init_flag = 1;
}
}
/* ----------------------- Start implementation -----------------------------*/
BOOL xMBPortSerialInit(UCHAR ucPORT, ULONG ulBaudRate, UCHAR ucDataBits,
eMBParity eParity)
{
/**
* set 485 mode receive and transmit control IO
* @note MODBUS_SLAVE_RT_CONTROL_PIN_INDEX need be defined by user
*/
rt_pin_mode(MODBUS_SLAVE_RT_CONTROL_PIN_INDEX, PIN_MODE_OUTPUT);
/* set serial name */
if (ucPORT == 1) {
#if defined(RT_USING_UART1) || defined(RT_USING_REMAP_UART1)
extern struct rt_serial_device serial1;
serial = &serial1;
#endif
} else if (ucPORT == 2) {
#if defined(RT_USING_UART2)
extern struct rt_serial_device serial2;
serial = &serial2;
#endif
} else if (ucPORT == 3) {
#if defined(RT_USING_UART3)
extern struct rt_serial_device serial3;
serial = &serial3;
#endif
}
/* set serial configure parameter */
serial->config.baud_rate = ulBaudRate;
serial->config.stop_bits = STOP_BITS_1;
switch(eParity){
case MB_PAR_NONE: {
serial->config.data_bits = DATA_BITS_8;
serial->config.parity = PARITY_NONE;
break;
}
case MB_PAR_ODD: {
serial->config.data_bits = DATA_BITS_9;
serial->config.parity = PARITY_ODD;
break;
}
case MB_PAR_EVEN: {
serial->config.data_bits = DATA_BITS_9;
serial->config.parity = PARITY_EVEN;
break;
}
}
/* set serial configure */
serial->ops->configure(serial, &(serial->config));
/* open serial device */
if (!serial->parent.open(&serial->parent,
RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_RX )) {
serial->parent.rx_indicate = serial_rx_ind;
} else {
return FALSE;
}
/* software initialize */
rt_thread_init(&thread_serial_soft_trans_irq,
"slave trans",
serial_soft_trans_irq,
RT_NULL,
serial_soft_trans_irq_stack,
sizeof(serial_soft_trans_irq_stack),
10, 5);
rt_thread_startup(&thread_serial_soft_trans_irq);
rt_event_init(&event_serial, "slave event", RT_IPC_FLAG_PRIO);
return TRUE;
}
void rt_init_thread_entry(void* parameter)
{
rt_components_init();
LED_init();
Motor_Init();
rt_kprintf("start device init\n");
//rt_hw_i2c1_init();
i2cInit();
rt_hw_spi2_init();
rt_hw_spi3_init();
rt_event_init(&ahrs_event, "ahrs", RT_IPC_FLAG_FIFO);
dmp_init();
sonar_init();
HMC5983_Init();
adns3080_Init();
//config_bt();
rt_thread_init(&led_thread,
"led",
led_thread_entry,
RT_NULL,
led_stack,
256, 16, 1);
rt_thread_startup(&led_thread);
spi_flash_init();
// bmp085_init("i2c1");
rt_kprintf("device init succeed\n");
if (dfs_mount("flash0", "/", "elm", 0, 0) == 0)
{
rt_kprintf("flash0 mount to /.\n");
}
else
{
rt_kprintf("flash0 mount to / failed.\n");
}
//default settings
PID_Init(&p_rate_pid, 0, 0, 0);
PID_Init(&r_rate_pid, 0, 0, 0);
PID_Init(&y_rate_pid, 0, 0, 0);
PID_Init(&p_angle_pid, 0, 0, 0);
PID_Init(&r_angle_pid, 0, 0, 0);
PID_Init(&y_angle_pid, 0, 0, 0);
PID_Init(&x_v_pid, 0, 0, 0);
PID_Init(&y_v_pid, 0, 0, 0);
PID_Init(&x_d_pid, 0, 0, 0);
PID_Init(&y_d_pid, 0, 0, 0);
PID_Init(&h_pid, 0, 0, 0);
load_settings(&settings, "/setting", &p_angle_pid, &p_rate_pid
, &r_angle_pid, &r_rate_pid
, &y_angle_pid, &y_rate_pid
, &x_d_pid, &x_v_pid
, &y_d_pid, &y_v_pid
, &h_pid);
settings.roll_min = settings.pitch_min = settings.yaw_min = 1000;
settings.th_min = 1000;
settings.roll_max = settings.pitch_max = settings.yaw_max = 2000;
settings.th_max = 2000;
// if(settings.pwm_init_mode)
// {
// Motor_Set(1000,1000,1000,1000);
//
// rt_thread_delay(RT_TICK_PER_SECOND*5);
//
// Motor_Set(0,0,0,0);
//
// settings.pwm_init_mode=0;
// save_settings(&settings,"/setting");
//
// rt_kprintf("pwm init finished!\n");
// }
get_pid();
PID_Set_Filt_Alpha(&p_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_rate_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&p_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&r_angle_pid, 1.0 / 166.0, 20.0);
PID_Set_Filt_Alpha(&y_angle_pid, 1.0 / 75.0, 20.0);
PID_Set_Filt_Alpha(&x_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_v_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&x_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&y_d_pid, 1.0 / 100.0, 20.0);
PID_Set_Filt_Alpha(&h_pid, 1.0 / 60.0, 20.0);
rt_thread_init(&control_thread,
"control",
control_thread_entry,
RT_NULL,
control_stack,
1024, 3, 5);
rt_thread_startup(&control_thread);
rt_thread_init(&correct_thread,
"correct",
correct_thread_entry,
RT_NULL,
correct_stack,
1024, 12, 1);
rt_thread_startup(&correct_thread);
LED1(5);
}
void mavlink_thread_entry(void *parameter)
{
rt_uint8_t cnt = 0; // counter used to generate 5Hz sending rate.
rt_uint8_t buf[MAVLINK_MAX_PACKET_LEN];
rt_uint16_t len;
mavlink_message_t msg;
// not gonna use this event yet
if (rt_event_init(&event_drdy, "drdy", RT_IPC_FLAG_FIFO) != RT_EOK)
rt_kprintf("event_drdy init error\n");
while(1)
{
/* Send PFD data at 25 Hz*/
// fill the pfd struct
mavlink_updatePFDStruct();
// pack the pfd message
mavlink_msg_pfd_pack(MY_SYS_ID,
200, // component ID - IMU
&msg,
mavlinkPFDStruct.roll,
mavlinkPFDStruct.pitch,
mavlinkPFDStruct.yaw,
mavlinkPFDStruct.altitude,
mavlinkPFDStruct.airspeed);
// copy the message to the send buffer
len = mavlink_msg_to_send_buffer(buf, &msg);
// "Send" the message
radioSendWrapper(buf, len);
/* Send NavD data at 5 Hz */
if (cnt >= 5)
{
cnt = 0;
// fill the NavD struct
mavlink_updateNavDStruct();
// pack the NavD message
mavlink_msg_navd_pack(MY_SYS_ID,
200, // component ID - IMU
&msg,
mavlinkNavDStruct.battV,
mavlinkNavDStruct.battI,
mavlinkNavDStruct.temp,
mavlinkNavDStruct.latitude,
mavlinkNavDStruct.longitude,
mavlinkNavDStruct.course,
mavlinkNavDStruct.groundspeed);
// copy the message to the send buffer
len = mavlink_msg_to_send_buffer(buf, &msg);
// "Send" the message
radioSendWrapper(buf, len);
rt_thread_delay(10); // delay for a shorter time
continue;
}
cnt++;
rt_thread_delay(25);
}
}
void init_sys_485(void)
{
// rt_sem_init(&uart485_1_rx_byte_sem, "u485-1", 0, RT_IPC_FLAG_PRIO);
// rt_sem_init(&uart485_2_rx_byte_sem, "u485-2", 0, RT_IPC_FLAG_PRIO);
// rt_sem_init(&uart485_3_rx_byte_sem, "u485-3", 0, RT_IPC_FLAG_PRIO);
rt_event_init(&em_protocol_data_event_set, "tl16_eve", RT_IPC_FLAG_PRIO);
rs485_recv_buf= (rt_uint8_t *)rt_malloc(FRAME_485_LEN);
if(rs485_recv_buf == RT_NULL) {
rs485_info(("func:%s(), line:%d malloc fail\n",__FUNCTION__, __LINE__));
return;
}
rt_memset(rs485_recv_buf, 0, FRAME_485_LEN);
dev_485_1 = rt_device_find(UART_485_1_DEV);
if (dev_485_1 != RT_NULL && rt_device_open(dev_485_1, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_1_DEV));
rt_device_set_rx_indicate(dev_485_1, uart485_1_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_1_DEV));
// goto err_ret;
}
dev_485_2 = rt_device_find(UART_485_2_DEV);
if (dev_485_2 != RT_NULL && rt_device_open(dev_485_2, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_2_DEV));
rt_device_set_rx_indicate(dev_485_2, uart485_2_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_2_DEV));
// goto err_ret;
}
dev_485_3 = rt_device_find(UART_485_3_DEV);
if (dev_485_3 != RT_NULL && rt_device_open(dev_485_3, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_3_DEV));
rt_device_set_rx_indicate(dev_485_3, uart485_3_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_3_DEV));
// goto err_ret;
}
dev_485_tl16_1 = rt_device_find(UART_485_TL16_1_DEV);
if (dev_485_tl16_1 != RT_NULL && rt_device_open(dev_485_tl16_1, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_TL16_1_DEV));
rt_device_set_rx_indicate(dev_485_tl16_1, uart485_tl16_1_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_TL16_1_DEV));
// goto err_ret;
}
dev_485_tl16_2 = rt_device_find(UART_485_TL16_2_DEV);
if (dev_485_tl16_2 != RT_NULL && rt_device_open(dev_485_tl16_2, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_TL16_2_DEV));
rt_device_set_rx_indicate(dev_485_tl16_2, uart485_tl16_2_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_TL16_2_DEV));
// goto err_ret;
}
dev_485_tl16_3 = rt_device_find(UART_485_TL16_3_DEV);
if (dev_485_tl16_3 != RT_NULL && rt_device_open(dev_485_tl16_3, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_TL16_3_DEV));
rt_device_set_rx_indicate(dev_485_tl16_3, uart485_tl16_3_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_TL16_3_DEV));
// goto err_ret;
}
dev_485_tl16_4 = rt_device_find(UART_485_TL16_4_DEV);
if (dev_485_tl16_4 != RT_NULL && rt_device_open(dev_485_tl16_4, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_TL16_4_DEV));
rt_device_set_rx_indicate(dev_485_tl16_4, uart485_tl16_4_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_TL16_4_DEV));
// goto err_ret;
}
dev_485_tl16_5 = rt_device_find(UART_485_TL16_5_DEV);
if (dev_485_tl16_5 != RT_NULL && rt_device_open(dev_485_tl16_5, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_TL16_5_DEV));
rt_device_set_rx_indicate(dev_485_tl16_5, uart485_tl16_5_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_TL16_5_DEV));
// goto err_ret;
}
dev_485_tl16_6 = rt_device_find(UART_485_TL16_6_DEV);
if (dev_485_tl16_6 != RT_NULL && rt_device_open(dev_485_tl16_6, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_TL16_6_DEV));
rt_device_set_rx_indicate(dev_485_tl16_6, uart485_tl16_6_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_TL16_6_DEV));
// goto err_ret;
}
dev_485_tl16_7 = rt_device_find(UART_485_TL16_7_DEV);
if (dev_485_tl16_7 != RT_NULL && rt_device_open(dev_485_tl16_7, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_TL16_7_DEV));
rt_device_set_rx_indicate(dev_485_tl16_7, uart485_tl16_7_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_TL16_7_DEV));
// goto err_ret;
}
dev_485_tl16_8 = rt_device_find(UART_485_TL16_8_DEV);
if (dev_485_tl16_8 != RT_NULL && rt_device_open(dev_485_tl16_8, RT_DEVICE_OFLAG_RDWR) == RT_EOK) {
rs485_info(("open device:%s succ\n", UART_485_TL16_8_DEV));
rt_device_set_rx_indicate(dev_485_tl16_8, uart485_tl16_8_rx_ind);
} else {
rs485_info(("can not find device:%s\n", UART_485_TL16_8_DEV));
// goto err_ret;
}
return;
//err_ret:
// rt_free(rs485_recv_buf);
// return;
}
/* ----------------------- Start implementation -----------------------------*/
BOOL
xMBPortEventInit( void )
{
rt_event_init(&xSlaveOsEvent,"slave event",RT_IPC_FLAG_PRIO);
return TRUE;
}
/* ----------------------- Start implementation -----------------------------*/
BOOL
xMBMasterPortEventInit( void )
{
rt_event_init(&xMasterOsEvent,"master event",RT_IPC_FLAG_PRIO);
return TRUE;
}
