FreeRTOSTimer::FreeRTOSTimer(tmrTIMER_CALLBACK pFunction, TIME_MS t, TimerType type)
{
	// xTimerCreate() copies the pointer for the name, so the tName variable
	// cannot simply be on the stack, it should be global or static
	static signed char tName[] = "Tmr";

	mTimerHandle = xTimerCreate(tName, OS_MS(t),
								type == TimerOneShot ? pdFALSE : pdTRUE,
								0,
								pFunction);
}
Exemple #2
0
bool I2C_Base::transfer(char deviceAddress, char firstReg, char* pData, unsigned int transferSize)
{
    bool status = false;
    if(mDisableOperation || !pData) {
        return status;
    }

    // If scheduler not running, perform polling transaction
    if(taskSCHEDULER_RUNNING != xTaskGetSchedulerState())
    {
        i2cKickOffTransfer(deviceAddress, firstReg, pData, transferSize);

        // Wait for transfer to finish
        const uint64_t timeout = sys_get_uptime_ms() + I2C_TIMEOUT_MS;
        while (!xSemaphoreTakeFromISR(mTransferCompleteSignal, NULL)) {
            if (sys_get_uptime_ms() > timeout) {
                break;
            }
        }

        status = (0 == mTransaction.error);
    }
    else if (xSemaphoreTake(mI2CMutex, OS_MS(I2C_TIMEOUT_MS)))
    {
        // Clear potential stale signal and start the transfer
        xSemaphoreTake(mTransferCompleteSignal, 0);
        i2cKickOffTransfer(deviceAddress, firstReg, pData, transferSize);

        // Wait for transfer to finish and copy the data if it was read mode
        if (xSemaphoreTake(mTransferCompleteSignal, OS_MS(I2C_TIMEOUT_MS))) {
            status = (0 == mTransaction.error);
        }

        xSemaphoreGive(mI2CMutex);
    }

    return status;
}
bool I2C_Base::readRegisters(char deviceAddress, char firstReg, char* pData, unsigned int bytesToRead)
{
    if(mDisableOperation) {
        return 0;
    }

    if(bytesToRead > sizeof(mI2CIOFrame.rwBuffer)) {
        return false;
    }

    // If scheduler not running, perform polling transaction
    if(taskSCHEDULER_RUNNING != xTaskGetSchedulerState())
    {
        NVIC_DisableIRQ(mIRQ);
        {
            i2cKickOffTransfer(i2cRead, deviceAddress, firstReg, pData, bytesToRead);
            do {
                // Wait until SI flag is set, then call i2cStateMachine()
                while(! (mpI2CRegs->I2CONSET & (1 << 3)) )
                {
                    ;
                }
            }while (readComplete != i2cStateMachine());
        }
        memcpy(pData, &mI2CIOFrame.rwBuffer[0], bytesToRead);
        NVIC_EnableIRQ(mIRQ);

        return (0 == mI2CIOFrame.error);
    }

    // Wait to get I2C Access, and take clear read complete signal if it's available
    xSemaphoreTake(mI2CMutex, portMAX_DELAY);
    xSemaphoreTake(mReadCompSig, 0);

    // Kick off the transfer and wait for read to finish
    i2cKickOffTransfer(i2cRead, deviceAddress, firstReg, pData, bytesToRead);
    xSemaphoreTake(mReadCompSig, OS_MS(I2C_READ_TIMEOUT_MS));

    // Transfer is successful if error is nonzero
    const bool success = (0 == mI2CIOFrame.error);
    if(success) {
        memcpy(pData, &mI2CIOFrame.rwBuffer[0], bytesToRead);
    }

    // After read is done, give the I2C access back.
    xSemaphoreGive(mI2CMutex);

    return success;
}
/**
 * Delays in milliseconds
 * @param delayMilliSec The delay in milliseconds.
 */
void delay_ms(unsigned int delayMilliSec)
{
    /**
     * If FreeRTOS is running, we should sleep the calling task instead
     * of polling using the full CPU.
     */
    if(isFreeRtosRunning())
    {
        vTaskDelay(OS_MS(delayMilliSec));
    }
    else
    {
        delay_us(1000 * delayMilliSec);
    }
}
Exemple #5
0
void sender(void *p)
{
    OSHANDLES *osHandles = (OSHANDLES*)p;
    char songName[15] = "ESpark.mp3";
    for(;;)
    {
        rprintf("    %s(): Sending song-name to Queue ...\n", __FUNCTION__);
        if(xQueueSend(osHandles->queue.songname, &songName[0], 100)) {
            rprintf("    Song Sent!\n");
        }
        else {
            rprintf("Timeout during Send!!!\n");
        }
        vTaskDelay(OS_MS(5000));
    }
}
Exemple #6
0
bool CAN_tx (can_t can, can_msg_t *pCanMsg, uint32_t timeout_ms)
{
    if (!CAN_VALID(can) || !pCanMsg || CAN_is_bus_off(can)) {
        return false;
    }

    bool ok = false;
    can_struct_t *pStruct = CAN_STRUCT_PTR(can);
    LPC_CAN_TypeDef *CANx = pStruct->pCanRegs;

    /* Try transmitting to one of the available buffers */
    taskENTER_CRITICAL();
    do {
        ok = CAN_tx_now(pStruct, pCanMsg);
    } while(0);
    taskEXIT_CRITICAL();

    /* If HW buffer not available, then just queue the message */
    if (!ok) {
        if (taskSCHEDULER_RUNNING == xTaskGetSchedulerState()) {
            ok = xQueueSend(pStruct->txQ, pCanMsg, OS_MS(timeout_ms));
        }
        else {
            ok = xQueueSend(pStruct->txQ, pCanMsg, 0);
        }

        /* There is possibility that before we queued the message, we got interrupted
         * and all hw buffers were emptied meanwhile, and our queued message will now
         * sit in the queue forever until another Tx interrupt takes place.
         * So we dequeue it here if all are empty and send it over.
         */
        taskENTER_CRITICAL();
        do {
            can_msg_t msg;
            if (tx_all_avail == (CANx->SR & tx_all_avail) &&
                xQueueReceive(pStruct->txQ, &msg, 0)
            ) {
                ok = CAN_tx_now(pStruct, &msg);
            }
        } while(0);
        taskEXIT_CRITICAL();
    }

    return ok;
}
Exemple #7
0
bool CAN_tx (can_t can, can_msg_t *pCanMsg, uint32_t timeout_ms)
{
    volatile LPC_CAN_TypeDef *CANx = CAN_ENUM_TO_REGS(can);
    const uint8_t cidx = CAN_INDEX(can);
    bool ok = false;

    if (!CANx || !pCanMsg) {
        return false;
    }

    /* Try transmitting to one of the available buffers */
    vPortEnterCritical();
    {
        ok = CAN_tx_now(CANx, pCanMsg);
    }
    vPortExitCritical();

    /* If all three buffers are busy, just queue the message */
    if (!ok) {
        if (taskSCHEDULER_RUNNING == xTaskGetSchedulerState()) {
            ok = xQueueSend(g_can_tx_qs[cidx], pCanMsg, OS_MS(timeout_ms));
        }
        else {
            ok = xQueueSendFromISR(g_can_tx_qs[cidx], pCanMsg, NULL);
        }

        /* There is possibility that before we queued the message, we got interrupted
         * and all hw buffers were emptied meanwhile, and our queued message will now
         * sit in the queue forever until another Tx interrupt takes place.
         * So we dequeue it here if all are empty and send it over.
         */
        vPortEnterCritical();
        {
            can_msg_t msg;
            if (tx_all_avail == (CANx->SR & tx_all_avail) &&
                xQueueReceiveFromISR(g_can_tx_qs[cidx], &msg, NULL)
            ) {
                ok = CAN_tx_now(CANx, &msg);
            }
        }
        vPortExitCritical();
    }

    return ok;
}
Exemple #8
0
/// Retrieves a packet from the queue handle with the given timeout.
static char wireless_get_queued_pkt(QueueHandle_t qhandle, mesh_packet_t *pkt, const uint32_t timeout_ms)
{
    char ok = 0;

    if (taskSCHEDULER_RUNNING == xTaskGetSchedulerState()) {
        ok = xQueueReceive(qhandle, pkt, OS_MS(timeout_ms));
    }
    else {
        uint64_t timeout_of_char = sys_get_uptime_ms() + timeout_ms;
        while (! (ok = xQueueReceive(qhandle, pkt, 0))) {
            if (sys_get_uptime_ms() > timeout_of_char) {
                break;
            }
        }
    }

    return ok;
}
Exemple #9
0
bool CAN_rx (can_t can, can_msg_t *pCanMsg, uint32_t timeout_ms)
{
    bool ok = false;

    if (CAN_VALID(can) && pCanMsg)
    {
        if (taskSCHEDULER_RUNNING == xTaskGetSchedulerState()) {
            ok = xQueueReceive(CAN_STRUCT_PTR(can)->rxQ, pCanMsg, OS_MS(timeout_ms));
        }
        else {
            uint64_t msg_timeout = sys_get_uptime_ms() + timeout_ms;
            while (! (ok = xQueueReceive(CAN_STRUCT_PTR(can)->rxQ, pCanMsg, 0))) {
                if (sys_get_uptime_ms() > msg_timeout) {
                    break;
                }
            }
        }
    }

    return ok;
}
Exemple #10
0
bool CAN_rx (can_t can, can_msg_t *pCanMsg, uint32_t timeout_ms)
{
    bool ok = false;

    if (CAN_VALID(can) && pCanMsg)
    {
        if (taskSCHEDULER_RUNNING == xTaskGetSchedulerState()) {
            ok = xQueueReceive(g_can_rx_qs[CAN_INDEX(can)], pCanMsg, OS_MS(timeout_ms));
        }
        else {
            uint64_t msg_timeout = sys_get_uptime_ms() + timeout_ms;
            while (! (ok = xQueueReceiveFromISR(g_can_rx_qs[CAN_INDEX(can)], pCanMsg, NULL))) {
                if (sys_get_uptime_ms() > msg_timeout) {
                    break;
                }
            }
        }
    }

    return ok;
}
Exemple #11
0
/**
 * This is the C task that is created multiple times.  Each task receives its
 * own parameter therefore differentiating between different tasks.
 */
void scheduler_c_task_private(void *task_ptr)
{
    scheduler_task& task = *(scheduler_task*)task_ptr;

    /* Have the responsible task call taskEntry() for each task */
    if (mTaskEntryTaskHandle == task.mHandle) {
        bool failure = false;

        print_strings(task.mName, " task calling taskEntry() for all tasks ... ");
        for (uint32_t i=0; i < gTaskList->size(); i++)
        {
            scheduler_task *t = gTaskList->at(i);
            if (!t->taskEntry()) {
                print_strings(t->mName, "  --> FAILED taskEntry()");
                failure = true;
            }
        }

        if (failure) {
            puts("ERROR: Killing FreeRTOS due to error(s)");
            vTaskEndScheduler();
        } else {
            /* Give permission for everyone to start the run() loop */
            for (uint32_t i=0; i < gTaskList->size(); i++)
            {
                xSemaphoreGive(mRunTask);
            }
        }
    }

    // Wait until we're given the go ahead by the task giving semaphore above
    xSemaphoreTake(mRunTask, portMAX_DELAY);

    portTickType xLastWakeTime = xTaskGetTickCount();
    portTickType xNextStatTime = 0;

    for (;;)
    {
#if (0 != configUSE_QUEUE_SETS)
        if (task.mQueueSet) {
            task.mQueueSetType = xQueueSelectFromSet(task.mQueueSet, task.mQueueSetBlockTime);
        }
#endif

        // Run the task code and suspend when an error occurs
        if (!task.run((void*)task.mParam)) {
            print_strings(task.mName, " --> FAILURE detected; suspending this task ...");
            vTaskSuspend(0);
        }
        ++(task.mRunCount);

        // Update the task statistics once in a short while :
        if (xTaskGetTickCount() > xNextStatTime) {
            xNextStatTime = xTaskGetTickCount() + OS_MS(task.mStatUpdateRateMs);
            task.mFreeStack = uxTaskGetStackHighWaterMark(task.mHandle);
            task.mCpuPercent = uxTaskGetCpuUsage(task.mHandle);
        }

        // Delay if set
        if (task.mTaskDelayMs) {
            vTaskDelayUntil( &xLastWakeTime, OS_MS(task.mTaskDelayMs));
        }
    }
}
Exemple #12
0
void flight_task(void *pvParameters)
{
	OSHANDLES *osHandles = (OSHANDLES*)pvParameters;

	setup_motors();
	write_motors_zero();

	memset(&(osHandles->flight_control), 0, sizeof(osHandles->flight_control));

	/* -----------------    P    I   D   limit   */
	PID_DATA pid_pitch = { 700, 001, 000, 100 };
	PID_DATA pid_roll =  { 700, 001, 000, 100 };
	PID_DATA pid_yaw =   {1200, 005, 000, 100 };
	osHandles->flight_settings.pid_pitch = &pid_pitch;
	osHandles->flight_settings.pid_roll = &pid_roll;
	osHandles->flight_settings.pid_yaw = &pid_yaw;

	osHandles->flight_control.tx_throttle = 1000;
	osHandles->flight_control.tx_yaw = 1500;
	osHandles->flight_control.tx_pitch = 1500;
	osHandles->flight_control.tx_roll = 1500;
	osHandles->flight_settings.pitch_roll_tx_scale = 16;
	osHandles->flight_settings.yaw_tx_scale = 16;

	SENSOR_DATA zero_vals = { 0 };
	vTaskDelay(OS_MS(500));
	init_wii_sensors();
	zero_wii_sensors(&zero_vals);

	//int pitch_integral = 0;
    //unsigned int last_ms = xTaskGetTickCount();
	unsigned int loop_count = 0;
	SENSOR_DATA sensor_vals;
	int pitch_offset = 0,roll_offset = 0,yaw_offset = 0;

	for(;;)  //this is a continuous task.
	{
		unsigned char data_type = update_wii_data(&sensor_vals, &zero_vals);
		if (data_type == 1){
			//unsigned int this_ms = xTaskGetTickCount();
			//pitch_integral += sensor_vals.pitch*(this_ms-last_ms); //deg/s * ms = degrees*1000
			//last_ms = this_ms;

			pitch_offset = calculate_pid( sensor_vals.pitch+1500, osHandles->flight_control.tx_pitch, &pid_pitch);
			roll_offset  = calculate_pid( sensor_vals.roll+1500,  osHandles->flight_control.tx_roll, &pid_roll);
			yaw_offset   = calculate_pid( sensor_vals.yaw+1500,   osHandles->flight_control.tx_yaw, &pid_yaw);
		}
		//osHandles->flight_control.tx_throttle;
		if (osHandles->flight_control.armed == 3){
			if (osHandles->flight_settings.flying_mode == PLUS_MODE){
				write_motors(
					osHandles->flight_control.tx_throttle + pitch_offset - yaw_offset, //front  //TODO: add option to change yaw direction.
					osHandles->flight_control.tx_throttle - pitch_offset - yaw_offset, //back
					osHandles->flight_control.tx_throttle + roll_offset + yaw_offset,  //left
					osHandles->flight_control.tx_throttle - roll_offset + yaw_offset   //right
					);
			}
			else if (osHandles->flight_settings.flying_mode == X_MODE){
			// Front = Front/Right, Back = Left/Rear, Left = Front/Left, Right = Right/Rear
				write_motors(
					osHandles->flight_control.tx_throttle + pitch_offset + roll_offset + yaw_offset, //front
					osHandles->flight_control.tx_throttle - pitch_offset - roll_offset + yaw_offset, //back
					osHandles->flight_control.tx_throttle - pitch_offset + roll_offset - yaw_offset,  //left
					osHandles->flight_control.tx_throttle + pitch_offset - roll_offset - yaw_offset   //right
					);
			}
			//safety disarm if not updated command recently enough.
			if (osHandles->flight_control.command_used_number++ > 75) { osHandles->flight_control.armed = 0; write_motors_zero(); }
		}
		else {
			if (osHandles->flight_control.please_update_sensors){ //allows the user interface task to zero the sensor values.
				osHandles->flight_control.please_update_sensors = 0;
				zero_wii_sensors(&zero_vals);
				pulse_motors(3, 200);
				if (osHandles->flight_control.telem_mode) {rprintf("zero-ed angles.\n");}
			}
			osHandles->flight_control.command_used_number++;
		}


    	if (!(loop_count%100)&& osHandles->flight_control.telem_mode) { rprintf("ofst[p,r,y]=[%i,%i,%i] <tx_p=%i> srs={%i-%i-%i}\n",  pitch_offset, roll_offset, yaw_offset, osHandles->flight_control.tx_pitch ,sensor_vals.pitch,sensor_vals.roll,sensor_vals.yaw); }

		//write_PWM_servo(2, calculate_pid(pitch_integral/1000, 0, &(osHandles->flight_settings->pid_roll)) + 1500 );

		vTaskDelay(MAX_WII_SENSOR_POLLING_RATE);
		loop_count++;
	}
}