示例#1
0
/**
 *******************************************************************************
 * @brief      Respond the request in the service request queue.	 
 * @param[in]  None
 * @param[out] None 
 * @retval     None  
 *
 * @par Description		 
 * @details    This function be called to respond the request in the service  
 *             request queue.
 * @note 
 *******************************************************************************
 */
void RespondSRQ(void)
{
#if CFG_MAX_SERVICE_REQUEST > 0
    U16 i;
    P_SQC pcell;
#endif
 
#if (CFG_TASK_WAITTING_EN > 0)
    if(TimeReq == TRUE)                 /* Time delay request?                */
    {
        TimeDispose();                  /* Yes,call handler                   */
        TimeReq = FALSE;                /* Reset time delay request false     */
    }
#endif
#if CFG_TMR_EN  > 0
    if(TimerReq == TRUE)                /* Timer request?                     */
    {
        TmrDispose();                   /* Yes,call handler                   */
        TimerReq = FALSE;               /* Reset timer request false          */
    }
#endif

#if CFG_MAX_SERVICE_REQUEST > 0
    pcell = &ServiceReq.cell[0];  /* Get the head item of service request list*/
    for(i=0;i<ServiceReq.cnt;i++,pcell++) 
    {
        switch(pcell->type)             /* Judge service request type         */
        {
#if CFG_SEM_EN > 0
            case SEM_REQ:               /* Semaphore post request,call handler*/
                  CoPostSem(pcell->id);
                  break;
#endif
#if CFG_MAILBOX_EN > 0
            case MBOX_REQ:              /* Mailbox post request,call handler  */
                  CoPostMail(pcell->id,pcell->arg);
                  break;
#endif
#if CFG_FLAG_EN > 0
            case FLAG_REQ:              /* Flag set request,call handler      */
                  CoSetFlag(pcell->id);
                  break;
#endif	 
#if CFG_QUEUE_EN > 0
            case QUEUE_REQ:             /* Queue post request,call handler    */
				  CoPostQueueMail(pcell->id,pcell->arg);
                  break;
#endif
            default:                    /* Others,break                       */
                  break;
		}
    pcell->type = 0;                    /* Initialize the service request cell*/
    pcell->id   = 0;
    pcell->arg  = 0;	
	}
    ServiceReq.cnt = 0;               /* Initialize the service request queue */
#endif
    IsrReq = FALSE;
}
示例#2
0
void JoyTimer(void){
	uint8_t key;
	key=Read_Joy();
  	if((key!=0) && (lastjoyKey!=key)){
		CoSetFlag(keyFlag[key-1]);
 //		CoClearFlag(keyFlag[key-1]);
  	}
  	lastjoyKey=key;
}
示例#3
0
void serialWatch(void) {
#ifdef SERIAL_UART1_PORT
    if (serialPort1 && serialAvailable(serialPort1))
	CoSetFlag(serialPort1->waitFlag);
#endif
#ifdef SERIAL_UART2_PORT
    if (serialPort2 && serialAvailable(serialPort2))
	CoSetFlag(serialPort2->waitFlag);
#endif
#ifdef SERIAL_UART3_PORT
    if (serialPort3 && serialAvailable(serialPort3))
	CoSetFlag(serialPort3->waitFlag);
#endif
#ifdef SERIAL_UART4_PORT
    if (serialPort4 && serialAvailable(serialPort4))
	CoSetFlag(serialPort4->waitFlag);
#endif
#ifdef SERIAL_UART5_PORT
    if (serialPort5 && serialAvailable(serialPort5))
	CoSetFlag(serialPort5->waitFlag);
#endif
#ifdef SERIAL_UART6_PORT
    if (serialPort6 && serialAvailable(serialPort6))
	CoSetFlag(serialPort6->waitFlag);
#endif
}
示例#4
0
void JoyTask (void* pdata){
	uint8_t last_joyKey,key;

	last_joyKey=Read_Joy();
  for (;;) {
	  	key=Read_Joy();
	  	if((key!=0) && (last_joyKey!=key)){
	  		CoSetFlag(keyFlag[key-1]);
	  //		CoClearFlag(keyFlag[key-1]);
	  	}
	  	last_joyKey=key;

		CoTimeDelay (0,0,0,50);
  }
}
示例#5
0
文件: flag.c 项目: ChrelleP/autoquad
StatusType isr_SetFlag(OS_FlagID id)
{
    if(OSSchedLock > 0)         /* If scheduler is locked,(the caller is ISR) */
    {
        /* Insert the request into service request queue                      */
        if(InsertInSRQ(FLAG_REQ,id,Co_NULL) == Co_FALSE)
        {
            return E_SEV_REQ_FULL;      /* The service requst queue is full   */
        }
        else
        {
            return E_OK;
        }
    }
    else
    {
        return(CoSetFlag(id));          /* The caller is not ISR, set the flag*/
    }
}
示例#6
0
void runTaskCode(void *unused) {
    uint32_t axis = 0;
    uint32_t loops = 0;

    AQ_NOTICE("Run task started\n");

    while (1) {
	// wait for data
	CoWaitForSingleFlag(imuData.sensorFlag, 0);

	// soft start GPS accuracy
	runData.accMask *= 0.999f;

	navUkfInertialUpdate();

	// record history for acc & mag & pressure readings for smoothing purposes
	// acc
	runData.sumAcc[0] -= runData.accHist[0][runData.sensorHistIndex];
	runData.sumAcc[1] -= runData.accHist[1][runData.sensorHistIndex];
	runData.sumAcc[2] -= runData.accHist[2][runData.sensorHistIndex];

	runData.accHist[0][runData.sensorHistIndex] = IMU_ACCX;
	runData.accHist[1][runData.sensorHistIndex] = IMU_ACCY;
	runData.accHist[2][runData.sensorHistIndex] = IMU_ACCZ;

	runData.sumAcc[0] += runData.accHist[0][runData.sensorHistIndex];
	runData.sumAcc[1] += runData.accHist[1][runData.sensorHistIndex];
	runData.sumAcc[2] += runData.accHist[2][runData.sensorHistIndex];

	// mag
	runData.sumMag[0] -= runData.magHist[0][runData.sensorHistIndex];
	runData.sumMag[1] -= runData.magHist[1][runData.sensorHistIndex];
	runData.sumMag[2] -= runData.magHist[2][runData.sensorHistIndex];

	runData.magHist[0][runData.sensorHistIndex] = IMU_MAGX;
	runData.magHist[1][runData.sensorHistIndex] = IMU_MAGY;
	runData.magHist[2][runData.sensorHistIndex] = IMU_MAGZ;

	runData.sumMag[0] += runData.magHist[0][runData.sensorHistIndex];
	runData.sumMag[1] += runData.magHist[1][runData.sensorHistIndex];
	runData.sumMag[2] += runData.magHist[2][runData.sensorHistIndex];

	// pressure
	runData.sumPres -= runData.presHist[runData.sensorHistIndex];
	runData.presHist[runData.sensorHistIndex] = AQ_PRESSURE;
	runData.sumPres += runData.presHist[runData.sensorHistIndex];

	runData.sensorHistIndex = (runData.sensorHistIndex + 1) % RUN_SENSOR_HIST;

	if (!((loops+1) % 20)) {
	   simDoAccUpdate(runData.sumAcc[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumAcc[2]*(1.0f / (float)RUN_SENSOR_HIST));
	}
	else if (!((loops+7) % 20)) {
	   simDoPresUpdate(runData.sumPres*(1.0f / (float)RUN_SENSOR_HIST));
	}
#ifndef USE_DIGITAL_IMU
	else if (!((loops+13) % 20) && AQ_MAG_ENABLED) {
	   simDoMagUpdate(runData.sumMag[0]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[1]*(1.0f / (float)RUN_SENSOR_HIST), runData.sumMag[2]*(1.0f / (float)RUN_SENSOR_HIST));
	}
#endif
	// optical flow update
	else if (navUkfData.flowCount >= 10 && !navUkfData.flowLock) {
	    navUkfFlowUpdate();
	}
	// only accept GPS updates if there is no optical flow
	else if (CoAcceptSingleFlag(gpsData.gpsPosFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.hAcc < NAV_MIN_GPS_ACC && gpsData.tDOP != 0.0f) {
	    navUkfGpsPosUpdate(gpsData.lastPosUpdate, gpsData.lat, gpsData.lon, gpsData.height, gpsData.hAcc + runData.accMask, gpsData.vAcc + runData.accMask);
	    CoClearFlag(gpsData.gpsPosFlag);
	    // refine static sea level pressure based on better GPS altitude fixes
	    if (gpsData.hAcc < runData.bestHacc && gpsData.hAcc < NAV_MIN_GPS_ACC) {
                navPressureAdjust(gpsData.height);
		runData.bestHacc = gpsData.hAcc;
	    }
	}
	else if (CoAcceptSingleFlag(gpsData.gpsVelFlag) == E_OK && navUkfData.flowQuality == 0.0f && gpsData.sAcc < NAV_MIN_GPS_ACC/2 && gpsData.tDOP != 0.0f) {
	    navUkfGpsVelUpdate(gpsData.lastVelUpdate, gpsData.velN, gpsData.velE, gpsData.velD, gpsData.sAcc + runData.accMask);
	    CoClearFlag(gpsData.gpsVelFlag);
	}
	// observe zero position
	else if (!((loops+4) % 20) && (gpsData.hAcc >= NAV_MIN_GPS_ACC || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
	    navUkfZeroPos();
	}
	// observer zero velocity
	else if (!((loops+10) % 20) && (gpsData.sAcc >= NAV_MIN_GPS_ACC/2 || gpsData.tDOP == 0.0f) && navUkfData.flowQuality == 0.0f) {
	    navUkfZeroVel();
	}
	// observe that the rates are exactly 0 if not flying or moving
	else if (!(supervisorData.state & STATE_FLYING)) {
	    float stdX, stdY, stdZ;

	    arm_std_f32(runData.accHist[0], RUN_SENSOR_HIST, &stdX);
	    arm_std_f32(runData.accHist[1], RUN_SENSOR_HIST, &stdY);
	    arm_std_f32(runData.accHist[2], RUN_SENSOR_HIST, &stdZ);

	    if ((stdX + stdY + stdZ) < (IMU_STATIC_STD*2)) {
		if (!((axis + 0) % 3))
		    navUkfZeroRate(IMU_RATEX, 0);
		else if (!((axis + 1) % 3))
		    navUkfZeroRate(IMU_RATEY, 1);
		else
		    navUkfZeroRate(IMU_RATEZ, 2);
		axis++;
	    }
	}

        navUkfFinish();
        altUkfProcess(AQ_PRESSURE);

        // determine which altitude estimate to use
        if (gpsData.hAcc > p[NAV_ALT_GPS_ACC]) {
            runData.altPos = &ALT_POS;
            runData.altVel = &ALT_VEL;
        }
        else {
            runData.altPos = &UKF_ALTITUDE;
            runData.altVel = &UKF_VELD;
        }

	CoSetFlag(runData.runFlag);	// new state data

	navNavigate();
#ifndef HAS_AIMU
	analogDecode();
#endif
	if (!(loops % (int)(1.0f / AQ_OUTER_TIMESTEP)))
	    loggerDoHeader();
	loggerDo();
	gimbalUpdate();

#ifdef CAN_CALIB
	canTxIMUData(loops);
#endif
        calibrate();

	loops++;
    }
}
示例#7
0
/**
 *******************************************************************************
 * @brief      Respond the request in the service request queue.
 * @param[in]  None
 * @param[out] None
 * @retval     None
 *
 * @par Description
 * @details    This function be called to respond the request in the service
 *             request queue.
 * @note
 *******************************************************************************
 */
void RespondSRQ(void) {

#if CFG_MAX_SERVICE_REQUEST > 0
	SQC cell;

#endif

#if (CFG_TASK_WAITTING_EN > 0)
	if(TimeReq == Co_TRUE) {               /* Time delay request?                */
		TimeDispose();                  /* Yes,call handler                   */
		TimeReq = Co_FALSE;                /* Reset time delay request Co_FALSE     */
	}
#endif
#if CFG_TMR_EN  > 0
	if(TimerReq == Co_TRUE) {              /* Timer request?                     */
		TmrDispose();                   /* Yes,call handler                   */
		TimerReq = Co_FALSE;               /* Reset timer request Co_FALSE          */
	}
#endif

#if CFG_MAX_SERVICE_REQUEST > 0

	while (ServiceReq.cnt != 0) {
		IRQ_DISABLE_SAVE ();            /* need to protect the following      */
		cell = ServiceReq.cell[ServiceReq.head];  /* extract one cell         */
		ServiceReq.head = (ServiceReq.head + 1) % /* move head (pop)          */
						  CFG_MAX_SERVICE_REQUEST;
		ServiceReq.cnt--;
		IRQ_ENABLE_RESTORE ();          /* now use the cell copy              */

		switch(cell.type) {             /* Judge service request type         */
#if CFG_SEM_EN > 0
			case SEM_REQ:                   /* Semaphore post request,call handler*/
				CoPostSem(cell.id);
				break;
#endif
#if CFG_MAILBOX_EN > 0
			case MBOX_REQ:                  /* Mailbox post request,call handler  */
				CoPostMail(cell.id, cell.arg);
				break;
#endif
#if CFG_FLAG_EN > 0
			case FLAG_REQ:                  /* Flag set request,call handler      */
				CoSetFlag(cell.id);
				break;
#endif
#if CFG_QUEUE_EN > 0
			case QUEUE_REQ:                 /* Queue post request,call handler    */
				CoPostQueueMail(cell.id, cell.arg);
				break;
#endif
			default:                        /* Others,break                       */
				break;
		}
	}
#endif
	IRQ_DISABLE_SAVE ();                /* need to protect the following      */

	if (ServiceReq.cnt == 0) {          /* another item in the queue already? */
		IsrReq = Co_FALSE;                 /* queue still empty here             */
	}
	IRQ_ENABLE_RESTORE ();              /* now it is done and return          */
}
示例#8
0
void btPushByte(uint8_t data)
{
  btTxFifo.push(data);
  CoSetFlag(btFlag); // Tell the Bt task something to do
}
示例#9
0
文件: main.c 项目: MejriTaoufik/CoOS
/**
 *******************************************************************************
 * @brief		Initialization task	  
 * @param[in] 	pdata	A pointer to parameter passed to task.	 
 * @param[out] 	None  
 * @retval		None
 *		 
 * @details	    This task is called to initial hardware and created tasks.
 *******************************************************************************
 */
void task_init(void *pdata)
{
	uart_printf (" [OK]. \n\r\n\r");
	uart_printf ("\r \"task_init\" task enter.		\n\r\n\r ");
	pdata = pdata; 
	 
    /* Initiate Time buffer for LCD display */
	chart[0] = time[2]/10 + '0';
	chart[1] = time[2]%10 + '0';
		
	chart[3] = time[1]/10 + '0';
	chart[4] = time[1]%10 + '0';

	chart[6] = time[0]/10 + '0';
	chart[7] = time[0]%10 + '0';	


	uart_printf ("\r Create the \"mut_uart\" mutex...      ");
	mut_uart = CoCreateMutex();
	if(mut_uart != E_CREATE_FAIL)
		uart_printf (" [OK]. \n");		
	else
		uart_printf (" [Fail]. \n");	


	uart_printf ("\r Create the \"mut_lcd\" mutex...       ");
	mut_lcd  = CoCreateMutex(); 
	if(mut_lcd != E_CREATE_FAIL)
		uart_printf (" [OK]. \n");		
	else
		uart_printf (" [Fail]. \n");	


	uart_printf ("\r Create the \"button_sel_flg\" flag... ");

	/*!< Manual reset flag,initial state:0 */
	button_sel_flg	= CoCreateFlag(Co_FALSE,0); 
	if(button_sel_flg != E_CREATE_FAIL)
		uart_printf (" [OK]. \n");		
	else
		uart_printf (" [Fail]. \n");	


	uart_printf ("\r Create the \"button_add_flag\" flag...");

	/*!< Manual reset flag,initial state:0	*/
	button_add_flg = CoCreateFlag(Co_FALSE,0);	
	if(button_add_flg != E_CREATE_FAIL)
		uart_printf (" [OK]. \n\n");		
	else
		uart_printf (" [Fail]. \n\n");	

	uart_printf ("\r Create the \"lcd_blink_flg\" flag...  ");
	lcd_blink_flg = CoCreateFlag(Co_FALSE,0); 	/*!< Manual reset flag,initial state:0	*/ 
	if(lcd_blink_flg != E_CREATE_FAIL)
		uart_printf (" [OK]. \n");		
	else
		uart_printf (" [Fail]. \n");
								  
	uart_printf ("\r Create the \"time_display_flg\" flag...");

	/*!< Manual reset flag,initial state:0	*/ 
	time_display_flg = CoCreateFlag(Co_FALSE,0); 
	if(time_display_flg != E_CREATE_FAIL)
		uart_printf (" [OK]. \n");		
	else
		uart_printf (" [Fail]. \n");

	/*!< Set flag to allow "time_display_flg" task run.	*/
	CoSetFlag(time_display_flg);
		

	uart_printf ("\r Create the first mailbox...         ");
	mbox0 = CoCreateMbox(EVENT_SORT_TYPE_FIFO);
 	if(mbox0 == E_CREATE_FAIL)
		uart_printf (" [Fail]. \n\n");		
	else
	    uart_printf (" [OK]. \n\n");
			



	/* Configure Peripheral */
	uart_printf ("\r Initial hardware in Board :     \n\r");

	uart_printf ("\r ADC initial...                      ");
	ADC_Configuration  ();
	uart_printf (" [OK]. \n");

	uart_printf ("\r RTC initial...                      ");
	RTC_Configuration  ();
	uart_printf (" [OK]. \n");

	uart_printf ("\r GPIO initial...                     ");
	GPIO_Configuration ();	
	uart_printf (" [OK]. \n");

	uart_printf ("\r External interrupt initial...       ");
	EXIT_Configuration ();
	uart_printf (" [OK]. \n");	

	uart_printf ("\r LCD initial...                      ");
	LCD_Configuration  ();		
	uart_printf (" [OK]. \n\n");
	

	/* Create Tasks */
	CoCreateTask(                       lcd_display_adc, 
	                                         (void *)0 , 
					                    LCD_DISPLAY_PRI ,
			     &lcd_display_adc_Stk[TASK_STK_SIZE-1] , 
					                     TASK_STK_SIZE
				 );

    CoCreateTask(    	              uart_print , 
	                                   (void *)0 , 
				                  UART_PRINT_PRI ,   
				 &uart_print_Stk[TASK_STK_SIZE-1],
					                TASK_STK_SIZE 
		        );
	CoCreateTask(                       led_blink , 
	                                    (void *)0 ,
	                                LCD_BLINK_PRI ,    
	             &led_display_Stk[TASK_STK_SIZE-1],
	                                 TASK_STK_SIZE 
	             );

	time_display_id = CoCreateTask(                         time_display, 
	                                                           (void *)0, 
								                       TIME_DISRPLAY_PRI,  
								    &time_display_Stk[TASK_STK_SIZE - 1], 
									                       TASK_STK_SIZE 
								  );

	CoCreateTask(                   time_set ,
			                       (void *)0 , 
			                    TIME_SET_PRI ,      
			   &time_set_Stk[TASK_STK_SIZE-1], 
			                    TASK_STK_SIZE 
			   );


	CoExitTask();	 /*!< Delete 'task_init' task. 	*/	
}
示例#10
0
文件: main.c 项目: MejriTaoufik/CoOS
/**
 *******************************************************************************
 * @brief		Set time task		  
 * @param[in] 	pdata	A pointer to parameter passed to task.	 
 * @param[out] 	None  
 * @retval		None
 *		 
 * @details	  	This task use to set time.  
 *******************************************************************************
 */
void time_set(void *pdata)
{
	static unsigned char bct = 0;			/* Button calc 					  */
	U16 evtmp = 0;

	pdata = pdata;
	for (;;)
	{
		evtmp = CoWaitForMultipleFlags(EVT_BUTTON_SEL|EVT_BUTTON_ADD,OPT_WAIT_ANY,0,&errinfo[20]);
		if(errinfo[20] != E_OK)
			uart_printf("\r Flag ERROR:\n");

		if (evtmp == EVT_BUTTON_SEL) 
		{
		 	bct++;
			switch (bct) 
			{	
				case 1: 
					timeflag = 1;
					if(lcd_blink_id == 0)
						lcd_blink_id = CoCreateTask (lcd_blink,(void *)0, LCD_BLINK_PRI ,&lcd_blink_Stk[TASK_STK_SIZE - 1], TASK_STK_SIZE );
					
					DisableRTCInterrupt();

					CoClearFlag(time_display_flg);
					CoSetFlag(lcd_blink_flg);
					break;
				case 2:
					timeflag = 2;
					break;
				case 3:
					timeflag = 3;
					break;
				case 4:	
					bct = 0; 
						
				   	CoClearFlag(lcd_blink_flg);
					CoSetFlag(time_display_flg);

					EnableRTCInterrupt();								
					break;
				default: break;
			};
			CoTickDelay(40);
			CoClearFlag(button_sel_flg);				
		}
		else if(evtmp == EVT_BUTTON_ADD) 
		{			
			CoEnterMutexSection(mut_lcd);			
		 	switch (bct)
			{
				case 1:
					time[0]++;
					if (time[0] >= 60) 
						time[0] = 0;
					chart[6] = time[0]/10 + '0';
					chart[7] = time[0]%10 + '0';
					break;				
				case 2:
					time[1]++;
					if (time[1] >= 60)
						time[1] = 0;
					chart[3] = time[1]/10 + '0';
					chart[4] = time[1]%10 + '0';
					break;				

				case 3:
					time[2]++;
					if (time[2] >= 24)
						time[2] = 0;
					chart[0] = time[2]/10 + '0';
					chart[1] = time[2]%10 + '0';
					break;

				default: break;
			}	
			set_cursor(7, 0);
			lcd_print (chart);
					
			CoLeaveMutexSection(mut_lcd);
			CoTickDelay(40);
			CoClearFlag(button_add_flg);						
		}
					
	}		
}