/*
*********************************************************************************************************
* Input PWM
*********************************************************************************************************
*
*********************************************************************************************************
*/
void InputTask(void *p_arg)
{
CPU_TS ts;
OS_ERR err;
TimerConfig();
SetUpTimer1();
SetUpTimer3();
while (1)
{
OSTimeDly(15,OS_OPT_TIME_DLY,&err); //Read every 150ms
if(TIM3->SR & (1 << 1)) //if status register is true go fetch the Period
{
Period = TIM3->CCR1;
}
if(TIM3->SR & (1 << 2)) //if status register is true go fetch the Duty Cycle
{
DutyCycle = TIM3->CCR2;
}
//Save la valeur de Commande dans la variable golbale
OSMutexPend(&mutTMR3,0,OS_OPT_PEND_BLOCKING,&ts,&err);
Command = DutyCycle;
OSMutexPost(&mutTMR3,OS_OPT_POST_NONE,&err);
}
}
void UART1_PrintCh (uint8_t ch)
{
#if 0
OS_ERR err;
CPU_TS ts;
OSMutexPend((OS_MUTEX *)&PC_UART_DEVICE,
(OS_TICK )0,
(OS_OPT )OS_OPT_PEND_BLOCKING,
(CPU_TS *)&ts,
(OS_ERR *)&err);
/* Loop until USARTy DR register is empty */
while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);
/* Send one byte from USARTy to USARTz */
USART_SendData(USART1, (uint8_t)ch);
OSMutexPost((OS_MUTEX *)&PC_UART_DEVICE,
(OS_OPT )OS_OPT_POST_NONE,
(OS_ERR *)&err);
#else
/* Loop until USARTy DR register is empty */
while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);
/* Send one byte from USARTy to USARTz */
USART_SendData(USART1, (uint8_t)ch);
#endif
}
void task2(task* pdata)
{
int start = 0;
int end;
int delay;
INT8U err;
int arrival = 3;
OSTCBCur->Period = pdata->period;
OSTCBCur->Exec = pdata->exec;
OSTCBCur->ExecTime = pdata->exec;
OSTCBCur->Start = 0;
OSTCBCur->Deadline = pdata->period + arrival;
OSTCBCur->Org_Deadline = pdata->period + arrival;
OSTimeDly(arrival);
while (1)
{
printf("\t%d\tTask_2\n",OSTimeGet());
mywait(OSTCBCur->Exec); // CPU time
printf("\t%d\tTask_2 get R2\t",OSTimeGet());
OSMutexPend(R2, 0, &err);
mywait(3); // R2
printf("\t%d\tTask_2 release R2",OSTimeGet());
OSMutexPost(R2);
end = OSTimeGet();
delay = OSTCBCur->Deadline-end; // let it delay 1 timetick to avoid running the while loop
OSTCBCur->Resp = end-start;
start += pdata->period;
OSTCBCur->Deadline += OSTCBCur->Period;
OSTCBCur->Org_Deadline = OSTCBCur->Deadline;
OSTimeDly(delay);
}
}
void motion_SetDefaultSpeed(float speed)
{
int i;
BOOL found = FALSE;
OSMutexPend(mtxDefaultSpeeds, 0, &OSLastError);
LOG_TEST_OS_ERROR(OSLastError);
defSpeed = speed;
//find appropriate speed and set mouse resolution
for(i=0; i<CALIB_SPEED_NB; i++)
{
if(speed == calibSpeed[i])
{
encoder_SetResolution(calibMouse[i][0], calibMouse[i][1]);
found = TRUE;
break;
}
}
motors_ConfigAllIMax(SLIPPAGE_RATIO * MAX(speed, 0.3f));
if(!found)
{
encoder_SetResolution(DEFAULT_LEFT_TICKS_PER_M, DEFAULT_RIGHT_TICKS_PER_M);
}
OSMutexPost(mtxDefaultSpeeds);
}
/******************************************************
TimeSet - takes the programmed time of the clock, and
* sets it to the TimeofDay
Passes in: ltime
Passes out: nothing
*******************************************************/
void TimeSet(TIME *ltime)
{
INT8U err;
OSMutexPend(ClockMutexKey,0,&err);
TimeOfDay = *ltime;
OSMutexPost(ClockMutexKey);
}
/******************************************************
TimeGet- sets the value of TimeofDay to ltime to be displyed
on the LCD
Passes in: nothing
Passes out : TimeOfDay
*******************************************************/
void TimeGet(TIME *ltime)
{
INT8U err;
OSMutexPend(ClockMutexKey,0,&err);
*ltime = TimeOfDay ;
OSMutexPost(ClockMutexKey);
}
int8_t avgAllArrays(){
INT8U err = OS_NO_ERR;
int16_t avgTempData[9];
int i,j;
for (i = 0; i < 9; i++) {
int16_t sum = 0;
for (j = 0; j < VALUE_NUM; j++) {
sum += (arrs[i])[j];
}
avgTempData[i] = sum/VALUE_NUM;
}
//get Semaphore for the avg Data
OSMutexPend(sensorDataMutex, 0, &err);
for(i = 0; i<9 ;i++){
avgData[i] = avgTempData[i];
}
//release Semaphore for the avg Data
err = OSMutexPost(sensorDataMutex);
return err;
}
void osReleaseMutex(OsMutex *mutex)
{
OS_ERR err;
//Release ownership of the mutex object
OSMutexPost(mutex, OS_OPT_POST_NONE, &err);
}
/* This function is called on leaving file functions to unlock the volume.
*/
void ff_rel_grant (
_SYNC_t sobj /* Sync object to be signaled */
)
{
OS_ERR err;
OSMutexPost(&sobj, OS_OPT_POST_NONE, &err); /* uC/OS-III */
}
static void appTaskCanReceive(void *pdata) {
uint8_t error;
canMessage_t msg;
/* Install the CAN interrupt handler and start the OS ticker
* (must be done in the highest priority task)
*/
canRxInterrupt(canHandler);
osStartTick();
/*
* Now execute the main task loop for this task
*/
while ( true ) {
OSSemPend(can1RxSem, 0, &error);
msg = can1RxBuf;
interfaceLedToggle(D1_LED);
OSMutexPend(displayMutex, 0, &error);
lcdSetTextPos(2,1);
lcdWrite("ID : %08x", msg.id);
lcdSetTextPos(2,2);
lcdWrite("LEN : %08x", msg.len);
lcdSetTextPos(2,3);
lcdWrite("DATA_A : %08x", msg.dataA);
lcdSetTextPos(2,4);
lcdWrite("DATA_B : %08x", msg.dataB);
error = OSMutexPost(displayMutex);
}
}
int Socket_TCPClientSendData(char *sendbuf, unsigned int len)
{
int total = 0;
int n = 0;
unsigned char err = 0;
static int cloud_error_cnt = 0;
fd_set writeset;
OSMutexPend(cloud_send_mutex_lock, 0, &err);
while (len != total)
{
/*Ì×½Ú×Ö¼¯ºÏÇå¿Õ */
FD_ZERO(&writeset);
/*¼ÓÈë¶ÁдÌ×½Ó×Ö¼¯ºÏ*/
FD_SET(TCPClientFd, &writeset);
/*¼ì²âÌ×½Ó×ÖÊÇ·ñ¿Éд*/
if(select(TCPClientFd+1, NULL, &writeset, NULL, NULL) == -1)
return -1;
if(FD_ISSET(TCPClientFd, &writeset))
{
n = send(TCPClientFd, (sendbuf + total), (len - total), 0);
if (n < 0)
{
log_err("Send TCP client data(len:%d, total:%d) ERROR.\n", len, total);
OSMutexPost(cloud_send_mutex_lock);
/* if send time error more than 5 timse, reset tcp connection */
cloud_error_cnt++;
if (cloud_error_cnt > 5) {
TCPClient_reset_flag = 0xa5;
}
return n;
}
total += n;
}
}
OSMutexPost(cloud_send_mutex_lock);
return total;
}
void motion_SetDefaultDecel(float decel)
{
OSMutexPend(mtxDefaultSpeeds, 0, &OSLastError);
LOG_TEST_OS_ERROR(OSLastError);
defDecel = decel;
OSMutexPost(mtxDefaultSpeeds);
}
void BSP_OS_GLCD_MutexPost (void)
{
OS_ERR err;
OSMutexPost(&GLCD_Mutex,
OS_OPT_POST_NONE,
&err);
}
void BSP_OS_RSPI0_MutexPost (void)
{
OS_ERR err;
OSMutexPost(&RSPI0_Mutex,
OS_OPT_POST_NONE,
&err);
}
/*!
* @brief Display data on the LCD, safely.
* @param[in] Position and data.
*/
void protected_lcd_display(uint8_t position, const uint8_t *data)
{
OS_ERR err;
OSMutexPend(&lcd_mutex, 0, OS_OPT_PEND_BLOCKING, 0, &err);
assert(OS_ERR_NONE == err);
DisplayLCD(position, data);
OSMutexPost(&lcd_mutex, OS_OPT_POST_NONE, &err);
assert(OS_ERR_NONE == err);
}
/*!
* @brief Display data on the LCD, safely.
* @param[in] Position and data.
*/
void protected_lcd_clear(void)
{
OS_ERR err;
OSMutexPend(&lcd_mutex, 0, OS_OPT_PEND_BLOCKING, 0, &err);
assert(OS_ERR_NONE == err);
ClearLCD();
OSMutexPost(&lcd_mutex, OS_OPT_POST_NONE, &err);
assert(OS_ERR_NONE == err);
}
/*FUNCTION**********************************************************************
*
* Function Name : OSA_MutexUnlock
* Description : This function is used to unlock a mutex.
*
*END**************************************************************************/
osa_status_t OSA_MutexUnlock(mutex_t *pMutex)
{
if (OS_ERR_NONE == OSMutexPost(*pMutex))
{
return kStatus_OSA_Success;
}
else
{
return kStatus_OSA_Error;
}
}
static void appTaskCanMonitor(void *pdata) {
uint8_t error;
while (true) {
OSMutexPend(displayMutex, 0, &error);
lcdSetTextPos(2,5);
lcdWrite("CAN1GSR: %08x", canStatus(CAN_PORT_1));
error = OSMutexPost(displayMutex);
OSTimeDly(20);
}
}
static void AppTaskObj1 (void *p_arg)
{
OS_ERR os_err;
OS_MSG_SIZE msg_size;
(void)p_arg;
while (DEF_TRUE) {
#if (OS_CFG_SEM_EN > 0u)
OSSemPend(&AppTaskObjSem,
0,
OS_OPT_PEND_BLOCKING,
0,
&os_err);
#endif
#if (OS_CFG_MUTEX_EN > 0u)
OSMutexPend(&AppTaskObjMutex,
0,
OS_OPT_PEND_BLOCKING,
0,
&os_err);
OSMutexPost(&AppTaskObjMutex,
OS_OPT_POST_NONE,
&os_err);
#endif
#if (OS_CFG_Q_EN > 0u)
OSQPend(&AppTaskObjQ,
0,
OS_OPT_PEND_BLOCKING,
&msg_size,
0,
&os_err);
#endif
#if (OS_CFG_FLAG_EN > 0u)
OSFlagPend(&AppTaskObjFlag,
DEF_BIT_00,
0,
OS_OPT_PEND_FLAG_SET_ALL + OS_OPT_PEND_FLAG_CONSUME + OS_OPT_PEND_BLOCKING,
0,
&os_err);
#endif
OSTimeDlyHMSM( 0u, 0u, 0u, 10u,
OS_OPT_TIME_HMSM_STRICT,
&os_err);
APP_TRACE_INFO(("Object test task 1 running ....\n"));
}
}
float motion_GetDefaultDecel()
{
float decel;
OSMutexPend(mtxDefaultSpeeds, 0, &OSLastError);
LOG_TEST_OS_ERROR(OSLastError);
decel = defDecel;
OSMutexPost(mtxDefaultSpeeds);
return decel;
}
static void test_task2(void* p_arg)
{
p_arg = p_arg;
while(1) {
OSMutexPost(p_mutex);
printk("produce ");
OSTimeDly(10);
}
}
void ff_rel_grant (
_SYNC_t sobj /* Sync object to be signaled */
)
{
// ReleaseMutex(sobj); /* Win32 */
// sig_sem(sobj); /* uITRON */
OSMutexPost(sobj); /* uC/OS-II */
// xSemaphoreGive(sobj); /* FreeRTOS */
}
/*!
* @brief Initialize the reentrant LCD driver.
*/
void protected_lcd_init(void)
{
OS_ERR err;
OSMutexCreate(&lcd_mutex, "LCD Mutex", &err);
assert(OS_ERR_NONE == err);
OSMutexPend(&lcd_mutex, 0, OS_OPT_PEND_BLOCKING, 0, &err);
assert(OS_ERR_NONE == err);
InitialiseLCD();
OSMutexPost(&lcd_mutex, OS_OPT_POST_NONE, &err);
assert(OS_ERR_NONE == err);
}
float motion_GetDefaultSpeed()
{
float speed;
OSMutexPend(mtxDefaultSpeeds, 0, &OSLastError);
LOG_TEST_OS_ERROR(OSLastError);
speed = defSpeed;
OSMutexPost(mtxDefaultSpeeds);
return speed;
}
void Solenoid::lock() {
INT8U err = OS_NO_ERR;
OSMutexPend(solenoidMutex, 0, &err);
if(err != OS_NO_ERR){
printf("Error pending on solenoid mutex\n");
return;
}
IOWR_ALTERA_AVALON_PIO_DATA(SOLENOID_CONTROLLER_BASE, LOCKED);
err = OSMutexPost(solenoidMutex);
if(err != OS_NO_ERR){
printf("Error posting to solenoid mutex\n");
}
}
static void AppTaskObj0 (void *p_arg)
{
OS_ERR os_err;
(void)p_arg;
while (DEF_TRUE) {
#if (OS_CFG_SEM_EN > 0u)
OSSemPost(&AppTaskObjSem,
OS_OPT_POST_1,
&os_err);
#endif
#if (OS_CFG_MUTEX_EN > 0u)
OSMutexPend(&AppTaskObjMutex,
0,
OS_OPT_PEND_BLOCKING,
0,
&os_err);
OSTimeDlyHMSM( 0u, 0u, 0u, 100u,
OS_OPT_TIME_HMSM_STRICT,
&os_err);
OSMutexPost(&AppTaskObjMutex,
OS_OPT_POST_NONE,
&os_err);
#endif
#if (OS_CFG_Q_EN > 0u)
OSQPost( &AppTaskObjQ,
(void *) 1u,
1u,
OS_OPT_POST_FIFO,
&os_err);
#endif
#if (OS_CFG_FLAG_EN > 0u)
OSFlagPost(&AppTaskObjFlag,
DEF_BIT_00,
OS_OPT_POST_FLAG_SET,
&os_err);
#endif
OSTimeDlyHMSM( 0u, 0u, 0u, 10u,
OS_OPT_TIME_HMSM_STRICT,
&os_err);
APP_TRACE_INFO(("Object test task 0 running ....\n"));
}
}
/*
*********************************************************************************************************
* App_Task_KEY()
*
* Description : This is an example of an application task.
*
* Argument(s) : p_arg is the argument passed to 'App_Task_KEY()' by 'OSTaskCreate()'.
*
* Return(s) : none.
*
* Caller(s) : This is a task.
*
* Notes : (1) The first line of code is used to prevent a compiler warning because 'p_arg' is not
* used. The compiler should not generate any code for this statement.
*********************************************************************************************************
*/
static void App_Task_KEY (void *p_arg)
{
OS_ERR os_err;
CPU_TS ts;
(void)p_arg;
while (DEF_TRUE) {
OSTimeDly ((OS_TICK )1,
(OS_OPT )OS_OPT_TIME_DLY,
(OS_ERR *)&os_err);
while (BSP_SW_Read (BSP_SW_1) == DEF_ON) {
OSTimeDly(1, OS_OPT_TIME_HMSM_STRICT, &os_err);
}
//OS_CRITICAL_ENTER();
OSQPost((OS_Q *)&AppQ, /* sends a message to another task using the message queue AppQ. */
(void *)1, /* sends a fixed message of value "1". */
(OS_MSG_SIZE)sizeof(void *),
(OS_OPT )OS_OPT_POST_FIFO,
(OS_ERR *)&os_err);
OSMutexPend((OS_MUTEX *)&AppMutex, /* waits on the mutual exclusion semaphore. */
(OS_TICK )0,
(OS_OPT )OS_OPT_PEND_BLOCKING,
(CPU_TS *)&ts,
(OS_ERR *)&os_err);
/* Access shared resource */
OSMutexPost((OS_MUTEX *)&AppMutex, /* must call OSMutexPost() to release the mutex. */
(OS_OPT )OS_OPT_POST_NONE,
(OS_ERR *)&os_err);
//BSP_LED_Toggle(BSP_LED_RED);
//OSTimeDlyResume(&App_Task_LED_TCB, &os_err);
while (BSP_SW_Read (BSP_SW_1) == DEF_OFF) {
OSTimeDly(1, OS_OPT_TIME_HMSM_STRICT, &os_err);
}
OSTimeDlyHMSM(0, 0, 0, 50,
OS_OPT_TIME_HMSM_STRICT, &os_err); /* Suspend the task execution for 50 milliseconds. */
}
}
/*
*********************************************************************************************************
* ReportAdcSample
*********************************************************************************************************
* Arguments:
* uChannel: ADC channel sepecification.
* pstrADControlBlock: pointer to the channel control block.
*********************************************************************************************************
*/
static void ReportAdcSample(CPU_INT08U uChannel,
ADC_CONTROL_BLOCK_t* pstrADControlBlock)
{
OS_ERR err;
CPU_TS ts;
OSMutexPend(&mutADC,0,OS_OPT_PEND_BLOCKING,&ts,&err);
strSemADC.uValue = ADC1->JDR1;
OSMutexPost(&mutADC,OS_OPT_POST_NONE,&err);
OSSemPost(&semADC_Complete,OS_OPT_POST_NONE,&err);
}
static void OS_TmrUnlock (void)
{
OS_ERR err;
#if OS_CFG_MUTEX_EN > 0u
OSMutexPost(&OSTmrMutex, /* Use a mutex to protect the timers */
OS_OPT_POST_NONE,
&err);
#else
OSSchedUnlock(&err); /* Lock the scheduler to protect the timers */
#endif
(void)&err;
}
/*FUNCTION**********************************************************************
*
* Function Name : OSA_MutexUnlock
* Description : This function is used to unlock a mutex.
*
*END**************************************************************************/
osa_status_t OSA_MutexUnlock(mutex_t *pMutex)
{
OS_ERR err;
OSMutexPost(pMutex, OS_OPT_POST_NONE, &err);
if (OS_ERR_NONE == err)
{
return kStatus_OSA_Success;
}
else
{
return kStatus_OSA_Error;
}
}