static void AppTimerCreate (void)
{
OS_ERR err;
/* 建立SPI1超时定时器 */
OSTmrCreate((OS_TMR *) &SPI1_OT_TIMER,
(CPU_CHAR *) "EPS Over Timer",
(OS_TICK ) 0,
(OS_TICK ) 1,
(OS_OPT ) OS_OPT_TMR_ONE_SHOT,
(OS_TMR_CALLBACK_PTR) 0,
(void *) 0,
(OS_ERR *) &err);
/* 建立通信1超时定时器 */
OSTmrCreate((OS_TMR *) &COM_OT_TIMER,
(CPU_CHAR *) "COM Over Timer",
(OS_TICK ) 2*10,
(OS_TICK ) 0,
(OS_OPT ) OS_OPT_TMR_ONE_SHOT,
(OS_TMR_CALLBACK_PTR) ComOT_CallBack,
(void *) 0,
(OS_ERR *) &err);
/* 建立GPS接收超时定时器 */
OSTmrCreate((OS_TMR *) &GPS_OT_TIMER,
(CPU_CHAR *) "GPS Over Timer",
(OS_TICK ) 2*10, /* 2s延迟 */
(OS_TICK ) 0,
(OS_OPT ) OS_OPT_TMR_ONE_SHOT,
(OS_TMR_CALLBACK_PTR) GPSOT_CallBack,
(void *) 0,
(OS_ERR *) &err);
}
void start_task(void *pdata)
{
OS_CPU_SR cpu_sr = 0;
pdata = pdata;
u8 err;
delay_init(168);
USART1_Init();//初始化
LED_Init();
BEEP_Init();
pringf("Hello world\n");
OS_ENTER_CRITICLE(); //进入临界区 无法被中断打断
//创建系统定时器
pTmrLed = OSTmrCreate( 0,//dly 计时前的延迟
OS_TMR_CFG_TICKS_PER_SEC/2,//周期 宏本身表示1s
OS_TMR_OPT_PERIODIC,//周期模式
(OS_TMR_CALLBACK)LedTmrCallback,
NULL,
NULL,
&err
);//定时器时间到了 自动调用回调函数
pTmrBeep = OSTmrCreate( 0,
OS_TMR_CFG_TICKS_PER_SEC/2,
OS_TMR_OPT_PERIODIC,
(OS_TMR_CALLBACK)BeepTmrCallback,
NULL,
NULL,
&err
);
//启动定时器
if( OSTmrStart(pTmrLED,&err) == OS_TRUE )
{
printf("LED TMR Start.\n");
}
else
printf("LED TMR Fail.\n");
if( OSTmrStart(pTmrBeep,&err) == OS_TRUE )
{
printf("BEEP TMR Start.\n");
}
else
printf("BEEP TMR Fail.\n");
OS_EXIT_CRITICLE(); //退出临界区 可以被中断打断
while(1)//一直延时 等待程序运行
OSTimeDlyHMSM(0,0,0,500);
}
/**********************************************************
TimeInit - function that initializes timer, mutex, and clock task.
* sets values to timeOfDay if reset button is hit
Creates: ClockTimer, ClockMutexKey, SecFlag
*********************************************************8*/
void TimeInit(void)
{
INT8U err;
TimeOfDay.hr = 0x0C;
TimeOfDay.min = 0x00;
TimeOfDay.sec = 0x00;
ClockTimer = OSTmrCreate(0,
10,
OS_TMR_OPT_PERIODIC,
ClockTimerFnct,
(void *)0,
"Clock Timer ",
&err);
OSTmrStart(ClockTimer, &err);
ClockMutexKey = (CLOCK_PIP, &err);
(void)OSTaskCreate(ClockTask,
(void *)0,
(void *)&ClockTaskStk[CLOCKTASK_STK_SIZE],
CLOCKTASK_PRIO);
SecFlag = OSSemCreate(0);
}
/*
* 函数名：Task_Tmr
* 描述 : 定时器应用任务
* 优先级为OS_CFG_PRIO_MAX-5u
* 输入 ：无
* 输出 : 无
*/
void Task_Tmr(void *p_arg)
{
OS_ERR err;
//定时器变量
OS_TMR MyTmr;
(void)p_arg;
//创建定时器
OSTmrCreate ((OS_TMR *)&MyTmr,
(CPU_CHAR *)"MyTimer",
(OS_TICK )100, //第一次延时设置为100，结合定时器的频率是100Hz，正好1s
(OS_TICK )100, //重复延时的时候100个TmrTick，结合定时器的频率是100Hz，正好1s
(OS_OPT )OS_OPT_TMR_PERIODIC,//模式设置为重复模式
(OS_TMR_CALLBACK_PTR )_cbOfTmr1, //回调函数
(void *)0, //参数设置为0
(OS_ERR *)err);
//启动定时器
OSTmrStart ((OS_TMR *)&MyTmr,
(OS_ERR *)err);
//保存定时器开始的时候的时间戳
ts_start = OS_TS_GET();
while (1)
{
//不断阻塞
OSTimeDlyHMSM(0, 0,1,0,OS_OPT_TIME_HMSM_STRICT,&err);
}
}
timer_t *timer_setup(int time_val, int type, timer_callback_func callback, void *callback_arg)
{
INT8U perr;
OS_TMR *tmr;
if (time_val < 100)
time_val = 100;
time_val = time_val * OS_TMR_CFG_TICKS_PER_SEC / 1000;
if ((callback == 0))
{
p_err("setup_timer err arg\n");
return 0;
}
if (type)
//== 1 repeat
{
p_dbg("repeat:%d\n", time_val);
tmr = OSTmrCreate(time_val, time_val, OS_TMR_OPT_PERIODIC, (OS_TMR_CALLBACK)callback, callback_arg, "", &perr);
}
else
{
p_dbg("one short:%d\n", time_val);
tmr = OSTmrCreate(time_val, time_val, OS_TMR_OPT_ONE_SHOT, (OS_TMR_CALLBACK)callback, callback_arg, "", &perr);
}
if (perr != OS_ERR_NONE)
{
tmr = 0;
p_err("setup_timer err\n");
}
tmr->priv = mem_malloc(sizeof(struct work_struct));
if(!tmr->priv){
OSTmrDel(tmr, &perr);
return 0;
}
init_work(tmr->priv, callback, callback_arg);
return (timer_t*)tmr;
}
void DYNAMIC_UTI(void* pdata)
{
INT8U err_tmr, err_tmr_start, i, err;
int j, switch_status;
Sem_DYNAMIC_UTI = OSSemCreate(0);
printf("DYNAMIC_UTI Task created!\n");
SWTimer_DYNAMIC_UTI = OSTmrCreate(0, DYNAMIC_UTI_PERIOD, OS_TMR_OPT_PERIODIC , Tmr_Callback_DYNAMIC_UTI_Task, (void *)0,"DYNAMIC_UTI",&err_tmr);
if(err_tmr == OS_ERR_NONE){
OSTmrStart(SWTimer_DYNAMIC_UTI, &err_tmr_start);
if(DEBUG)
printf("Timer for DYNAMIC_UTI is created and Started \n");
}
else {
printf("Error while creating DYNAMIC_UTI task timer \n");
}
while(1)
{
switch_status = switches_pressed();
switch_status = switch_status & 1008; // Mask for SW4 - SW9
switch_status = switch_status >> 4;
printf("Before loop : Time : %d, SwitchStatus : %d \n", alt_timestamp() / (alt_timestamp_freq()/1000), switch_status);
//printf("switch_status : %d\n",switch_status);
if(switch_status > 50)
switch_status = 50;
// Loop running for 6 unit of time
// for Optimization Level Os : j = 4600000 ;
// for No Optimization : j = 3444
for(i=0; i<switch_status; i++){
for(j=0; j<4600000; j++){
}
}
printf("After loop : Time : %d \n", alt_timestamp() / (alt_timestamp_freq()/1000));
OSSemPend(Sem_DYNAMIC_UTI, 0, &err);
if(err == OS_ERR_NONE)
{
}
else {
printf("Sem_DYNAMIC_UTI Pend Error ------ \n");
}
}
}
/*
*********************************************************************************************************
* AppLedIinit
*********************************************************************************************************
*
*********************************************************************************************************
*/
void AppLedInit()
{
OS_ERR err;
OSTmrCreate(&tmrLedCtr1,
"Timer Control Led",
7,
0,
OS_OPT_TMR_ONE_SHOT,
AppLedOff1,
NULL,
&err);
RCC->APB2ENR |= 1<<6; /* Configure PE10 (open drain) */
GPIOE->ODR |= 1 << 10;
GPIOE->CRH = (GPIOE->CRH & 0xfffff0ff) | 0x00000600;
/* Turn LED1ON */
AppLedOn1();
}
void SIM90x_Init(const struct TRANSPORT_IF *tp)
{
uint32_t resp;
uint32_t retry = 0;
OS_ERR error;
SIM90x_Transport = (struct TRANSPORT_IF*)tp;
///////////////
OSTmrCreate(&IdleTimer,
"IdleTimer",
SYS_MsToTick(1200),
SYS_MsToTick(1200),
OS_OPT_TMR_ONE_SHOT,
(OS_TMR_CALLBACK_PTR)0,
(void*)0,
&error );
///////////////
initPort();
Term_Init(SIM90x_Transport);
////////////////
SIM90x_PowerUp();
while(1)
{
SYS_DelayMs(500);
Debug(DEBUG_SIM908, "\n\nInit SIM908 retry %d times !!!! \n", retry);
retry ++;
// Echo mode off
Debug(DEBUG_SIM908, "\nATE0; Echo mode off\n");
SIM90x_Flush();
SIM90x_WriteLine("ATE0", 1000);
resp = SIM90x_WaitResponseList(1000, "OK", 0);
Debug(DEBUG_SIM908, "Response: %s\n", SIM90x_ResponseBuffer);
if( resp == 0 )
continue;
// Disable network registration unsolicited result code
Debug(DEBUG_SIM908, "\nAT+CREG=0; Network Registration \n");
SIM90x_Flush();
SIM90x_WriteLine("AT+CREG=0", 1000);
resp = SIM90x_WaitResponseList(1000, "OK", 0);
Debug(DEBUG_SIM908, "Response: %s\n", SIM90x_ResponseBuffer);
if( resp == 0 )
continue;
Debug(DEBUG_SIM908, "\nAT+CPIN? Enter PIN \n");
SIM90x_Flush();
SIM90x_WriteLine("AT+CPIN?", 1000);
resp = SIM90x_WaitResponseList(1000, "+CPIN: READY", 0);
Debug(DEBUG_SIM908, "Response: %s\n", SIM90x_ResponseBuffer);
if( resp == 0 )
continue;
break;
}
SIM90x_StartIdleTime();
}
//开始任务函数
void start_task(void *p_arg)
{
OS_ERR err;
CPU_SR_ALLOC();
p_arg = p_arg;
CPU_Init();
#if OS_CFG_STAT_TASK_EN > 0u
OSStatTaskCPUUsageInit(&err); //统计任务
#endif
#ifdef CPU_CFG_INT_DIS_MEAS_EN //如果使能了测量中断关闭时间
CPU_IntDisMeasMaxCurReset();
#endif
#if OS_CFG_SCHED_ROUND_ROBIN_EN //当使用时间片轮转的时候
//使能时间片轮转调度功能,时间片长度为1个系统时钟节拍，既1*5=5ms
OSSchedRoundRobinCfg(DEF_ENABLED,1,&err);
#endif
OS_CRITICAL_ENTER(); //进入临界区
//创建定时器1
OSTmrCreate((OS_TMR *)&tmr1, //定时器1
(CPU_CHAR *)"tmr1", //定时器名字
(OS_TICK )0, //0ms
(OS_TICK )50, //50*10=500ms
(OS_OPT )OS_OPT_TMR_PERIODIC, //周期模式
(OS_TMR_CALLBACK_PTR)tmr1_callback,//定时器1回调函数
(void *)0, //参数为0
(OS_ERR *)&err); //返回的错误码
//创建主任务
OSTaskCreate((OS_TCB * )&Main_TaskTCB,
(CPU_CHAR * )"Main task",
(OS_TASK_PTR )main_task,
(void * )0,
(OS_PRIO )MAIN_TASK_PRIO,
(CPU_STK * )&MAIN_TASK_STK[0],
(CPU_STK_SIZE)MAIN_STK_SIZE/10,
(CPU_STK_SIZE)MAIN_STK_SIZE,
(OS_MSG_QTY )0,
(OS_TICK )0,
(void * )0,
(OS_OPT )OS_OPT_TASK_STK_CHK|OS_OPT_TASK_STK_CLR,
(OS_ERR * )&err);
//创建MSGDIS任务
OSTaskCreate((OS_TCB * )&Msgdis_TaskTCB,
(CPU_CHAR * )"Msgdis task",
(OS_TASK_PTR )msgdis_task,
(void * )0,
(OS_PRIO )MSGDIS_TASK_PRIO,
(CPU_STK * )&MSGDIS_TASK_STK[0],
(CPU_STK_SIZE)MSGDIS_STK_SIZE/10,
(CPU_STK_SIZE)MSGDIS_STK_SIZE,
(OS_MSG_QTY )TASK_Q_NUM, //任务Msgdis_task需要使用内建消息队列，消息队列长度为4
(OS_TICK )0,
(void * )0,
(OS_OPT )OS_OPT_TASK_STK_CHK|OS_OPT_TASK_STK_CLR,
(OS_ERR * )&err);
OS_CRITICAL_EXIT(); //退出临界区
OSTaskDel((OS_TCB*)0,&err); //删除start_task任务自身
}
void AppTaskRadioControl(void *p_arg)
{
OS_ERR os_err;
uint8_t i = 0;
opQueueElem_t *opQueueElemPtr = NULL;
radioFrame *rfData = &radioData.txCell;
AppTaskRadioControlInit();
rfData->uart_frame.frame_type = RADIO_FRAME_NORMAL;
rfData->uart_frame.uart_sender_addr = RADIO_UART_ADDRESS;
rfData->uart_frame.function = RADIO_FUNC_SEND;
rfData->uart_frame.radio_target_addr = RADIO_ADDR_NODE_1;
rfData->uart_frame.payload_bytes_nb = RADIO_PAYLOAD_SIZE;
OSTmrCreate(&rfHeartbeatTimeout, "Heartbeat timeout", 0, 5, OS_OPT_TMR_PERIODIC, rfHeartbeatCallback, 0, &os_err);
OSTmrStart((OS_TMR *)&rfHeartbeatTimeout, &os_err);
while(1) {
opQueueElemPtr = receive_from_radio_queue();
if(opQueueElemPtr != NULL) {
OSTmrStart((OS_TMR *)&rfHeartbeatTimeout, &os_err);
for(i=5; i<RADIO_FRAME_SIZE; i++) {
rfData->iodata.byte[i] = 0x00;
}
switch(opQueueElemPtr->operation)
{
case RADIO_OP_DRIVE:
rfData->uart_frame.payload.operation = opQueueElemPtr->operation;
rfData->uart_frame.payload.driveData.controller = opQueueElemPtr->ctrl;
rfData->uart_frame.payload.driveData.direction = opQueueElemPtr->opAction;
rfData->uart_frame.payload.driveData.speed_0 = opQueueElemPtr->val_0;
rfData->uart_frame.payload.driveData.speed_1 = opQueueElemPtr->val_1;
break;
case RADIO_OP_LIGHTING:
rfData->uart_frame.payload.operation = opQueueElemPtr->operation;
rfData->uart_frame.payload.lightingData.controller = opQueueElemPtr->ctrl;
rfData->uart_frame.payload.lightingData.lightingType = opQueueElemPtr->opAction;
rfData->uart_frame.payload.lightingData.lightingState = opQueueElemPtr->val_0;
break;
case RADIO_OP_SOUND_SIG:
rfData->uart_frame.payload.operation = opQueueElemPtr->operation;
rfData->uart_frame.payload.soundSignalData.controller = opQueueElemPtr->ctrl;
rfData->uart_frame.payload.soundSignalData.soundSignalStatus = opQueueElemPtr->opAction;
break;
case RADIO_OP_HEARTBEAT:
rfData->uart_frame.payload.operation = opQueueElemPtr->operation;
break;
default: { }
}
radio_frame_transmit_handler(rfData);
}
else {
if(rfHeartbeatToSend == true) {
rfHeartbeatToSend = false;
if(scrSaverStarted == false) {
opQueueElem_t opQueueElem = {0};
// heartbeat messages are sustained only when SS is disabled
opQueueElem.operation = RADIO_OP_HEARTBEAT;
send_to_radio_queue(&opQueueElem);
}
}
}
}
}
/*****************************************************************************
* Function Name // task_start
* Descriptor // before create multi_task, we create a task_start task
* // in this example, this task display the cpu usage
* Input
* Output
* Return
****************************************************************************/
void task_start (void *data)
{
extern void CreateUserTask(void);
extern void RestoreParamToDefault(void);
int err;
u8 mac_addr[6];
u8 tx_gain[12];
u8 errstatus;
tls_irq_init();
/* initial os ticker */
tls_os_timer_init();
#if OS_TASK_STAT_EN > 0
/* Initialize uC/OS-II's statistics */
OSStatInit();
#endif
tls_spi_slave_sel(SPI_SLAVE_FLASH);
tls_spi_init();
tls_fls_init();
tls_fls_gd25qxx_drv_install();
tls_restore_param_from_backup();
RestoreParamToDefault();
//tls_param_to_default();
err = tls_param_init();
if (err) {
TLS_DBGPRT_INFO("read spi param err\n");
}
tls_fwup_init();
/* ¶ÁÈ¡efuseÖеÄmacµØÖ· */
tls_get_mac_addr(mac_addr);
tls_get_tx_gain(tx_gain);
TLS_DBGPRT_INFO("tx gain ");
TLS_DBGPRT_DUMP(&tx_gain[0], 12);
TLS_DBGPRT_INFO("mac addr ");
TLS_DBGPRT_DUMP(&mac_addr[0], 6);
if(tls_wl_init(tx_gain, &mac_addr[0]) == NULL){
TLS_DBGPRT_INFO("wl driver initial failured\n");
}
if (wpa_supplicant_init(mac_addr)) {
TLS_DBGPRT_INFO("supplicant initial failured\n");
}
tls_ethernet_init();
tls_sys_init();
#if TLS_CONFIG_HOSTIF
tls_hostif_init();
#endif
#if (TLS_CONFIG_HOSTIF && TLS_CONFIG_HS_SPI)
tls_hspi_init();
#endif
#if (TLS_CONFIG_HOSTIF &&TLS_CONFIG_UART)
tls_uart_init();
#endif
tls_sys_auto_mode_run();
tls_netif_add_status_event(tls_main_net_status_changed);
BLINKTIMER = OSTmrCreate(0, 10, 2, BlinkTimerProc, NULL, NULL, &errstatus);
if (BLINKTIMER){
OSTmrStart(BLINKTIMER, &errstatus);
}
tls_wifi_one_shot_init();
disp_version_info();
/* User start here */
CreateUserTask();
for (;;)
{
tls_os_time_delay(0x10000000);
}
}
void SwitchIO(void* pdata)
{
INT8U err_tmr, err_tmr_start, err_sem, switch_status;
Sem_SwitchIO = OSSemCreate(0);
Sem_SwitchIO_IP = OSSemCreate(0); // For getting user input
printf("SwitchIO Task created!\n");
SWTimer_SwitchIO = OSTmrCreate(0, SwitchIO_PERIOD, OS_TMR_OPT_PERIODIC , Tmr_Callback_SwitchIO_Task, (void *)0,"SwitchIO",&err_tmr);
if(err_tmr == OS_ERR_NONE){
OSTmrStart(SWTimer_SwitchIO, &err_tmr_start);
if(DEBUG)
printf("Timer for SwitchIO is created and Started \n");
}
else {
printf("Error while creating SwitchIO task timer \n");
if(err_tmr == OS_ERR_TMR_NAME_TOO_LONG)
printf(" OS_ERR_TMR_NAME_TOO_LONG : SwitchIO TASK \n");
}
while(1)
{
if(DEBUG)
printf("SwitchIO task \n");
switch_status = switches_pressed();
if(DEBUG)
printf("Switch Status : %d \n", switch_status);
if(switch_status & 1) // Engine pressed
{
engine = on;
}
else if(CUR_VELOCITY == 0){
engine = off;
}
if(switch_status & 2) // Top Gear pressed
{
top_gear = on;
}
else {
top_gear = off;
cruise_control = off;
}
OSSemPost(Sem_SwitchIO_IP);
if(DEBUG)
printf("SEM ACCESS SwitchIO TASK\n\n");
OSSemPend(Sem_SwitchIO, 0, &err_sem);
if(DEBUG)
printf("SwitchIO Task in Loop\n");
}
}
void ButtonIO(void* pdata)
{
INT8U err_tmr, err_tmr_start, err_sem;
int button_status;
Sem_ButtonIO = OSSemCreate(0);
Sem_ButtonIO_IP = OSSemCreate(0); // For getting user Input
printf("ButtonIO Task created!\n");
SWTimer_ButtonIO = OSTmrCreate(0, ButtonIO_PERIOD, OS_TMR_OPT_PERIODIC , Tmr_Callback_ButtonIO_Task, (void *)0,"ButtonIO",&err_tmr);
if(err_tmr == OS_ERR_NONE){
if(DEBUG)
printf("Timer for ButtonIO is created\n");
OSTmrStart(SWTimer_ButtonIO, &err_tmr_start);
if(err_tmr_start == OS_ERR_NONE){
if(DEBUG)
printf("Timer Started in ButtonIO \n");
}
else {
printf("Problem starting timer in ButtonIO TASK \n");
}
}
else {
printf("Error while creating ButtonIO task timer \n");
if(err_tmr == OS_ERR_TMR_INVALID_OPT)
printf(" OS_ERR_TMR_INVALID_OPT : ButtonIO TASK \n");
}
while(1)
{
if(DEBUG)
printf("ButtonIO task \n");
button_status = buttons_pressed();
if(DEBUG)
printf("Button Status : %d, %x \n",button_status, button_status & 15);
button_status = button_status & 15;
if(button_status & 4) // Brake Pedal pressed
{
brake_pedal = on;
cruise_control = off;
}
else {
brake_pedal = off;
}
if((button_status & 1) && (engine == on)) // Gas Pedal pressed
{
gas_pedal = on;
cruise_control = off;
}
else {
gas_pedal = off;
}
// Cruse Contol pressed
if((button_status & 2) && (top_gear == on) && (CUR_VELOCITY >= Threshold_Velocity_CC) && (gas_pedal == off) && (brake_pedal == off))
{
if(DEBUG_IO)
printf("Cruise control detected -----");
cruise_control = on;
}
OSSemPost(Sem_ButtonIO_IP);
if(DEBUG)
printf("SEM ACCESS ButtonIO TASK\n\n");
OSSemPend(Sem_ButtonIO, 0, &err_sem);
if(DEBUG)
printf("ButtonIO Task in Loop\n");
}
}
void ControlTask(void* pdata)
{
INT8U err, err_tmr_2, err_tmr_start, err_sem;
INT8U throttle = 0; /* Value between 0 and 80, which is interpreted as between 0.0V and 8.0V */
void* msg;
INT16S* current_velocity, diff_velocity;
Sem_ControlTask = OSSemCreate(0);
printf("Control Task created!\n");
SWTimer_ControlTask = OSTmrCreate(0, CONTROL_PERIOD, OS_TMR_OPT_PERIODIC , Tmr_Callback_Control_Task, (void *)0,"S/W Timer 1",&err_tmr_2);
if(err_tmr_2 == OS_ERR_NONE){
if(DEBUG)
printf("Timer for CONTROL TASK is created and Started\n");
OSTmrStart(SWTimer_ControlTask, &err_tmr_start);
}
else {
printf("Error while creating control task timer \n");
if(err_tmr_2 == OS_ERR_TMR_NON_AVAIL)
printf(" OS_ERR_TMR_NON_AVAIL : CONTROL TASK \n");
}
while(1)
{
if(DEBUG)
printf("Control task waiting for MBoxPost_Velocity \n\n");
msg = OSMboxPend(Mbox_Velocity, 0, &err);
current_velocity = (INT16S*) msg;
// Set throttle value if gas pedal is pressed
if((gas_pedal == on) && (engine == on)){
throttle = throttle + 10;
if(throttle > 80)
throttle = 80; // Max value of throttle 0 - 80
if(DEBUG_IO)
printf("********** Acclerate Gas_Pedal : %d **********\n", throttle);
}
else {
throttle = 0;
}
if(engine == off){
throttle = 0;
}
// Check for cruise control
if((cruise_control == on) && (engine == on)){
if(REF_VELOCITY == 0)
REF_VELOCITY = *current_velocity;
if(DEBUG_IO)
printf("********** Cruise control Active ********** REF : %d \n",REF_VELOCITY);
show_target_velocity((INT8U) (REF_VELOCITY/10));
if(*current_velocity > REF_VELOCITY){ // Decrease throttle value
diff_velocity = *current_velocity - REF_VELOCITY;
if(diff_velocity > 15){
throttle = 0;
}
else if(diff_velocity > 10){
throttle = throttle - 30;
}
else {
throttle = throttle - 10;
}
if(throttle <= 0) // Throttle 0-80
throttle = 0;
if(DEBUG_IO)
printf("DEC throttle : %d \n", throttle);
}
else {
diff_velocity = REF_VELOCITY - *current_velocity;
if(diff_velocity > 15){
throttle = 80;
}
else if (diff_velocity > 10){
throttle = throttle + 30;
}
else {
throttle = throttle + 10;
}
if(throttle >= 80)
throttle = 80;
if(DEBUG_IO)
printf("INC throttle : %d \n", throttle);
}
}
else {
//cruise_control = off;
REF_VELOCITY = 0;
IOWR_ALTERA_AVALON_PIO_DATA(DE2_PIO_HEX_HIGH28_BASE,0x0000);
}
err = OSMboxPost(Mbox_Throttle, (void *) &throttle);
if(DEBUG)
printf("MBoxPost_Throttle done by CONTROL TASK \n");
if(DEBUG)
printf("SEM ACCESS CONTROL TASK\n\n");
OSSemPend(Sem_ControlTask, 0, &err_sem);
if(DEBUG)
printf("Control Task in Loop\n");
}
}
/*
* The task 'VehicleTask' updates the current velocity of the vehicle
*/
void VehicleTask(void* pdata)
{
INT8U err, err_tmr_1, err_tmr_start, err_sem;
void* msg;
INT8U* throttle;
INT8S acceleration; /* Value between 40 and -20 (4.0 m/s^2 and -2.0 m/s^2) */
INT8S retardation; /* Value between 20 and -10 (2.0 m/s^2 and -1.0 m/s^2) */
INT16U position = 0; /* Value between 0 and 20000 (0.0 m and 2000.0 m) */
INT16S wind_factor; /* Value between -10 and 20 (2.0 m/s^2 and -1.0 m/s^2) */
Sem_VehicleTask = OSSemCreate(0);
//OSSemPend(Sem_VehicleTask,0,&err_sem);
printf("Vehicle task created!\n");
SWTimer_VehicleTask = OSTmrCreate(0, VEHICLE_PERIOD, OS_TMR_OPT_PERIODIC, Tmr_Callback_Vehicle_Task, (void *)0,"S/W Timer 2",&err_tmr_1);
if(err_tmr_1 == OS_ERR_NONE){
if(DEBUG)
printf("Timer for VECHICLE TASK is created\n");
OSTmrStart(SWTimer_VehicleTask, &err_tmr_start);
if(err_tmr_start == OS_ERR_NONE){
if(DEBUG)
printf("Timer started in VEHICLE TASK \n");
}
else {
printf("Problem starting timer in VEHICLE TASK \n");
}
}
else {
printf("Error while creating Vehicle task timer \n");
if(err_tmr_1 == OS_ERR_TMR_INVALID_DLY)
printf(" Delay INVALID : VEHICLE TASK\n");
}
while(1)
{
// Wait for user inputs
OSSemPend(Sem_SwitchIO_IP , 0, &err_sem);
OSSemPend(Sem_ButtonIO_IP, 0, &err_sem);
if((brake_pedal == on) && (CUR_VELOCITY > 0)){
CUR_VELOCITY = CUR_VELOCITY - 10;
// if(CUR_VELOCITY < -200) // Max value for velocity
//CUR_VELOCITY = -200;
if(DEBUG_IO)
printf("********** Brake brake_pedal : %d ********** \n", CUR_VELOCITY);
}
err = OSMboxPost(Mbox_Velocity, (void *) &CUR_VELOCITY);
if(DEBUG)
printf("Vehicle task Posted MBoxPost_Velocity \n");
if(DEBUG)
printf("SEM ACCESS VEHICLE TASK\n\n");
OSSemPend(Sem_VehicleTask,0,&err_sem);
/* Non-blocking read of mailbox:
- message in mailbox: update throttle
- no message: use old throttle
*/
if(DEBUG)
printf("Vehicle task waiting for MBoxPost_Throttle for 1 unit time \n");
msg = OSMboxPend(Mbox_Throttle, 1, &err);
if (err == OS_NO_ERR)
throttle = (INT8U*) msg;
if(DEBUG)
printf("Vehicle task GOT MBoxPost_Throttle \n");
/* Retardation : Factor of Terrain and Wind Resistance */
if (CUR_VELOCITY > 0)
wind_factor = CUR_VELOCITY * CUR_VELOCITY / 10000 + 1;
else
wind_factor = (-1) * CUR_VELOCITY * CUR_VELOCITY / 10000 + 1;
if (position < 4000)
retardation = wind_factor; // even ground
else if (position < 8000)
retardation = wind_factor + 15; // traveling uphill
else if (position < 12000)
retardation = wind_factor + 25; // traveling steep uphill
else if (position < 16000)
retardation = wind_factor; // even ground
else if (position < 20000)
retardation = wind_factor - 10; //traveling downhill
else
retardation = wind_factor - 5 ; // traveling steep downhill
acceleration = *throttle / 2 - retardation;
position = adjust_position(position, CUR_VELOCITY, acceleration, 300);
CUR_VELOCITY = adjust_velocity(CUR_VELOCITY, acceleration, brake_pedal, 300);
printf("Position: %dm\n", position / 10);
printf("Velocity: %4.1fm/s\n", CUR_VELOCITY /10.0);
printf("Throttle: %dV Time : %d \n\n", *throttle / 10, (int) (alt_timestamp() / (alt_timestamp_freq()/1000)));
alt_timestamp_start();
/*
INT32U stk_size;
err = OSTaskStkChk(16, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used OverLoadDetection : %d \n", stk_size);
err = OSTaskStkChk(14, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used DYNAMIV_UTI : %d \n", stk_size);
err = OSTaskStkChk(12, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used CONTROLTASK: %d \n", stk_size);
err = OSTaskStkChk(10, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used VehicleTask: %d \n", stk_size);
err = OSTaskStkChk(8, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used SwitchIO: %d \n", stk_size);
err = OSTaskStkChk(7, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used ButtonIO: %d \n", stk_size);
err = OSTaskStkChk(6, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used WatchDog: %d \n", stk_size);
err = OSTaskStkChk(5, &stk_data);
stk_size = stk_data.OSUsed;
printf("Stack Used Start Task: %d \n", stk_size);
*/
show_velocity_on_sevenseg((INT8S) (CUR_VELOCITY / 10));
show_active_signals();
show_position(position);
// OSSemPend(Sem_VehicleTask,0,&err_sem);
}
}
