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

use {
    TailByte,
    TransportFrame,
    UavcanFrame,
    UavcanHeader,
    UavcanIndexable,
};

use serializer::{
    SerializationResult,
    Serializer,
};



pub struct FrameGenerator<B: UavcanIndexable> {
    serializer: Serializer<B>,
    started: bool,
    id: u32,
    toggle: bool,
    transfer_id: u8,
}

impl<B: UavcanIndexable> FrameGenerator<B> {
    pub fn from_uavcan_frame<H: UavcanHeader, F: UavcanFrame<H, B>>(frame: F, transfer_id: u8) -> Self {
        let (header, body) = frame.to_parts();
        Self{
            serializer: Serializer::from_structure(body),
            started: false,
            id: header.to_id(),
            toggle: false,
            transfer_id: transfer_id,
        }
    }
    
    pub fn next_transport_frame<T: TransportFrame>(&mut self) -> Option<T> {
        let remaining_bits = self.serializer.remaining_bits();
        let max_data_length = T::max_data_length();
        let max_payload_length = max_data_length - 1;
        let mut transport_frame = T::with_length(self.id, max_data_length);

        
        let first_of_multi_frame = !self.started && (remaining_bits > max_payload_length*8);

        if remaining_bits == 0 {
            return None;
        } else if first_of_multi_frame {
            // TODO: calc crc
            self.serializer.serialize(&mut transport_frame.data_as_mut()[2..max_data_length-1]);
            transport_frame.data_as_mut()[max_data_length-1] = TailByte{start_of_transfer: !self.started, end_of_transfer: false, toggle: self.toggle, transfer_id: self.transfer_id}.into();
        } else {
            if let SerializationResult::Finished(i) = self.serializer.serialize(&mut transport_frame.data_as_mut()[0..max_data_length-1]){
                let frame_length = (i+7)/8 + 1;
                transport_frame.set_data_length(frame_length);
                transport_frame.data_as_mut()[frame_length-1] = TailByte{start_of_transfer: !self.started, end_of_transfer: true, toggle: self.toggle, transfer_id: self.transfer_id}.into();
            }
        }

        self.started = true;
        self.toggle = !self.toggle;
        
        return Some(transport_frame);
    }
}




#[cfg(test)]
mod tests {

    use{
        UavcanIndexable,
        UavcanPrimitiveField,
        UavcanHeader,
        UavcanFrame,
        TailByte,
    };
    
    use headers::{
        MessageFrameHeader,
    };

    use types::{
        Uint2,
        Uint3,
        Uint16,
        Uint32,
    };
    
    use can_frame::{
        CanFrame,
        CanID,
    };

    use frame_generator::{
        FrameGenerator,
    };
    
    #[test]
    fn serialize_node_status_frame() {

        #[derive(UavcanIndexable, Default)]
        struct NodeStatus {
            uptime_sec: Uint32,
            health: Uint2,
            mode: Uint3,
            sub_mode: Uint3,
            vendor_specific_status_code: Uint16,
        }

        #[derive(UavcanFrame, Default)]
        struct NodeStatusMessage {
            header: MessageFrameHeader,
            body: NodeStatus,
        }
            
        
        let can_frame = CanFrame{id: CanID::Extended(MessageFrameHeader::from_id(0xaa).to_id()), dlc: 8, data: [1, 0, 0, 0, 0b10001110, 5, 0, TailByte{start_of_transfer: true, end_of_transfer: true, toggle: false, transfer_id: 0}.into()]};
        
        let uavcan_frame = NodeStatusMessage{
            header: MessageFrameHeader::from_id(0xaa),
            body: NodeStatus{
                uptime_sec: 1.into(),
                health: 2.into(),
                mode: 3.into(),
                sub_mode: 4.into(),
                vendor_specific_status_code: 5.into(),
            },
        };

        let mut frame_generator = FrameGenerator::from_uavcan_frame(uavcan_frame, 0);

        assert_eq!(frame_generator.next_transport_frame(), Some(can_frame));
        assert_eq!(frame_generator.next_transport_frame::<CanFrame>(), None);
        
    }

}