├── README.md ├── keywords.txt ├── .vscode └── c_cpp_properties.json ├── frc_can_core.h ├── LICENSE.md ├── ThirdPartyNotices.txt ├── examples └── FRC_CAN_Communication │ └── FRC_CAN_Communication.ino ├── frc_CAN.h ├── FRC_CAN.cpp ├── frc_mcp2515.h └── mcp2515.cpp /README.md: -------------------------------------------------------------------------------- 1 | FRC Arduino MCP2515 CAN interface library 2 | --------------------------------------------------------- 3 | 4 | Documentation incoming -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | MCP2515 KEYWORD1 2 | CAN KEYWORD1 3 | 4 | CAN_5KBPS LITERAL1 5 | CAN_10KBPS LITERAL1 6 | CAN_20KBPS LITERAL1 7 | CAN_40KBPS LITERAL1 8 | CAN_50KBPS LITERAL1 9 | CAN_80KBPS LITERAL1 10 | CAN_100KBPS LITERAL1 11 | CAN_125KBPS LITERAL1 12 | CAN_200KBPS LITERAL1 13 | CAN_250KBPS LITERAL1 14 | CAN_500KBPS LITERAL1 15 | CAN_1000KBPS LITERAL1 -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Win32", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "C:/arduino-1.8.10/hardware/arduino/avr/cores/arduino", 8 | "C:/arduino-1.8.10/hardware/arduino/avr/variants/standard", 9 | "Z:/Documents/Arduino/libraries/MCP_CAN_lib", 10 | "C:/arduino-1.8.10/libraries/Servo/src", 11 | "C:/arduino-1.8.10/hardware/arduino/avr/libraries/SPI/src", 12 | "C:/arduino-1.8.10/hardware/arduino/avr/libraries/EEPROM/src" 13 | ], 14 | "defines": [ 15 | "F_CPU=16000000L", 16 | "ARDUINO=10810", 17 | "ARDUINO_AVR_UNO", 18 | "ARDUINO_ARCH_AVR" 19 | ], 20 | "windowsSdkVersion": "10.0.18362.0", 21 | "compilerPath": "C:/arduino-1.8.10/hardware/tools/avr/bin/avr-gcc.exe", 22 | "cStandard": "c11", 23 | "cppStandard": "c++11", 24 | "intelliSenseMode": "gcc-x86", 25 | "compilerArgs": [ 26 | "-mmcu=atmega328p" 27 | ], 28 | "forcedInclude": [ 29 | "C:/arduino-1.8.10/hardware/arduino/avr/cores/arduino/Arduino.h" 30 | ] 31 | } 32 | ], 33 | "version": 4 34 | } -------------------------------------------------------------------------------- /frc_can_core.h: -------------------------------------------------------------------------------- 1 | #ifndef CAN_H_ 2 | #define CAN_H_ 3 | 4 | #include 5 | 6 | 7 | typedef unsigned char __u8; 8 | typedef unsigned short __u16; 9 | typedef unsigned long __u32; 10 | 11 | 12 | /* special address description flags for the CAN_ID */ 13 | #define CAN_EFF_FLAG 0x80000000UL /* EFF/SFF is set in the MSB */ 14 | #define CAN_RTR_FLAG 0x40000000UL /* remote transmission request */ 15 | #define CAN_ERR_FLAG 0x20000000UL /* error message frame */ 16 | 17 | /* valid bits in CAN ID for frame formats */ 18 | #define CAN_SFF_MASK 0x000007FFUL /* standard frame format (SFF) */ 19 | #define CAN_EFF_MASK 0x1FFFFFFFUL /* extended frame format (EFF) */ 20 | #define CAN_ERR_MASK 0x1FFFFFFFUL /* omit EFF, RTR, ERR flags */ 21 | 22 | /* 23 | * Controller Area Network Identifier structure 24 | * 25 | * bit 0-28 : CAN identifier (11/29 bit) 26 | * bit 29 : error message frame flag (0 = data frame, 1 = error message) 27 | * bit 30 : remote transmission request flag (1 = rtr frame) 28 | * bit 31 : frame format flag (0 = standard 11 bit, 1 = extended 29 bit) 29 | */ 30 | typedef __u32 canid_t; 31 | 32 | #define CAN_SFF_ID_BITS 11 33 | #define CAN_EFF_ID_BITS 29 34 | 35 | /* CAN payload length and DLC definitions according to ISO 11898-1 */ 36 | #define CAN_MAX_DLC 8 37 | #define CAN_MAX_DLEN 8 38 | 39 | struct can_frame { 40 | canid_t can_id; /* 32 bit CAN_ID + EFF/RTR/ERR flags */ 41 | __u8 can_dlc; /* frame payload length in byte (0 .. CAN_MAX_DLEN) */ 42 | __u8 data[CAN_MAX_DLEN] __attribute__((aligned(8))); 43 | }; 44 | 45 | #endif /* CAN_H_ */ 46 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | 2 | Copyright (c) 2019 FIRST 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | * Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of the FIRST nor the 13 | names of its contributors may be used to endorse or promote products 14 | derived from this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY 17 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR 19 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR 20 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /ThirdPartyNotices.txt: -------------------------------------------------------------------------------- 1 | ============================================================================== 2 | Copyrights and Licenses for Third Party Software Distributed with WPILib 3 | ============================================================================== 4 | The WPILib software contains code written by third parties. The copyrights, 5 | license, and restrictions which apply to each piece of software is included 6 | later in this file and/or inside of the individual applicable source files. 7 | 8 | The disclaimer of warranty in the WPILib license above applies to all code in 9 | WPILib, and nothing in any of the other licenses gives permission to use the 10 | names of FIRST nor the names of the WPILib contributors to endorse or promote 11 | products derived from this software. 12 | 13 | ============================================================================== 14 | MCP2515 LICENSE 15 | ============================================================================== 16 | 17 | The MIT License (MIT) 18 | 19 | Copyright (c) 2013 Seeed Technology Inc. Copyright (c) 2016 Dmitry 20 | 21 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 22 | 23 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 24 | 25 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /examples/FRC_CAN_Communication/FRC_CAN_Communication.ino: -------------------------------------------------------------------------------- 1 | 2 | #include "frc_mcp2515.h" 3 | #include "frc_CAN.h" 4 | 5 | // Define the CS pin and the interrupt pin 6 | #define CAN_CS 10 7 | #define CAN_INTERRUPT 2 8 | 9 | // Create an MCP2515 device. Only need to create 1 of these 10 | frc::MCP2515 mcp2515{CAN_CS}; 11 | 12 | // Create an FRC CAN Device. You can create up to 16 of these in 1 progam 13 | // Any more will overflow a global array 14 | frc::CAN frcCANDevice{1}; 15 | 16 | // Callback function. This will be called any time a new message is received 17 | // Matching one of the enabled devices. 18 | void CANCallback(frc::CAN* can, int apiId, bool rtr, const frc::CANData& data) { 19 | Serial.println(apiId, HEX); 20 | } 21 | 22 | // Callback function for any messages not matching a known device. 23 | // This would still have flags for RTR and Extended set, its a raw ID 24 | void UnknownMessageCallback(uint32_t id, const frc::CANData& data) { 25 | 26 | } 27 | 28 | 29 | void setup() { 30 | // Initialize the MCP2515. If any error values are set, initialization failed 31 | auto err = mcp2515.reset(); 32 | // CAN rate must be 1000KBPS to work with the FRC Ecosystem 33 | // Clock rate must match clock rate of CAN Board. 34 | err = mcp2515.setBitrate(frc::CAN_1000KBPS, frc::CAN_CLOCK::MCP_8MHZ); 35 | 36 | // Set up to normal CAN mode 37 | err = mcp2515.setNormalMode(); 38 | 39 | // Prepare our interrupt pin 40 | pinMode(CAN_INTERRUPT, INPUT); 41 | 42 | // Set up FRC CAN to be able to use the CAN Impl and callbacks 43 | // Last parameter can be set to nullptr if unknown messages should be skipped 44 | frc::CAN::SetCANImpl(&mcp2515, CAN_INTERRUPT, CANCallback, UnknownMessageCallback); 45 | 46 | // All CAN Devices must be added to the read list. Otherwise they will not be handled correctly. 47 | frcCANDevice.AddToReadList(); 48 | } 49 | 50 | unsigned long long lastSend20Ms = 0; 51 | int count = 0; 52 | 53 | void loop() { 54 | // Update must be called every loop in order to receive messages 55 | frc::CAN::Update(); 56 | 57 | uint8_t data[8]; 58 | 59 | // Writes can happen any time, this uses a periodic send 60 | auto now = millis(); 61 | if (now - lastSend20Ms > 20) { 62 | // 20 ms periodic 63 | memset(data, 0, 8); 64 | data[0] = count; 65 | count++; 66 | 67 | frcCANDevice.WritePacket(data, 1, 1); 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /frc_CAN.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace frc 6 | { 7 | class MCP2515; 8 | struct CANData 9 | { 10 | uint8_t data[8]; 11 | uint8_t length; 12 | uint32_t timestamp; 13 | }; 14 | 15 | enum class CANDeviceType 16 | { 17 | kBroadcast = 0, 18 | kRobotController = 1, 19 | kMotorController = 2, 20 | kRelayController = 3, 21 | kGyroSensor = 4, 22 | kAccelerometer = 5, 23 | kUltrasonicSensor = 6, 24 | kGearToothSensor = 7, 25 | kPowerDistribution = 8, 26 | kPneumatics = 9, 27 | kMiscellaneous = 10, 28 | kFirmwareUpdate = 31 29 | }; 30 | 31 | enum class CANManufacturer 32 | { 33 | kBroadcast = 0, 34 | kNI = 1, 35 | kLM = 2, 36 | kDEKA = 3, 37 | kCTRE = 4, 38 | kREV = 5, 39 | kGrapple = 6, 40 | kMS = 7, 41 | kTeamUse = 8, 42 | kKauaiLabs = 9, 43 | kCopperforge = 10, 44 | kPWF = 11, 45 | kStudica = 12 46 | }; 47 | 48 | constexpr uint32_t createCANId(uint16_t apiId, uint8_t deviceId, uint8_t manufacturer, uint8_t deviceType) 49 | { 50 | return (static_cast(deviceType) & 0x1F) << 24 | (static_cast(manufacturer) & 0xFF) << 16 | (apiId & 0x3FF) << 6 | (deviceId & 0x3F); 51 | } 52 | 53 | class CAN 54 | { 55 | public: 56 | typedef void (*MessageCallback)(CAN *can, int apiId, bool rtr, const CANData& message); 57 | typedef void (*UnknownMessageCallback)(uint32_t id, const CANData& message); 58 | static void SetCANImpl(MCP2515 *mcp2515, uint8_t interruptPin, MessageCallback messageCallback, UnknownMessageCallback unknownCallback); 59 | 60 | static void Update(); 61 | 62 | constexpr CAN(int deviceId, CANManufacturer deviceManufacturer, CANDeviceType deviceType) 63 | : m_messageMask{createCANId(0, deviceId, static_cast(deviceManufacturer), static_cast(deviceType))} 64 | { 65 | } 66 | 67 | constexpr explicit CAN(int deviceId) : CAN{deviceId, CANManufacturer::kTeamUse, CANDeviceType::kMiscellaneous} 68 | { 69 | } 70 | 71 | void AddToReadList(); 72 | 73 | void RemoveFromReadList(); 74 | 75 | bool WritePacket(const uint8_t *data, uint8_t length, int apiId); 76 | 77 | bool WriteRTRFrame(uint8_t length, int apiId); 78 | 79 | private: 80 | static MCP2515* m_mcp2515; 81 | static uint8_t m_interruptPin; 82 | static CAN::MessageCallback m_messageCb; 83 | static CAN::UnknownMessageCallback m_unknownMessageCb; 84 | static CAN* m_canClasses[16]; 85 | static uint8_t m_canCount; 86 | 87 | int16_t matchesId(int32_t id) { 88 | constexpr int32_t mask = 0x1FFF003F; 89 | constexpr int32_t filter = 0x0000FFC0; 90 | if ((id & mask) == m_messageMask) { 91 | return (id & filter) >> 6; 92 | } 93 | return -1; 94 | } 95 | uint32_t m_messageMask = 0; 96 | }; 97 | } // namespace frc -------------------------------------------------------------------------------- /FRC_CAN.cpp: -------------------------------------------------------------------------------- 1 | #include "frc_CAN.h" 2 | #include "frc_mcp2515.h" 3 | 4 | using namespace frc; 5 | 6 | frc::MCP2515 *CAN::m_mcp2515 = nullptr; 7 | uint8_t CAN::m_interruptPin = 0; 8 | CAN::MessageCallback CAN::m_messageCb = nullptr; 9 | CAN::UnknownMessageCallback CAN::m_unknownMessageCb = nullptr; 10 | CAN *CAN::m_canClasses[16]; 11 | uint8_t CAN::m_canCount = 0; 12 | 13 | void CAN::SetCANImpl(MCP2515 *mcp, uint8_t iPin, CAN::MessageCallback cb, CAN::UnknownMessageCallback ucb) 14 | { 15 | m_mcp2515 = mcp; 16 | m_interruptPin = iPin; 17 | m_messageCb = cb; 18 | m_unknownMessageCb = ucb; 19 | } 20 | 21 | void CAN::Update() 22 | { 23 | can_frame frame; 24 | if (!digitalRead(m_interruptPin)) 25 | { 26 | auto err = m_mcp2515->readMessage(&frame); 27 | if (err == MCP2515::ERROR::ERROR_OK) 28 | { 29 | auto masked = frame.can_id & CAN_EFF_MASK; 30 | for (uint8_t i = 0; i < m_canCount; i++) 31 | { 32 | auto canClass = m_canClasses[i]; 33 | if (canClass == nullptr) 34 | return; 35 | auto matchesId = canClass->matchesId(masked); 36 | if (matchesId >= 0) 37 | { 38 | CANData message; 39 | message.length = frame.can_dlc; 40 | message.timestamp = millis(); 41 | memcpy(message.data, frame.data, message.length); 42 | m_messageCb(canClass, matchesId, (frame.can_id & CAN_RTR_FLAG) != 0, message); 43 | return; 44 | } 45 | } 46 | if (m_unknownMessageCb) 47 | { 48 | CANData message; 49 | message.length = frame.can_dlc; 50 | message.timestamp = millis(); 51 | memcpy(message.data, frame.data, message.length); 52 | m_unknownMessageCb(frame.can_id, message); 53 | } 54 | } 55 | } 56 | } 57 | 58 | void CAN::AddToReadList() 59 | { 60 | m_canClasses[m_canCount] = this; 61 | m_canCount++; 62 | } 63 | 64 | void CAN::RemoveFromReadList() 65 | { 66 | for (int i = 0; i < m_canCount; i++) 67 | { 68 | if (m_canClasses[i] == this) 69 | { 70 | m_canClasses[i] == nullptr; 71 | } 72 | } 73 | } 74 | 75 | bool CAN::WritePacket(const uint8_t *data, uint8_t length, int apiId) 76 | { 77 | can_frame frame; 78 | memcpy(frame.data, data, length); 79 | frame.can_dlc = length; 80 | frame.can_id = m_messageMask; 81 | frame.can_id |= apiId << 6; 82 | frame.can_id |= CAN_EFF_FLAG; 83 | 84 | return m_mcp2515->sendMessage(&frame) == MCP2515::ERROR::ERROR_OK; 85 | } 86 | 87 | bool CAN::WriteRTRFrame(uint8_t length, int apiId) 88 | { 89 | can_frame frame; 90 | memset(frame.data, 0, 8); 91 | frame.can_dlc = length; 92 | frame.can_id = m_messageMask; 93 | frame.can_id |= apiId << 6; 94 | frame.can_id |= CAN_EFF_FLAG; 95 | frame.can_id |= CAN_RTR_FLAG; 96 | 97 | return m_mcp2515->sendMessage(&frame) == MCP2515::ERROR::ERROR_OK; 98 | } 99 | -------------------------------------------------------------------------------- /frc_mcp2515.h: -------------------------------------------------------------------------------- 1 | #ifndef _FRC_MCP2515_H_ 2 | #define _FRC_MCP2515_H_ 3 | 4 | #include 5 | #include "frc_can_core.h" 6 | 7 | /* 8 | * Speed 8M 9 | */ 10 | #define MCP_8MHz_1000kBPS_CFG1 (0x00) 11 | #define MCP_8MHz_1000kBPS_CFG2 (0x80) 12 | #define MCP_8MHz_1000kBPS_CFG3 (0x80) 13 | 14 | #define MCP_8MHz_500kBPS_CFG1 (0x00) 15 | #define MCP_8MHz_500kBPS_CFG2 (0x90) 16 | #define MCP_8MHz_500kBPS_CFG3 (0x82) 17 | 18 | #define MCP_8MHz_250kBPS_CFG1 (0x00) 19 | #define MCP_8MHz_250kBPS_CFG2 (0xB1) 20 | #define MCP_8MHz_250kBPS_CFG3 (0x85) 21 | 22 | #define MCP_8MHz_200kBPS_CFG1 (0x00) 23 | #define MCP_8MHz_200kBPS_CFG2 (0xB4) 24 | #define MCP_8MHz_200kBPS_CFG3 (0x86) 25 | 26 | #define MCP_8MHz_125kBPS_CFG1 (0x01) 27 | #define MCP_8MHz_125kBPS_CFG2 (0xB1) 28 | #define MCP_8MHz_125kBPS_CFG3 (0x85) 29 | 30 | #define MCP_8MHz_100kBPS_CFG1 (0x01) 31 | #define MCP_8MHz_100kBPS_CFG2 (0xB4) 32 | #define MCP_8MHz_100kBPS_CFG3 (0x86) 33 | 34 | #define MCP_8MHz_80kBPS_CFG1 (0x01) 35 | #define MCP_8MHz_80kBPS_CFG2 (0xBF) 36 | #define MCP_8MHz_80kBPS_CFG3 (0x87) 37 | 38 | #define MCP_8MHz_50kBPS_CFG1 (0x03) 39 | #define MCP_8MHz_50kBPS_CFG2 (0xB4) 40 | #define MCP_8MHz_50kBPS_CFG3 (0x86) 41 | 42 | #define MCP_8MHz_40kBPS_CFG1 (0x03) 43 | #define MCP_8MHz_40kBPS_CFG2 (0xBF) 44 | #define MCP_8MHz_40kBPS_CFG3 (0x87) 45 | 46 | #define MCP_8MHz_33k3BPS_CFG1 (0x47) 47 | #define MCP_8MHz_33k3BPS_CFG2 (0xE2) 48 | #define MCP_8MHz_33k3BPS_CFG3 (0x85) 49 | 50 | #define MCP_8MHz_31k25BPS_CFG1 (0x07) 51 | #define MCP_8MHz_31k25BPS_CFG2 (0xA4) 52 | #define MCP_8MHz_31k25BPS_CFG3 (0x84) 53 | 54 | #define MCP_8MHz_20kBPS_CFG1 (0x07) 55 | #define MCP_8MHz_20kBPS_CFG2 (0xBF) 56 | #define MCP_8MHz_20kBPS_CFG3 (0x87) 57 | 58 | #define MCP_8MHz_10kBPS_CFG1 (0x0F) 59 | #define MCP_8MHz_10kBPS_CFG2 (0xBF) 60 | #define MCP_8MHz_10kBPS_CFG3 (0x87) 61 | 62 | #define MCP_8MHz_5kBPS_CFG1 (0x1F) 63 | #define MCP_8MHz_5kBPS_CFG2 (0xBF) 64 | #define MCP_8MHz_5kBPS_CFG3 (0x87) 65 | 66 | /* 67 | * speed 16M 68 | */ 69 | #define MCP_16MHz_1000kBPS_CFG1 (0x00) 70 | #define MCP_16MHz_1000kBPS_CFG2 (0xD0) 71 | #define MCP_16MHz_1000kBPS_CFG3 (0x82) 72 | 73 | #define MCP_16MHz_500kBPS_CFG1 (0x00) 74 | #define MCP_16MHz_500kBPS_CFG2 (0xF0) 75 | #define MCP_16MHz_500kBPS_CFG3 (0x86) 76 | 77 | #define MCP_16MHz_250kBPS_CFG1 (0x41) 78 | #define MCP_16MHz_250kBPS_CFG2 (0xF1) 79 | #define MCP_16MHz_250kBPS_CFG3 (0x85) 80 | 81 | #define MCP_16MHz_200kBPS_CFG1 (0x01) 82 | #define MCP_16MHz_200kBPS_CFG2 (0xFA) 83 | #define MCP_16MHz_200kBPS_CFG3 (0x87) 84 | 85 | #define MCP_16MHz_125kBPS_CFG1 (0x03) 86 | #define MCP_16MHz_125kBPS_CFG2 (0xF0) 87 | #define MCP_16MHz_125kBPS_CFG3 (0x86) 88 | 89 | #define MCP_16MHz_100kBPS_CFG1 (0x03) 90 | #define MCP_16MHz_100kBPS_CFG2 (0xFA) 91 | #define MCP_16MHz_100kBPS_CFG3 (0x87) 92 | 93 | #define MCP_16MHz_80kBPS_CFG1 (0x03) 94 | #define MCP_16MHz_80kBPS_CFG2 (0xFF) 95 | #define MCP_16MHz_80kBPS_CFG3 (0x87) 96 | 97 | #define MCP_16MHz_83k3BPS_CFG1 (0x03) 98 | #define MCP_16MHz_83k3BPS_CFG2 (0xBE) 99 | #define MCP_16MHz_83k3BPS_CFG3 (0x07) 100 | 101 | #define MCP_16MHz_50kBPS_CFG1 (0x07) 102 | #define MCP_16MHz_50kBPS_CFG2 (0xFA) 103 | #define MCP_16MHz_50kBPS_CFG3 (0x87) 104 | 105 | #define MCP_16MHz_40kBPS_CFG1 (0x07) 106 | #define MCP_16MHz_40kBPS_CFG2 (0xFF) 107 | #define MCP_16MHz_40kBPS_CFG3 (0x87) 108 | 109 | #define MCP_16MHz_33k3BPS_CFG1 (0x4E) 110 | #define MCP_16MHz_33k3BPS_CFG2 (0xF1) 111 | #define MCP_16MHz_33k3BPS_CFG3 (0x85) 112 | 113 | #define MCP_16MHz_20kBPS_CFG1 (0x0F) 114 | #define MCP_16MHz_20kBPS_CFG2 (0xFF) 115 | #define MCP_16MHz_20kBPS_CFG3 (0x87) 116 | 117 | #define MCP_16MHz_10kBPS_CFG1 (0x1F) 118 | #define MCP_16MHz_10kBPS_CFG2 (0xFF) 119 | #define MCP_16MHz_10kBPS_CFG3 (0x87) 120 | 121 | #define MCP_16MHz_5kBPS_CFG1 (0x3F) 122 | #define MCP_16MHz_5kBPS_CFG2 (0xFF) 123 | #define MCP_16MHz_5kBPS_CFG3 (0x87) 124 | 125 | /* 126 | * speed 20M 127 | */ 128 | #define MCP_20MHz_1000kBPS_CFG1 (0x00) 129 | #define MCP_20MHz_1000kBPS_CFG2 (0xD9) 130 | #define MCP_20MHz_1000kBPS_CFG3 (0x82) 131 | 132 | #define MCP_20MHz_500kBPS_CFG1 (0x00) 133 | #define MCP_20MHz_500kBPS_CFG2 (0xFA) 134 | #define MCP_20MHz_500kBPS_CFG3 (0x87) 135 | 136 | #define MCP_20MHz_250kBPS_CFG1 (0x41) 137 | #define MCP_20MHz_250kBPS_CFG2 (0xFB) 138 | #define MCP_20MHz_250kBPS_CFG3 (0x86) 139 | 140 | #define MCP_20MHz_200kBPS_CFG1 (0x01) 141 | #define MCP_20MHz_200kBPS_CFG2 (0xFF) 142 | #define MCP_20MHz_200kBPS_CFG3 (0x87) 143 | 144 | #define MCP_20MHz_125kBPS_CFG1 (0x03) 145 | #define MCP_20MHz_125kBPS_CFG2 (0xFA) 146 | #define MCP_20MHz_125kBPS_CFG3 (0x87) 147 | 148 | #define MCP_20MHz_100kBPS_CFG1 (0x04) 149 | #define MCP_20MHz_100kBPS_CFG2 (0xFA) 150 | #define MCP_20MHz_100kBPS_CFG3 (0x87) 151 | 152 | #define MCP_20MHz_83k3BPS_CFG1 (0x04) 153 | #define MCP_20MHz_83k3BPS_CFG2 (0xFE) 154 | #define MCP_20MHz_83k3BPS_CFG3 (0x87) 155 | 156 | #define MCP_20MHz_80kBPS_CFG1 (0x04) 157 | #define MCP_20MHz_80kBPS_CFG2 (0xFF) 158 | #define MCP_20MHz_80kBPS_CFG3 (0x87) 159 | 160 | #define MCP_20MHz_50kBPS_CFG1 (0x09) 161 | #define MCP_20MHz_50kBPS_CFG2 (0xFA) 162 | #define MCP_20MHz_50kBPS_CFG3 (0x87) 163 | 164 | #define MCP_20MHz_40kBPS_CFG1 (0x09) 165 | #define MCP_20MHz_40kBPS_CFG2 (0xFF) 166 | #define MCP_20MHz_40kBPS_CFG3 (0x87) 167 | 168 | #define MCP_20MHz_33k3BPS_CFG1 (0x0B) 169 | #define MCP_20MHz_33k3BPS_CFG2 (0xFF) 170 | #define MCP_20MHz_33k3BPS_CFG3 (0x87) 171 | 172 | namespace frc { 173 | 174 | enum CAN_CLOCK { 175 | MCP_20MHZ, 176 | MCP_16MHZ, 177 | MCP_8MHZ 178 | }; 179 | 180 | enum CAN_SPEED { 181 | CAN_5KBPS, 182 | CAN_10KBPS, 183 | CAN_20KBPS, 184 | CAN_31K25BPS, 185 | CAN_33KBPS, 186 | CAN_40KBPS, 187 | CAN_50KBPS, 188 | CAN_80KBPS, 189 | CAN_83K3BPS, 190 | CAN_95KBPS, 191 | CAN_100KBPS, 192 | CAN_125KBPS, 193 | CAN_200KBPS, 194 | CAN_250KBPS, 195 | CAN_500KBPS, 196 | CAN_1000KBPS 197 | }; 198 | 199 | enum CAN_CLKOUT { 200 | CLKOUT_DISABLE = -1, 201 | CLKOUT_DIV1 = 0x0, 202 | CLKOUT_DIV2 = 0x1, 203 | CLKOUT_DIV4 = 0x2, 204 | CLKOUT_DIV8 = 0x3, 205 | }; 206 | 207 | class MCP2515 208 | { 209 | public: 210 | enum ERROR { 211 | ERROR_OK = 0, 212 | ERROR_FAIL = 1, 213 | ERROR_ALLTXBUSY = 2, 214 | ERROR_FAILINIT = 3, 215 | ERROR_FAILTX = 4, 216 | ERROR_NOMSG = 5 217 | }; 218 | 219 | enum MASK { 220 | MASK0, 221 | MASK1 222 | }; 223 | 224 | enum RXF { 225 | RXF0 = 0, 226 | RXF1 = 1, 227 | RXF2 = 2, 228 | RXF3 = 3, 229 | RXF4 = 4, 230 | RXF5 = 5 231 | }; 232 | 233 | enum RXBn { 234 | RXB0 = 0, 235 | RXB1 = 1 236 | }; 237 | 238 | enum TXBn { 239 | TXB0 = 0, 240 | TXB1 = 1, 241 | TXB2 = 2 242 | }; 243 | 244 | enum /*class*/ CANINTF : uint8_t { 245 | CANINTF_RX0IF = 0x01, 246 | CANINTF_RX1IF = 0x02, 247 | CANINTF_TX0IF = 0x04, 248 | CANINTF_TX1IF = 0x08, 249 | CANINTF_TX2IF = 0x10, 250 | CANINTF_ERRIF = 0x20, 251 | CANINTF_WAKIF = 0x40, 252 | CANINTF_MERRF = 0x80 253 | }; 254 | 255 | enum /*class*/ EFLG : uint8_t { 256 | EFLG_RX1OVR = (1<<7), 257 | EFLG_RX0OVR = (1<<6), 258 | EFLG_TXBO = (1<<5), 259 | EFLG_TXEP = (1<<4), 260 | EFLG_RXEP = (1<<3), 261 | EFLG_TXWAR = (1<<2), 262 | EFLG_RXWAR = (1<<1), 263 | EFLG_EWARN = (1<<0) 264 | }; 265 | 266 | private: 267 | static const uint8_t CANCTRL_REQOP = 0xE0; 268 | static const uint8_t CANCTRL_ABAT = 0x10; 269 | static const uint8_t CANCTRL_OSM = 0x08; 270 | static const uint8_t CANCTRL_CLKEN = 0x04; 271 | static const uint8_t CANCTRL_CLKPRE = 0x03; 272 | 273 | enum /*class*/ CANCTRL_REQOP_MODE : uint8_t { 274 | CANCTRL_REQOP_NORMAL = 0x00, 275 | CANCTRL_REQOP_SLEEP = 0x20, 276 | CANCTRL_REQOP_LOOPBACK = 0x40, 277 | CANCTRL_REQOP_LISTENONLY = 0x60, 278 | CANCTRL_REQOP_CONFIG = 0x80, 279 | CANCTRL_REQOP_POWERUP = 0xE0 280 | }; 281 | 282 | static const uint8_t CANSTAT_OPMOD = 0xE0; 283 | static const uint8_t CANSTAT_ICOD = 0x0E; 284 | 285 | static const uint8_t CNF3_SOF = 0x80; 286 | 287 | static const uint8_t TXB_EXIDE_MASK = 0x08; 288 | static const uint8_t DLC_MASK = 0x0F; 289 | static const uint8_t RTR_MASK = 0x40; 290 | 291 | static const uint8_t RXBnCTRL_RXM_STD = 0x20; 292 | static const uint8_t RXBnCTRL_RXM_EXT = 0x40; 293 | static const uint8_t RXBnCTRL_RXM_STDEXT = 0x00; 294 | static const uint8_t RXBnCTRL_RXM_MASK = 0x60; 295 | static const uint8_t RXBnCTRL_RTR = 0x08; 296 | static const uint8_t RXB0CTRL_BUKT = 0x04; 297 | 298 | static const uint8_t MCP_SIDH = 0; 299 | static const uint8_t MCP_SIDL = 1; 300 | static const uint8_t MCP_EID8 = 2; 301 | static const uint8_t MCP_EID0 = 3; 302 | static const uint8_t MCP_DLC = 4; 303 | static const uint8_t MCP_DATA = 5; 304 | 305 | enum /*class*/ STAT : uint8_t { 306 | STAT_RX0IF = (1<<0), 307 | STAT_RX1IF = (1<<1) 308 | }; 309 | 310 | static const uint8_t STAT_RXIF_MASK = STAT_RX0IF | STAT_RX1IF; 311 | 312 | enum /*class*/ TXBnCTRL : uint8_t { 313 | TXB_ABTF = 0x40, 314 | TXB_MLOA = 0x20, 315 | TXB_TXERR = 0x10, 316 | TXB_TXREQ = 0x08, 317 | TXB_TXIE = 0x04, 318 | TXB_TXP = 0x03 319 | }; 320 | 321 | static const uint8_t EFLG_ERRORMASK = EFLG_RX1OVR 322 | | EFLG_RX0OVR 323 | | EFLG_TXBO 324 | | EFLG_TXEP 325 | | EFLG_RXEP; 326 | 327 | enum /*class*/ INSTRUCTION : uint8_t { 328 | INSTRUCTION_WRITE = 0x02, 329 | INSTRUCTION_READ = 0x03, 330 | INSTRUCTION_BITMOD = 0x05, 331 | INSTRUCTION_LOAD_TX0 = 0x40, 332 | INSTRUCTION_LOAD_TX1 = 0x42, 333 | INSTRUCTION_LOAD_TX2 = 0x44, 334 | INSTRUCTION_RTS_TX0 = 0x81, 335 | INSTRUCTION_RTS_TX1 = 0x82, 336 | INSTRUCTION_RTS_TX2 = 0x84, 337 | INSTRUCTION_RTS_ALL = 0x87, 338 | INSTRUCTION_READ_RX0 = 0x90, 339 | INSTRUCTION_READ_RX1 = 0x94, 340 | INSTRUCTION_READ_STATUS = 0xA0, 341 | INSTRUCTION_RX_STATUS = 0xB0, 342 | INSTRUCTION_RESET = 0xC0 343 | }; 344 | 345 | enum /*class*/ REGISTER : uint8_t { 346 | MCP_RXF0SIDH = 0x00, 347 | MCP_RXF0SIDL = 0x01, 348 | MCP_RXF0EID8 = 0x02, 349 | MCP_RXF0EID0 = 0x03, 350 | MCP_RXF1SIDH = 0x04, 351 | MCP_RXF1SIDL = 0x05, 352 | MCP_RXF1EID8 = 0x06, 353 | MCP_RXF1EID0 = 0x07, 354 | MCP_RXF2SIDH = 0x08, 355 | MCP_RXF2SIDL = 0x09, 356 | MCP_RXF2EID8 = 0x0A, 357 | MCP_RXF2EID0 = 0x0B, 358 | MCP_CANSTAT = 0x0E, 359 | MCP_CANCTRL = 0x0F, 360 | MCP_RXF3SIDH = 0x10, 361 | MCP_RXF3SIDL = 0x11, 362 | MCP_RXF3EID8 = 0x12, 363 | MCP_RXF3EID0 = 0x13, 364 | MCP_RXF4SIDH = 0x14, 365 | MCP_RXF4SIDL = 0x15, 366 | MCP_RXF4EID8 = 0x16, 367 | MCP_RXF4EID0 = 0x17, 368 | MCP_RXF5SIDH = 0x18, 369 | MCP_RXF5SIDL = 0x19, 370 | MCP_RXF5EID8 = 0x1A, 371 | MCP_RXF5EID0 = 0x1B, 372 | MCP_TEC = 0x1C, 373 | MCP_REC = 0x1D, 374 | MCP_RXM0SIDH = 0x20, 375 | MCP_RXM0SIDL = 0x21, 376 | MCP_RXM0EID8 = 0x22, 377 | MCP_RXM0EID0 = 0x23, 378 | MCP_RXM1SIDH = 0x24, 379 | MCP_RXM1SIDL = 0x25, 380 | MCP_RXM1EID8 = 0x26, 381 | MCP_RXM1EID0 = 0x27, 382 | MCP_CNF3 = 0x28, 383 | MCP_CNF2 = 0x29, 384 | MCP_CNF1 = 0x2A, 385 | MCP_CANINTE = 0x2B, 386 | MCP_CANINTF = 0x2C, 387 | MCP_EFLG = 0x2D, 388 | MCP_TXB0CTRL = 0x30, 389 | MCP_TXB0SIDH = 0x31, 390 | MCP_TXB0SIDL = 0x32, 391 | MCP_TXB0EID8 = 0x33, 392 | MCP_TXB0EID0 = 0x34, 393 | MCP_TXB0DLC = 0x35, 394 | MCP_TXB0DATA = 0x36, 395 | MCP_TXB1CTRL = 0x40, 396 | MCP_TXB1SIDH = 0x41, 397 | MCP_TXB1SIDL = 0x42, 398 | MCP_TXB1EID8 = 0x43, 399 | MCP_TXB1EID0 = 0x44, 400 | MCP_TXB1DLC = 0x45, 401 | MCP_TXB1DATA = 0x46, 402 | MCP_TXB2CTRL = 0x50, 403 | MCP_TXB2SIDH = 0x51, 404 | MCP_TXB2SIDL = 0x52, 405 | MCP_TXB2EID8 = 0x53, 406 | MCP_TXB2EID0 = 0x54, 407 | MCP_TXB2DLC = 0x55, 408 | MCP_TXB2DATA = 0x56, 409 | MCP_RXB0CTRL = 0x60, 410 | MCP_RXB0SIDH = 0x61, 411 | MCP_RXB0SIDL = 0x62, 412 | MCP_RXB0EID8 = 0x63, 413 | MCP_RXB0EID0 = 0x64, 414 | MCP_RXB0DLC = 0x65, 415 | MCP_RXB0DATA = 0x66, 416 | MCP_RXB1CTRL = 0x70, 417 | MCP_RXB1SIDH = 0x71, 418 | MCP_RXB1SIDL = 0x72, 419 | MCP_RXB1EID8 = 0x73, 420 | MCP_RXB1EID0 = 0x74, 421 | MCP_RXB1DLC = 0x75, 422 | MCP_RXB1DATA = 0x76 423 | }; 424 | 425 | static const uint32_t SPI_CLOCK = 10000000; // 10MHz 426 | 427 | static const int N_TXBUFFERS = 3; 428 | static const int N_RXBUFFERS = 2; 429 | 430 | static const struct TXBn_REGS { 431 | REGISTER CTRL; 432 | REGISTER SIDH; 433 | REGISTER DATA; 434 | } TXB[N_TXBUFFERS]; 435 | 436 | static const struct RXBn_REGS { 437 | REGISTER CTRL; 438 | REGISTER SIDH; 439 | REGISTER DATA; 440 | CANINTF CANINTF_RXnIF; 441 | } RXB[N_RXBUFFERS]; 442 | 443 | uint8_t SPICS; 444 | 445 | private: 446 | 447 | void startSPI(); 448 | void endSPI(); 449 | 450 | ERROR setMode(const CANCTRL_REQOP_MODE mode); 451 | 452 | uint8_t readRegister(const REGISTER reg); 453 | void readRegisters(const REGISTER reg, uint8_t values[], const uint8_t n); 454 | void setRegister(const REGISTER reg, const uint8_t value); 455 | void setRegisters(const REGISTER reg, const uint8_t values[], const uint8_t n); 456 | void modifyRegister(const REGISTER reg, const uint8_t mask, const uint8_t data); 457 | 458 | void prepareId(uint8_t *buffer, const bool ext, const uint32_t id); 459 | 460 | public: 461 | MCP2515(const uint8_t _CS); 462 | ERROR reset(void); 463 | ERROR setConfigMode(); 464 | ERROR setListenOnlyMode(); 465 | ERROR setSleepMode(); 466 | ERROR setLoopbackMode(); 467 | ERROR setNormalMode(); 468 | ERROR setClkOut(const CAN_CLKOUT divisor); 469 | ERROR setBitrate(const CAN_SPEED canSpeed); 470 | ERROR setBitrate(const CAN_SPEED canSpeed, const CAN_CLOCK canClock); 471 | ERROR setFilterMask(const MASK num, const bool ext, const uint32_t ulData); 472 | ERROR setFilter(const RXF num, const bool ext, const uint32_t ulData); 473 | ERROR sendMessage(const TXBn txbn, const struct can_frame *frame); 474 | ERROR sendMessage(const struct can_frame *frame); 475 | ERROR readMessage(const RXBn rxbn, struct can_frame *frame); 476 | ERROR readMessage(struct can_frame *frame); 477 | bool checkReceive(void); 478 | bool checkError(void); 479 | uint8_t getErrorFlags(void); 480 | void clearRXnOVRFlags(void); 481 | uint8_t getInterrupts(void); 482 | uint8_t getInterruptMask(void); 483 | void clearInterrupts(void); 484 | void clearTXInterrupts(void); 485 | uint8_t getStatus(void); 486 | void clearRXnOVR(void); 487 | void clearMERR(); 488 | void clearERRIF(); 489 | }; 490 | 491 | } 492 | 493 | #endif 494 | -------------------------------------------------------------------------------- /mcp2515.cpp: -------------------------------------------------------------------------------- 1 | #include "frc_mcp2515.h" 2 | 3 | using namespace frc; 4 | 5 | const struct MCP2515::TXBn_REGS MCP2515::TXB[MCP2515::N_TXBUFFERS] = { 6 | {MCP_TXB0CTRL, MCP_TXB0SIDH, MCP_TXB0DATA}, 7 | {MCP_TXB1CTRL, MCP_TXB1SIDH, MCP_TXB1DATA}, 8 | {MCP_TXB2CTRL, MCP_TXB2SIDH, MCP_TXB2DATA} 9 | }; 10 | 11 | const struct MCP2515::RXBn_REGS MCP2515::RXB[N_RXBUFFERS] = { 12 | {MCP_RXB0CTRL, MCP_RXB0SIDH, MCP_RXB0DATA, CANINTF_RX0IF}, 13 | {MCP_RXB1CTRL, MCP_RXB1SIDH, MCP_RXB1DATA, CANINTF_RX1IF} 14 | }; 15 | 16 | MCP2515::MCP2515(const uint8_t _CS) 17 | { 18 | SPI.begin(); 19 | 20 | SPICS = _CS; 21 | pinMode(SPICS, OUTPUT); 22 | endSPI(); 23 | } 24 | 25 | void MCP2515::startSPI() { 26 | SPI.beginTransaction(SPISettings(SPI_CLOCK, MSBFIRST, SPI_MODE0)); 27 | digitalWrite(SPICS, LOW); 28 | } 29 | 30 | void MCP2515::endSPI() { 31 | digitalWrite(SPICS, HIGH); 32 | SPI.endTransaction(); 33 | } 34 | 35 | MCP2515::ERROR MCP2515::reset(void) 36 | { 37 | startSPI(); 38 | SPI.transfer(INSTRUCTION_RESET); 39 | endSPI(); 40 | 41 | delay(10); 42 | 43 | uint8_t zeros[14]; 44 | memset(zeros, 0, sizeof(zeros)); 45 | setRegisters(MCP_TXB0CTRL, zeros, 14); 46 | setRegisters(MCP_TXB1CTRL, zeros, 14); 47 | setRegisters(MCP_TXB2CTRL, zeros, 14); 48 | 49 | setRegister(MCP_RXB0CTRL, 0); 50 | setRegister(MCP_RXB1CTRL, 0); 51 | 52 | setRegister(MCP_CANINTE, CANINTF_RX0IF | CANINTF_RX1IF); 53 | 54 | modifyRegister(MCP_RXB0CTRL, 55 | RXBnCTRL_RXM_MASK | RXB0CTRL_BUKT, 56 | RXBnCTRL_RXM_STDEXT | RXB0CTRL_BUKT); 57 | modifyRegister(MCP_RXB1CTRL, RXBnCTRL_RXM_MASK, RXBnCTRL_RXM_STDEXT); 58 | 59 | // clear filters and masks 60 | /*RXF filters[] = {RXF0, RXF1, RXF2, RXF3, RXF4, RXF5}; 61 | for (int i=0; i<6; i++) { 62 | ERROR result = setFilter(filters[i], true, 0); 63 | if (result != ERROR_OK) { 64 | return result; 65 | } 66 | } 67 | 68 | MASK masks[] = {MASK0, MASK1}; 69 | for (int i=0; i<2; i++) { 70 | ERROR result = setFilterMask(masks[i], true, 0); 71 | if (result != ERROR_OK) { 72 | return result; 73 | } 74 | }*/ 75 | 76 | return ERROR_OK; 77 | } 78 | 79 | uint8_t MCP2515::readRegister(const REGISTER reg) 80 | { 81 | startSPI(); 82 | SPI.transfer(INSTRUCTION_READ); 83 | SPI.transfer(reg); 84 | uint8_t ret = SPI.transfer(0x00); 85 | endSPI(); 86 | 87 | return ret; 88 | } 89 | 90 | void MCP2515::readRegisters(const REGISTER reg, uint8_t values[], const uint8_t n) 91 | { 92 | startSPI(); 93 | SPI.transfer(INSTRUCTION_READ); 94 | SPI.transfer(reg); 95 | // mcp2515 has auto-increment of address-pointer 96 | for (uint8_t i=0; i> 8); 512 | canid = (uint16_t)(id >> 16); 513 | buffer[MCP_SIDL] = (uint8_t) (canid & 0x03); 514 | buffer[MCP_SIDL] += (uint8_t) ((canid & 0x1C) << 3); 515 | buffer[MCP_SIDL] |= TXB_EXIDE_MASK; 516 | buffer[MCP_SIDH] = (uint8_t) (canid >> 5); 517 | } else { 518 | buffer[MCP_SIDH] = (uint8_t) (canid >> 3); 519 | buffer[MCP_SIDL] = (uint8_t) ((canid & 0x07 ) << 5); 520 | buffer[MCP_EID0] = 0; 521 | buffer[MCP_EID8] = 0; 522 | } 523 | } 524 | 525 | MCP2515::ERROR MCP2515::setFilterMask(const MASK mask, const bool ext, const uint32_t ulData) 526 | { 527 | ERROR res = setConfigMode(); 528 | if (res != ERROR_OK) { 529 | return res; 530 | } 531 | 532 | uint8_t tbufdata[4]; 533 | prepareId(tbufdata, ext, ulData); 534 | 535 | REGISTER reg; 536 | switch (mask) { 537 | case MASK0: reg = MCP_RXM0SIDH; break; 538 | case MASK1: reg = MCP_RXM1SIDH; break; 539 | default: 540 | return ERROR_FAIL; 541 | } 542 | 543 | setRegisters(reg, tbufdata, 4); 544 | 545 | return ERROR_OK; 546 | } 547 | 548 | MCP2515::ERROR MCP2515::setFilter(const RXF num, const bool ext, const uint32_t ulData) 549 | { 550 | ERROR res = setConfigMode(); 551 | if (res != ERROR_OK) { 552 | return res; 553 | } 554 | 555 | REGISTER reg; 556 | 557 | switch (num) { 558 | case RXF0: reg = MCP_RXF0SIDH; break; 559 | case RXF1: reg = MCP_RXF1SIDH; break; 560 | case RXF2: reg = MCP_RXF2SIDH; break; 561 | case RXF3: reg = MCP_RXF3SIDH; break; 562 | case RXF4: reg = MCP_RXF4SIDH; break; 563 | case RXF5: reg = MCP_RXF5SIDH; break; 564 | default: 565 | return ERROR_FAIL; 566 | } 567 | 568 | uint8_t tbufdata[4]; 569 | prepareId(tbufdata, ext, ulData); 570 | setRegisters(reg, tbufdata, 4); 571 | 572 | return ERROR_OK; 573 | } 574 | 575 | MCP2515::ERROR MCP2515::sendMessage(const TXBn txbn, const struct can_frame *frame) 576 | { 577 | const struct TXBn_REGS *txbuf = &TXB[txbn]; 578 | 579 | uint8_t data[13]; 580 | 581 | bool ext = (frame->can_id & CAN_EFF_FLAG); 582 | bool rtr = (frame->can_id & CAN_RTR_FLAG); 583 | uint32_t id = (frame->can_id & (ext ? CAN_EFF_MASK : CAN_SFF_MASK)); 584 | 585 | prepareId(data, ext, id); 586 | 587 | data[MCP_DLC] = rtr ? (frame->can_dlc | RTR_MASK) : frame->can_dlc; 588 | 589 | memcpy(&data[MCP_DATA], frame->data, frame->can_dlc); 590 | 591 | setRegisters(txbuf->SIDH, data, 5 + frame->can_dlc); 592 | 593 | modifyRegister(txbuf->CTRL, TXB_TXREQ, TXB_TXREQ); 594 | 595 | return ERROR_OK; 596 | } 597 | 598 | MCP2515::ERROR MCP2515::sendMessage(const struct can_frame *frame) 599 | { 600 | if (frame->can_dlc > CAN_MAX_DLEN) { 601 | return ERROR_FAILTX; 602 | } 603 | 604 | TXBn txBuffers[N_TXBUFFERS] = {TXB0, TXB1, TXB2}; 605 | 606 | for (int i=0; iCTRL); 609 | if ( (ctrlval & TXB_TXREQ) == 0 ) { 610 | return sendMessage(txBuffers[i], frame); 611 | } 612 | } 613 | 614 | return ERROR_FAILTX; 615 | } 616 | 617 | MCP2515::ERROR MCP2515::readMessage(const RXBn rxbn, struct can_frame *frame) 618 | { 619 | const struct RXBn_REGS *rxb = &RXB[rxbn]; 620 | 621 | uint8_t tbufdata[5]; 622 | 623 | readRegisters(rxb->SIDH, tbufdata, 5); 624 | 625 | uint32_t id = (tbufdata[MCP_SIDH]<<3) + (tbufdata[MCP_SIDL]>>5); 626 | 627 | if ( (tbufdata[MCP_SIDL] & TXB_EXIDE_MASK) == TXB_EXIDE_MASK ) { 628 | id = (id<<2) + (tbufdata[MCP_SIDL] & 0x03); 629 | id = (id<<8) + tbufdata[MCP_EID8]; 630 | id = (id<<8) + tbufdata[MCP_EID0]; 631 | id |= CAN_EFF_FLAG; 632 | } 633 | 634 | uint8_t dlc = (tbufdata[MCP_DLC] & DLC_MASK); 635 | if (dlc > CAN_MAX_DLEN) { 636 | return ERROR_FAIL; 637 | } 638 | 639 | uint8_t ctrl = readRegister(rxb->CTRL); 640 | if (ctrl & RXBnCTRL_RTR) { 641 | id |= CAN_RTR_FLAG; 642 | } 643 | 644 | frame->can_id = id; 645 | frame->can_dlc = dlc; 646 | 647 | readRegisters(rxb->DATA, frame->data, dlc); 648 | 649 | modifyRegister(MCP_CANINTF, rxb->CANINTF_RXnIF, 0); 650 | 651 | return ERROR_OK; 652 | } 653 | 654 | MCP2515::ERROR MCP2515::readMessage(struct can_frame *frame) 655 | { 656 | ERROR rc; 657 | uint8_t stat = getStatus(); 658 | 659 | if ( stat & STAT_RX0IF ) { 660 | rc = readMessage(RXB0, frame); 661 | } else if ( stat & STAT_RX1IF ) { 662 | rc = readMessage(RXB1, frame); 663 | } else { 664 | rc = ERROR_NOMSG; 665 | } 666 | 667 | return rc; 668 | } 669 | 670 | bool MCP2515::checkReceive(void) 671 | { 672 | uint8_t res = getStatus(); 673 | if ( res & STAT_RXIF_MASK ) { 674 | return true; 675 | } else { 676 | return false; 677 | } 678 | } 679 | 680 | bool MCP2515::checkError(void) 681 | { 682 | uint8_t eflg = getErrorFlags(); 683 | 684 | if ( eflg & EFLG_ERRORMASK ) { 685 | return true; 686 | } else { 687 | return false; 688 | } 689 | } 690 | 691 | uint8_t MCP2515::getErrorFlags(void) 692 | { 693 | return readRegister(MCP_EFLG); 694 | } 695 | 696 | void MCP2515::clearRXnOVRFlags(void) 697 | { 698 | modifyRegister(MCP_EFLG, EFLG_RX0OVR | EFLG_RX1OVR, 0); 699 | } 700 | 701 | uint8_t MCP2515::getInterrupts(void) 702 | { 703 | return readRegister(MCP_CANINTF); 704 | } 705 | 706 | void MCP2515::clearInterrupts(void) 707 | { 708 | setRegister(MCP_CANINTF, 0); 709 | } 710 | 711 | uint8_t MCP2515::getInterruptMask(void) 712 | { 713 | return readRegister(MCP_CANINTE); 714 | } 715 | 716 | void MCP2515::clearTXInterrupts(void) 717 | { 718 | modifyRegister(MCP_CANINTF, (CANINTF_TX0IF | CANINTF_TX1IF | CANINTF_TX2IF), 0); 719 | } 720 | 721 | void MCP2515::clearRXnOVR(void) 722 | { 723 | uint8_t eflg = getErrorFlags(); 724 | if (eflg != 0) { 725 | clearRXnOVRFlags(); 726 | clearInterrupts(); 727 | //modifyRegister(MCP_CANINTF, CANINTF_ERRIF, 0); 728 | } 729 | 730 | } 731 | 732 | void MCP2515::clearMERR() 733 | { 734 | //modifyRegister(MCP_EFLG, EFLG_RX0OVR | EFLG_RX1OVR, 0); 735 | //clearInterrupts(); 736 | modifyRegister(MCP_CANINTF, CANINTF_MERRF, 0); 737 | } 738 | 739 | void MCP2515::clearERRIF() 740 | { 741 | //modifyRegister(MCP_EFLG, EFLG_RX0OVR | EFLG_RX1OVR, 0); 742 | //clearInterrupts(); 743 | modifyRegister(MCP_CANINTF, CANINTF_ERRIF, 0); 744 | } 745 | --------------------------------------------------------------------------------