Adding WIP refactor for MQTT + settings

This commit is contained in:
Ryan Summers 2021-01-26 14:28:06 +01:00
parent 659a6879f7
commit f0e7c153ba
6 changed files with 337 additions and 146 deletions

View File

@ -45,6 +45,7 @@ enum-iterator = "0.6.0"
paste = "1" paste = "1"
dsp = { path = "dsp" } dsp = { path = "dsp" }
ad9959 = { path = "ad9959" } ad9959 = { path = "ad9959" }
minimq = "0.1.0"
[dependencies.mcp23017] [dependencies.mcp23017]
git = "https://github.com/mrd0ll4r/mcp23017.git" git = "https://github.com/mrd0ll4r/mcp23017.git"

View File

@ -25,6 +25,12 @@ const TCP_TX_BUFFER_SIZE: usize = 8192;
// The number of cascaded IIR biquads per channel. Select 1 or 2! // The number of cascaded IIR biquads per channel. Select 1 or 2!
const IIR_CASCADE_LENGTH: usize = 1; const IIR_CASCADE_LENGTH: usize = 1;
#[derive(miniconf::StringSet)]
struct Settings {
afe_gain: [hardware::AfeGain; 2],
iir: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
}
#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
const APP: () = { const APP: () = {
struct Resources { struct Resources {
@ -32,6 +38,7 @@ const APP: () = {
adcs: (Adc0Input, Adc1Input), adcs: (Adc0Input, Adc1Input),
dacs: (Dac0Output, Dac1Output), dacs: (Dac0Output, Dac1Output),
net_interface: hardware::Ethernet, net_interface: hardware::Ethernet,
mqtt_interface: hardware::MqttInterface<Settings>,
// Format: iir_state[ch][cascade-no][coeff] // Format: iir_state[ch][cascade-no][coeff]
#[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])]
@ -107,26 +114,8 @@ const APP: () = {
} }
} }
#[idle(resources=[net_interface, iir_state, iir_ch, afes])] #[idle(resources=[mqtt_interface], spawn=[settings_update])]
fn idle(mut c: idle::Context) -> ! { fn idle(mut c: idle::Context) -> ! {
let mut socket_set_entries: [_; 8] = Default::default();
let mut sockets =
smoltcp::socket::SocketSet::new(&mut socket_set_entries[..]);
let mut rx_storage = [0; TCP_RX_BUFFER_SIZE];
let mut tx_storage = [0; TCP_TX_BUFFER_SIZE];
let tcp_handle = {
let tcp_rx_buffer =
smoltcp::socket::TcpSocketBuffer::new(&mut rx_storage[..]);
let tcp_tx_buffer =
smoltcp::socket::TcpSocketBuffer::new(&mut tx_storage[..]);
let tcp_socket =
smoltcp::socket::TcpSocket::new(tcp_rx_buffer, tcp_tx_buffer);
sockets.add(tcp_socket)
};
let mut server = server::Server::new();
let mut time = 0u32; let mut time = 0u32;
let mut next_ms = Instant::now(); let mut next_ms = Instant::now();
@ -141,125 +130,26 @@ const APP: () = {
time += 1; time += 1;
} }
{ let sleep = c.resources.network_stack.update(
let socket =
&mut *sockets.get::<smoltcp::socket::TcpSocket>(tcp_handle);
if socket.state() == smoltcp::socket::TcpState::CloseWait {
socket.close();
} else if !(socket.is_open() || socket.is_listening()) {
socket
.listen(1235)
.unwrap_or_else(|e| warn!("TCP listen error: {:?}", e));
} else {
server.poll(socket, |req| {
info!("Got request: {:?}", req);
stabilizer::route_request!(req,
readable_attributes: [
"stabilizer/iir/state": (|| {
let state = c.resources.iir_state.lock(|iir_state|
server::Status {
t: time,
x0: iir_state[0][0][0],
y0: iir_state[0][0][2],
x1: iir_state[1][0][0],
y1: iir_state[1][0][2],
});
Ok::<server::Status, ()>(state)
}),
// "_b" means cascades 2nd IIR
"stabilizer/iir_b/state": (|| { let state = c.resources.iir_state.lock(|iir_state|
server::Status {
t: time,
x0: iir_state[0][IIR_CASCADE_LENGTH-1][0],
y0: iir_state[0][IIR_CASCADE_LENGTH-1][2],
x1: iir_state[1][IIR_CASCADE_LENGTH-1][0],
y1: iir_state[1][IIR_CASCADE_LENGTH-1][2],
});
Ok::<server::Status, ()>(state)
}),
"stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()),
"stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain())
],
modifiable_attributes: [
"stabilizer/iir0/state": server::IirRequest, (|req: server::IirRequest| {
c.resources.iir_ch.lock(|iir_ch| {
if req.channel > 1 {
return Err(());
}
iir_ch[req.channel as usize][0] = req.iir;
Ok::<server::IirRequest, ()>(req)
})
}),
"stabilizer/iir1/state": server::IirRequest, (|req: server::IirRequest| {
c.resources.iir_ch.lock(|iir_ch| {
if req.channel > 1 {
return Err(());
}
iir_ch[req.channel as usize][0] = req.iir;
Ok::<server::IirRequest, ()>(req)
})
}),
"stabilizer/iir_b0/state": server::IirRequest, (|req: server::IirRequest| {
c.resources.iir_ch.lock(|iir_ch| {
if req.channel > 1 {
return Err(());
}
iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir;
Ok::<server::IirRequest, ()>(req)
})
}),
"stabilizer/iir_b1/state": server::IirRequest,(|req: server::IirRequest| {
c.resources.iir_ch.lock(|iir_ch| {
if req.channel > 1 {
return Err(());
}
iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir;
Ok::<server::IirRequest, ()>(req)
})
}),
"stabilizer/afe0/gain": hardware::AfeGain, (|gain| {
c.resources.afes.0.set_gain(gain);
Ok::<(), ()>(())
}),
"stabilizer/afe1/gain": hardware::AfeGain, (|gain| {
c.resources.afes.1.set_gain(gain);
Ok::<(), ()>(())
})
]
)
});
}
}
let sleep = match c.resources.net_interface.poll(
&mut sockets,
smoltcp::time::Instant::from_millis(time as i64), smoltcp::time::Instant::from_millis(time as i64),
) { );
Ok(changed) => !changed,
Err(smoltcp::Error::Unrecognized) => true,
Err(e) => {
info!("iface poll error: {:?}", e);
true
}
};
if sleep { match c.resources.mqtt_interface.lock(|interface| interface.update(time).unwrap()) {
cortex_m::asm::wfi(); Action::Sleep => cortex_m::asm::wfi(),
Action::Continue => {},
Action::CommitSettings => c.spawn.settings_update().unwrap();
} }
} }
} }
#[task(priority = 1, resources=[mqtt_interface, afes, iir_ch])]
fn settings_update(c: settings_update::Context) {
let settings = c.resources.mqtt_interface.current_settings();
c.resources.iir_ch.lock(|iir_ch| *iir_ch = settings.iir);
c.resources.afes.0.set_gain(settings.afe_gain[0]);
c.resources.afes.1.set_gain(settings.afe_gain[1]);
}
#[task(binds = ETH, priority = 1)] #[task(binds = ETH, priority = 1)]
fn eth(_: eth::Context) { fn eth(_: eth::Context) {
unsafe { hal::ethernet::interrupt_handler() } unsafe { hal::ethernet::interrupt_handler() }

View File

@ -21,20 +21,12 @@ use embedded_hal::digital::v2::{InputPin, OutputPin};
use super::{ use super::{
adc, afe, dac, design_parameters, digital_input_stamper, eeprom, pounder, adc, afe, dac, design_parameters, digital_input_stamper, eeprom, pounder,
timers, DdsOutput, Ethernet, AFE0, AFE1, smoltcp_nal::NetStorage, timers, DdsOutput, NetworkStack, AFE0, AFE1,
}; };
// Network storage definition for the ethernet interface.
struct NetStorage {
ip_addrs: [smoltcp::wire::IpCidr; 1],
neighbor_cache:
[Option<(smoltcp::wire::IpAddress, smoltcp::iface::Neighbor)>; 8],
routes_storage: [Option<(smoltcp::wire::IpCidr, smoltcp::iface::Route)>; 1],
}
/// The available networking devices on Stabilizer. /// The available networking devices on Stabilizer.
pub struct NetworkDevices { pub struct NetworkDevices {
pub interface: Ethernet, pub stack: NetworkStack,
pub phy: ethernet::phy::LAN8742A<ethernet::EthernetMAC>, pub phy: ethernet::phy::LAN8742A<ethernet::EthernetMAC>,
} }
@ -71,7 +63,11 @@ static mut NET_STORE: NetStorage = NetStorage {
smoltcp::wire::Ipv6Cidr::SOLICITED_NODE_PREFIX, smoltcp::wire::Ipv6Cidr::SOLICITED_NODE_PREFIX,
)], )],
neighbor_cache: [None; 8], neighbor_cache: [None; 8],
routes_storage: [None; 1], routes_cache: [None; 8],
sockets: [None; 1],
tx_storage: [0; 4096],
rx_storage: [0; 4096],
}; };
/// Configure the stabilizer hardware for operation. /// Configure the stabilizer hardware for operation.
@ -515,7 +511,7 @@ pub fn setup(
); );
let default_v4_gw = Ipv4Address::new(10, 0, 16, 1); let default_v4_gw = Ipv4Address::new(10, 0, 16, 1);
let mut routes = Routes::new(&mut store.routes_storage[..]); let mut routes = Routes::new(&mut store.routes_cache[..]);
routes.add_default_ipv4_route(default_v4_gw).unwrap(); routes.add_default_ipv4_route(default_v4_gw).unwrap();
let neighbor_cache = let neighbor_cache =
@ -528,8 +524,26 @@ pub fn setup(
.routes(routes) .routes(routes)
.finalize(); .finalize();
let sockets = {
// Note(unsafe): Configuration is only called once, so we only access the global
// storage a single time.
let socket_storage = unsafe { &mut NET_STORE.sockets };
let mut sockets = smoltcp::socket::SocketSet::new(socket_storage);
let tcp_socket = {
// Note(unsafe): Configuration is only called once, so we only access the global
// storage a single time.
let rx_storage = unsafe { &mut NET_STORE.rx_storage[..] };
let tx_storage = unsafe { &mut NET_STORE.tx_storage[..] };
smoltcp::socket::TcpSocket::new(rx_storage, tx_storage)
};
sockets.add(tcp_socket);
sockets
};
NetworkDevices { NetworkDevices {
interface, stack: NetworkStack::new(interface, sockets),
phy: lan8742a, phy: lan8742a,
} }
}; };

View File

@ -15,6 +15,7 @@ mod design_parameters;
mod digital_input_stamper; mod digital_input_stamper;
mod eeprom; mod eeprom;
mod pounder; mod pounder;
mod smoltcp_nal;
mod timers; mod timers;
pub use adc::{Adc0Input, Adc1Input}; pub use adc::{Adc0Input, Adc1Input};
@ -36,13 +37,15 @@ pub type AFE1 = afe::ProgrammableGainAmplifier<
>; >;
// Type alias for the ethernet interface on Stabilizer. // Type alias for the ethernet interface on Stabilizer.
pub type Ethernet = smoltcp::iface::EthernetInterface< type Ethernet = smoltcp::iface::EthernetInterface<
'static, 'static,
'static, 'static,
'static, 'static,
hal::ethernet::EthernetDMA<'static>, hal::ethernet::EthernetDMA<'static>,
>; >;
pub type NetworkStack = smoltcp_nal::NetworkStack<'static, 'static, 'static>;
pub use configuration::{setup, PounderDevices, StabilizerDevices}; pub use configuration::{setup, PounderDevices, StabilizerDevices};
#[inline(never)] #[inline(never)]

View File

@ -0,0 +1,102 @@
use super::NetworkStack;
use minimq::{QoS, Error, Property, MqttClient};
pub enum Action {
Continue,
Sleep,
CommitSettings,
}
struct MqttInterface<T: miniconf::StringSet> {
client: MqttClient<minimq::consts::U256, NetworkStack>,
subscribed: bool,
settings: T,
}
impl<T> MqttInterface<T>
where
T: miniconf::StringSet
{
pub fn new(stack: NetworkStack, settings: T) -> Self {
let client: MqttClient<minimq::consts::U256, _> = MqttClient::new(
IpAddr::V4(Ipv4Addr::new(10, 0, 0, 1)),
"stabilizer",
stack).unwrap();
Self {
client,
subscribed: false,
settings,
}
}
pub fn current_settings(&self) -> &T {
&self.settings
}
pub fn update(&mut self, time: u32) -> Result<bool, ()> {
let sleep = self.client.network_stack.update(smoltcp::time::Instant::from_millis(time as i64));
if !self.subscribed && self.client.is_connected().unwrap() {
self.client.subscribe("stabilizer/settings/#", &[]);
self.client.subscribe("stabilizer/commit", &[]);
}
let mut commit = false;
match self.client.poll(|client, topic, message, properties| {
let split = topic.split('/').iter();
// TODO: Verify topic ID against our ID.
let id = split.next().unwrap();
// Process the command
let command = split.next().unwrap();
let response: String<consts::U512> = match command {
"settings" => {
// Handle settings failures
let mut response: String<consts::U512> = String::new();
match self.settings.string_set(split.peekable(), message) {
Ok(_) => write!(&mut response, "{} written", topic).unwrap(),
Err(error) => {
write!(&mut response, "Settings failure: {}", error).unwrap();
}
};
response
},
"commit" => {
commit = true;
String::from("Committing pending settings");
}
};
// Publish the response to the request over MQTT using the ResponseTopic property if
// possible. Otherwise, default to a logging topic.
if let Property::ResponseTopic(topic) = properties.iter().find(|&prop| {
if let Property::ResponseTopic(_) = *prop {
true
} else {
false
}
}).or(Some(&Property::ResponseTopic("stabilizer/log"))).unwrap() {
self.client.publish(topic, &response.into_bytes(), QoS::AtMostOnce, &[]).unwrap();
}
}) {
Ok(_) => {},
Err(Error::Disconnected) => self.subscribed = false,
Err(err) => error!("Unexpected error: {:?}", err)
};
let action = if commit {
Action::Commit
} else if sleep {
Action::Sleep
} else {
Action::Continue
};
Ok(action)
}
}

181
src/hardware/smoltcp_nal.rs Normal file
View File

@ -0,0 +1,181 @@
use core::cell::RefCell;
///! Network abstraction layer for smoltcp.
use heapless::{consts, Vec};
use minimq::embedded_nal::{self as nal, nb};
use super::Ethernet;
pub struct NetStorage {
pub ip_addrs: [smoltcp::wire::IpCidr; 1],
pub sockets: [Option<smoltcp::socket::SocketSetItem<'static, 'static>>; 1],
pub neighbor_cache:
[Option<(smoltcp::wire::IpAddress, smoltcp::iface::Neighbor)>; 8],
pub routes_cache:
[Option<(smoltcp::wire::IpCidr, smoltcp::iface::Route)>; 8],
pub tx_storage: [u8; 4096],
pub rx_storage: [u8; 4096],
}
#[derive(Debug)]
pub enum NetworkError {
NoSocket,
ConnectionFailure,
ReadFailure,
WriteFailure,
Unsupported,
}
pub struct NetworkStack<'a, 'b, 'c> {
network_interface: RefCell<Ethernet>,
sockets: RefCell<smoltcp::socket::SocketSet<'a, 'b, 'c>>,
next_port: RefCell<u16>,
unused_handles: RefCell<Vec<smoltcp::socket::SocketHandle, consts::U16>>,
}
impl<'a, 'b, 'c> NetworkStack<'a, 'b, 'c> {
pub fn new(
interface: Ethernet,
sockets: smoltcp::socket::SocketSet<'a, 'b, 'c>,
) -> Self {
let mut unused_handles: Vec<
smoltcp::socket::SocketHandle,
consts::U16,
> = Vec::new();
for socket in sockets.iter() {
unused_handles.push(socket.handle()).unwrap();
}
NetworkStack {
network_interface: RefCell::new(interface),
sockets: RefCell::new(sockets),
next_port: RefCell::new(49152),
unused_handles: RefCell::new(unused_handles),
}
}
pub fn update(&self, time: u32) -> bool {
match self.network_interface.borrow_mut().poll(
&mut self.sockets.borrow_mut(),
smoltcp::time::Instant::from_millis(time as i64),
) {
Ok(changed) => changed == false,
Err(e) => {
info!("{:?}", e);
true
}
}
}
fn get_ephemeral_port(&self) -> u16 {
// Get the next ephemeral port
let current_port = self.next_port.borrow().clone();
let (next, wrap) = self.next_port.borrow().overflowing_add(1);
*self.next_port.borrow_mut() = if wrap { 49152 } else { next };
return current_port;
}
}
impl<'a, 'b, 'c> NetworkStack<'a, 'b, 'c> {
fn open(
&self,
_mode: nal::Mode,
) -> Result<smoltcp::socket::SocketHandle, NetworkError> {
match self.unused_handles.borrow_mut().pop() {
Some(handle) => {
// Abort any active connections on the handle.
let mut sockets = self.sockets.borrow_mut();
let internal_socket: &mut smoltcp::socket::TcpSocket =
&mut *sockets.get(handle);
internal_socket.abort();
Ok(handle)
}
None => Err(NetworkError::NoSocket),
}
}
fn connect(
&self,
socket: smoltcp::socket::SocketHandle,
remote: nal::SocketAddr,
) -> Result<smoltcp::socket::SocketHandle, NetworkError> {
let mut sockets = self.sockets.borrow_mut();
let internal_socket: &mut smoltcp::socket::TcpSocket =
&mut *sockets.get(socket);
// If we're already in the process of connecting, ignore the request silently.
if internal_socket.is_open() {
return Ok(socket);
}
match remote.ip() {
nal::IpAddr::V4(addr) => {
let octets = addr.octets();
let address = smoltcp::wire::Ipv4Address::new(
octets[0], octets[1], octets[2], octets[3],
);
internal_socket
.connect(
(address, remote.port()),
self.get_ephemeral_port(),
)
.map_err(|_| NetworkError::ConnectionFailure)?;
Ok(socket)
}
// We only support IPv4.
_ => Err(NetworkError::Unsupported),
}
}
fn is_connected(
&self,
socket: &smoltcp::socket::SocketHandle,
) -> Result<bool, NetworkError> {
let mut sockets = self.sockets.borrow_mut();
let socket: &mut smoltcp::socket::TcpSocket =
&mut *sockets.get(*socket);
Ok(socket.may_send() && socket.may_recv())
}
fn write(
&self,
socket: &mut smoltcp::socket::SocketHandle,
buffer: &[u8],
) -> nb::Result<usize, NetworkError> {
let mut sockets = self.sockets.borrow_mut();
let socket: &mut smoltcp::socket::TcpSocket =
&mut *sockets.get(*socket);
socket
.send_slice(buffer)
.map_err(|_| nb::Error::Other(NetworkError::WriteFailure))
}
fn read(
&self,
socket: &mut smoltcp::socket::SocketHandle,
buffer: &mut [u8],
) -> nb::Result<usize, NetworkError> {
let mut sockets = self.sockets.borrow_mut();
let socket: &mut smoltcp::socket::TcpSocket =
&mut *sockets.get(*socket);
socket
.recv_slice(buffer)
.map_err(|_| nb::Error::Other(NetworkError::ReadFailure))
}
fn close(
&self,
socket: smoltcp::socket::SocketHandle,
) -> Result<(), NetworkError> {
let mut sockets = self.sockets.borrow_mut();
let internal_socket: &mut smoltcp::socket::TcpSocket =
&mut *sockets.get(socket);
internal_socket.close();
self.unused_handles.borrow_mut().push(socket).unwrap();
Ok(())
}
}