thequux/film-scanner
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed up nostd support, verified that sqrt and cbrt routines work to within 1ULP

Browse Source
This commit is contained in:
TQ Hirsch
2022-04-25 22:36:05 +02:00
parent 8016a8c7f8
commit 1e1cac4f06
7 changed files with 75 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
firmware/film-scanner-fw/.cargo/config.toml
View File

@@ -20,7 +20,7 @@ rustflags = [
]
[build]
target = "thumbv7em-none-eabihf"
target = "thumbv6m-none-eabi"
[env]
DEFMT_LOG = "debug"

48
firmware/film-scanner-fw/src/main.rs
View File

@@ -15,8 +15,10 @@ mod app {
use core::num::Wrapping;
use embedded_hal::digital::v2::OutputPin;
use embedded_time::duration::Extensions;
use jerk_control::executor::CommandQueue;
use jerk_control::planner::Planner;
use rp_pico::hal;
use rp_pico::hal::{self, gpio::pin::{Output, PushPull, PullDownDisabled, Pin}};
use rp_pico::pac;
use rp_pico::XOSC_CRYSTAL_FREQ;
use rtic::mutex_prelude::{TupleExt04, TupleExt05};
@@ -69,11 +71,26 @@ mod app {
pub active: bool,
}
pub struct StepperProcess<Pin> {
pub struct StepperProcess<StepPin, DirPin>
where StepPin: rp2040_hal::gpio::PinId,
DirPin: rp2040_hal::gpio::PinId {
pub executor: executor::CommandQueue,
pub pin: Pin,
pub step: rp_pico::hal::gpio::Pin<StepPin, Output<PushPull>>,
pub dir: rp_pico::hal::gpio::Pin<DirPin, Output<PushPull>>,
}
impl<StepPin, DirPin> StepperProcess<StepPin, DirPin>
where StepPin: rp2040_hal::gpio::PinId,
DirPin: rp2040_hal::gpio::PinId {
pub fn new(step: Pin<StepPin, PullDownDisabled>, dir: Pin<DirPin, PullDownDisabled>) -> Self {
Self {
executor: CommandQueue::new(),
step: step.into_push_pull_output(),
dir: dir.into_push_pull_output(),
}
}
}
#[shared]
struct Shared {
@@ -89,9 +106,11 @@ mod app {
}
use rp2040_hal::gpio::pin::bank0;
#[local]
struct Local {
executor1: StepperProcess<rp_pico::hal::gpio::Pin<hal::gpio::pin::bank0::>>,
executor1: StepperProcess<bank0::Gpio1, bank0::Gpio2>,
}
#[init(local = [usb_bus: Option<usb_device::bus::UsbBusAllocator<hal::usb::UsbBus>> = None])]
@@ -178,7 +197,9 @@ mod app {
serial,
usb_dev,
},
Local {},
Local {
executor1: StepperProcess::new(pins.gpio1, pins.gpio2),
},
init::Monotonics(),
)
}
@@ -188,9 +209,24 @@ mod app {
binds = TIMER_IRQ_0,
priority = 4,
shared = [timer, alarm, serial],
local = [tog: bool = true],
local = [executor1],
)]
fn timer_irq(mut _cx: timer_irq::Context) {
let executor: &mut StepperProcess<_, _> = _cx.local.executor1;
if executor.executor.direction() {
executor.dir.set_high();
} else {
executor.dir.set_low();
}
if executor.executor.step_early() {
executor.step.set_high();
} else {
executor.step.set_low();
}
executor.executor.step_late();
}
/// Usb interrupt handler. Runs every time the host requests new data.

2
firmware/jerk_control/src/executor.rs
View File

@@ -36,7 +36,7 @@ impl CommandQueue {
// Returns true if step signal should be high. This does the first, constant time part of stepping.
// Note that step_late needs to be called as well to advance the segment.
pub fn step_early(&mut self) -> bool {
let cur_segment = &mut self.segments[self.cur_segment as usize];
self.p.0 = (self.p + cur_segment.delta[0]).0.rem_euclid(self.step_size);

2
firmware/jerk_control/src/lib.rs
View File

@@ -1,4 +1,4 @@
// #![no_std]
#![no_std]
pub mod planner;
pub mod executor;

26
firmware/jerk_control/src/math_ext.rs
View File

@@ -41,12 +41,22 @@ fn sqrt_approx(value: f32) -> f32 {
pub fn cbrt_approx(value: f32) -> f32 {
let value = value.to_bits();
const EXP_BIAS: i32 = 0x3f80_0000;
//const EXP_BIAS: i32 = 0x3f80_0000;
f32::from_bits((value & 0x8000_0000) |
(((value as i32 & 0x7fff_ffff) - EXP_BIAS) / 3 + EXP_BIAS) as u32)
(((value & 0x7fff_ffff) / 3).wrapping_add(0x2a5119f2)))
}
fn frexp(value: f32) -> (i32, f32) {
let value = value.to_bits();
(((value >> 23) & 0xff) as i32 - 126, // we go to [0.5,1), whereas the mantissa represents [1,2)
f32::from_bits(value & 0x7f_ffff | (126 << 23)))
}
fn ldexp(exp: i32, mantissa: f32) -> f32 {
f32::from_bits(mantissa.to_bits().wrapping_add((exp as u32) << 23))
}
impl Roots for f32 {
fn sqrt(self) -> Self {
let mut estimate = sqrt_approx(self);
@@ -56,16 +66,18 @@ impl Roots for f32 {
estimate
}
//#[cfg(ignore)]
fn cbrt(self) -> Self {
let mut estimate = cbrt_approx(self); // TODO: improve this with bit twiddling
let mut estimate = cbrt_approx(self) as f64; // TODO: improve this with bit twiddling
let a = self as f64;
// Use halley's method to refine the approximation.
for _ in 0..3 {
for _ in 0..2 {
let e3 = estimate * estimate * estimate;
estimate = estimate * (e3 + self + self) / ( e3 + e3 + self);
estimate = estimate * (e3 + a + a) / ( e3 + e3 + a);
}
estimate
estimate as f32
}
}

1
firmware/jerk_control/src/planner.rs
View File

@@ -5,6 +5,7 @@
use core::num::Wrapping;
use core::option::Option;
use crate::math_ext::Roots as _;
pub struct Config {
/// Max jerk, in mm/s^3

33
firmware/motion-tester/src/bin/math_test.rs
View File

@@ -5,43 +5,30 @@ fn ulp(f: f32) -> f32 {
}
fn fn_err(actual: f32, calc: f32) -> f32 {
(calc - actual).abs() / actual
(calc - actual).abs() / ulp(actual)
}
fn test_all(fname: &str, actual: fn(f32) -> f32, calc: fn(f32) -> f32) {
let mut max_err = 0f32;
let mut max_err_value = 0f32;
(0x01..=0xfe).into_par_iter()
let errors =
(0x01..=0xfe).into_par_iter()
.map(|exp| {
(0x0..=0x7f_ffff).map(|mant| f32::from_bits((exp << 23) + mant))
let max_err_place = (0x0..=0x7f_ffff).map(|mant| f32::from_bits((exp << 23) + mant))
.max_by_key(|value| fn_err(actual(*value), calc(*value)).to_bits())
.unwrap()
.unwrap();
(exp, max_err_place, fn_err(actual(max_err_place), calc(max_err_place)))
})
.map(|value| (value,fn_err(actual(value), calc(value))))
.collect::<Vec<_>>();
for i in (0x0400_0000..0x0f7f_ffffu32).map(f32::from_bits) {
// range of normalized floats...
let err = fn_err(actual(i), calc(i));
if err > max_err {
max_err = err;
max_err_value = i;
}
for (exp, base, err) in errors {
println!("{},{},{}", exp as i32 - 127,base, err);
}
let max_err_pct = max_err * 100f32;
println!("{}: {}% @ {}", fname, max_err_pct, max_err_value);
println!(
"Value: 0x{:08x}\n
Calculated: 0x{:08x}\n
Actual: 0x{:08x}",
max_err_value.to_bits(),
calc(max_err_value).to_bits(), actual(max_err_value).to_bits())
}
fn main() {
// test_all("sqrt", f32::sqrt, jerk_control::math_ext::Roots::sqrt);
//test_all("sqrt", f32::sqrt, jerk_control::math_ext::Roots::sqrt);
test_all("cbrt", f32::cbrt, jerk_control::math_ext::Roots::cbrt);
}
}