CAN/src/teensy_can.cpp at 275a82f9f9e91ca1fd1344803e86fff64af1e464 · NU-Formula-Racing/CAN · GitHub

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
#if defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41)
#include "teensy_can.h"

template <uint8_t bus_num>
std::vector<ICANRXMessage *> TeensyCAN<bus_num>::rx_messages_{};

FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_256> can_bus_1;
FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_256> can_bus_2;
FlexCAN_T4<CAN3, RX_SIZE_256, TX_SIZE_256> can_bus_3;

template <uint8_t bus_num>
void TeensyCAN<bus_num>::Initialize(BaudRate baud)
{
    // Repeated code due to limitations of C++11, look into alternatives without repeated code
    if (bus_num == 2)
    {
        can_bus_2.begin();
        can_bus_2.setBaudRate(static_cast<uint32_t>(baud));
        can_bus_2.enableFIFO();
        can_bus_2.enableFIFOInterrupt();
        can_bus_2.onReceive(ProcessMessage);
    }
    else if (bus_num == 3)
    {
        can_bus_3.begin();
        can_bus_3.setBaudRate(static_cast<uint32_t>(baud));
        can_bus_3.enableFIFO();
        can_bus_3.enableFIFOInterrupt();
        can_bus_3.onReceive(ProcessMessage);
    }
    else
    {
        can_bus_1.begin();
        can_bus_1.setBaudRate(static_cast<uint32_t>(baud));
        can_bus_1.enableFIFO();
        can_bus_1.enableFIFOInterrupt();
        can_bus_1.onReceive(ProcessMessage);
    }
}

template <uint8_t bus_num>
void TeensyCAN<bus_num>::Tick()
{
    // Repeated code due to limitations of C++11, look into alternatives without repeated code
    uint8_t remaining = 1;
    const uint8_t kMaxEvents{100};
    for (uint8_t counter = 0; counter < kMaxEvents && remaining != 0; counter++)
    {
        if (bus_num == 2)
        {
            remaining = can_bus_2.events();
        }
        else if (bus_num == 3)
        {
            remaining = can_bus_3.events();
        }
        else
        {
            remaining = can_bus_1.events();
        }
    }
}

template <uint8_t bus_num>
bool TeensyCAN<bus_num>::SendMessage(CANMessage &msg)
{
    CAN_message_t msg_t;
    msg_t.id = msg.id_;
    msg_t.flags.extended = msg.extended_id_;
    msg_t.len = msg.len_;

    for (int i = 0; i < 8; i++)
    {
        msg_t.buf[i] = msg.data_[i];
    }

    // Repeated code due to limitations of C++11, look into alternatives without repeated code
    if (bus_num == 2)
    {
        can_bus_2.write(msg_t);
    }
    else if (bus_num == 3)
    {
        can_bus_3.write(msg_t);
    }
    else
    {
        can_bus_1.write(msg_t);
    }

    return true;
}

template <uint8_t bus_num>
_MB_ptr TeensyCAN<bus_num>::ProcessMessage = [](const CAN_message_t &msg)
{
    std::array<uint8_t, 8> msg_data{};
    memcpy(msg_data.data(), msg.buf, 8);
    CANMessage received_message{static_cast<uint32_t>(msg.id), msg.len, msg_data};
    for (size_t i = 0; i < rx_messages_.size(); i++)
    {
        rx_messages_[i]->DecodeSignals(received_message);
    }
};

template <uint8_t bus_num>
ICAN::ErrorStatus TeensyCAN<bus_num>::GetErrorStatus()
{
    CAN_error_t err;
    // Repeated code due to limitations of C++11, look into alternatives without repeated code
    if (bus_num == 2)
    {
        can_bus_2.error(err, false);
    }
    else if (bus_num == 3)
    {
        can_bus_3.error(err, false);
    }
    else
    {
        can_bus_1.error(err, false);
    }

    if (err.FLT_CONF == "Error Active")
    {
        return ICAN::ErrorStatus::ERROR_ACTIVE;
    }
    else if (err.FLT_CONF == "Error Passive")
    {
        return ICAN::ErrorStatus::ERROR_PASSIVE;
    }
    else if (err.FLT_CONF == "Bus off")
    {
        return ICAN::ErrorStatus::BUS_OFF;
    }
}
#endif