diff --git a/embassy-rp/src/pio_programs/clock_divider.rs b/embassy-rp/src/pio_programs/clock_divider.rs new file mode 100644 index 000000000..02e353f53 --- /dev/null +++ b/embassy-rp/src/pio_programs/clock_divider.rs @@ -0,0 +1,25 @@ +//! Helper functions for calculating PIO clock dividers + +use fixed::traits::ToFixed; +use fixed::types::extra::U8; + +use crate::clocks::clk_sys_freq; + +/// Calculate a PIO clock divider value based on the desired target frequency. +/// +/// # Arguments +/// +/// * `target_hz` - The desired PIO clock frequency in Hz +/// +/// # Returns +/// +/// A fixed-point divider value suitable for use in a PIO state machine configuration +#[inline] +pub fn calculate_pio_clock_divider(target_hz: u32) -> fixed::FixedU32 { + // Requires a non-zero frequency + assert!(target_hz > 0, "PIO clock frequency cannot be zero"); + + // Calculate the divider + let divider = (clk_sys_freq() + target_hz / 2) / target_hz; + divider.to_fixed() +} diff --git a/embassy-rp/src/pio_programs/hd44780.rs b/embassy-rp/src/pio_programs/hd44780.rs index 3aa54495f..546c85a89 100644 --- a/embassy-rp/src/pio_programs/hd44780.rs +++ b/embassy-rp/src/pio_programs/hd44780.rs @@ -1,11 +1,11 @@ //! [HD44780 display driver](https://www.sparkfun.com/datasheets/LCD/HD44780.pdf) -use crate::clocks::clk_sys_freq; use crate::dma::{AnyChannel, Channel}; use crate::pio::{ Common, Config, Direction, FifoJoin, Instance, Irq, LoadedProgram, PioPin, ShiftConfig, ShiftDirection, StateMachine, }; +use crate::pio_programs::clock_divider::calculate_pio_clock_divider; use crate::Peri; /// This struct represents a HD44780 program that takes command words ( <0:4>) @@ -136,10 +136,8 @@ impl<'l, P: Instance, const S: usize> PioHD44780<'l, P, S> { let mut cfg = Config::default(); cfg.use_program(&word_prg.prg, &[&e]); - // Scale the divider based on system clock frequency - // Original: 125 at 125 MHz (1 MHz PIO clock) - let word_divider = (clk_sys_freq() / 1_000_000) as u8; // Target 1 MHz PIO clock - cfg.clock_divider = word_divider.into(); + // Target 1 MHz PIO clock (each cycle is 1µs) + cfg.clock_divider = calculate_pio_clock_divider(1_000_000); cfg.set_out_pins(&[&db4, &db5, &db6, &db7]); cfg.shift_out = ShiftConfig { @@ -167,10 +165,8 @@ impl<'l, P: Instance, const S: usize> PioHD44780<'l, P, S> { let mut cfg = Config::default(); cfg.use_program(&seq_prg.prg, &[&e]); - // Original: 8 at 125 MHz (~15.6 MHz PIO clock) - // Comment says ~64ns/insn which is 1/(15.6 MHz) = ~64ns - let seq_divider = (clk_sys_freq() / 15_600_000) as u8; // Target ~15.6 MHz PIO clock (~64ns/insn) - cfg.clock_divider = seq_divider.into(); + // Target ~15.6 MHz PIO clock (~64ns/insn) + cfg.clock_divider = calculate_pio_clock_divider(15_600_000); cfg.set_jmp_pin(&db7); cfg.set_set_pins(&[&rs, &rw]); diff --git a/embassy-rp/src/pio_programs/mod.rs b/embassy-rp/src/pio_programs/mod.rs index 74537825b..8eac328b3 100644 --- a/embassy-rp/src/pio_programs/mod.rs +++ b/embassy-rp/src/pio_programs/mod.rs @@ -1,5 +1,6 @@ //! Pre-built pio programs for common interfaces +pub mod clock_divider; pub mod hd44780; pub mod i2s; pub mod onewire; diff --git a/embassy-rp/src/pio_programs/rotary_encoder.rs b/embassy-rp/src/pio_programs/rotary_encoder.rs index 71567a602..70b3795e9 100644 --- a/embassy-rp/src/pio_programs/rotary_encoder.rs +++ b/embassy-rp/src/pio_programs/rotary_encoder.rs @@ -1,12 +1,10 @@ //! PIO backed quadrature encoder -use fixed::traits::ToFixed; - -use crate::clocks::clk_sys_freq; use crate::gpio::Pull; use crate::pio::{ Common, Config, Direction as PioDirection, FifoJoin, Instance, LoadedProgram, PioPin, ShiftDirection, StateMachine, }; +use crate::pio_programs::clock_divider::calculate_pio_clock_divider; use crate::Peri; /// This struct represents an Encoder program loaded into pio instruction memory. @@ -50,10 +48,8 @@ impl<'d, T: Instance, const SM: usize> PioEncoder<'d, T, SM> { cfg.fifo_join = FifoJoin::RxOnly; cfg.shift_in.direction = ShiftDirection::Left; - // Original: 10_000 at 125 MHz (12.5 KHz PIO clock) - // Scale divider to maintain same PIO clock frequency at different system clocks - let divider = (clk_sys_freq() as f32 / 12_500.0).to_fixed(); - cfg.clock_divider = divider; + // Target 12.5 KHz PIO clock + cfg.clock_divider = calculate_pio_clock_divider(12_500); cfg.use_program(&program.prg, &[]); sm.set_config(&cfg); diff --git a/embassy-rp/src/pio_programs/stepper.rs b/embassy-rp/src/pio_programs/stepper.rs index 6878c32f5..0e9a8daf9 100644 --- a/embassy-rp/src/pio_programs/stepper.rs +++ b/embassy-rp/src/pio_programs/stepper.rs @@ -2,12 +2,8 @@ use core::mem::{self, MaybeUninit}; -use fixed::traits::ToFixed; -use fixed::types::extra::U8; -use fixed::FixedU32; - -use crate::clocks::clk_sys_freq; use crate::pio::{Common, Config, Direction, Instance, Irq, LoadedProgram, PioPin, StateMachine}; +use crate::pio_programs::clock_divider::calculate_pio_clock_divider; use crate::Peri; /// This struct represents a Stepper driver program loaded into pio instruction memory. @@ -65,7 +61,9 @@ impl<'d, T: Instance, const SM: usize> PioStepper<'d, T, SM> { sm.set_pin_dirs(Direction::Out, &[&pin0, &pin1, &pin2, &pin3]); let mut cfg = Config::default(); cfg.set_out_pins(&[&pin0, &pin1, &pin2, &pin3]); - cfg.clock_divider = (clk_sys_freq() / (100 * 136)).to_fixed(); + + cfg.clock_divider = calculate_pio_clock_divider(100 * 136); + cfg.use_program(&program.prg, &[]); sm.set_config(&cfg); sm.set_enable(true); @@ -74,9 +72,11 @@ impl<'d, T: Instance, const SM: usize> PioStepper<'d, T, SM> { /// Set pulse frequency pub fn set_frequency(&mut self, freq: u32) { - let clock_divider: FixedU32 = (clk_sys_freq() / (freq * 136)).to_fixed(); - assert!(clock_divider <= 65536, "clkdiv must be <= 65536"); - assert!(clock_divider >= 1, "clkdiv must be >= 1"); + let clock_divider = calculate_pio_clock_divider(freq * 136); + let divider_f32 = clock_divider.to_num::(); + assert!(divider_f32 <= 65536.0, "clkdiv must be <= 65536"); + assert!(divider_f32 >= 1.0, "clkdiv must be >= 1"); + self.sm.set_clock_divider(clock_divider); self.sm.clkdiv_restart(); }