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);
}
