Fix CoreMutex FreeRTOS ISR logic (#1510)

cores/rp2040/CoreMutex.cpp

@@ -30,10 +30,15 @@ CoreMutex::CoreMutex(mutex_t *mutex, uint8_t option) {
     _option = option;
     if (__isFreeRTOS) {
         auto m = __get_freertos_mutex_for_ptr(mutex);
-        if (__freertos_check_if_in_isr() && !__freertos_mutex_take_from_isr(m)) {
-            return;
+        if (__freertos_check_if_in_isr()) {
+            if (!__freertos_mutex_take_from_isr(m)) {
+                return;
+            }
+            // At this point we have the mutex in ISR
+        } else {
+            // Grab the mutex normally, possibly waking other tasks to get it
+            __freertos_mutex_take(m);
         }
-        __freertos_mutex_take(m);
     } else {
         uint32_t owner;
         if (!mutex_try_enter(_mutex, &owner)) {