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); }
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); } }
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)); } }
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; }
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; }
/// 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; }
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; }
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; }
/** * 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)); } } }
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++; } }