/**
* This function will let current thread sleep for some ticks.
*
* @param tick the sleep ticks
*
* @return RT_EOK
*
*/
rt_err_t rt_thread_sleep(rt_tick_t tick)
{
register rt_base_t temp;
struct rt_thread *thread;
/* disable interrupt */
temp = rt_hw_interrupt_disable();
/* set to current thread */
thread = rt_current_thread;
RT_ASSERT(thread != RT_NULL);
/* suspend thread */
rt_thread_suspend(thread);
/* reset the timeout of thread timer and start it */
rt_timer_control(&(thread->thread_timer), RT_TIMER_CTRL_SET_TIME, &tick);
rt_timer_start(&(thread->thread_timer));
/* enable interrupt */
rt_hw_interrupt_enable(temp);
rt_schedule();
/* clear error number of this thread to RT_EOK */
if (thread->error == -RT_ETIMEOUT)
thread->error = RT_EOK;
return RT_EOK;
}
rt_err_t rt_completion_wait(struct rt_completion *completion, rt_int32_t timeout)
{
rt_err_t result;
rt_base_t level;
rt_thread_t thread;
RT_ASSERT(completion != RT_NULL);
result = RT_EOK;
thread = rt_thread_self();
level = rt_hw_interrupt_disable();
if (completion->flag != RT_COMPLETED)
{
/* only one thread can suspend on complete */
RT_ASSERT(rt_list_isempty(&(completion->suspended_list)));
if (timeout == 0)
{
result = -RT_ETIMEOUT;
goto __exit;
}
else
{
/* reset thread error number */
thread->error = RT_EOK;
/* suspend thread */
rt_thread_suspend(thread);
/* add to suspended list */
rt_list_insert_before(&(completion->suspended_list), &(thread->tlist));
/* current context checking */
RT_DEBUG_NOT_IN_INTERRUPT;
/* start timer */
if (timeout > 0)
{
/* reset the timeout of thread timer and start it */
rt_timer_control(&(thread->thread_timer), RT_TIMER_CTRL_SET_TIME, &timeout);
rt_timer_start(&(thread->thread_timer));
}
/* enable interrupt */
rt_hw_interrupt_enable(level);
/* do schedule */
rt_schedule();
/* thread is waked up */
result = thread->error;
level = rt_hw_interrupt_disable();
/* clean completed flag */
completion->flag = RT_UNCOMPLETED;
}
}
__exit:
rt_hw_interrupt_enable(level);
return result;
}
/**
* This function will suspend a thread for a specified IPC object and put the
* thread into suspend queue of IPC object
*
* @param ipc the IPC object
* @param thread the thread object to be suspended
*
* @return the operation status, RT_EOK on successful
*/
rt_inline rt_err_t rt_ipc_object_suspend(struct rt_ipc_object *ipc, struct rt_thread *thread)
{
/* suspend thread */
rt_thread_suspend(thread);
ipc->suspend_thread_count ++;
switch (ipc->parent.flag)
{
case RT_IPC_FLAG_FIFO:
rt_list_insert_before(&(ipc->suspend_thread), &(thread->tlist));
break;
case RT_IPC_FLAG_PRIO:
{
struct rt_list_node* n;
struct rt_thread* sthread;
/* find a suitable position */
for (n = ipc->suspend_thread.next; n != &(ipc->suspend_thread);
n = n->next)
{
sthread = rt_list_entry(n, struct rt_thread, tlist);
/* find out */
if (thread->current_priority < sthread->current_priority) break;
}
rt_list_insert_before(&(ipc->suspend_thread), &(thread->tlist));
}
break;
}
return RT_EOK;
}
/* 线程2入口 */
static void thread2_entry(void* parameter)
{
/* 延时10个OS Tick */
rt_thread_delay(10);
/* 挂起线程1 */
rt_thread_suspend(tid1);
/* 延时10个OS Tick */
rt_thread_delay(10);
/* 线程2自动退出 */
tid2 = RT_NULL;
}
/* 线程1入口 */
static void thread1_entry(void* parameter)
{
/* 低优先级线程1开始运行 */
rt_kprintf("thread1 startup%d\n");
/* 挂起自身 */
rt_kprintf("suspend thread self\n");
rt_thread_suspend(tid1);
/* 主动执行线程调度 */
rt_schedule();
/* 当线程1被唤醒时 */
rt_kprintf("thread1 resumed\n");
}
/**
* This function will suspend a thread to a specified list. IPC object or some double-queue
* object (mailbox etc.) contains this kind of list.
*
* @param ipc the IPC object
* @param thread the thread object to be suspended
*
* @return the operation status, RT_EOK on successful
*/
rt_inline rt_err_t rt_ipc_list_suspend(rt_list_t *list, struct rt_thread *thread, rt_uint8_t flag)
{
/* suspend thread */
rt_thread_suspend(thread);
switch (flag)
{
case RT_IPC_FLAG_FIFO:
rt_list_insert_before(list, &(thread->tlist));
break;
case RT_IPC_FLAG_PRIO:
{
struct rt_list_node *n;
struct rt_thread *sthread;
/* find a suitable position */
for (n = list->next; n != list; n = n->next)
{
sthread = rt_list_entry(n, struct rt_thread, tlist);
/* find out */
if (thread->current_priority < sthread->current_priority)
{
/* insert this thread before the sthread */
rt_list_insert_before(&(sthread->tlist), &(thread->tlist));
break;
}
}
/* not found a suitable position, append to the end of suspend_thread list */
if (n == list)
rt_list_insert_before(list, &(thread->tlist));
}
break;
}
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");
}
}
}
/* system timer thread entry */
static void rt_thread_timer_entry(void *parameter)
{
rt_tick_t next_timeout;
while (1)
{
/* get the next timeout tick */
next_timeout = rt_timer_list_next_timeout(rt_soft_timer_list);
if (next_timeout == RT_TICK_MAX)
{
/* no software timer exist, suspend self. */
rt_thread_suspend(rt_thread_self());
rt_schedule();
}
else
{
rt_tick_t current_tick;
/* get current tick */
current_tick = rt_tick_get();
if ((next_timeout - current_tick) < RT_TICK_MAX/2)
{
/* get the delta timeout tick */
next_timeout = next_timeout - current_tick;
rt_thread_delay_hmsm(next_timeout);
}
}
/* lock scheduler */
rt_enter_critical();
/* check software timer */
rt_soft_timer_check();
/* unlock scheduler */
rt_exit_critical();
}
}
/**
* This function will allocate a block from memory pool
*
* @param mp the memory pool object
* @param time the waiting time
*
* @return the allocated memory block or RT_NULL on allocated failed
*/
void *rt_mp_alloc(rt_mp_t mp, rt_int32_t time)
{
rt_uint8_t *block_ptr;
register rt_base_t level;
struct rt_thread *thread;
/* disable interrupt */
level = rt_hw_interrupt_disable();
if (mp->block_free_count)
{
/* memory block is available. decrease the free block counter */
mp->block_free_count --;
/* get block from block list */
block_ptr = mp->block_list;
mp->block_list = *(rt_uint8_t **)block_ptr;
/* point to memory pool */
*(rt_uint8_t **)block_ptr = (rt_uint8_t *)mp;
}
else
{
/* memory block is unavailable. */
if (time == 0)
{
/* enable interrupt */
rt_hw_interrupt_enable(level);
return RT_NULL;
}
else
{
RT_DEBUG_NOT_IN_INTERRUPT;
/* get current thread */
thread = rt_thread_self();
/* need suspend thread */
rt_thread_suspend(thread);
rt_list_insert_after(&(mp->suspend_thread), &(thread->tlist));
mp->suspend_thread_count ++;
if (time > 0)
{
/* init thread timer and start it */
rt_timer_control(&(thread->thread_timer), RT_TIMER_CTRL_SET_TIME, &time);
rt_timer_start(&(thread->thread_timer));
}
/* enable interrupt */
rt_hw_interrupt_enable(level);
/* do a schedule */
rt_schedule();
if (thread->error != RT_EOK)
return RT_NULL;
/* disable interrupt */
level = rt_hw_interrupt_disable();
/* decrease free block */
mp->block_free_count --;
/* get block from block list */
block_ptr = mp->block_list;
mp->block_list = *(rt_uint8_t **)block_ptr;
/* point to memory pool */
*(rt_uint8_t **)block_ptr = (rt_uint8_t *)mp;
}
}
/* enable interrupt */
rt_hw_interrupt_enable(level);
RT_OBJECT_HOOK_CALL(rt_mp_alloc_hook, (mp, (rt_uint8_t *)(block_ptr + sizeof(rt_uint8_t *))));
return (rt_uint8_t *)(block_ptr + sizeof(rt_uint8_t *));
}
void thread_gps_upgrade_udisk( void* parameter )
{
#define READ_PACKET_SIZE 1012
void ( *msg )( void *p );
int fd = -1, size;
u32 count = 0;
rt_uint8_t *pdata; /*数据*/
rt_uint8_t buf[32];
rt_uint8_t ch_h, ch_l;
rt_err_t res;
LENGTH_BUF uart_buf;
rt_uint32_t file_datalen; /*升级文件长度*/
rt_uint8_t file_matchcode[2]; /*文件匹配码*/
rt_uint16_t packet_num;
rt_uint16_t crc;
msg = parameter;
ptr_mem_packet = rt_malloc( READ_PACKET_SIZE+20 );
if( ptr_mem_packet == RT_NULL )
{
msg( "E内存不足" );
return;
}
/*查找U盘*/
while( 1 )
{
if( rt_device_find( "udisk" ) == RT_NULL ) /*没有找到*/
{
count++;
if( count <= 10 )
{
msg( "I等待U盘插入" );
BD_upgrad_contr=1;
}else
{
msg( "EU盘不存在" ); /*指示U盘不存在*/
BD_upgrad_contr=1;
goto end_upgrade_usb_0;
}
rt_thread_delay( RT_TICK_PER_SECOND );
}else
{
msg( "I查找升级文件" );
break;
}
WatchDog_Feed();
}
/*查找指定文件BEIDOU.IMG*/
fd = open( "/udisk/BEIDOU.IMG", O_RDONLY, 0 );
if( fd >= 0 )
{
msg( "I分析文件" );
}else
{
msg( "E升级文件不存在" );
goto end_upgrade_usb_0;
}
size = read( fd, ptr_mem_packet, 16 );
pdata = ptr_mem_packet;
if( ( *pdata != 0x54 ) || ( *( pdata + 1 ) != 0x44 ) )
{
msg( "E文件头错误" );
goto end_upgrade_usb_1;
}
ch_h = ( *( pdata + 9 ) & 0xf0 ) >> 4;
ch_l = ( *( pdata + 9 ) & 0xf );
sprintf( buf, "I版本:%d.%d.%d", ch_h, ch_l, *( pdata + 10 ) );
msg( buf );
/*升级数据长度*/
file_datalen =0;
file_datalen = ( *( pdata + 11 ) ) << 24;
file_datalen |= ( *( pdata + 12 ) ) << 16;
file_datalen |= ( *( pdata + 13 ) ) << 8;
file_datalen |= *( pdata + 14 );
rt_kprintf("file_datalen=%x",file_datalen);
/*文件匹配码在尾部*/
count = 0;
ch_h = 0;
do{
res = read( fd, ptr_mem_packet,READ_PACKET_SIZE );
if(res) count = res;
WatchDog_Feed();
}while(res>0);
//rt_kprintf("res=%02x\r\n",res);
//if(res==0) res=READ_PACKET_SIZE;
if( ( ptr_mem_packet[count - 1] != 0x54 ) || ( ptr_mem_packet[count - 2] != 0x44 ) )
{
msg( "E文件尾错误" );
goto end_upgrade_usb_1;
}
file_matchcode[0] = ptr_mem_packet[count - 6];
file_matchcode[1] = ptr_mem_packet[count - 5];
rt_kprintf("file datalen=%x matchcode=%02x%02x",file_datalen,file_matchcode[0],file_matchcode[1]);
close( fd );
msg( "I配置端口" );
fd = open( "/udisk/BEIDOU.IMG", O_RDONLY, 0 );
if( fd < 0 )
{
msg( "E升级文件不存在" );
goto end_upgrade_usb_0;
}
/*开始升级*/
/*停止gps线程*/
rt_thread_suspend( &thread_gps );
/*进入升级状态*/
memcpy( buf, "\x40\x30\xC0\x00\x03\x00\x01\x34\x21\x0D\x0A", 11 );
dev_gps_write( &dev_gps, 0, buf, 11 );
rt_thread_delay( RT_TICK_PER_SECOND );
/*版本查询*/
count = 0;
dev_gps_write( &dev_gps, 0, "\x40\x10\xC0\x00\x10\x00\x01\xC2\x84\x0D\x0A", 11 );
while( 1 )
{
rt_thread_delay(RT_TICK_PER_SECOND/10);
res = rt_mq_recv( &mq_gps, (void*)&uart_buf, 124, 5 );
if( res == RT_EOK ) //收到一包数据
{
count=0; // clear
rt_kprintf("\r\n版本查询\r\n");
for( ch_h = 0; ch_h < uart_buf.wr; ch_h++ )
{
rt_kprintf( "%02x ", uart_buf.body[ch_h] );
}
rt_kprintf( "\r\n" );
if( ( uart_buf.wr == 15 ) && ( uart_buf.body[4] == 0x02 ) ) /*进入升级状态*/
{
ch_h = ( uart_buf.body[7] & 0xf0 ) >> 4;
ch_l = ( uart_buf.body[7] & 0xf );
sprintf( buf, "I版本:%d.%d.%d", ch_h, ch_l, uart_buf.body[8] );
msg( buf );
break;
}
}else /*超时*/
rt_err_t rt_data_queue_push(struct rt_data_queue *queue,
const void *data_ptr,
rt_size_t data_size,
rt_int32_t timeout)
{
rt_uint16_t mask;
rt_ubase_t level;
rt_thread_t thread;
rt_err_t result;
RT_ASSERT(queue != RT_NULL);
result = RT_EOK;
thread = rt_thread_self();
mask = queue->size - 1;
level = rt_hw_interrupt_disable();
while (queue->put_index - queue->get_index == queue->size)
{
queue->waiting_lwm = RT_TRUE;
/* queue is full */
if (timeout == 0)
{
result = -RT_ETIMEOUT;
goto __exit;
}
/* current context checking */
RT_DEBUG_NOT_IN_INTERRUPT;
/* reset thread error number */
thread->error = RT_EOK;
/* suspend thread on the push list */
rt_thread_suspend(thread);
rt_list_insert_before(&(queue->suspended_push_list), &(thread->tlist));
/* start timer */
if (timeout > 0)
{
/* reset the timeout of thread timer and start it */
rt_timer_control(&(thread->thread_timer),
RT_TIMER_CTRL_SET_TIME,
&timeout);
rt_timer_start(&(thread->thread_timer));
}
/* enable interrupt */
rt_hw_interrupt_enable(level);
/* do schedule */
rt_schedule();
/* thread is waked up */
result = thread->error;
level = rt_hw_interrupt_disable();
if (result != RT_EOK) goto __exit;
}
queue->queue[queue->put_index & mask].data_ptr = data_ptr;
queue->queue[queue->put_index & mask].data_size = data_size;
queue->put_index += 1;
if (!rt_list_isempty(&(queue->suspended_pop_list)))
{
/* there is at least one thread in suspended list */
/* get thread entry */
thread = rt_list_entry(queue->suspended_pop_list.next,
struct rt_thread,
tlist);
/* resume it */
rt_thread_resume(thread);
rt_hw_interrupt_enable(level);
/* perform a schedule */
rt_schedule();
return result;
}
