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

libraries/AP_HAL_ChibiOS/Device.cpp

@@ -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),
-                         name,
-                         thread_priority,           /* Initial priority.    */
-                         DeviceBus::bus_thread,    /* Thread function.     */
-                         this);                     /* Thread parameter.    */
+        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) {

libraries/AP_HAL_ChibiOS/Scheduler.cpp

@@ -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,

libraries/AP_HAL_ChibiOS/UARTDriver.cpp

@@ -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,

libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c

@@ -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
 
 

libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h

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