1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
|
use crate::{
analog::{Ac, AcCapture, Adc, AdcChannel},
fixpt::{fixpt, Fixpt},
hw::mcu,
mains::Mains,
mutex::{MainCtx, MutexCell},
pi::{Pi, PiParams},
speedo::{MotorSpeed, Speedo},
timer::{timer_get, timer_get_large, LargeTimestamp, RelLargeTimestamp, RelTimestamp},
triac::Triac,
};
const RPMPI_DT: RelLargeTimestamp = RelLargeTimestamp::from_millis(10);
const RPMPI_PARAMS: PiParams = PiParams {
kp: fixpt!(10 / 1), //TODO
ki: fixpt!(1 / 10), //TODO
ilim: fixpt!(10 / 1),
};
/// Convert 0..0x3FF to 0..128 Hz to 0..8 16Hz
fn setpoint_to_f(adc: u16) -> Fixpt {
Fixpt::from_fraction(adc as i16, 8 * 16)
}
/// Convert -8..8 16Hz into pi..0 radians.
/// Convert pi..0 radians into 20..0 ms.
fn f_to_trig_offs(f: Fixpt) -> Fixpt {
let fmin = Fixpt::from_int(-8);
let fmax = Fixpt::from_int(8);
let f = f.max(fmin);
let f = f.min(fmax);
let fact = fixpt!(20 / 16);
(fmax - f) * fact
}
#[allow(non_snake_case)]
pub struct SysPeriph {
pub AC: mcu::AC,
pub ADC: mcu::ADC,
pub PORTA: mcu::PORTA,
pub PORTB: mcu::PORTB,
pub TC1: mcu::TC1,
}
#[derive(Copy, Clone, PartialEq, Eq)]
#[repr(u8)]
enum SysState {
/// POR system check.
Check,
/// Up and running.
Run,
}
pub struct System {
ac: Ac,
adc: Adc,
speedo: Speedo,
mains: Mains,
rpm_pi: Pi,
next_rpm_pi: MutexCell<LargeTimestamp>,
triac: Triac,
}
impl System {
pub const fn new() -> Self {
Self {
ac: Ac::new(),
adc: Adc::new(),
speedo: Speedo::new(),
mains: Mains::new(),
rpm_pi: Pi::new(),
next_rpm_pi: MutexCell::new(LargeTimestamp::new()),
triac: Triac::new(),
}
}
pub fn init(&self, m: &MainCtx<'_>, sp: &SysPeriph) {
self.adc.init(m, sp);
self.adc.enable(
m,
AdcChannel::Setpoint.mask() | AdcChannel::ShuntDiff.mask() | AdcChannel::ShuntHi.mask(),
);
self.ac.init(sp);
self.triac.shutoff(m);
}
/*
fn run_initial_check(&self, cs: CriticalSection<'_>, _sp: &SysPeriph) {
let adc = self.adc.borrow(cs);
let Some(_setpoint) = adc.get_result(AdcChannel::Setpoint) else {
return;
};
//TODO
let Some(_shuntdiff) = adc.get_result(AdcChannel::ShuntDiff) else {
return;
};
//TODO
let Some(_shunthi) = adc.get_result(AdcChannel::ShuntHi) else {
return;
};
//TODO
self.speedo.borrow_mut(cs).reset();
self.state.set(cs, SysState::Run);
}
*/
#[allow(dead_code)]
fn debug(&self, m: &MainCtx<'_>, sp: &SysPeriph, ticks: i8) {
sp.PORTB.portb.modify(|_, w| w.pb6().set_bit());
let end = timer_get(&m.to_any()) + RelTimestamp::from_ticks(ticks);
while timer_get(&m.to_any()) < end {}
sp.PORTB.portb.modify(|_, w| w.pb6().clear_bit());
}
pub fn run(&self, m: &MainCtx<'_>, sp: &SysPeriph, ac: AcCapture) {
self.speedo.update(m, &ac);
let speedo_hz = self.speedo.get_speed(m).unwrap_or(MotorSpeed::zero());
let phase_update = self.mains.run(m, sp);
let phase = self.mains.get_phase(m);
let phaseref = self.mains.get_phaseref(m);
self.adc.run(m, sp);
let setpoint = self.adc.get_result(m, AdcChannel::Setpoint);
let shuntdiff = self.adc.get_result(m, AdcChannel::ShuntDiff);
let shunthi = self.adc.get_result(m, AdcChannel::ShuntHi);
let now = timer_get_large(m);
if now >= self.next_rpm_pi.get(m) {
self.next_rpm_pi.set(m, now + RPMPI_DT);
if let Some(setpoint) = setpoint {
let setpoint = setpoint_to_f(setpoint);
let y = self
.rpm_pi
.run(m, &RPMPI_PARAMS, setpoint, speedo_hz.as_16hz());
//let y = setpoint;
let phi_offs_ms = f_to_trig_offs(y);
//self.debug(m, sp, phi_offs_ms.to_int() as i8);
self.triac.set_phi_offs_ms(m, phi_offs_ms);
} else {
self.triac.shutoff(m);
}
}
self.triac.run(m, sp, phase_update, phase, phaseref);
}
}
// vim: ts=4 sw=4 expandtab
|
