Compare commits
6 Commits
20732c0de1
...
aa4bb8bae8
Author | SHA1 | Date |
---|---|---|
morgan | aa4bb8bae8 | |
morgan | a2e5eba767 | |
morgan | c4780aa608 | |
morgan | e6045be0f8 | |
morgan | acbbd8d79a | |
morgan | 6bc867dd63 |
|
@ -407,7 +407,6 @@ pub mod wrpll {
|
|||
pub fn get_period_error() -> i32 {
|
||||
// n * BEATING_PERIOD - REF_TAG(n) mod BEATING_PERIOD
|
||||
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 {
|
||||
period_error -= BEATING_PERIOD
|
||||
|
@ -516,23 +515,20 @@ pub mod wrpll {
|
|||
fn helper_pll() -> Result<(), &'static str> {
|
||||
let period_err = tag_collector::get_period_error();
|
||||
|
||||
let h_adpll;
|
||||
unsafe {
|
||||
let h_adpll = unsafe {
|
||||
H_INTEGRATOR += period_err * KI;
|
||||
h_adpll = (BASE_ADPLL + period_err * KP + H_INTEGRATOR).clamp(-ADPLL_MAX, ADPLL_MAX);
|
||||
}
|
||||
(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 m_adpll;
|
||||
unsafe {
|
||||
let m_adpll = unsafe {
|
||||
M_INTEGRATOR += phase_err * KI;
|
||||
m_adpll = (BASE_ADPLL + phase_err * KP + M_INTEGRATOR).clamp(-ADPLL_MAX, ADPLL_MAX);
|
||||
}
|
||||
(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