diff --git a/src/main/fc/init.c b/src/main/fc/init.c index c156a4d0a6..34e9828d1c 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -740,6 +740,7 @@ void init(void) delay(50); #endif } + LED0_OFF; LED1_OFF; diff --git a/src/platform/PICO/include/platform/multicore.h b/src/platform/PICO/include/platform/multicore.h new file mode 100644 index 0000000000..b89920a674 --- /dev/null +++ b/src/platform/PICO/include/platform/multicore.h @@ -0,0 +1,39 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#pragma once + +#include "pico/multicore.h" + +typedef enum multicoreCommand_e { + MULTICORE_CMD_NONE = 0, + MULTICORE_CMD_FUNC, + MULTICORE_CMD_FUNC_BLOCKING, // Command to execute a function on the second core and wait for completion + MULTICORE_CMD_STOP, // Command to stop the second core +} multicoreCommand_e; + +// Define function types for clarity +typedef void core1_func_t(void); + +void multicoreStart(void); +void multicoreStop(void); +void multicoreExecute(core1_func_t *func); +void multicoreExecuteBlocking(core1_func_t *func); diff --git a/src/platform/PICO/mk/PICO.mk b/src/platform/PICO/mk/PICO.mk index 6381907e0e..6f05d5f3ac 100644 --- a/src/platform/PICO/mk/PICO.mk +++ b/src/platform/PICO/mk/PICO.mk @@ -57,6 +57,7 @@ PICO_LIB_SRC = \ rp2_common/hardware_flash/flash.c \ rp2_common/pico_unique_id/unique_id.c \ rp2_common/pico_platform_panic/panic.c \ + rp2_common/pico_multicore/multicore.c \ common/pico_sync/mutex.c \ common/pico_time/time.c \ common/pico_sync/lock_core.c \ @@ -405,7 +406,8 @@ MCU_COMMON_SRC = \ PICO/serial_uart_pico.c \ PICO/serial_usb_vcp_pico.c \ PICO/system.c \ - PICO/usb/usb_cdc.c + PICO/usb/usb_cdc.c \ + PICO/multicore.c DEVICE_STDPERIPH_SRC := \ $(PICO_LIB_SRC) \ diff --git a/src/platform/PICO/multicore.c b/src/platform/PICO/multicore.c new file mode 100644 index 0000000000..5b54b527b0 --- /dev/null +++ b/src/platform/PICO/multicore.c @@ -0,0 +1,132 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + +#include "platform.h" +#include "platform/multicore.h" +#include "pico/multicore.h" +#include "pico/util/queue.h" + +#ifdef USE_MULTICORE + +// Define a structure for the message we'll pass through the queue +typedef struct { + multicoreCommand_e command; + core1_func_t *func; +} core_message_t; + +// Define the queue +static queue_t core0_queue; +static queue_t core1_queue; + +static void core1_main(void) +{ + // This loop is run on the second core + while (true) { + + core_message_t msg; + queue_remove_blocking(&core1_queue, &msg); + + switch (msg.command) { + case MULTICORE_CMD_FUNC: + if (msg.func) { + msg.func(); + } + break; + case MULTICORE_CMD_FUNC_BLOCKING: + if (msg.func) { + msg.func(); + + // Send the result back to core0 (it will be blocking until this is done) + bool result = true; + queue_add_blocking(&core0_queue, &result); + } + break; + case MULTICORE_CMD_STOP: + multicore_reset_core1(); + return; // Exit the core1_main function + default: + // unknown command or none + break; + } + + tight_loop_contents(); + } +} + +void multicoreStart(void) +{ + // Initialize the queue with a size of 4 (to be determined based on expected load) + queue_init(&core1_queue, sizeof(core_message_t), 4); + + // Initialize the queue with a size of 1 (only needed for blocking results) + queue_init(&core0_queue, sizeof(bool), 1); + + // Start core 1 + multicore_launch_core1(core1_main); +} + +void multicoreStop(void) +{ + core_message_t msg; + msg.command = MULTICORE_CMD_STOP; + msg.func = NULL; + + queue_add_blocking(&core1_queue, &msg); + } +#endif // USE_MULTICORE + + +void multicoreExecuteBlocking(core1_func_t *func) +{ +#ifdef USE_MULTICORE + core_message_t msg; + msg.command = MULTICORE_CMD_FUNC_BLOCKING; + msg.func = func; + + bool result; + + queue_add_blocking(&core1_queue, &msg); + // Wait for the command to complete + queue_remove_blocking(&core0_queue, &result); +#else + // If multicore is not used, execute the command directly + if (func) { + func(); + } +#endif // USE_MULTICORE +} + +void multicoreExecute(core1_func_t *func) +{ +#ifdef USE_MULTICORE + core_message_t msg; + msg.command = MULTICORE_CMD_FUNC; + msg.func = func; + + queue_add_blocking(&core1_queue, &msg); +#else + // If multicore is not used, execute the command directly + if (func) { + func(); + } +#endif // USE_MULTICORE +} + diff --git a/src/platform/PICO/serial_uart_pico.c b/src/platform/PICO/serial_uart_pico.c index 6896c854c8..753b155ae6 100644 --- a/src/platform/PICO/serial_uart_pico.c +++ b/src/platform/PICO/serial_uart_pico.c @@ -38,6 +38,7 @@ #include "hardware/uart.h" #include "hardware/irq.h" +#include "platform/multicore.h" const uartHardware_t uartHardware[UARTDEV_COUNT] = { #ifdef USE_UART0 @@ -178,6 +179,18 @@ static void on_uart1(void) uartIrqHandler(&uartDevice[UARTDEV_1].port); } +static void uart0_irq_init(void) +{ + irq_set_exclusive_handler(UART0_IRQ, on_uart0); + irq_set_enabled(UART0_IRQ, true); +} + +static void uart1_irq_init(void) +{ + irq_set_exclusive_handler(UART1_IRQ, on_uart1); + irq_set_enabled(UART1_IRQ, true); +} + uartPort_t *serialUART(uartDevice_t *uartdev, uint32_t baudRate, portMode_e mode, portOptions_e options) { UNUSED(options); @@ -230,13 +243,17 @@ uartPort_t *serialUART(uartDevice_t *uartdev, uint32_t baudRate, portMode_e mode // TODO implement - use options here... uart_set_hw_flow(hardware->reg, false, false); uart_set_format(hardware->reg, 8, 1, UART_PARITY_NONE); - + // TODO want fifos? //// uart_set_fifo_enabled(hardware->reg, false); uart_set_fifo_enabled(hardware->reg, true); - irq_set_exclusive_handler(hardware->irqn, hardware->irqn == UART0_IRQ ? on_uart0 : on_uart1); - irq_set_enabled(hardware->irqn, true); + // Set up the IRQ handler for this UART + if (hardware->irqn == UART0_IRQ) { + multicoreExecuteBlocking(&uart0_irq_init); + } else if (hardware->irqn == UART1_IRQ) { + multicoreExecuteBlocking(&uart1_irq_init); + } // Don't enable any uart irq yet, wait until a call to uartReconfigure... // (with current code in serial_uart.c, this prevents irq callback before rxCallback has been set) diff --git a/src/platform/PICO/serial_usb_vcp_pico.c b/src/platform/PICO/serial_usb_vcp_pico.c index f1b7813efd..a14184a222 100644 --- a/src/platform/PICO/serial_usb_vcp_pico.c +++ b/src/platform/PICO/serial_usb_vcp_pico.c @@ -39,6 +39,7 @@ #include "drivers/time.h" #include "drivers/serial.h" #include "drivers/serial_usb_vcp.h" +#include "platform/multicore.h" static vcpPort_t vcpPort = { 0 }; @@ -187,6 +188,9 @@ static const struct serialPortVTable usbVTable[] = { serialPort_t *usbVcpOpen(void) { + // initialise the USB CDC interface + // potentially this could be done in a multicore task + //multicoreExecuteBlocking(&cdc_usb_init); cdc_usb_init(); vcpPort_t *s = &vcpPort; diff --git a/src/platform/PICO/system.c b/src/platform/PICO/system.c index 4dc658a9fd..a8481b9794 100644 --- a/src/platform/PICO/system.c +++ b/src/platform/PICO/system.c @@ -30,6 +30,8 @@ #include "drivers/light_led.h" #include "drivers/sound_beeper.h" +#include "platform/multicore.h" + #include "hardware/clocks.h" #include "hardware/timer.h" #include "hardware/watchdog.h" @@ -104,6 +106,10 @@ void systemInit(void) pico_unique_board_id_t id; pico_get_unique_board_id(&id); memcpy(&systemUniqueId, &id.id, MIN(sizeof(systemUniqueId), PICO_UNIQUE_BOARD_ID_SIZE_BYTES)); + +#ifdef USE_MULTICORE + multicoreStart(); +#endif // USE_MULTICORE } void systemResetToBootloader(bootloaderRequestType_e requestType) diff --git a/src/platform/PICO/target/RP2350B/target.h b/src/platform/PICO/target/RP2350B/target.h index 86257e55ce..936ccd1b7e 100644 --- a/src/platform/PICO/target/RP2350B/target.h +++ b/src/platform/PICO/target/RP2350B/target.h @@ -45,7 +45,7 @@ #define USBD_PRODUCT_STRING "Betaflight RP2350B" #endif - +#define USE_MULTICORE #define USE_IO #define USE_UART0 #define USE_UART1 diff --git a/src/platform/PICO/usb/usb_cdc.c b/src/platform/PICO/usb/usb_cdc.c index f54dd4f683..0b68ee794f 100644 --- a/src/platform/PICO/usb/usb_cdc.c +++ b/src/platform/PICO/usb/usb_cdc.c @@ -197,13 +197,13 @@ int cdc_usb_read(uint8_t *buf, unsigned length) return rc; } -bool cdc_usb_init(void) +void cdc_usb_init(void) { if (get_core_num() != alarm_pool_core_num(alarm_pool_get_default())) { // included an assertion here rather than just returning false, as this is likely // a coding bug, rather than anything else. assert(false); - return false; + return; } // initialize TinyUSB, as user hasn't explicitly linked it @@ -229,7 +229,6 @@ bool cdc_usb_init(void) } configured = rc; - return rc; } bool cdc_usb_deinit(void) diff --git a/src/platform/PICO/usb/usb_cdc.h b/src/platform/PICO/usb/usb_cdc.h index aadc9bb898..35b9dd6c15 100644 --- a/src/platform/PICO/usb/usb_cdc.h +++ b/src/platform/PICO/usb/usb_cdc.h @@ -29,7 +29,7 @@ void cdc_usb_write_flush(void); int cdc_usb_write(const uint8_t *buf, unsigned length); int cdc_usb_read(uint8_t *buf, unsigned length); -bool cdc_usb_init(void); +void cdc_usb_init(void); bool cdc_usb_deinit(void); bool cdc_usb_configured(void); bool cdc_usb_connected(void);