trinamic 0.0.0-beta2

Trinamic Motion Control Language (TMCL) in Rust
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

use bxcan;
use bxcan::Id;
use bxcan::{Frame, StandardId};
use core::convert::TryInto;
use core::future::Future;
use nb::block;

pub struct BxCanInterface<T: bxcan::Instance> {
    can: bxcan::Can<T>,
}

impl<T: bxcan::Instance> BxCanInterface<T> {
    pub fn new(can: bxcan::Can<T>) -> Self {
        Self { can: can }
    }
}

impl<T: bxcan::Instance + 'static> crate::TMCLConnnection for BxCanInterface<T> {
    type SendFuture<'a> = impl Future<Output = ()>;
    type ReceiveFuture<'a> = impl Future<Output = [u8; 8]>;

    // Send the bytearray [data] representing a TMCL command. The length of
    // [data] is 9. The hostID and module_id parameters may be used for extended
    // addressing options available on the implemented communication interface.
    fn _send<'a>(
        &'a mut self,
        host_id: u16,
        module_id: u16,
        data: [u8; 8],
    ) -> Self::SendFuture<'a> {
        async move {
            let frame_tx = Frame::new_data(
                StandardId::new(module_id).unwrap(),
                [
                    data[1], data[2], data[3], data[4], data[5], data[6], data[7],
                ],
            );

            block!(self.can.transmit(&frame_tx)).unwrap();
        }
    }
    //
    // Receive a TMCL reply and return it as a bytearray. The length of the
    // returned byte array is 9. The hostID and module_id parameters may be used
    // for extended addressing options available on the implemented
    // communication interface.
    fn _recv<'a>(&'a mut self, host_id: u16, module_id: u16) -> Self::ReceiveFuture<'a> {
        async move {
            let frame_rx = block!(self.can.receive()).unwrap();

            let id = match frame_rx.id() {
                Id::Extended(id) => id.as_raw() as u8,
                Id::Standard(id) => id.as_raw() as u8,
            };
            let data: [u8; 7] = match (frame_rx.data().unwrap() as &[u8]).try_into() {
                Ok(data) => data,
                Err(_) => [0; 7],
            };

            [
                id, data[0], data[1], data[2], data[3], data[4], data[5], data[6],
            ]
        }
    }
}