wrpll fw & tag collector: use static mut
parent
06266eba84
commit
611c08262f
|
@ -328,8 +328,6 @@ fn set_adpll(dcxo: i2c::DCXO, adpll: i32) -> Result<(), &'static str> {
|
|||
#[cfg(has_wrpll)]
|
||||
pub mod wrpll {
|
||||
|
||||
use libcortex_a9::mutex::Mutex;
|
||||
|
||||
use super::*;
|
||||
|
||||
const BEATING_PERIOD: i32 = 0x8000;
|
||||
|
@ -340,9 +338,9 @@ pub mod wrpll {
|
|||
const KP: i32 = 6;
|
||||
const KI: i32 = 2;
|
||||
|
||||
static BASE_ADPLL: Mutex<i32> = Mutex::new(0);
|
||||
static H_INTEGRATOR: Mutex<i32> = Mutex::new(0);
|
||||
static M_INTEGRATOR: Mutex<i32> = Mutex::new(0);
|
||||
static mut BASE_ADPLL: i32 = 0;
|
||||
static mut H_INTEGRATOR: i32 = 0;
|
||||
static mut M_INTEGRATOR: i32 = 0;
|
||||
|
||||
#[derive(Clone, Copy)]
|
||||
pub enum ISR {
|
||||
|
@ -357,37 +355,41 @@ pub mod wrpll {
|
|||
static TAG_OFFSET: Mutex<u32> = Mutex::new(19050);
|
||||
#[cfg(wrpll_ref_clk = "SMA_CLKIN")]
|
||||
static TAG_OFFSET: Mutex<u32> = Mutex::new(0);
|
||||
static REF_TAG: Mutex<u32> = Mutex::new(0);
|
||||
static REF_TAG_READY: Mutex<bool> = Mutex::new(false);
|
||||
static MAIN_TAG: Mutex<u32> = Mutex::new(0);
|
||||
static MAIN_TAG_READY: Mutex<bool> = Mutex::new(false);
|
||||
static mut REF_TAG: u32 = 0;
|
||||
static mut REF_TAG_READY: bool = false;
|
||||
static mut MAIN_TAG: u32 = 0;
|
||||
static mut MAIN_TAG_READY: bool = false;
|
||||
|
||||
pub fn reset() {
|
||||
clear_phase_diff_ready();
|
||||
*REF_TAG.lock() = 0;
|
||||
*MAIN_TAG.lock() = 0;
|
||||
unsafe {
|
||||
REF_TAG = 0;
|
||||
MAIN_TAG = 0;
|
||||
}
|
||||
}
|
||||
|
||||
pub fn clear_phase_diff_ready() {
|
||||
*REF_TAG_READY.lock() = false;
|
||||
*MAIN_TAG_READY.lock() = false;
|
||||
unsafe {
|
||||
REF_TAG_READY = false;
|
||||
MAIN_TAG_READY = false;
|
||||
}
|
||||
}
|
||||
|
||||
pub fn collect_tags(interrupt: ISR) {
|
||||
match interrupt {
|
||||
ISR::RefTag => {
|
||||
*REF_TAG.lock() = unsafe { csr::wrpll::ref_tag_read() };
|
||||
*REF_TAG_READY.lock() = true;
|
||||
}
|
||||
ISR::MainTag => {
|
||||
*MAIN_TAG.lock() = unsafe { csr::wrpll::main_tag_read() };
|
||||
*MAIN_TAG_READY.lock() = true;
|
||||
}
|
||||
ISR::RefTag => unsafe {
|
||||
REF_TAG = csr::wrpll::ref_tag_read();
|
||||
REF_TAG_READY = true;
|
||||
},
|
||||
ISR::MainTag => unsafe {
|
||||
MAIN_TAG = csr::wrpll::main_tag_read();
|
||||
MAIN_TAG_READY = true;
|
||||
},
|
||||
}
|
||||
}
|
||||
|
||||
pub fn phase_diff_ready() -> bool {
|
||||
*REF_TAG_READY.lock() && *MAIN_TAG_READY.lock()
|
||||
unsafe { REF_TAG_READY && MAIN_TAG_READY }
|
||||
}
|
||||
|
||||
#[cfg(feature = "calibrate_wrpll_skew")]
|
||||
|
@ -402,7 +404,7 @@ pub mod wrpll {
|
|||
|
||||
pub fn get_period_error() -> i32 {
|
||||
// n * BEATING_PERIOD - REF_TAG(n) mod BEATING_PERIOD
|
||||
let mut period_error = (*REF_TAG.lock()).overflowing_neg().0.rem_euclid(BEATING_PERIOD as u32) as i32;
|
||||
let mut period_error = unsafe { REF_TAG.overflowing_neg().0.rem_euclid(BEATING_PERIOD as u32) as i32 };
|
||||
|
||||
// mapping tags from [0, 2π] -> [-π, π]
|
||||
if period_error > BEATING_HALFPERIOD {
|
||||
|
@ -413,10 +415,8 @@ pub mod wrpll {
|
|||
|
||||
pub fn get_phase_error() -> i32 {
|
||||
// MAIN_TAG(n) - REF_TAG(n) - TAG_OFFSET mod BEATING_PERIOD
|
||||
let mut phase_error = (*MAIN_TAG.lock())
|
||||
.overflowing_sub(*REF_TAG.lock() + *TAG_OFFSET.lock())
|
||||
.0
|
||||
.rem_euclid(BEATING_PERIOD as u32) as i32;
|
||||
let mut phase_error =
|
||||
unsafe { MAIN_TAG.overflowing_sub(REF_TAG).0.rem_euclid(BEATING_PERIOD as u32) as i32 };
|
||||
|
||||
// mapping tags from [0, 2π] -> [-π, π]
|
||||
if phase_error > BEATING_HALFPERIOD {
|
||||
|
@ -440,11 +440,11 @@ pub mod wrpll {
|
|||
|error: i32| ((error as f64 * FREQ_DIV as f64 * 1e6) / (0.0001164 * (1 << COUNTER_WIDTH) as f64)) as i32;
|
||||
|
||||
let (ref_count, main_count) = get_freq_counts();
|
||||
let mut base_adpll_lock = BASE_ADPLL.lock();
|
||||
*base_adpll_lock = count2adpll(ref_count as i32 - main_count as i32);
|
||||
set_adpll(i2c::DCXO::Main, *base_adpll_lock)?;
|
||||
set_adpll(i2c::DCXO::Helper, *base_adpll_lock)?;
|
||||
|
||||
unsafe {
|
||||
BASE_ADPLL = count2adpll(ref_count as i32 - main_count as i32);
|
||||
set_adpll(i2c::DCXO::Main, BASE_ADPLL)?;
|
||||
set_adpll(i2c::DCXO::Helper, BASE_ADPLL)?;
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
|
@ -463,8 +463,10 @@ pub mod wrpll {
|
|||
}
|
||||
|
||||
fn reset_plls(timer: &mut GlobalTimer) -> Result<(), &'static str> {
|
||||
*H_INTEGRATOR.lock() = 0;
|
||||
*M_INTEGRATOR.lock() = 0;
|
||||
unsafe {
|
||||
H_INTEGRATOR = 0;
|
||||
M_INTEGRATOR = 0;
|
||||
}
|
||||
set_adpll(i2c::DCXO::Main, 0)?;
|
||||
set_adpll(i2c::DCXO::Helper, 0)?;
|
||||
// wait for adpll to transfer and DCXO to settle
|
||||
|
@ -507,27 +509,25 @@ pub mod wrpll {
|
|||
|
||||
fn helper_pll() -> Result<(), &'static str> {
|
||||
let period_err = tag_collector::get_period_error();
|
||||
let mut integrator_lock = H_INTEGRATOR.lock();
|
||||
|
||||
*integrator_lock += period_err * KI;
|
||||
let mut h_adpll = *BASE_ADPLL.lock() + period_err * KP + *integrator_lock;
|
||||
|
||||
h_adpll = h_adpll.clamp(-ADPLL_MAX, ADPLL_MAX);
|
||||
let h_adpll;
|
||||
unsafe {
|
||||
H_INTEGRATOR += period_err * KI;
|
||||
h_adpll = (BASE_ADPLL + period_err * KP + H_INTEGRATOR).clamp(-ADPLL_MAX, ADPLL_MAX);
|
||||
}
|
||||
set_adpll(i2c::DCXO::Helper, h_adpll)?;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn main_pll() -> Result<(), &'static str> {
|
||||
let phase_err = tag_collector::get_phase_error();
|
||||
let mut integrator_lock = M_INTEGRATOR.lock();
|
||||
|
||||
*integrator_lock += phase_err * KI;
|
||||
let mut m_adpll = *BASE_ADPLL.lock() + phase_err * KP + *integrator_lock;
|
||||
|
||||
m_adpll = m_adpll.clamp(-ADPLL_MAX, ADPLL_MAX);
|
||||
let m_adpll;
|
||||
unsafe {
|
||||
M_INTEGRATOR += phase_err * KI;
|
||||
m_adpll = (BASE_ADPLL + phase_err * KP + M_INTEGRATOR).clamp(-ADPLL_MAX, ADPLL_MAX);
|
||||
}
|
||||
set_adpll(i2c::DCXO::Main, m_adpll)?;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue