Skip to content

RP2350 Port - #32995

Open
davidbuzz wants to merge 72 commits into
ArduPilot:masterfrom
davidbuzz:rp2350-v5-squashed-and-cleaned-and-rebased
Open

davidbuzz wants to merge 72 commits into
ArduPilot:masterfrom
davidbuzz:rp2350-v5-squashed-and-cleaned-and-rebased

Conversation

@davidbuzz

@davidbuzz davidbuzz commented May 6, 2026 •

Copy link
Copy Markdown
Contributor

Summary

RP2350 support in AP_HAL_ChibiOS: dual-core SMP, and the PIO-based peripherals the chip needs in place of the STM32 timer/DMA paths ArduPilot normally relies on. This is the first non-STM32 hardware in this HAL.

The port flies. RPI_UAVFC is a real flight controller running this, with bidirectional DShot600, an eRPM-driven harmonic notch, analog OSD and microSD logging in the air. The head runs it at 200 MHz with a 100 MHz flash clock in continuous read, with more of both cores' hot paths in SRAM, and that configuration flew three times on 2026-11-07, including RTL by switch and the port's first Auto mission: scheduler load 29-41% in flight against 54-69% at 225 MHz, no long loops, and nothing dropped from the log in flight.

Classification & Testing (check all that apply and add your own)

  • Checked by a human programmer
  • Tested on hardware
  • Logs available on request
  • Automated test(s) verify changes (e.g. unit test, autotest)

Description

Feature support

Feature Status Notes
Dual-core SMP Yes First SMP implementation in ChibiOS. IMU, rate loop and DShot on core1; main loop and EKF on core0. See "Cores, and what runs where" below
USB CDC Yes Console and MAVLink
Hardware UARTs Yes The chip has two PL011s
PIO UARTs Yes Two more, since two hardware UARTs are not enough for a flight controller. Inverted and half-duplex too: INOVER/OUTOVER on the pad, and a dedicated 8E2 receive program for SBUS
SPI Yes IMU and microSD
I2C Yes Internal baro, external GPS/compass
ADC Yes Including the RP2350B pins above GPIO29
PWM output Yes 4 motor outputs
Bidirectional DShot600 Yes PIO driven. Flown
Harmonic notch from eRPM Yes Per-motor; the four motors span 164-212 Hz so per-motor tracking is not optional
NeoPixel / serial LED Yes PIO driven
Analog OSD Yes PIO scan-out. Costs about 0.1% of core0, because PIO and DMA do the video timing. Detects PAL or NTSC from the camera's field rate and re-checks it while disarmed
MSP DisplayPort Yes
microSD logging Yes SPI mode
Parameter storage Yes XIP flash
Watchdog Yes 2 s hardware watchdog, with the monitor thread's lockup detection. Lockup-tested on RPI_UAVFC: resets, detects the reset and restores the persistent data. On by default on all three boards, as on any ChibiOS board
Bootloader Yes RP2350 IMAGE_DEF; flash over SWD or UF2
VTX control and power switching Yes SmartAudio
Battery voltage monitoring Yes
Analog RSSI Yes
External compass Yes I2C. No compass on the board itself, so without one the EKF stays in constant-position mode
Battery current monitoring Partial Reads, but does not track load on the tested vehicle; scale is still a placeholder
DShot rates other than 600 No Anything else raises a configuration error at boot
OneShot / OneShot125 No The RP2350 PWM slice has no EGR equivalent. Would need the PWM wrap IRQ - novel work, not a port
BLHeli passthrough No
CAN / DroneCAN No No hardware for it
ProfiLED No NeoPixel only
Hardware SPI OSD No No device on RPI_UAVFC. On Laurel it contends with microSD for SPI1
SBUS Yes, with #33057 Inverted 8E2 on a PIO UART, which assembles whole frames, aligns them on the 2 ms wire gap and debounces the failsafe flag; the PL011 path sets parity, stop bits and INOVER as well. Wire the receiver to the RADIO pad and set SERIAL3_OPTIONS 1. Alignment is deliberately enforced at the PIO layer, because it is the last one that knows the wire timing - a batched port has none left by the time the bytes reach the decoder, which is what #33057 is for. Not yet run against a receiver on this port, where CRSF is what has flown
SBUS on the dedicated pad (GPIO41) No Its only hardware-UART function is UART1_RX, which the GPS owns, and a third PIO UART is mapped to PIO1, which the analog OSD claims. Use the RADIO pad
Serial ESC telemetry pad No Can only reach the UART the GPS owns. Use bidirectional DShot instead
IOMCU No Outputs are driven directly
Safety switch No Deliberate board choice
IMU heater No
Hardware RTC No
External AHRS No Compiled out: AP_AHRS updates every backend it is built with on every loop, which cost core0 about 1% with none fitted
Dual USB CDC No Attempted, enumerated, never passed bytes. Abandoned

Boards

RPI_UAVFC is the flight target and the one that matters. It has flown.

Laurel was the bring-up board - an RP2350B carrier used to get the platform working before RPI_UAVFC existed. It is bench-verified, not a flight target, and does not need flying. Its feature table is kept in-tree as the record of what was proven on hardware during bring-up.

Pico2 is a reference target for anyone wanting to run this on a bare Pico 2. It compiles and boots; nothing in its feature table is marked hardware-verified.

Cores, and what runs where

Two Cortex-M33s, and one 16 KB XIP instruction cache shared between them. Both facts drive the split: the flight-critical chain gets a core to itself, and that core is kept out of flash so the other one can have the cache.

Assignment is one header, rp2350/rp2350_core_affinity.h, with a define per thread. IRQ routing follows for free - ChibiOS routes a peripheral's IRQ to whichever core called the driver start function, and each bus thread does its own init on first wakeup, so the IRQ goes where the thread went. Moving a driver between cores is a one-line change with no NVIC work.

Core 0 Core 1
main loop at 200 Hz, EKF3 inline, AHRS, navigation, GCS, logging SPI0 bus thread: IMU FIFO at 3200 Hz, gyro LPF, harmonic notch
CRSF RC input on the PIO0 interrupt, USB, UART, I2C rate thread at 1600 Hz: three AC_PID updates, AP_MotorsMatrix mixing
SPI1 microSD, parameter storage, PIO OSD rcout thread: bidirectional DShot600 at 1600 Hz, GCR decode, eRPM

Those rates are R2 Rev C with the ICM-56686, which takes the ICM-456xy path: an 800 Hz base backend rate, so INS_GYRO_RATE 2 lands on 3200 Hz where an ICM42688P's 1000 Hz base gives 4000 Hz. FSTRATE_DIV 2 halves that into the rate thread. SERVO_DSHOT_RATE multiplies the rate-loop rate and not SCHED_LOOP_RATE, which is a trap worth naming - at 1 the DShot frame rate is simply the rate-loop rate, and 0 is a fixed 1 kHz regardless. The equivalent ICM42688P build runs 4000 / 2000 / 2000 Hz.

Parameter storage has to stay on core0, because the XIP lockout below is driven from core0 and parks core1. A flash write issued from core1 would park the core doing the writing.

The obvious move with a spare core is to put more on it, and that is wrong here. Adding USB, UART, I2C and RC input to core1 during Laurel bring-up took the main loop from 296 Hz to 225 Hz: every extra thread there is more context switches, and every SMP spinlock acquire on one core stalls the other. Core1 runs the fewest threads the IMU-to-motors chain needs and nothing else.

flowchart TB
  subgraph C1["CORE 1 - gyro sample to DShot frame"]
    direction LR
    A["SPI0 bus thread<br/>IMU FIFO 3200 Hz<br/>gyro LPF, harmonic notch"]
    B["rate thread 1600 Hz<br/>AC_PID x3<br/>AP_MotorsMatrix mixing"]
    C["rcout thread<br/>bidir DShot600 1600 Hz<br/>GCR decode, eRPM"]
    A --> B --> C
  end
  subgraph C0["CORE 0 - everything else"]
    direction LR
    M["main loop 200 Hz<br/>EKF3 inline, AHRS<br/>nav, GCS, logging"]
    R["CRSF RC in<br/>PIO0 IRQ, 2.1 kHz"]
    S["storage, USB, UART, I2C<br/>SPI1 microSD, PIO OSD"]
  end
  SY["Scratch Y<br/>SRAM9, 4 KB<br/>core1 dedicated I-bus"]
  SR["striped SRAM, 512 KB<br/>registry relocations"]
  SX["Scratch X<br/>SRAM8, 4 KB<br/>core0 dedicated I-bus"]
  XIP["QSPI flash via XIP<br/>shared 16 KB cache<br/>disabled during flash writes"]

  C1 -- "9.7%" --> SY
  C1 -- "77.1%" --> SR
  C1 -- "13.2%" --> XIP
  C0 -- "8.3%" --> SX
  C0 -- "19.4%" --> SR
  C0 -- "72.2%" --> XIP
Loading

Edge labels are where each core fetches instructions from, as a share of non-idle PC samples. Box rates are the ICM-56686 build; every measured share here and below came off the ICM42688P build at 4000 Hz gyro, 2000 Hz rate loop and SERVO_DSHOT_RATE 1.

IMU to motors, in SRAM

Everything on core1 between the gyro sample and the DShot frame is relocated out of flash: the FIFO read, the filters, the rate controller and its PIDs, the mixer, and the bidirectional DShot send and decode. Two mechanisms do it. A registry file lists functions to move into striped SRAM, and two 4 KB scratch banks take the hottest leaves - SRAM8 sits on core0's dedicated instruction bus and SRAM9 on core1's, so a fetch from either is single-cycle and contends with nothing.

The DShot chain is the measurement that made the case, because it was entirely flash-resident and had never been profiled. Same board, same rates, on-chip PC sampler, shares of core1 non-idle samples:

PWM baseline bidir DShot, in flash bidir DShot, relocated
flash 1.8% 65.6% 17.7%
SRAM 97.2% 34.1% 73.3%
scratch 1.0% 0.2% 9.0%

Core1 idle went from 21.9% of emitted samples to 51.4%, so the core is doing about half the work it was. RCOutput_pico::read_telemetry alone fell from 19.2% to 3.5%.

That 5.5x settles something worth stating, because the opposite assumption is the easy one to make: a high sample count does not by itself mean code is fetch-bound. AP_MotorsMatrix is core1's largest single consumer at around 2.1% armed, and moving it to scratch gained nothing - it is data-heavy mixing math, not fetch-bound. The only way to tell the two apart was to move it and measure again.

Why core0 keeps the flash

Core1 being in SRAM is not only about core1. The XIP cache is shared, so core1's fetches evict core0's working set, and the failure that exposed it was not subtle: once the bidirectional DShot chain started fetching from flash, CRSF RC input went into continuous failsafe. The PIO UART has no DMA, so every RX interrupt drains the FIFO in software - a byte every 24 us at 420 kbaud - and that handler was being fetched from flash on the core that was losing the cache. It now sits in Scratch X together with the rest of the PIO0 interrupt path.

There is a second reason core1 cannot be in flash. The QMI controller disables XIP for the duration of every erase and page program, so a fetch from flash inside that window returns DECERR and faults. Core0 parks core1 first, over the SIO doorbell:

sequenceDiagram
  participant C0 as Core 0, storage
  participant Q as QMI / XIP
  participant C1 as Core 1
  C0->>C1: SIO doorbell, IRQ26
  C1->>C1: mask all IRQs, spin in .ramtext
  C1-->>C0: ack
  C0->>Q: exit XIP
  C0->>Q: erase or program
  C0->>Q: enter XIP
  C0->>C1: release
  C1->>C1: restore IRQs, resume
Loading

Core1 spins in .ramtext with its interrupts masked until core0 releases it. Flash writes come only from parameter storage, so this happens at boot and on a GCS parameter save, never in flight, and perf_report emits XIPpark: n=<count> max=<us> so the cost is visible rather than assumed. IRQ26 runs at CORTEX_MINIMUM_PRIORITY deliberately: at priority 0 it fires inside ChibiOS kernel locks and deadlocks against the SMP spinlock.

What falls out is the asymmetry the whole arrangement is aiming for. Bidirectional DShot600 at 2 kHz, shares of non-idle samples:

core 1 core 0
idle 58.3% 53.9%
flash 13.2% 72.2%
SRAM 77.1% 19.4%
scratch 9.7% 8.3%

Every one of core1's top 24 functions is SRAM-resident, and nothing in flash appears until 0.4%. Core0 at 72% flash is the intended state, not an unfinished one: it has the cache largely to itself, and it is the core that can absorb a miss.

Core0 relocation does pay where it is affordable. AP_DAL was 3.9% of core0 non-idle in 7.2 KB and entirely flash-resident; relocating it measured 8.0x and 9.9x across two independent A/B pairs, against boot-to-boot drift of 18% and 6% on the two halves. It is measured as a ratio of DAL samples to NavEKF3 samples rather than as an absolute share, because absolute core0 load moves by 20 points across a reflash depending on EKF and GPS state, and the first attempt at this read as a 28% win that was almost entirely that drift. Core0 relocation has since gone further, to the AHRS update, core0's tick and DMA vectors and newlib's memcmp and Euler trig: 7 KB of SRAM that took the armed scheduler load from 31% to 25.5% at 200 MHz and core0's non-idle time in flash from 59% to 50%.

Still to do

What this costs boards that will never see an RP2350

Shared-library changes in this diff land flash on every ChibiOS board. MatekF405 copter built at this branch's merge-base is 873644 B and at its head 873708 B, so +64 B. An nm diff of the two ELFs puts it here:

bytes
SPI clock guard +32 SPIDevice::clock_pulse
UART thread start-up +26 UARTDriver
RCOutput DMA handle checks +16 DShot send paths
Scheduler::delay() in 32-bit microseconds -12

The FTP, logger and microSD changes that made up most of the earlier figure are out of this diff and proposed on their own. What stays in shared code is one GCS_MAVLink commit that lets a board set the FTP thread priority and TX back-pressure through #ifndef defaults carrying master's values, which the RP2350 MCU definition overrides; it adds 0 B to other boards.

Dependent PRs

This branch is the PR branch and carries only the RP2350 port. Work that is not RP2350-specific has been split out and is not in this diff:

PR What State
ArduPilot/ChibiOS#113 RP2350 support in ChibiOS, including SMP and boot flash continuous read Open. Must merge first, and ArduPilot must move to a ChibiOS carrying it
chibios-upstream/chibios#581 Optional RP2350 boot flash continuous read and deselect time in upstream ChibiOS Open. The upstream form of the continuous read ArduPilot/ChibiOS#113 carries; not needed by this PR
#33055 AP_Baro: DPS280/310 poll rate Open
#33056 AP_Filesystem: memset d_name before strncpy Open, approved
#33057 AP_RCProtocol/SBUS: anchor framing on the header and resync on failure Open. SBUS on this port needs it: upstream requires a wire-time gap before a header, which a batched PIO or DMA port does not preserve
#33058 AP_RCProtocol: backend polling and CRSF startup guards Open, changes requested
#34436 Copter: honour the harmonic notch loop rate option in the rate thread Open. Found here: the rate thread re-centred every notch at the rate loop rate whatever INS_HNTCH_OPTS said, so the "update at loop rate" option did nothing. Dropped from this branch - with the option set, which is how this board flies, the behaviour is identical either way, so the port does not need it
#34368 AP_InertialSensor: ICM-56686 IREG offsets and FIFO_CONFIG2 Merged 2026-09-15. Was needed for RPI_UAVFC R2 Rev C, which specifies that IMU
#34473 AP_Logger: mark start_new_log() IO steps for the IO timer watchdog Open. Found here: a stall inside start_new_log() was reported as "stuck thread ()" with no hint of which step. Dropped from this branch, which flies without it
#34474 GCS_MAVLink: wake the FTP worker on request and stop it spinning forever Open. Found here. Dropped from this branch; FTP on the board has been rechecked without it
#34476 AP_HAL_ChibiOS: honour AP_FATFS_MAX_IO_SIZE on the microSD SPI path Open. Not needed for the port to work, but highly desirable for logging: with it and #34477 RPI_UAVFC logs at about 265 KB/s instead of 115 KB/s. Dropped from this branch; a bench test without it logs correctly at the reduced rate
#34477 AP_HAL_ChibiOS: write microSD blocks in one SPI exchange Open, needs ArduPilot/ChibiOS#115. Not needed for the port to work, but highly desirable for logging, as above. Dropped from this branch
#34478 AP_HAL_ChibiOS: recover microSD parameter storage after an I/O error Open. Found here, but the RP2350 boards keep parameters in flash, so the port does not use it. Dropped from this branch
#34430 AP_Relay: don't re-apply a relay pin's mode on every read Dropped. Re-applying the mode is deliberate - it takes a pin back from scripting driving it as a GPIO. The rail drop it guarded against on RPI_UAVFC was the RP2350 pad driver releasing the output while rewriting the mode, now fixed in ArduPilot/ChibiOS#113
#34437 AP_Math: keep angle shaping in float Open. Dropped from this branch: setting HAL_WITH_POSTYPE_DOUBLE 0 for the RP2350 boards makes the shaping path float throughout, and saves 8180 B of flash on RPI_UAVFC, so the port no longer depends on it
#34497 AP_OSD: do not leave a hole in the backend list Open. Found here: with OSD_TYPE set to TXONLY or to an OSD that fails to start (for instance MSP DisplayPort with no MSP port configured), and a second OSD in OSD_TYPE2 (for instance the PIO overlay), the OSD thread calls through a null slot and the autopilot crashes a few seconds after boot, on master as on this branch. Dropped from this branch; until it merges, run the PIO overlay as OSD_TYPE, or give OSD_TYPE an OSD that starts
#34443 AP_Param: enforce @readonly defaults over stale stored values Open. Found here: @READONLY in a board's defaults.parm loses to a stored value, and the parameter being read-only then stops MAVLink correcting it. The ODID boards use it to lock the RemoteID parameters. Dropped from this branch along with the boards' @readonly defaults
#34438 AP_HAL_ChibiOS: fix the threads.txt worst-slice mark Merged 2026-09-28
#34530 AP_HAL_ChibiOS: count unallocated core memory in memory.txt Open. Found here: @SYS/memory.txt showed a few hundred bytes free on RP2350 with 160 KB to spare, because it left out the part of the default heap the core allocator has not handed out yet. Every ChibiOS board's first region reads low the same way. Not in this branch; MEMINFO was already right

ChibiOS#113 is the gating item. The rest can land in any order; this PR does not contain their changes and does not conflict with them.

How the commits are laid out

The branch was rebuilt from 330 commits into 36, and is 71 after review and the move to 200 MHz. Code that STM32 boards also build is in a few commits by area: AP_HAL, the common hwdef code and generator, waf, the UART, bus, RC, scheduler and storage parts of AP_HAL_ChibiOS, and one commit per library touched. Each of those says in its message what, if anything, changes for existing boards. The five that do change STM32 output come last, after all the RP2350 work: the UART thread start-up race, RCOutput's DMA handle checks, the INS rate-thread signalling, the SPI clock guard, and Scheduler::delay() timing in 32-bit microseconds, which drops a 64-bit division from every pass of its loop. With --consistent-builds, MatekH743 and MatekF405 copter are byte-identical across the 15 RP2350 commits for the move to 200 MHz that come just before them. The RP2350-only work follows in logical groups: PIO drivers, profiling, the shared rp2350/ files, the MCU and common board files, one commit per board, bootloaders and tools.

Every commit builds for MatekF405 and SITL, and each RP2350 board builds from the commit that adds it. The history before the rebuild is on andyp1per:rp2350-v7.

On sharing the HAL

AP_HAL_ChibiOS was STM32-only before this, and when this PR opened it was an open question whether RP2350 belonged in the same HAL, in an overlay, or in a hard fork. Having built it, the shared HAL is the right answer. The STM32 paths are not distorted by it, and the genuinely chip-specific work - the PIO peripherals, SMP, the XIP relocation registry - lives in its own files rather than as conditionals threaded through shared code.

Vehicle code is almost untouched: 14 changed lines in ArduCopter, and no mention of RP2350 or ChibiOS in any vehicle directory. Board-dependent values go through HAL defaults a board overrides.

The one piece that is genuinely novel rather than ported is the XIP relocation registry, and it earns its keep: relocating the notch coefficient update and the rate-loop logging chain alone took core1 from 69.7% to 58.2% busy armed. See "Cores, and what runs where" above.

@davidbuzz

Copy link
Copy Markdown
Contributor Author

its tested and boots, and runs scheduler, and does 95% of the things youd expect from a port at this stage, its not perfect, but is an excellent dicussion point.

@davidbuzz

Copy link
Copy Markdown
Contributor Author

tridge wanted to know the tools/cpuinfo info...

./waf configure --board=Laurel --debug
./waf --targets=tool/CPUInfo --upload

Connect /dev/serial/by-id/usb-ArduPilot_Laurel_B8CE48E2E19D881E67A96B1B-if00 source_system=255
Log Directory:
Telemetry log: mav.tlog
CPUInfo cycle 1
SYSCLK 375MHz
Type sizes:
char : 1
short : 2
int : 4
long : 4
long long : 8
bool : 1
void* : 4

Operation timings:
Note: timings for some operations are very data dependent
nop 0.0024 usec/call
micros() 0.0451 usec/call
micros16() 0.0284 usec/call
millis() 0.3196 usec/call
millis16() 0.3191 usec/call
micros64() 0.2268 usec/call
hrt_micros32() 0.0532 usec/call
hrt_micros64() 0.1607 usec/call
hrt_millis32() 0.2488 usec/call
hrt_millis64() 0.2601 usec/call
fadd 0.0256 usec/call
fsub 0.0254 usec/call
fmul 0.0376 usec/call
fdiv /= 0.0594 usec/call
fdiv 2/x 0.0640 usec/call
dadd 0.2640 usec/call
dsub 0.2542 usec/call
dmul 0.1356 usec/call
ddiv 0.1702 usec/call
sinf() 0.4584 usec/call
cosf() 0.4886 usec/call
tanf() 0.5704 usec/call
acosf() 0.4890 usec/call
asinf() 0.5784 usec/call
atan2f() 0.6554 usec/call
sqrtf() 0.0540 usec/call
sin() 0.4584 usec/call
cos() 0.4772 usec/call
tan() 0.5632 usec/call
acos() 0.4776 usec/call
asin() 0.5608 usec/call
atan2() 0.6618 usec/call
sqrt() 0.0540 usec/call
sq() 0.0196 usec/call
powf(v,2) 0.2954 usec/call
powf(v,3.1) 1.3268 usec/call
iadd8 0.0242 usec/call
isub8 0.0236 usec/call
imul8 0.0280 usec/call
idiv8 0.0358 usec/call
iadd16 0.0316 usec/call
isub16 0.0348 usec/call
imul16 0.0278 usec/call
idiv16 0.0478 usec/call
iadd32 0.0200 usec/call
isub32 0.0200 usec/call
imul32 0.0214 usec/call
idiv32 0.0236 usec/call
iadd64 0.0570 usec/call
isub64 0.0524 usec/call

I have modiled this to remove these as they crashed it: [ the hardware has double support, but its wip ]
cpuinfo_printf("printing NaN: %f\n", (double)sqrtf(-1.0f));
cpuinfo_printf("printing +Inf: %f\n", (double)(1.0f/0.0f));
cpuinfo_printf("printing -Inf: %f\n", (double)(-1.0f/0.0f));

i also removed this line:
TIMEIT("EKF",v_out = ekf.test(), 5);
.. as presumably, at this time, it uses doubles and no worky.

@davidbuzz
davidbuzz force-pushed the rp2350-v5-squashed-and-cleaned-and-rebased branch 7 times, most recently from f42f8f0 to 9e923a8 Compare May 7, 2026 04:38
@davidbuzz

Copy link
Copy Markdown
Contributor Author
  • cleaned up the __RAMFUNC2__ all over the place by using a registry file to hold all the symbols instead, and using linker magic to still get them into ram.
  • removed spurious =0; and =0.0f;
  • removed hundreds of lines of unnecessary whitespace changes.
  • re-based and re-labeled commits so they represent new reality.
  • got --targets=tool/CPUInfo to work well enough to get most info.

[Only other thing that was suggested in the eu dev call that i havent yet tried is explicity turning off multicore support and runing a strictly single-core variant, as Tridge seems to thing that might be a lot better. ]

@davidbuzz
davidbuzz force-pushed the rp2350-v5-squashed-and-cleaned-and-rebased branch from cbdf1e9 to d2728d3 Compare May 7, 2026 21:41
@davidbuzz

Copy link
Copy Markdown
Contributor Author

Given that the system has hardware single-precision-float but not hardware double, this means double performance appears to be about 10x slower than single , so this target uses HAL_WITH_EKF_DOUBLE=0. Also, this PR includes about a dozen double/float ambiguity fixes that help teh compiler use float instead of double. ( adding the 'f' after a constant ).

@davidbuzz

davidbuzz commented May 8, 2026 •

Copy link
Copy Markdown
Contributor Author

RP2350 Laurel @ 375 MHz — Single-Core vs SMP CPUInfo Benchmark

Operation Single-Core (µs) SMP (µs) Ratio (SMP/SC)
Clocks / timers
nop 0.0022 0.0021 0.95x
micros() 0.0450 0.0480 1.07x
micros16() 0.0298 0.0300 1.01x
millis() 0.2911 0.3207 1.10x
millis16() 0.3074 0.3365 1.09x
micros64() 0.1963 0.2317 1.18x
hrt_micros32() 0.0664 0.0684 1.03x
hrt_micros64() 0.1937 0.2148 1.11x
hrt_millis32() 0.2852 0.3110 1.09x
hrt_millis64() 0.2879 0.3126 1.09x
Single-precision float
fadd 0.0246 0.0274 1.11x
fsub 0.0242 0.0242 1.00x
fmul 0.0242 0.0244 1.01x
fdiv /= 0.0632 0.0586 0.93x
fdiv 2/x 0.0592 0.0546 0.92x
sinf() 0.4526 0.3942 0.87x
cosf() 0.4324 0.3692 0.85x
tanf() 0.6456 1.1814 1.83x
acosf() 0.1560 0.3416 2.19x
asinf() 0.1920 0.3718 1.94x
atan2f() 0.6236 0.6434 1.03x
sqrtf() 0.0546 0.0550 1.01x
sq() 0.0222 0.0192 0.86x
powf(v,2) 0.0194 0.0200 1.03x
powf(v,3.1) 1.2726 0.3604 0.28x
Double-precision float
dadd 0.2720 0.2792 1.03x
dsub 0.2616 0.2614 1.00x
dmul 0.1360 0.1390 1.02x
ddiv 0.1668 0.1616 0.97x
sin() 0.4574 0.3930 0.86x
cos() 1.1980 0.3902 0.33x
tan() 0.8764 0.5894 0.67x
acos() 0.1572 0.3458 2.20x
asin() 0.1814 0.3656 2.02x
atan2() 0.6098 0.6172 1.01x
sqrt() 0.0558 0.0536 0.96x
Integer
iadd8 0.0230 0.0230 1.00x
isub8 0.0244 0.0242 0.99x
imul8 0.0296 0.0308 1.04x
idiv8 0.0240 0.0228 0.95x
iadd16 0.0230 0.0240 1.04x
isub16 0.0248 0.0242 0.98x
imul16 0.0268 0.0270 1.01x
idiv16 0.0342 0.0336 0.98x
iadd32 0.0202 0.0202 1.00x
isub32 0.0218 0.0234 1.07x
imul32 0.0234 0.0214 0.91x
idiv32 0.0232 0.0232 1.00x
iadd64 0.0522 0.0548 1.05x
isub64 0.0576 0.0576 1.00x
imul64 0.0748 0.0786 1.05x
idiv64 0.2700 0.2710 1.00x
Memory
memcpy128 1.8092 1.8049 1.00x
memset128 2.4583 2.5441 1.04x
delay(1) 218.412 230.048 1.05x
OS primitives
SEM 0.5974 0.7908 1.32x
chSysLock 0.0085 0.0664 7.81x
chMtxLock 0.2034 0.3502 1.72x
chThdYield 0.1140 0.1708 1.50x

Note: SP/DP microbench — float call: 0.01911 vs 0.01914 µs; double call: 0.25248 vs 0.26200 µs.
Trig/transcendental variations between runs are likely cache/pipeline state effects, not SMP overhead.
The dominant SMP cost is chSysLock: LDREX/STREX hardware spinlock adds ~58 ns per lock/unlock pair.

I did 2x minor variants of this PR to get the aboev numbers, they are two latest commits over here: https://github.com/davidbuzz/ardupilot/tree/rp2350-v5-etc-single-core-vs-smp

@tpwrules tpwrules left a comment •

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a big PR and there are many things to comment on. Unsure what is draft and what is permanent so consider that when reading my comments.

  1. I did not really look at AP_HAL_ChibiOS or the ChibiOS changes themselves. Are you actually using the Raspberry Pi Pico SDK? I am also not passing judgment on the structure of the HAL at this point.
  2. There is a lot of funny stuff with FP on these boards and when using the SDK, including hard vs soft float calling convention, the double precision accelerator, -fsingle-precision-constant, and our double precision EKF. These all need to be nailed down, and mismatches may explain various divergent results. I am pretty sure double precision EKF is disabled by default.
  3. The overwhelming limit on this chip is XIP bandwidth. There are various clock and ratio tweaks that can be done to improve this; I don't know what is done but a gross overclock is likely counterproductive. I further have serious doubts that having two cores contend over XIP (and even RAM) bandwidth will improve performance. The structure of the core 1 offload computations is also suspect. Please completely remove this feature and treat it as a single-core chip. Multi-core could be considered in later PRs.
  4. There are still lots of little things to clean up. I have tried to focus my review on changes that could simplify the scope and get this PR (or one like it) in easier.

Comment thread ArduCopter/Attitude.cpp Resolved
Comment thread ArduCopter/Attitude.cpp Resolved
Comment thread libraries/AP_AHRS/AP_AHRS.cpp Resolved
Comment thread libraries/AP_Baro/AP_Baro_DPS280.cpp Resolved
Comment thread libraries/GCS_MAVLink/GCS_FTP.h Resolved
Comment thread Tools/AP_Bootloader/bl_protocol.cpp Resolved
Comment thread Tools/AP_Bootloader/bl_protocol.cpp Resolved
Comment thread Tools/AP_Bootloader/support.cpp Resolved
@tpwrules tpwrules mentioned this pull request May 11, 2026
4 of 9 tasks
@davidbuzz
davidbuzz force-pushed the rp2350-v5-squashed-and-cleaned-and-rebased branch from 13b7951 to 4530364 Compare May 15, 2026 08:49
@davidbuzz

davidbuzz commented May 19, 2026 •

Copy link
Copy Markdown
Contributor Author

@tpwrules - care to take another gander? I've cleaned it up a lot. :-) Eg, all the sync and async stuff on the second core is gone, and its now fully SMP enabled on the second core. I've also removed the 4x drive-by fixes that u sggested need to be PRs into their own small PRs.

@davidbuzz

davidbuzz commented May 19, 2026 •

Copy link
Copy Markdown
Contributor Author

Oh, and i also wanted to note that this PR [currently] sits on-top of ChibiOS master, so we wont be able to merge this till the next release of chibios is done, and ArduPilot is using it. [ Andy's working on that ], so dont panic, we have a plan for that. :-)
EDIT: have a PR on top of our ChibiOS that contains only the RP fixes now

andyp1per and others added 28 commits November 11, 2026 18:23
BRD_IDLE_STATS timed idle and run time in one object, entered from the
idle thread of every core. On RP2350 one core would start a
measurement and the other stop it, subtracting two unrelated cycle
counters, so the reported load was neither core's. Each core now keeps
its own measurement, scaled by the rate of the counter it reads, and
per-core getters report them.

No change to compiled output on STM32 boards.
On SMP the threads.txt header gives load and peak for each core, and
get_system_load() returns the busier one, so the takeoff check's CPU
load test sees a saturated rate-loop core rather than an average.

No change to compiled output on STM32 boards.
With the rate loop on core1 the main loop's load says nothing about
that core; the idle-thread stats give load and peak for both.
On a half-duplex PIO UART the receive interrupt drops our own echo
while _hd_echo_active is set, and only flush() or available() cleared
it. Any reply that arrived before the caller next looked was dropped
as echo too. SmartAudio flushes straight after writing, so it worked;
FPort and SRXL2 do not flush.

uart_tx now raises flag sm just before it releases the line, by which
time the receiver has pushed the last echoed byte at the centre of its
stop bit, and the interrupt purges the RX FIFO and closes the window
there, unless a byte is still in the FIFO or being shifted out. The
window now opens in the transmit drain as each byte goes to the FIFO,
not in write(), so it opens and closes under the same port lock and
neither core can close it on a byte the other has just sent. The
receive programs move up one slot for the extra instruction, which
takes the block to 30 of its 32 slots.

On RPI_UAVFC with a SmartAudio v2 VTX on SERIAL4, and AP_SmartAudio
changed for the test to skip its flush and read 150 ms after each
request, the 35 s after boot went from 276 bytes sent and none
received to one request and its 11 byte reply. Unchanged SmartAudio
still reads the VTX at boot and sets channel, power and pit mode, over
three runs before and after.

No change to compiled output on STM32 boards.
Five loops empty a PIO UART's receive FIFO: the drain and the echo
purge in the interrupt, and the purges in begin(), discard_input() and
before the interrupt is enabled. Each stopped only when FSTAT read
empty. Two run in the interrupt and two under the port lock with
kernel interrupts masked, so a FIFO that never read empty would hang
the core until the watchdog reset it, and the interrupt on the other
core would spin on the lock behind it.

Each now reads at most 16 entries, twice the joined FIFO's depth. A
byte takes 24 us even at CRSF's 420 kbaud, far longer than a pass, so
the limit is not reached in normal running. A drain cut short
leaves RXNEMPTY raised and the interrupt takes the rest on its next
entry. The four purges share one inline helper, so the interrupt still
runs entirely from SRAM.

SmartAudio on RPI_UAVFC still reads the VTX at boot and sets channel,
power and pit mode.

No change to compiled output on STM32 boards.
@SYS/flash.bin serves the internal flash from 0x08000000, where it
sits on STM32. A board whose flash is elsewhere can now set
AP_FILESYSTEM_SYS_FLASH_BASE.

No change to compiled output on STM32 boards.
On RP2350 the boot flash is the XIP window at 0x10000000, and
0x08000000 is not flash. On RPI_UAVFC a fetch of @SYS/flash.bin now
returns the bootloader as programmed, followed by the parameter
storage block.

No change to compiled output on STM32 boards.
…theses

The placement script emitted every "(archive)" entry as a linker pattern
without checking it, so a member the toolchain names differently stayed
in flash with no warning: newer newlib names libm's objects
libm_a-<name>.o rather than lib_a-<name>.o, which silently put the math
library back in flash on a GCC 14 build. Resolve each entry against the
toolchain's own libm and libc, accepting either spelling, and count a
member that exists under neither as a miss under --strict. Read symbols
with the cross nm and c++filt named by a new --toolchain option,
defaulting to arm-none-eabi.

It also cut a demangled name at its first "(", so no entry could name a
decltype return type (MAX, MIN) or a template argument such as
Bitmask<(unsigned short)256>. Drop a leading decltype (...) and cut at
the first "(" outside template brackets, falling back to the old rule
for names like operator< that never return to depth 0. The one entry
written for the old cut, "DerivativeFilter<float,", now names the three
methods the copter links.

No change to the RPI_UAVFC copter binary with GCC 10.
So the script reads symbols with the binutils of the toolchain that built
them. No change to the RPI_UAVFC copter binary.
It already read symbols with arm-none-eabi-nm; demangle with the matching
c++filt rather than whatever the host provides.
Core1's rate loop runs from SRAM but still called into flash on every
pass: the thrust curve, the current limit and AP_Motors_limit::set_rpy
in the motor output, the ChibiOS waits, wakes and locks of rcout,
trigger_groups and dshot_send, the spinlock release reached from PendSV,
the SIO FIFO interrupt, the rate-loop logging checks, the DShot command
and telemetry request path, the dynamic notch update and a few
arithmetic leaves. Core1's fetches also cost core0, which shares the
XIP cache and queues behind them at the QMI.

Move them, and return two functions to flash that gained nothing in
SRAM: NavEKF3_core::FuseAirspeed, which runs only with an airspeed
sensor, and chvprintf, reached only from a bare printf() through
__wrap_vprintf in flash.

Armed at low throttle on RPI_UAVFC at 225 MHz, two runs each: the motor
path took core1 from 55% to 50% busy and its non-idle time in flash
from 21.5% to 15%. The rest, measured with bidirectional DShot, logging
and the PIO OSD running, took that from 11.4% to 0.2%, core1 from 52%
to 46% busy, core0 from 58% to 55% and the scheduler load from 38.7% to
36%. The heap ends 32 bytes smaller. A 150 s armed soak was clean.
Adds RP_QMI_CONT_READ_MODE and RP_QMI_MIN_DESELECT to the RP2350 port.
Both are off unless a board defines them.
…divider

The decode took its scale from a fixed 75 MHz, so the system clock had
to be a multiple of 75 MHz. Take the scale from the 16.8 divider the
state machine is given instead, which 200 MHz reaches within 0.1%
(75.07 MHz), and keep only the check that the divider is at least 1.

No change at 225 or 150 MHz: the dividers are the same, and the decode
thresholds come out at 3, 9, 14 and 20 samples at 225, 200 and 150 MHz.
The RPI_UAVFC copter binary differs only in one ULP of the scale and a
WITH_SEMAPHORE line number.
The divider was already computed from the system clock; only the
comment claimed an integer. No change to compiled output.
…flash

At 225 MHz the flash could only run at 75 MHz, as divider 2 put the
read sample too close to the edge; 200 MHz divides to 100 MHz. Each
board now takes 200 MHz from a 1200 MHz VCO, the flash at 100 MHz
(CLKDIV 2, RXDELAY 2), 3 clocks (15 ns) between reads where the bootrom
leaves 7, and continuous read, which drops the 8-clock command from
every XIP cache miss and checks itself at boot.

On RPI_UAVFC (W25Q32JV, JEDEC ID EF4016) a sweep with both cores halted,
reading the app twice per setting through the uncached XIP alias, read
every byte right with the sample 7.5 to 15 ns after the falling edge
and none at 5 or 17.5 ns; RXDELAY 2 samples at 10 ns. Armed on the
bench with DShot, logging and the OSD running, continuous read and the
shorter deselect took the scheduler load from 46% to 40%, and 200 MHz
with the faster flash from 36% to 31%. An armed soak at each step read
30 MB of flash back correctly, bidirectional DShot decoded with no
errors, the OSD looked unchanged, and parameter writes, reboots and
uploads into and out of continuous read all worked.

Laurel booted on it and read its whole 4 MB back correctly over MAVLink
FTP, twice disarmed and once armed, with continuous read engaged; its
boot ROM sets Dual I/O, so a miss there takes 48 flash clocks rather
than 28. A Pico 2 read its whole flash back correctly twice, in Quad
I/O with continuous read engaged; its W25Q32RV deselect time is not
checked against a datasheet. Neither has had its own sweep. RPI_UAVFC
flew it three times on 2026-11-07, at 29-41% scheduler load in flight
against 54-69% at 225 MHz, with no long loops.
Core0 still fetched from flash on every main loop: its tick alarm and
DMA vectors (Vector40, Vector68; core1's were already in SRAM), newlib's
memcmp, which AP_DAL::WriteLogMessage runs on every replay message,
asinf and acosf with the sqrtf and fabsf they call, and most of the
AHRS update: the EKF3 and DCM result copies, update_state, the trig and
view updates, DCM's normalisation, the airspeed estimates and the
matrix, quaternion and vector maths under them.

Move those, taking every AHRS function above 1% of core0's busy time
per KB. AP_AHRS_External::get_results would qualify, but its work is
wasted with no external AHRS fitted. The NavEKF3 getters get_results
calls are each under 0.06% and stay in flash.

Armed on RPI_UAVFC at 200 MHz with DShot, logging and the OSD running,
two runs each: the vectors and libc took the scheduler load from 31.1%
to 29.3% and core0 from 51% to 49% busy; the AHRS took the load from
29.7% to 25.5% and core0 from 49.3% to 46.1%. Core0's non-idle time in
flash went from 59% to 50%, and core1 is unchanged. 6976 bytes of SRAM.
AP_AHRS updates every backend it is built with on every loop and copies
out its results, whether or not AHRS_EKF_TYPE selects it. With no
external AHRS fitted, AP_AHRS_External::get_results and the
AP_ExternalAHRS getters under it took about 1% of core0's busy time, all
of it fetched from flash.

Armed on RPI_UAVFC at 200 MHz with DShot, logging and the OSD running,
two runs each: scheduler load 25.5% to 24.9%, core0 46.1% to 45.0% busy,
core1 unchanged, and DShot decoded with no errors. 40 KB less flash and
544 bytes more heap.
The PL022 runs at clk_peri / (CPSR x (1 + SCR)), and SCR was taken as
clk_peri / (CPSR x f) rounded down. That never exceeds the request, but
when the division is exact it runs one step slow: at 200 MHz the
microSD's 25 MHz ran at 20 MHz, its 400 kHz init at 398 kHz and the
IMUs' 2 MHz at 1.96 MHz. Take the smallest divisor whose clock does not
exceed the request instead. The IMUs' 8 and 16 MHz are unchanged.

On RPI_UAVFC, armed for 45 s with 140 KB/s of logging, the run at
20 MHz dropped 518 messages with the buffer down to 0.8 KB free; the
same run at 25 MHz, twice, dropped none and kept at least 26 KB free.
Every log read back over MAVLink FTP and parsed with no errors, the
25 MHz ones 60% faster.
Bring the RPI_UAVFC, Laurel and Pico2 READMEs and hardware notes, and
the defaults.parm comments, into line with the 200 MHz core clock and
100 MHz flash clock. No default changes; RPI_UAVFC's firmware differs
only in the embedded copy of its defaults.parm comment.
…lacement

Record how continuous read works and what a cache miss costs: an 8-byte
line is 36 flash clocks, 28 in continuous read, or 56 and 48 on
Laurel's Dual I/O flash. Record that core1's flash fetches cost core0
at the QMI as well as in the cache, how template and decltype names are
written in the registries, and which spellings of a newlib archive
member the placement script accepts.

Give the clock, XIP miss, SPI and WS2812 figures at 200 MHz, which all
three boards now run, how to measure a board's flash read window, and
the boot constraint on RXDELAY. Measure free memory with MEMINFO:
@SYS/memory.txt counts only the heap's free list, 2 KB on RPI_UAVFC
while available_memory() reported 153 KB. Add the reboot HardFault to
the known issues.

Documentation and comments only.
On a bench Pico 2 the boot ROM sets Quad I/O (EBh) and continuous read
engages with the W25Q32RV: at 200 MHz with the 100 MHz flash clock the
whole 4 MB read back correctly over MAVLink FTP twice, and three
parameter writes each survived a reboot with continuous read back on.
Comments only.
RPI_UAVFC flew the 200 MHz flash settings three times on 2026-11-07,
including RTL by switch and the port's first Auto mission, and the
Pico 2 has run them on the bench, as Laurel has. RTL's landing and the
GPS failsafe remain unflown.
thread_init() raises the caller above the new thread's priority while
creating it, so the thread does not run before uart_thread_ctx is
assigned. uart_thread() already waits for that; this just avoids the
wait.

_flush(), _write() and set_unbuffered_writes() skip chEvtSignal() while
uart_thread_ctx is still null, instead of signalling a null thread.

These are the only parts of the RP2350 UART support that change the
compiled output on existing boards.

Co-authored-by: Andy Piper <github@andypiper.com>
can_send_dshot_pulse() and serial_led_send() read rcout_micros(), so
both sides of the subtraction from last_dmar_send_us use
rcout_timer_t. With the default 64-bit rcout_timer_t that is
micros64() as before.

dshot_send(), dshot_send_command() and dshot_collect_dma_locks()
null-check dma_handle before locking or unlocking it, for groups that
send DShot without a DMA channel, as the RP2350 PIO output
does.

Co-authored-by: David Buzz <davidbuzz@gmail.com>
push_next_gyro_sample() signalled the rate thread while still holding
the fast rate buffer mutex. The rate thread runs at a higher priority,
so it woke, blocked on that mutex and handed back through priority
inheritance: an extra context switch pair per gyro sample on the way
to the rate loop. The signal now comes after the mutex is released.

Co-authored-by: David Buzz <davidbuzz@gmail.com>
clock_pulse() ignored acquire_bus(), which on RP2350 fails when the
bus could not get its DMA channels, and went on to start a transfer
on a driver with null channels. Give the semaphore back and fail.
delay() divided a 64-bit microsecond count by 1000 on every pass of its
1 ms loop. On RP2350, where the M33 has no 64-bit divide, that was 85%
of the calls to __aeabi_uldivmod (a hardware breakpoint on it, armed on
RPI_UAVFC) and took 64-bit division from 1.12% of core0's busy time to
0.39%.

A uint16_t delay is at most 65.5 s, far inside the 32-bit wrap, and
elapsed_us / 1000 < ms holds exactly when elapsed_us < ms * 1000, so
the loop ends at the same time as before. Changes compiled output on
every ChibiOS board.
The RP2350 hardware UARTs use the SIO driver, and _line_config_sem is
declared and taken only for HAL_USE_SERIAL, so their parity, stop bit
and option changes run unlocked. Two threads changing the parity can
leave the port on one parity while get_parity() reports the other, and
a parity change racing a stop bit change can write back the stop bits
the other has just replaced: the race master fixed for STM32 UARTs.

Declare the lock for HAL_USE_SIO too, and take it in the SIO branches
as the HAL_USE_SERIAL path does. sio_apply_framing() takes _write_mutex
inside it, and nothing holds _write_mutex while changing line settings,
so the two are always taken in the same order.

On STM32 boards the only change to compiled output is the line number
two existing WITH_SEMAPHORE() calls further down the file record.
@andyp1per

Copy link
Copy Markdown
Contributor

Status against the review of 20c6054, now at 6a9e836:

  • SIO line settings lock: fixed in 6a9e836. _line_config_sem is now declared for HAL_USE_SIO and taken in the SIO branches of _configure_parity(), _set_stop_bits() and _set_options(). sio_apply_framing() takes _write_mutex inside it, and nothing holds _write_mutex while changing line settings, so the two locks are always taken in the same order. On STM32 the only change is the line number two existing WITH_SEMAPHORE calls record.
  • PIOUART numbering: chibios_hwdef.py now errors when SERIAL_ORDER lists PIOUARTs out of order or with a gap (f0797ea).
  • SMP commit message: reworded in 6599145.
  • Bootloaders: after the repin.
  • The CI failures at 20c6054 were runner DNS failures in apt.

@AP-Review

Copy link
Copy Markdown

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: REQUEST CHANGES

Reviewed at head 6a9e836fb0.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/32995/35.html#pr32995

Still blocked by the unmerged ChibiOS#113 pin. Reviewed at 6a9e836, the live head, which moved from 20c6054 by one new commit and two amended ones, with no rebase.

Since the previous review, the SIO UART line-settings lock, the PIOUART numbering check and the SMP commit message have been fixed.

Blocking

  1. modules/ChibiOS: ChibiOS pin (still open). CI's check-merge-commits now fails on it at "Check branch conventions", because e8bdbce and 9b8f9f6 both reference ChibiOS commits not yet on master.
Minor (3)

Not fully reviewed: behaviour on RP2350 hardware (clocks and flash timing, watchdog-reset boot, DShot telemetry, OSD, SPI/microSD, GET_CRC readback and erase timing, core1 start-up, the reported flights), CI and SITL at this head (62 of 97 checks were still queued or running), the UART lock race on target, and the ChibiOS#113 port code beyond the parts this PR calls.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Projects

None yet

Development

Successfully merging this pull request may close these issues.