void motor_driver_update_trajectory(motor_driver_t *d, trajectory_chunk_t *traj) { chBSemWait(&d->lock); if (d->control_mode != MOTOR_CONTROL_MODE_TRAJECTORY) { d->setpt.trajectory = chPoolAlloc(d->traj_buffer_pool); float *traj_mem = chPoolAlloc(d->traj_buffer_points_pool); if (d->setpt.trajectory == NULL || traj_mem == NULL) { chSysHalt("motor driver out of memory (trajectory buffer allocation)"); } trajectory_init(d->setpt.trajectory, traj_mem, d->traj_buffer_nb_points, 4, traj->sampling_time_us); d->control_mode = MOTOR_CONTROL_MODE_TRAJECTORY; } int ret = trajectory_apply_chunk(d->setpt.trajectory, traj); switch (ret) { case TRAJECTORY_ERROR_TIMESTEP_MISMATCH: chSysHalt("TRAJECTORY_ERROR_TIMESTEP_MISMATCH"); break; case TRAJECTORY_ERROR_CHUNK_TOO_OLD: chSysHalt("TRAJECTORY_ERROR_CHUNK_TOO_OLD"); break; case TRAJECTORY_ERROR_DIMENSION_MISMATCH: chSysHalt("TRAJECTORY_ERROR_DIMENSION_MISMATCH"); break; case TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER: log_message("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER"); // chSysHalt("TRAJECTORY_ERROR_CHUNK_OUT_OF_ORER"); break; } d->update_period = MOTOR_CONTROL_UPDATE_PERIOD_TRAJECTORY; chBSemSignal(&d->lock); }
static void dyn2_execute(void) { int i; tprio_t prio = chThdGetPriority(); /* Adding the WAs to the pool. */ for (i = 0; i < 4; i++) chPoolFree(&mp1, wa[i]); /* Starting threads from the memory pool. */ threads[0] = chThdCreateFromMemoryPool(&mp1, prio-1, thread, "A"); threads[1] = chThdCreateFromMemoryPool(&mp1, prio-2, thread, "B"); threads[2] = chThdCreateFromMemoryPool(&mp1, prio-3, thread, "C"); threads[3] = chThdCreateFromMemoryPool(&mp1, prio-4, thread, "D"); threads[4] = chThdCreateFromMemoryPool(&mp1, prio-5, thread, "E"); test_assert(1, (threads[0] != NULL) && (threads[1] != NULL) && (threads[2] != NULL) && (threads[3] != NULL) && (threads[4] == NULL), "thread creation failed"); /* Claiming the memory from terminated threads. */ test_wait_threads(); test_assert_sequence(2, "ABCD"); /* Now the pool must be full again. */ for (i = 0; i < 4; i++) test_assert(3, chPoolAlloc(&mp1) != NULL, "pool list empty"); test_assert(4, chPoolAlloc(&mp1) == NULL, "pool list not empty"); }
static void pools1_execute(void) { int i; /* Adding the WAs to the pool.*/ chPoolLoadArray(&mp1, wa[0], MAX_THREADS); /* Emptying the pool.*/ for (i = 0; i < MAX_THREADS; i++) test_assert(1, chPoolAlloc(&mp1) != NULL, "list empty"); /* Now must be empty.*/ test_assert(2, chPoolAlloc(&mp1) == NULL, "list not empty"); /* Adding the WAs to the pool, one by one this time.*/ for (i = 0; i < MAX_THREADS; i++) chPoolFree(&mp1, wa[i]); /* Emptying the pool again.*/ for (i = 0; i < MAX_THREADS; i++) test_assert(3, chPoolAlloc(&mp1) != NULL, "list empty"); /* Now must be empty again.*/ test_assert(4, chPoolAlloc(&mp1) == NULL, "list not empty"); /* Covering the case where a provider is unable to return more memory.*/ chPoolInit(&mp1, 16, null_provider); test_assert(5, chPoolAlloc(&mp1) == NULL, "provider returned memory"); }
static void test_005_001_execute(void) { unsigned i; /* [5.1.1] Adding the objects to the pool using chPoolLoadArray().*/ test_set_step(1); { chPoolLoadArray(&mp1, objects, MEMORY_POOL_SIZE); } /* [5.1.2] Emptying the pool using chPoolAlloc().*/ test_set_step(2); { for (i = 0; i < MEMORY_POOL_SIZE; i++) test_assert(chPoolAlloc(&mp1) != NULL, "list empty"); } /* [5.1.3] Now must be empty.*/ test_set_step(3); { test_assert(chPoolAlloc(&mp1) == NULL, "list not empty"); } /* [5.1.4] Adding the objects to the pool using chPoolFree().*/ test_set_step(4); { for (i = 0; i < MEMORY_POOL_SIZE; i++) chPoolFree(&mp1, &objects[i]); } /* [5.1.5] Emptying the pool using chPoolAlloc() again.*/ test_set_step(5); { for (i = 0; i < MEMORY_POOL_SIZE; i++) test_assert(chPoolAlloc(&mp1) != NULL, "list empty"); } /* [5.1.6] Now must be empty again.*/ test_set_step(6); { test_assert(chPoolAlloc(&mp1) == NULL, "list not empty"); } /* [5.1.7] Covering the case where a provider is unable to return more memory.*/ test_set_step(7); { chPoolObjectInit(&mp1, sizeof (uint32_t), null_provider); test_assert(chPoolAlloc(&mp1) == NULL, "provider returned memory"); } }
/** * @brief Creates a new thread allocating the memory from the specified * memory pool. * @pre The configuration options @p CH_USE_DYNAMIC and @p CH_USE_MEMPOOLS * must be enabled in order to use this function. * @note A thread can terminate by calling @p chThdExit() or by simply * returning from its main function. * @note The memory allocated for the thread is not released when the thread * terminates but when a @p chThdWait() is performed. * * @param[in] mp pointer to the memory pool object * @param[in] prio the priority level for the new thread * @param[in] pf the thread function * @param[in] arg an argument passed to the thread function. It can be * @p NULL. * @return The pointer to the @p Thread structure allocated for * the thread into the working space area. * @retval NULL if the memory pool is empty. * * @api */ Thread *chThdCreateFromMemoryPool(MemoryPool *mp, tprio_t prio, tfunc_t pf, void *arg) { void *wsp; Thread *tp; chDbgCheck(mp != NULL, "chThdCreateFromMemoryPool"); wsp = chPoolAlloc(mp); if (wsp == NULL) return NULL; #if CH_DBG_FILL_THREADS _thread_memfill((uint8_t *)wsp, (uint8_t *)wsp + sizeof(Thread), CH_THREAD_FILL_VALUE); _thread_memfill((uint8_t *)wsp + sizeof(Thread), (uint8_t *)wsp + mp->mp_object_size, CH_STACK_FILL_VALUE); #endif chSysLock(); tp = chThdCreateI(wsp, mp->mp_object_size, prio, pf, arg); tp->p_flags = THD_MEM_MODE_MEMPOOL; tp->p_mpool = mp; chSchWakeupS(tp, RDY_OK); chSysUnlock(); return tp; }
/** * @brief Creates a new thread allocating the memory from the specified * memory pool. * @pre The configuration options @p CH_CFG_USE_DYNAMIC and * @p CH_CFG_USE_MEMPOOLS must be enabled in order to use this * function. * @note A thread can terminate by calling @p chThdExit() or by simply * returning from its main function. * @note The memory allocated for the thread is not released when the thread * terminates but when a @p chThdWait() is performed. * * @param[in] mp pointer to the memory pool object * @param[in] prio the priority level for the new thread * @param[in] pf the thread function * @param[in] arg an argument passed to the thread function. It can be * @p NULL. * @return The pointer to the @p thread_t structure allocated for * the thread into the working space area. * @retval NULL if the memory pool is empty. * * @api */ thread_t *chThdCreateFromMemoryPool(memory_pool_t *mp, tprio_t prio, tfunc_t pf, void *arg) { void *wsp; thread_t *tp; chDbgCheck(mp != NULL); wsp = chPoolAlloc(mp); if (wsp == NULL) { return NULL; } #if CH_DBG_FILL_THREADS == TRUE _thread_memfill((uint8_t *)wsp, (uint8_t *)wsp + sizeof(thread_t), CH_DBG_THREAD_FILL_VALUE); _thread_memfill((uint8_t *)wsp + sizeof(thread_t), (uint8_t *)wsp + mp->mp_object_size, CH_DBG_STACK_FILL_VALUE); #endif chSysLock(); tp = chThdCreateI(wsp, mp->mp_object_size, prio, pf, arg); tp->p_flags = CH_FLAG_MODE_MPOOL; tp->p_mpool = mp; chSchWakeupS(tp, MSG_OK); chSysUnlock(); return tp; }
static dyn_element_t *dyn_create_object_pool(const char *name, dyn_list_t *dlp, memory_pool_t *mp) { dyn_element_t *dep; chDbgCheck(name != NULL); /* Checking if an object object with this name has already been created.*/ dep = dyn_list_find(name, dlp); if (dep != NULL) { return NULL; } /* Allocating space for the new object.*/ dep = (dyn_element_t *)chPoolAlloc(mp); if (dep == NULL) { return NULL; } /* Initializing object list element.*/ /*lint -save -e668 [] Lint is confused by the above chDbgCheck() and incorrectly assumes that strncpy() could receive a NULL pointer.*/ strncpy(dep->name, name, CH_CFG_FACTORY_MAX_NAMES_LENGTH); /*lint -restore*/ dep->refs = (ucnt_t)1; dep->next = dlp->next; /* Updating factory list.*/ dlp->next = (dyn_element_t *)dep; return dep; }
/** * * @brief Appends an item to a queue. * * @param[in] queuep pointer to instance of @p struct pios_queue * @param[in] itemp pointer to item which will be appended to the queue * @param[in] timeout_ms timeout for appending item to queue in milliseconds * * @returns true on success or false on timeout or failure * */ bool PIOS_Queue_Send(struct pios_queue *queuep, const void *itemp, uint32_t timeout_ms) { void *buf = chPoolAlloc(&queuep->mp); if (buf == NULL) return false; memcpy(buf, itemp, queuep->mp.mp_object_size); systime_t timeout; if (timeout_ms == PIOS_QUEUE_TIMEOUT_MAX) timeout = TIME_INFINITE; else if (timeout_ms == 0) timeout = TIME_IMMEDIATE; else timeout = MS2ST(timeout_ms); msg_t result = chMBPost(&queuep->mb, (msg_t)buf, timeout); if (result != RDY_OK) { chPoolFree(&queuep->mp, buf); return false; } return true; }
/** * @brief Create a mutex. * @note @p mutex_def is not used. * @note Can involve memory allocation. */ osMutexId osMutexCreate(const osMutexDef_t *mutex_def) { (void)mutex_def; binary_semaphore_t *mtx = chPoolAlloc(&sempool); chBSemObjectInit(mtx, false); return mtx; }
/** * @brief Create a semaphore. * @note @p semaphore_def is not used. * @note Can involve memory allocation. */ osSemaphoreId osSemaphoreCreate(const osSemaphoreDef_t *semaphore_def, int32_t count) { (void)semaphore_def; semaphore_t *sem = chPoolAlloc(&sempool); chSemObjectInit(sem, (cnt_t)count); return sem; }
void rfm69_log_s64(uint8_t channel, int64_t data) { char *msg; msg = (void*)chPoolAlloc(&rfm69_mp); msg[4] = (char)(1 | conf.location << 4); msg[5] = (char)channel; memcpy(msg, (void*)&halGetCounterValue(), 4); memcpy(&msg[8], &data, 8); chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE); }
void rfm69_log_c(uint8_t channel, const char* data) { volatile char *msg; msg = (char*)chPoolAlloc(&rfm69_mp); msg[4] = (char)(0 | conf.location << 4); msg[5] = (char)channel; memcpy((void*)msg, (void*)&halGetCounterValue(), 4); memcpy((void*)&msg[8], data, 8); chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE); }
void rfm69_log_f(uint8_t channel, float data_a, float data_b) { char *msg; msg = (void*)chPoolAlloc(&rfm69_mp); msg[4] = (char)(9 | conf.location << 4); msg[5] = (char)channel; memcpy(msg, (void*)&halGetCounterValue(), 4); memcpy(&msg[8], &data_a, 4); memcpy(&msg[12], &data_b, 4); chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE); }
/** * @brief Create a timer. */ osTimerId osTimerCreate(const osTimerDef_t *timer_def, os_timer_type type, void *argument) { osTimerId timer = chPoolAlloc(&timpool); chVTObjectInit(&timer->vt); timer->ptimer = timer_def->ptimer; timer->type = type; timer->argument = argument; return timer; }
/** * @brief Allocates a mail object from a mail pool. * @pre The mail pool must be already been initialized. * * @param[in] mlp pointer to a @p MailPool structure * @param[in] time the number of ticks before the operation timeouts, * the following special values are allowed: * - @a TIME_IMMEDIATE immediate timeout. * - @a TIME_INFINITE no timeout. * . * @return The mail object. * @retval NULL timeout expired. * * @api */ void *mailCreate(MailPool *mlp, systime_t time) { msg_t msg; void *mailp; msg = chSemWaitTimeout(&mlp->sem, time); if (msg != RDY_OK) return NULL; mailp = chPoolAlloc(&mlp->pool); chDbgAssert(mailp != NULL, "mailCreate(), #1", "empty pool"); return mailp; }
void rfm69_log_u16(uint8_t channel, uint16_t data_a, uint16_t data_b, uint16_t data_c, uint16_t data_d) { char *msg; msg = (void*)chPoolAlloc(&rfm69_mp); msg[4] = (char)(6 | conf.location << 4); msg[5] = (char)channel; memcpy(msg, (void*)&halGetCounterValue(), 4); memcpy(&msg[8], &data_a, 2); memcpy(&msg[10], &data_b, 2); memcpy(&msg[12], &data_c, 2); memcpy(&msg[14], &data_d, 2); chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE); }
void logMsg(char *str) { if (logenabled) { msg_t m; m = (msg_t) chPoolAlloc(&logMP); if ((void *) m != NULL ) { strncpy((char *) m, str, LOG_MSG_MAX_LENGTH); ((char *) m)[LOG_MSG_MAX_LENGTH - 1] = '\0'; chMBPost(&logMB, m, TIME_IMMEDIATE ); } } }
/** * @brief Creates a new thread allocating the memory from the specified * memory pool. * @pre The configuration options @p CH_CFG_USE_DYNAMIC and * @p CH_CFG_USE_MEMPOOLS must be enabled in order to use this * function. * @pre The pool must be initialized to contain only objects with * alignment @p PORT_WORKING_AREA_ALIGN. * @note A thread can terminate by calling @p chThdExit() or by simply * returning from its main function. * @note The memory allocated for the thread is not released automatically, * it is responsibility of the creator thread to call @p chThdWait() * and then release the allocated memory. * * @param[in] mp pointer to the memory pool object * @param[in] prio the priority level for the new thread * @param[in] pf the thread function * @param[in] arg an argument passed to the thread function. It can be * @p NULL. * @return The pointer to the @p thread_t structure allocated for * the thread into the working space area. * @retval NULL if the memory pool is empty. * * @api */ thread_t *chThdCreateFromMemoryPool(memory_pool_t *mp, tprio_t prio, tfunc_t pf, void *arg) { void *wsp; chDbgCheck(mp != NULL); wsp = chPoolAlloc(mp); if (wsp == NULL) { return NULL; } #if CH_DBG_FILL_THREADS == TRUE _thread_memfill((uint8_t *)wsp, (uint8_t *)wsp + mp->object_size, CH_DBG_STACK_FILL_VALUE); #endif return chThdCreateStatic(wsp, mp->object_size, prio, pf, arg); }
/** * @brief Creates a new thread allocating the memory from the specified * memory pool. * @pre The configuration options @p CH_CFG_USE_DYNAMIC and * @p CH_CFG_USE_MEMPOOLS must be enabled in order to use this * function. * @pre The pool must be initialized to contain only objects with * alignment @p PORT_WORKING_AREA_ALIGN. * @note A thread can terminate by calling @p chThdExit() or by simply * returning from its main function. * @note The memory allocated for the thread is not released automatically, * it is responsibility of the creator thread to call @p chThdWait() * and then release the allocated memory. * * @param[in] mp pointer to the memory pool object * @param[in] name thread name * @param[in] prio the priority level for the new thread * @param[in] pf the thread function * @param[in] arg an argument passed to the thread function. It can be * @p NULL. * @return The pointer to the @p thread_t structure allocated for * the thread into the working space area. * @retval NULL if the memory pool is empty. * * @api */ thread_t *chThdCreateFromMemoryPool(memory_pool_t *mp, const char *name, tprio_t prio, tfunc_t pf, void *arg) { thread_t *tp; void *wsp; chDbgCheck(mp != NULL); wsp = chPoolAlloc(mp); if (wsp == NULL) { return NULL; } thread_descriptor_t td = { name, wsp, (stkalign_t *)((uint8_t *)wsp + mp->object_size), prio, pf, arg }; #if CH_DBG_FILL_THREADS == TRUE _thread_memfill((uint8_t *)wsp, (uint8_t *)wsp + mp->object_size, CH_DBG_STACK_FILL_VALUE); #endif chSysLock(); tp = chThdCreateSuspendedI(&td); tp->flags = CH_FLAG_MODE_MPOOL; tp->mpool = mp; chSchWakeupS(tp, MSG_OK); chSysUnlock(); return tp; }
void *MemoryPool::alloc(void) { return chPoolAlloc(&pool); }
void* usbAllocMailboxBuffer(void){ return chPoolAlloc(&usbMemPool); }