use embedded_hal::pwm::SetDutyCycle;
use crate::PolarityMode;
use crate::brightness::{self, Brightness};
pub trait GammaMap {
fn map(&self, raw: Brightness) -> Brightness;
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub enum GammaCorrection {
Linear,
CieLStar,
}
#[rustfmt::skip]
const CIE_PREFIX: [u8; 16] = [
0, 9, 18, 26, 33, 39, 44, 48,
52, 56, 60, 63, 66, 69, 72, 74,
];
#[rustfmt::skip]
const CIE_KNOTS: [u8; 16] = [
77, 107, 129, 146, 160, 173, 184, 194,
204, 212, 221, 228, 236, 242, 249, 255,
];
#[inline]
fn cie8_map(raw: u8) -> u8 {
if raw < 16 {
CIE_PREFIX[raw as usize]
} else if raw == 255 {
255 } else {
let idx = ((raw - 16) >> 4) as usize;
let lo = CIE_KNOTS[idx];
let diff = CIE_KNOTS[idx + 1].wrapping_sub(lo);
let frac = raw & 0x0F;
let mut offset = 0u8;
let mut lost = 0u8;
if frac & 8 != 0 {
offset += diff >> 1;
lost += diff & 1;
}
if frac & 4 != 0 {
offset += diff >> 2;
lost += (diff & 3) >> 1;
}
if frac & 2 != 0 {
offset += diff >> 3;
lost += (diff & 7) >> 2;
}
if frac & 1 != 0 {
offset += diff >> 4;
lost += (diff & 15) >> 3;
}
if lost >= 4 {
offset += 1;
} lo.wrapping_add(offset)
}
}
#[cfg(not(feature = "brightness-12bit"))]
impl GammaMap for GammaCorrection {
#[inline]
fn map(&self, raw: Brightness) -> Brightness {
match self {
Self::Linear => raw,
Self::CieLStar => cie8_map(raw),
}
}
}
#[cfg(feature = "brightness-12bit")]
impl GammaMap for GammaCorrection {
#[inline]
fn map(&self, raw: Brightness) -> Brightness {
match self {
Self::Linear => raw,
Self::CieLStar => {
let r: u16 = raw.into();
if r == 0 {
return brightness::min_value();
}
if r >= 4095 {
return brightness::max_value();
}
let base = (r >> 4) as u8; let frac = (r & 0x0F) as u8;
let lo = cie8_map(base) as u16;
let hi = cie8_map(base.saturating_add(1)) as u16;
let v8 = lo + ((hi - lo) * frac as u16 + 8) / 16;
let v12 = (v8 as u32 * 4095 + 127) / 255;
Brightness::new(v12 as u16)
}
}
}
}
pub struct PwmLed<P: SetDutyCycle, G: GammaMap = GammaCorrection> {
pin: P,
gamma: G,
polarity: PolarityMode,
max_duty: u16,
duty_scale: u32,
brightness: Brightness,
}
impl<P: SetDutyCycle, G: GammaMap> PwmLed<P, G> {
pub fn new(pin: P, gamma: G, polarity: PolarityMode) -> Result<Self, P::Error> {
let max_duty = pin.max_duty_cycle();
let duty_scale = max_duty as u32 * brightness::DUTY_SCALE_NUM;
let mut led = Self {
pin,
gamma,
polarity,
max_duty,
duty_scale,
brightness: Brightness::default(),
};
led.off()?;
Ok(led)
}
#[inline]
pub fn from_pin(pin: P, gamma: G, polarity: PolarityMode) -> Self {
let max_duty = pin.max_duty_cycle();
let duty_scale = max_duty as u32 * brightness::DUTY_SCALE_NUM;
Self {
pin,
gamma,
polarity,
max_duty,
duty_scale,
brightness: Brightness::default(),
}
}
pub fn set_brightness(&mut self, raw: Brightness) -> Result<(), P::Error> {
let corrected = self.gamma.map(raw);
let duty_raw = self.polarity.map_duty(corrected);
let duty = ((brightness::to_u32(duty_raw) * self.duty_scale + 32768) >> 16) as u16;
self.pin.set_duty_cycle(duty)?;
self.brightness = raw;
Ok(())
}
#[inline]
pub fn set_brightness_percent(&mut self, pct: u8) -> Result<(), P::Error> {
self.set_brightness(brightness::from_percent(pct))
}
#[inline]
pub fn set_duty_raw(&mut self, duty: u16) -> Result<(), P::Error> {
self.pin.set_duty_cycle(duty)
}
#[inline]
pub fn off(&mut self) -> Result<(), P::Error> {
self.set_brightness(Brightness::default())
}
#[inline]
pub fn on(&mut self) -> Result<(), P::Error> {
self.set_brightness(brightness::max_value())
}
#[inline]
pub fn brightness(&self) -> Brightness {
self.brightness
}
#[inline]
pub fn is_on(&self) -> bool {
brightness::to_u32(self.brightness) > 0
}
#[inline]
pub fn is_off(&self) -> bool {
brightness::to_u32(self.brightness) == 0
}
#[inline]
pub fn gamma(&self) -> &G {
&self.gamma
}
#[inline]
pub fn polarity(&self) -> PolarityMode {
self.polarity
}
#[inline]
pub fn max_duty(&self) -> u16 {
self.max_duty
}
#[inline]
pub fn release(self) -> P {
self.pin
}
}
impl<P: SetDutyCycle, G: GammaMap> embedded_hal::pwm::ErrorType for PwmLed<P, G> {
type Error = P::Error;
}
impl<P: SetDutyCycle, G: GammaMap> SetDutyCycle for PwmLed<P, G> {
fn max_duty_cycle(&self) -> u16 {
self.max_duty
}
fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
self.pin.set_duty_cycle(duty)
}
}
impl<P: SetDutyCycle, G: GammaMap> core::fmt::Debug for PwmLed<P, G>
where
G: core::fmt::Debug,
{
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
f.debug_struct("PwmLed")
.field("gamma", &self.gamma)
.field("polarity", &self.polarity)
.field("max_duty", &self.max_duty)
.field("brightness", &brightness::to_u32(self.brightness))
.finish()
}
}
#[cfg(feature = "defmt")]
impl<P: SetDutyCycle, G: GammaMap> defmt::Format for PwmLed<P, G>
where
G: defmt::Format,
{
fn format(&self, fmt: defmt::Formatter) {
defmt::write!(
fmt,
"PwmLed {{ gamma: {}, polarity: {}, max_duty: {}, brightness: {} }}",
self.gamma,
self.polarity,
self.max_duty,
brightness::to_u32(self.brightness)
)
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::PolarityMode;
fn cie_reference(raw: u8) -> u8 {
CIE_LSTAR[raw as usize]
}
include!(concat!(env!("OUT_DIR"), "/gamma_tables.rs"));
#[test]
fn cie_prefix_exact() {
for raw in 0..16u8 {
assert_eq!(
cie8_map(raw),
cie_reference(raw),
"prefix mismatch at raw={raw}"
);
}
}
#[test]
fn cie_knots_exact() {
for raw in (16..=255).step_by(16) {
let raw = raw as u8;
assert_eq!(
cie8_map(raw),
cie_reference(raw),
"knot mismatch at raw={raw}"
);
}
}
#[test]
fn cie_knots_endpoints() {
assert_eq!(CIE_KNOTS[0], 77);
assert_eq!(CIE_KNOTS[15], 255);
}
#[test]
fn cie_interp_max_error() {
let mut max_err = 0u16;
for raw in 16..=255u8 {
let got = cie8_map(raw);
let ref_val = cie_reference(raw);
let err = (got as i16 - ref_val as i16).unsigned_abs();
max_err = max_err.max(err);
}
assert!(max_err <= 4, "CIE L* interp max error {max_err} > 4");
}
#[test]
fn cie_lstar_monotonic() {
let mut prev = 0u8;
for raw in 0..=255u8 {
let val = cie8_map(raw);
assert!(
val >= prev,
"CIE L* non-monotonic at raw={raw}: {prev} -> {val}"
);
prev = val;
}
}
#[test]
fn cie_lstar_endpoints() {
assert_eq!(cie8_map(0), 0);
assert_eq!(cie8_map(255), 255);
}
#[test]
fn cie_lstar_at_midpoint() {
assert_eq!(cie8_map(128), 194);
}
#[test]
fn cie_lstar_low_end_smooth() {
let mut prev = 0u8;
for raw in 1..=10u8 {
let val = cie8_map(raw);
let step = val - prev;
assert!(
step <= 9,
"raw={raw}: step {step} too large (prev={prev}, val={val})"
);
prev = val;
}
assert_eq!(cie8_map(1), 9);
}
#[test]
fn linear_is_identity() {
for raw in 0..=255u8 {
let b = brightness::from_u32_clamped(raw as u32);
assert_eq!(GammaCorrection::Linear.map(b), b);
}
}
#[cfg(feature = "brightness-12bit")]
#[test]
fn cie12_lstar_endpoints() {
use arbitrary_int::u12;
assert_eq!(GammaCorrection::CieLStar.map(u12::new(0)), u12::new(0));
assert_eq!(
GammaCorrection::CieLStar.map(u12::new(4095)),
u12::new(4095)
);
}
#[cfg(feature = "brightness-12bit")]
#[test]
fn cie12_lstar_monotonic() {
let mut prev = 0u16;
for raw in 0..=4095u16 {
let b = arbitrary_int::u12::new(raw);
let val: u16 = GammaCorrection::CieLStar.map(b).into();
assert!(
val >= prev,
"CIE L* 12-bit non-monotonic at raw={raw}: {prev} -> {val}"
);
prev = val;
}
}
#[cfg(feature = "brightness-12bit")]
#[test]
fn cie12_lstar_agrees_with_8bit_at_knots() {
for raw in (0..=255u16).step_by(16) {
let raw12 = (raw * 16).min(4095);
let got: u16 = GammaCorrection::CieLStar
.map(arbitrary_int::u12::new(raw12))
.into();
let expected_8 = cie8_map(raw as u8) as u16;
let expected_12 = ((expected_8 as u32 * 4095 + 127) / 255) as u16;
let diff = got.abs_diff(expected_12);
assert!(
diff <= 1,
"at raw12={raw12}: got {got}, expected ~{expected_12}, diff={diff}"
);
}
}
#[test]
fn polarity_off_and_full_on_invert() {
assert_eq!(
brightness::to_u32(PolarityMode::ActiveHigh.map_duty(brightness::from_u32_clamped(0))),
0
);
assert_eq!(
brightness::to_u32(PolarityMode::ActiveHigh.map_duty(brightness::max_value())),
brightness::MAX_BRIGHTNESS
);
assert_eq!(
brightness::to_u32(PolarityMode::ActiveLow.map_duty(brightness::from_u32_clamped(0))),
brightness::MAX_BRIGHTNESS
);
assert_eq!(
brightness::to_u32(PolarityMode::ActiveLow.map_duty(brightness::max_value())),
0
);
}
use embedded_hal_mock::eh1::pwm::Mock as PwmMock;
use embedded_hal_mock::eh1::pwm::Transaction as PwmTrans;
const MAX_DUTY: u16 = 1000;
#[test]
fn new_sets_off() {
let e = [
PwmTrans::max_duty_cycle(MAX_DUTY),
PwmTrans::set_duty_cycle(0),
];
let led = PwmLed::new(
PwmMock::new(&e),
GammaCorrection::Linear,
PolarityMode::ActiveHigh,
)
.unwrap();
assert_eq!(brightness::to_u32(led.brightness()), 0);
assert!(led.is_off());
assert!(!led.is_on());
led.release().done();
}
#[test]
fn from_pin_no_touch() {
let e = [PwmTrans::max_duty_cycle(MAX_DUTY)];
let led = PwmLed::from_pin(
PwmMock::new(&e),
GammaCorrection::Linear,
PolarityMode::ActiveHigh,
);
assert_eq!(brightness::to_u32(led.brightness()), 0);
led.release().done();
}
#[test]
fn set_brightness_active_high() {
let e = [
PwmTrans::max_duty_cycle(MAX_DUTY),
PwmTrans::set_duty_cycle(0),
PwmTrans::set_duty_cycle(MAX_DUTY),
];
let mut led = PwmLed::new(
PwmMock::new(&e),
GammaCorrection::Linear,
PolarityMode::ActiveHigh,
)
.unwrap();
led.set_brightness(brightness::max_value()).unwrap();
assert_eq!(
brightness::to_u32(led.brightness()),
brightness::MAX_BRIGHTNESS
);
assert!(led.is_on());
led.release().done();
}
#[test]
fn set_brightness_active_low_polarity_inverts_duty() {
let e = [
PwmTrans::max_duty_cycle(MAX_DUTY),
PwmTrans::set_duty_cycle(MAX_DUTY), PwmTrans::set_duty_cycle(0), ];
let mut led = PwmLed::new(
PwmMock::new(&e),
GammaCorrection::Linear,
PolarityMode::ActiveLow,
)
.unwrap();
led.set_brightness(brightness::max_value()).unwrap();
assert_eq!(
brightness::to_u32(led.brightness()),
brightness::MAX_BRIGHTNESS
);
assert!(led.is_on());
led.release().done();
}
#[test]
fn on_and_off_track_correctly() {
let e = [
PwmTrans::max_duty_cycle(MAX_DUTY),
PwmTrans::set_duty_cycle(0), PwmTrans::set_duty_cycle(MAX_DUTY), PwmTrans::set_duty_cycle(0), ];
let mut led = PwmLed::new(
PwmMock::new(&e),
GammaCorrection::Linear,
PolarityMode::ActiveHigh,
)
.unwrap();
led.on().unwrap();
assert!(led.is_on());
assert!(!led.is_off());
led.off().unwrap();
assert!(!led.is_on());
assert!(led.is_off());
led.release().done();
}
#[test]
fn brightness_percent_rounding() {
let e = [
PwmTrans::max_duty_cycle(MAX_DUTY),
PwmTrans::set_duty_cycle(0),
PwmTrans::set_duty_cycle(MAX_DUTY),
];
let mut led = PwmLed::new(
PwmMock::new(&e),
GammaCorrection::Linear,
PolarityMode::ActiveHigh,
)
.unwrap();
led.set_brightness_percent(100).unwrap();
assert_eq!(
brightness::to_u32(led.brightness()),
brightness::MAX_BRIGHTNESS
);
led.release().done();
}
#[test]
fn custom_gamma_map() {
struct InvertGamma;
impl GammaMap for InvertGamma {
fn map(&self, raw: Brightness) -> Brightness {
brightness::max_value() - raw
}
}
let e = [
PwmTrans::max_duty_cycle(MAX_DUTY),
PwmTrans::set_duty_cycle(MAX_DUTY), PwmTrans::set_duty_cycle(0), ];
let mut led = PwmLed::new(PwmMock::new(&e), InvertGamma, PolarityMode::ActiveHigh).unwrap();
led.set_brightness(brightness::max_value()).unwrap();
led.release().done();
}
#[test]
fn set_duty_raw_bypasses_gamma() {
let e = [
PwmTrans::max_duty_cycle(MAX_DUTY),
PwmTrans::set_duty_cycle(0),
PwmTrans::set_duty_cycle(500),
];
let mut led = PwmLed::new(
PwmMock::new(&e),
GammaCorrection::Linear,
PolarityMode::ActiveHigh,
)
.unwrap();
led.set_duty_raw(500).unwrap();
assert_eq!(brightness::to_u32(led.brightness()), 0);
led.release().done();
}
#[test]
fn set_duty_cycle_trait_passthrough() {
let e = [
PwmTrans::max_duty_cycle(MAX_DUTY),
PwmTrans::set_duty_cycle(0),
PwmTrans::set_duty_cycle(777),
];
let mut led = PwmLed::new(
PwmMock::new(&e),
GammaCorrection::Linear,
PolarityMode::ActiveHigh,
)
.unwrap();
embedded_hal::pwm::SetDutyCycle::set_duty_cycle(&mut led, 777).unwrap();
assert_eq!(brightness::to_u32(led.brightness()), 0); led.release().done();
}
#[test]
fn accessors() {
let e = [PwmTrans::max_duty_cycle(MAX_DUTY)];
let led = PwmLed::from_pin(
PwmMock::new(&e),
GammaCorrection::CieLStar,
PolarityMode::ActiveLow,
);
assert_eq!(led.polarity(), PolarityMode::ActiveLow);
assert_eq!(led.max_duty(), MAX_DUTY);
let _ = led.gamma();
assert_eq!(brightness::to_u32(led.brightness()), 0);
led.release().done();
}
}