HAL_ChibiOS: allocate threads from any heap

this fixes a failure on MatekF405-Wing where it fails to allocate the
SPI thread for the IMU
This commit is contained in:
Andrew Tridgell 2019-02-27 11:14:58 +11:00
parent 1191b59b08
commit 64681cb05c
5 changed files with 36 additions and 10 deletions

View File

@ -114,12 +114,14 @@ AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t pe
break;
}
thread_ctx = chThdCreateFromHeap(NULL,
THD_WORKING_AREA_SIZE(1024),
thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(1024),
name,
thread_priority, /* Initial priority. */
DeviceBus::bus_thread, /* Thread function. */
this); /* Thread parameter. */
if (thread_ctx == nullptr) {
AP_HAL::panic("Failed to create bus thread %s", name);
}
}
DeviceBus::callback_info *callback = new DeviceBus::callback_info;
if (callback == nullptr) {

View File

@ -460,8 +460,7 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_
break;
}
}
thread_t *thread_ctx = chThdCreateFromHeap(NULL,
THD_WORKING_AREA_SIZE(stack_size),
thread_t *thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(stack_size),
name,
thread_priority,
thread_create_trampoline,

View File

@ -101,8 +101,7 @@ void UARTDriver::thread_init(void)
return;
}
#if CH_CFG_USE_HEAP == TRUE
uart_thread_ctx = chThdCreateFromHeap(NULL,
THD_WORKING_AREA_SIZE(2048),
uart_thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(2048),
"apm_uart",
APM_UART_PRIORITY,
uart_thread,

View File

@ -224,6 +224,31 @@ size_t mem_available(void)
return totalp;
}
/*
allocate a thread on any available heap
*/
thread_t *thread_create_alloc(size_t size,
const char *name, tprio_t prio,
tfunc_t pf, void *arg)
{
thread_t *ret;
// first try default heap
ret = chThdCreateFromHeap(NULL, size, name, prio, pf, arg);
if (ret != NULL) {
return ret;
}
// now try other heaps
uint8_t i;
for (i=1; i<NUM_MEMORY_REGIONS; i++) {
ret = chThdCreateFromHeap(&heaps[i], size, name, prio, pf, arg);
if (ret != NULL) {
return ret;
}
}
return NULL;
}
#endif // CH_CFG_USE_HEAP

View File

@ -32,6 +32,7 @@ void show_stack_usage(void);
size_t mem_available(void);
void *malloc_dma(size_t size);
void *malloc_fastmem(size_t size);
thread_t *thread_create_alloc(size_t size, const char *name, tprio_t prio, tfunc_t pf, void *arg);
// flush all dcache
void memory_flush_all(void);