├── README.md ├── canard ├── README.md └── example │ ├── build_opt.h │ ├── can.c │ ├── example.ino │ ├── main.c │ ├── src │ └── libcanard │ │ ├── canard.c │ │ ├── canard.h │ │ ├── canard_internals.h │ │ └── drivers │ │ ├── README.md │ │ ├── libuavcan_vs_libcanard_arch.png │ │ └── stm32 │ │ ├── _internal_bxcan.h │ │ ├── canard_stm32.c │ │ └── canard_stm32.h │ ├── uavcan.c │ └── uavcan.h ├── stm32f072 ├── README.md └── stm32f072.ino ├── stm32f103 ├── README.md └── stm32f103.ino ├── stm32f303 ├── README.md └── stm32f303.ino ├── stm32f405 └── README.md ├── stm32f407 ├── README.md ├── _platformio.ini └── stm32f407.ino ├── stm32f413 ├── README.md └── stm32f413.ino ├── stm32f446 ├── README.md └── stm32f446.ino ├── stm32f722 ├── README.md └── stm32f722.ino ├── stm32f767 ├── README.md └── stm32f767.ino ├── stm32l452 ├── README.md └── stm32l452.ino └── stm32l496 ├── README.md └── stm32l496.ino /README.md: -------------------------------------------------------------------------------- 1 | # Arduino-STM32-CAN 2 | Basic Extended CAN(BxCan) communication example for Arduino Core STM32. 3 | 4 | 5 | # Hardware requirements 6 | 7 | - STM32F072 Development Board. 8 | - STM32F103 Development Board. 9 | - STM32F303 Development Board. 10 | - STM32F405 Development Board. 11 | - STM32F407 Development Board. 12 | - STM32F413 Development Board. 13 | - STM32F446 Development Board. 14 | - STM32F722 Development Board. 15 | - STM32F767 Development Board. 16 | - STM32L452 Development Board. 17 | - STM32L496 Development Board. 18 | - CAN Transceiver. 19 | MCP2551/2561/2562(5V) 20 | TJA1040/1050/1051/1055(5V) 21 | SN65HVD230/231/232(3.3V) 22 | 23 | __NOTE__ 24 | 3V CAN Transceivers are fully interoperable with 5V CAN Transceivers. 25 | Check [here](http://www.ti.com/lit/an/slla337/slla337.pdf). 26 | 27 | 28 | # Software requirements 29 | - Arduino IDE 30 | I used V1.8.19 31 | ![Arduino_V1_8_19](https://github.com/user-attachments/assets/f3179568-cce6-4762-99e3-509852c7fa5d) 32 | 33 | - Arduino core support for STM32 based boards. 34 | https://github.com/stm32duino/Arduino_Core_STM32 35 | ___Note for Core version___ 36 | Core version 2.8 or later requires Arduino IDE 2.x. 37 | I used version 2.7.1. 38 | This is the final version available for Arduino IDE 1.x. 39 | ![](https://img.shields.io/badge/_IMPORTANT-important) 40 | __There is some core library for STM32.__ 41 | __It doesn't work with [Arduino STM32](https://github.com/rogerclarkmelbourne/Arduino_STM32).__ 42 | ![STM32_Core_2 7 1](https://github.com/user-attachments/assets/e135c6b2-61f5-4ac8-a4e5-8878a29a2be4) 43 | 44 | # Wirering 45 | - USB power supply (board with 5V pin) 46 | STM32 CAN RX GPIO has 5V tolerant. 47 | ![STM32F103-CAN-5V](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/b344eaaf-9b73-4321-a84b-d0715269126c) 48 | 49 | - Board without 5V pin, such as BlackPill, or ST-Link power supply 50 | ![STM32F103-CAN-3V3](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/4bf2508f-ac6f-4b5b-8ded-e700beced1ca) 51 | 52 | # My test circuit 53 | The Nano on the left is a receiver with a terminating resistor. 54 | The Nano on the right is a transmitter without a terminating resistor. 55 | The STM32 transceiver has a terminating resistor. 56 | - Using SN65HVD230 57 | ![Test-Circuit-SN65HVD230](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/cd69745e-c7c9-4419-aeb0-4611c17f800a) 58 | 59 | - Using TJA1051 60 | ![Test-Circuit-TJA1051](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/1cddb8be-1a83-473e-ae6c-d63528da4e81) 61 | 62 | # Transmitter 63 | 64 | - from STM32F072 65 | ![STM32F072_Send](https://user-images.githubusercontent.com/6020549/109580416-e8917380-7b3d-11eb-915e-5d13c6f5fe3f.jpg) 66 | 67 | - from STM32F103 68 | ![STM32F103_Send](https://user-images.githubusercontent.com/6020549/80896902-3d9e0680-8d2e-11ea-9add-0a102f43c3a7.jpg) 69 | 70 | - from STM32F303 71 | ![STM32F303_Send](https://user-images.githubusercontent.com/6020549/80896905-4262ba80-8d2e-11ea-9c3b-3f4871a947bb.jpg) 72 | 73 | - from STM32F407 74 | ![STM32F405_Send](https://user-images.githubusercontent.com/6020549/80896908-45f64180-8d2e-11ea-91a3-c34fdb48725b.jpg) 75 | 76 | - from STM32F413 77 | ![stm32f413_transfer](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/29dc0e5b-e7ab-4aaf-aee2-e57b6185b2a3) 78 | 79 | - from STM32F446 80 | ![stm32f446_transfer](https://user-images.githubusercontent.com/6020549/110085501-8900c580-7dd4-11eb-9fb7-311c21800719.jpg) 81 | 82 | - from STM32F767 83 | ![stm32f767_transfer](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/39d5fc41-8b22-4f28-be5d-e65fa936e0ac) 84 | 85 | - from STM32L452 86 | ![stm32l452_transfer](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/d92ab5c4-b708-4b5f-8114-7a59914402c8) 87 | 88 | # Receiver 89 | ![STM32-Receive](https://user-images.githubusercontent.com/6020549/75561089-c2984580-5a89-11ea-80d0-90a2af235b80.jpg) 90 | 91 | 92 | # Communication with Arduino-UNO 93 | You can use [this](https://github.com/coryjfowler/MCP_CAN_lib) library. 94 | 95 | # Communication with Arduino-DUE 96 | You can use [this](https://github.com/collin80/due_can) library. 97 | ![STM32F103-CAN-DUO](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/7bd4e8dd-77b2-4b25-9123-a3596a006e5f) 98 | 99 | # Communication with ESP8266 100 | You can use [this](https://github.com/coryjfowler/MCP_CAN_lib) library. 101 | ![STM32F103-CAN-ESP8266](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/b7046d49-7e4a-40b0-bbc4-7a55fc5dc427) 102 | 103 | # Communication with ESP32 104 | ESP-IDE has a CAN Network example. 105 | https://github.com/espressif/esp-idf/tree/master/examples/peripherals/twai/twai_network 106 | You can use [it](https://github.com/nopnop2002/esp-idf-candump). 107 | ![STM32F103-CAN-ESP32](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/fbf48367-d134-43f3-9891-ff714b38eb32) 108 | 109 | __Notes on ESP32 bitrate__ 110 | In ESP-IDF V4, CONFIG_ESP32_REV_MIN >= 2 means "halve communication speed". 111 | This is to support slower bitrates below 25Kbps. 112 | This fuature can be controlled by CONFIG_ESP32_REV_MIN. 113 | This fuature is enabled when CONFIG_ESP32_REV_MIN >= 2. 114 | See [here](https://www.esp32.com/viewtopic.php?t=15581) for detail. 115 | This specification is not available in ESP-IDF V5. 116 | 117 | 118 | # Communication with Raspberry Pi 119 | Edit /boot/config.txt and reboot. 120 | ``` 121 | dtparam=spi=on 122 | dtoverlay=mcp2515-can0,oscillator=16000000,interrupt=25 123 | dtoverlay=spi-bcm2835-overlay 124 | ``` 125 | 126 | 127 | Make sure the device is ready after reboot. 128 | ``` 129 | $ dmesg | grep mcp251x 130 | [ 19.992025] mcp251x spi0.0 can0: MCP2515 successfully initialized. 131 | 132 | $ ls /sys/bus/spi/devices/spi0.0 133 | driver modalias net of_node power statistics subsystem uevent 134 | 135 | $ ls /sys/bus/spi/devices/spi0.0/net 136 | can0 137 | 138 | $ sudo /sbin/ip link set can0 up type can bitrate 1000000 139 | 140 | $ dmesg | grep can0 141 | [ 19.992025] mcp251x spi0.0 can0: MCP2515 successfully initialized. 142 | [ 1309.525795] IPv6: ADDRCONF(NETDEV_CHANGE): can0: link becomes ready 143 | 144 | $ sudo ifconfig can0 145 | can0: flags=193 mtu 16 146 | unspec 00-00-00-00-00-00-00-00-00-00-00-00-00-00-00-00 txqueuelen 10 (UNSPEC) 147 | RX packets 0 bytes 0 (0.0 B) 148 | RX errors 0 dropped 0 overruns 0 frame 0 149 | TX packets 0 bytes 0 (0.0 B) 150 | TX errors 0 dropped 0 overruns 0 carrier 0 collisions 0 151 | 152 | $ sudo ifconfig can0 txqueuelen 1000 153 | ``` 154 | 155 | Install can-utils. 156 | ``` 157 | $ sudo apt-get install can-utils 158 | 159 | # Receiver 160 | $ candump can0 161 | 162 | # Transmitter 163 | $ cansend can0 123#11223344AABBCCDD 164 | ``` 165 | 166 | ![STM32F103-CAN-RPI](https://github.com/nopnop2002/Arduino-STM32-CAN/assets/6020549/bebd8b14-5565-4cc8-b6ba-82e23c1ef14c) 167 | 168 | # Troubleshooting 169 | There is a module of SN65HVD230 like this. 170 | ![SN65HVD230-1](https://user-images.githubusercontent.com/6020549/80897499-4d204e00-8d34-11ea-80c9-3dc41b1addab.JPG) 171 | 172 | There is a __120 ohms__ terminating resistor on the left side. 173 | ![SN65HVD230-22](https://user-images.githubusercontent.com/6020549/89281044-74185400-d684-11ea-9f55-830e0e9e6424.JPG) 174 | 175 | A transmission fail will occur. 176 | ![SendFail](https://user-images.githubusercontent.com/6020549/80897131-98d0f880-8d30-11ea-96b6-05e50ac740a3.jpg) 177 | 178 | I have removed the terminating resistor. 179 | And I used a external resistance of __150 ohms__. 180 | A transmission fail is fixed. 181 | ![SN65HVD230-33](https://user-images.githubusercontent.com/6020549/89280710-f7857580-d683-11ea-9b36-12e36910e7d9.JPG) 182 | 183 | If the transmission fails, these are the possible causes. 184 | - There is no receiving app on CanBus. 185 | - The speed does not match the receiver. 186 | - There is no terminating resistor on the CanBus. 187 | - There are three terminating resistors on the CanBus. 188 | - The resistance value of the terminating resistor is incorrect. 189 | - 3.3V transceiver and 5V transceiver are mixed. 190 | Use +1V Ground Shift using Split Termination. 191 | Check out Figure 7 [here](http://www.ti.com/lit/an/slla337/slla337.pdf). 192 | - Stub length in CAN bus is too long. 193 | ISO11898 Standard specifies a maximum bus length of 40m and maximum stub length of 0.3m at 1Mbps. 194 | See [here](https://e2e.ti.com/support/interface-group/interface/f/interface-forum/378932/iso1050-can-bus-stub-length). 195 | - Noise effects. 196 | It occurs when the wire cable is thin or wire cable is crossing. 197 | It is preferable to use wires that are noise-proof. 198 | 199 | I changed the single wire to twisted wire. It's fixed. 200 | ![CAN-SendFail](https://user-images.githubusercontent.com/6020549/185782335-66d1f534-5220-40a2-aee9-5d4a438c59a0.jpg) 201 | 202 | # Using PlatformIO 203 | Arduino-IDE only supports ST-LINK V2.1 adapters, but OpenOCD used by PlatformIO supports both V2.0 and V2.1. 204 | With ST-LINK, there is no need to change boot mode when writing firmware. 205 | PlatformIO allows you to use cheap Chinese ST-LINK adapters like this one. 206 | You can get it at a low price (about $2). 207 | ![ST-LINK-1](https://user-images.githubusercontent.com/6020549/221345715-b86e0a93-bdf4-46dd-a9b9-c05a27b042a4.JPG) 208 | ![ST-LINK-2](https://user-images.githubusercontent.com/6020549/221345711-7749b557-d55f-442f-8390-3632c63d5239.JPG) 209 | 210 | Connect the ST-LINK adapter and the STM32 development board as follows. 211 | |ST-LINK|STM32| 212 | |:---|:---| 213 | |3.3V|3.3V| 214 | |GND|GND| 215 | |SWDIO|SWIO(=PA13)| 216 | |SWCLK|SWCLK(=PA14)| 217 | 218 | ![platformio-2](https://user-images.githubusercontent.com/6020549/221346022-c199e146-2f17-4021-871c-5e1b502ffde7.JPG) 219 | 220 | # Advanced use of BxCan 221 | Skip if you don't need advanced usage. 222 | Advanced usage has different purposes depending on the requirements of the application. 223 | So I can't give you a concrete code example. 224 | 225 | ### About transmission 226 | STM32 has three transmit(TX) mailboxes. 227 | Three TX mailboxes are provided to the software for setting up messages. 228 | This sample code uses only the first TX mailbox and blocks the application until the send is successfully completed. 229 | There are two ways to know if the send was successful without blocking. 230 | Read the reference manual carefully and modify the source code as needed. 231 | 232 | - Uses a transmission completion interrupt 233 | You need to set the CAN interrupt enable register (CAN_IER) appropriately. 234 | 235 | - Check TX mailbox space before sending 236 | An application can store transmission requests in three TX mailboxes simultaneously. 237 | When there are requests for multiple TX mailboxes, the transmission scheduler decides which mailbox has to be transmitted first. 238 | You can specify which mailbox has priority using the CAN Master Control Register (CAN_MCR). 239 | You can tell if a mailbox is free using the CAN Transmit Status Register (CAN_TSR). 240 | When all three TX mailboxes are pending, no new messages can be sent until the transmit is canceled or completed. 241 | When all three TX mailboxes are pending, you can choose to cancel the pending transmission, wait until the transmission is complete, or give up on the new transmission. 242 | This choice depends on your application requirements. 243 | When all three TX mailboxes are pending for a long time, it could be a transmission error. 244 | CAN Error Status Register (CAN_ESR) should be checked. 245 | 246 | ### About reception 247 | STM32 has two receive(RX) mailboxes. 248 | Each RX Mailbox allows access to a 3-level depth FIFO, the access being offered only to the oldest received message in the FIFO. 249 | This sample code uses only the first RX mailbox. 250 | The RX mailbox is closely related to the receive filter settings. 251 | By properly setting the receive filter, you can sort the received messages into two RX Mailboxes. 252 | For example, high priority messages can be stored in the first RX MailBox and low priority messages can be stored in the second RX MailBox. 253 | Read the reference manual carefully and modify the source code as needed. 254 | 255 | # Reference 256 | 257 | https://github.com/nopnop2002/Robotell-USB-CAN-Python 258 | 259 | https://github.com/nopnop2002/esp-idf-CANBus-Monitor 260 | 261 | 262 | -------------------------------------------------------------------------------- /canard/README.md: -------------------------------------------------------------------------------- 1 | # canard 2 | I forked from [here](https://github.com/geosmall/UAVCAN-for-STM32-Arduino). 3 | 4 | # Changes from the original 5 | 6 | - Communicate with other libraries 7 | The original supports transmission of 8 bytes or more, but is not compatible with other libraries. 8 | This example can only transfer up to 8 bytes of data, but can communicate with other libraries. 9 | 10 | - Support STM32F405/407 11 | 12 | # API 13 | - Initialize Hardware 14 | transfer_id = CAN_HW_Init(uint8_t serial); 15 | 16 | - Initialize Library 17 | void initCanard(uint32_t speed); 18 | 19 | - Send CAN Packet from queue 20 | void sendCanard(void); 21 | 22 | - Receive CAN Packet 23 | bool receiveCanard(uint32_t * can_id, uint8_t * payload, uint8_t * payload_size); 24 | 25 | - Push CAN Packet to TX queue 26 | void queueCanard(uint32_t can_id, uint8_t * payload, uint8_t payload_size); 27 | 28 | -------------------------------------------------------------------------------- /canard/example/build_opt.h: -------------------------------------------------------------------------------- 1 | -DUSE_FULL_LL_DRIVER 2 | -DHAL_CAN_MODULE_ENABLED 3 | -------------------------------------------------------------------------------- /canard/example/can.c: -------------------------------------------------------------------------------- 1 | #include "stm32_def.h" 2 | 3 | //void CAN_HW_Init(void) { 4 | uint32_t CAN_HW_Init(uint8_t serial) { 5 | 6 | GPIO_InitTypeDef GPIO_InitStruct; 7 | 8 | // GPIO Ports Clock Enable 9 | __HAL_RCC_GPIOA_CLK_ENABLE(); 10 | 11 | // CAN1 clock enable 12 | __HAL_RCC_CAN1_CLK_ENABLE(); 13 | 14 | // CAN GPIO Configuration 15 | // PA11 ------> CAN_RX 16 | // PA12 ------> CAN_TX 17 | 18 | #if defined (STM32F103x6) || defined (STM32F103xB) 19 | printstr("Board is STM32F103x6/F103xB"); 20 | println(); 21 | GPIO_InitStruct.Pin = GPIO_PIN_11; 22 | GPIO_InitStruct.Mode = GPIO_MODE_INPUT; 23 | GPIO_InitStruct.Pull = GPIO_NOPULL; 24 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 25 | 26 | GPIO_InitStruct.Pin = GPIO_PIN_12; 27 | GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; 28 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; 29 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 30 | return 0x103 << 8 + serial; // 0x103XX 31 | 32 | #elif defined (STM32F303x8) || defined (STM32F303xC) || defined (STM32F303xE) 33 | printstr("Board is STM32F303x8/F303xC/F303xE"); 34 | println(); 35 | GPIO_InitStruct.Pin = GPIO_PIN_11 | GPIO_PIN_12; 36 | GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; 37 | GPIO_InitStruct.Pull = GPIO_NOPULL; 38 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; 39 | GPIO_InitStruct.Alternate = GPIO_AF9_CAN; 40 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 41 | return 0x303 << 8 + serial; // 0x303XX 42 | 43 | #elif defined (STM32F405xx) || defined (STM32F415xx) || defined (STM32F407xx) || defined (STM32F417xx) 44 | printstr("Board is STM32F405xx/F415xx/F407xx/417xx"); 45 | println(); 46 | GPIO_InitStruct.Pin = GPIO_PIN_11 | GPIO_PIN_12; 47 | GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; 48 | GPIO_InitStruct.Pull = GPIO_NOPULL; 49 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; 50 | GPIO_InitStruct.Alternate = GPIO_AF9_CAN1; 51 | HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); 52 | return 0x405 << 8 + serial; // 0x405XX 53 | 54 | #else 55 | #error "Warning untested processor variant" 56 | #endif 57 | 58 | } 59 | -------------------------------------------------------------------------------- /canard/example/example.ino: -------------------------------------------------------------------------------- 1 | 2 | extern "C" void printbegin(int baudrate) { 3 | Serial.begin(baudrate); 4 | } 5 | 6 | 7 | extern "C" void println(void) { 8 | Serial.println(); 9 | } 10 | 11 | extern "C" void printstr(char* str) { 12 | Serial.print(str); 13 | } 14 | 15 | extern "C" void printint(uint32_t num) { 16 | Serial.print(num, HEX); 17 | } 18 | 19 | extern "C" void printframe(uint32_t id, uint8_t data_len, uint8_t * data) { 20 | if (id > 0x7ff) { 21 | Serial.print("Extended ID: 0x"); 22 | if (id < 0x10000000) Serial.print("0"); 23 | if (id < 0x1000000) Serial.print("00"); 24 | if (id < 0x100000) Serial.print("000"); 25 | if (id < 0x10000) Serial.print("0000"); 26 | Serial.print(id, HEX); 27 | } else { 28 | Serial.print("Standard ID: 0x"); 29 | if (id < 0x100) Serial.print("0"); 30 | if (id < 0x10) Serial.print("00"); 31 | Serial.print(id, HEX); 32 | Serial.print(" "); 33 | } 34 | Serial.print(" DLC: "); 35 | Serial.print(data_len); 36 | Serial.print(" Data: "); 37 | for(int i=0;i= interval) { 44 | previousMillis = currentMillis; 45 | digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); 46 | //can_id = 0x12345678; 47 | for(int i=0;i<8;i++){ 48 | payload[i] = i; 49 | } 50 | queueCanard(transfer_id, payload, 8); 51 | } 52 | } 53 | -------------------------------------------------------------------------------- /canard/example/src/libcanard/canard_internals.h: -------------------------------------------------------------------------------- 1 | /* 2 | * The MIT License (MIT) 3 | * 4 | * Copyright (c) 2016-2017 UAVCAN Team 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to deal 8 | * in the Software without restriction, including without limitation the rights 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | * copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in all 14 | * copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | * SOFTWARE. 23 | * 24 | * Contributors: https://github.com/UAVCAN/libcanard/contributors 25 | */ 26 | 27 | /* 28 | * This file holds function declarations that expose the library's internal definitions for unit testing. 29 | * It is NOT part of the library's API and should not even be looked at by the user. 30 | */ 31 | 32 | #ifndef CANARD_INTERNALS_H 33 | #define CANARD_INTERNALS_H 34 | 35 | #include "canard.h" 36 | 37 | #ifdef __cplusplus 38 | extern "C" { 39 | #endif 40 | 41 | /// This macro is needed only for testing and development. Do not redefine this in production. 42 | #ifndef CANARD_INTERNAL 43 | # define CANARD_INTERNAL static 44 | #endif 45 | 46 | 47 | CANARD_INTERNAL CanardRxState* traverseRxStates(CanardInstance* ins, 48 | uint32_t transfer_descriptor); 49 | 50 | CANARD_INTERNAL CanardRxState* createRxState(CanardPoolAllocator* allocator, 51 | uint32_t transfer_descriptor); 52 | 53 | CANARD_INTERNAL CanardRxState* prependRxState(CanardInstance* ins, 54 | uint32_t transfer_descriptor); 55 | 56 | CANARD_INTERNAL CanardRxState* findRxState(CanardRxState* state, 57 | uint32_t transfer_descriptor); 58 | 59 | CANARD_INTERNAL int16_t bufferBlockPushBytes(CanardPoolAllocator* allocator, 60 | CanardRxState* state, 61 | const uint8_t* data, 62 | uint8_t data_len); 63 | 64 | CANARD_INTERNAL CanardBufferBlock* createBufferBlock(CanardPoolAllocator* allocator); 65 | 66 | CANARD_INTERNAL CanardTransferType extractTransferType(uint32_t id); 67 | 68 | CANARD_INTERNAL uint16_t extractDataType(uint32_t id); 69 | 70 | CANARD_INTERNAL void pushTxQueue(CanardInstance* ins, 71 | CanardTxQueueItem* item); 72 | 73 | CANARD_INTERNAL bool isPriorityHigher(uint32_t self, 74 | uint32_t other); 75 | 76 | CANARD_INTERNAL CanardTxQueueItem* createTxItem(CanardPoolAllocator* allocator); 77 | 78 | CANARD_INTERNAL void prepareForNextTransfer(CanardRxState* state); 79 | 80 | CANARD_INTERNAL int16_t computeTransferIDForwardDistance(uint8_t a, 81 | uint8_t b); 82 | 83 | CANARD_INTERNAL void incrementTransferID(uint8_t* transfer_id); 84 | 85 | CANARD_INTERNAL uint64_t releaseStatePayload(CanardInstance* ins, 86 | CanardRxState* rxstate); 87 | 88 | /// Returns the number of frames enqueued 89 | CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins, 90 | uint32_t can_id, 91 | uint8_t* transfer_id, 92 | uint16_t crc, 93 | const uint8_t* payload, 94 | uint16_t payload_len); 95 | 96 | CANARD_INTERNAL void copyBitArray(const uint8_t* src, 97 | uint32_t src_offset, 98 | uint32_t src_len, 99 | uint8_t* dst, 100 | uint32_t dst_offset); 101 | 102 | /** 103 | * Moves specified bits from the scattered transfer storage to a specified contiguous buffer. 104 | * Returns the number of bits copied, or negated error code. 105 | */ 106 | CANARD_INTERNAL int16_t descatterTransferPayload(const CanardRxTransfer* transfer, 107 | uint32_t bit_offset, 108 | uint8_t bit_length, 109 | void* output); 110 | 111 | CANARD_INTERNAL bool isBigEndian(void); 112 | 113 | CANARD_INTERNAL void swapByteOrder(void* data, unsigned size); 114 | 115 | /* 116 | * Transfer CRC 117 | */ 118 | CANARD_INTERNAL uint16_t crcAddByte(uint16_t crc_val, 119 | uint8_t byte); 120 | 121 | CANARD_INTERNAL uint16_t crcAddSignature(uint16_t crc_val, 122 | uint64_t data_type_signature); 123 | 124 | CANARD_INTERNAL uint16_t crcAdd(uint16_t crc_val, 125 | const uint8_t* bytes, 126 | size_t len); 127 | 128 | /** 129 | * Inits a memory allocator. 130 | * 131 | * @param [in] allocator The memory allocator to initialize. 132 | * @param [in] buf The buffer used by the memory allocator. 133 | * @param [in] buf_len The number of blocks in buf. 134 | */ 135 | CANARD_INTERNAL void initPoolAllocator(CanardPoolAllocator* allocator, 136 | CanardPoolAllocatorBlock* buf, 137 | uint16_t buf_len); 138 | 139 | /** 140 | * Allocates a block from the given pool allocator. 141 | */ 142 | CANARD_INTERNAL void* allocateBlock(CanardPoolAllocator* allocator); 143 | 144 | /** 145 | * Frees a memory block previously returned by canardAllocateBlock. 146 | */ 147 | CANARD_INTERNAL void freeBlock(CanardPoolAllocator* allocator, 148 | void* p); 149 | 150 | 151 | #ifdef __cplusplus 152 | } 153 | #endif 154 | #endif 155 | -------------------------------------------------------------------------------- /canard/example/src/libcanard/drivers/README.md: -------------------------------------------------------------------------------- 1 | # Libcanard Drivers 2 | 3 | This directory contains implementations of platform-specific components for Libcanard. 4 | Each sub-directory contains (or should contain) a dedicated README file with driver specific documentation. 5 | 6 | ## Porting Guide 7 | 8 | Existing drivers should be used as a reference for implementation of one's own custom drivers. 9 | Libcanard does not interact with the underlying platform drivers directly, 10 | but does so via the application. 11 | Therefore, there is no need in a dedicated porting guide. 12 | This is unlike Libuavcan, which is more complex and does have a well-defined interface between 13 | the library and the platform. 14 | 15 | ![libuavcan vs libcanard arch](libuavcan_vs_libcanard_arch.png) 16 | -------------------------------------------------------------------------------- /canard/example/src/libcanard/drivers/libuavcan_vs_libcanard_arch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nopnop2002/Arduino-STM32-CAN/3915696e04ee8d2eb3eefd08622cbbe5211d2155/canard/example/src/libcanard/drivers/libuavcan_vs_libcanard_arch.png -------------------------------------------------------------------------------- /canard/example/src/libcanard/drivers/stm32/_internal_bxcan.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 UAVCAN Team 3 | * 4 | * Distributed under the MIT License, available in the file LICENSE. 5 | * 6 | * Author: Pavel Kirienko 7 | */ 8 | 9 | #ifndef CANARD_STM32_BXCAN_H 10 | #define CANARD_STM32_BXCAN_H 11 | 12 | #include 13 | 14 | 15 | typedef struct 16 | { 17 | volatile uint32_t TIR; 18 | volatile uint32_t TDTR; 19 | volatile uint32_t TDLR; 20 | volatile uint32_t TDHR; 21 | } CanardSTM32TxMailboxType; 22 | 23 | typedef struct 24 | { 25 | volatile uint32_t RIR; 26 | volatile uint32_t RDTR; 27 | volatile uint32_t RDLR; 28 | volatile uint32_t RDHR; 29 | } CanardSTM32RxMailboxType; 30 | 31 | typedef struct 32 | { 33 | volatile uint32_t FR1; 34 | volatile uint32_t FR2; 35 | } CanardSTM32FilterRegisterType; 36 | 37 | typedef struct 38 | { 39 | volatile uint32_t MCR; ///< CAN master control register 0x000 40 | volatile uint32_t MSR; ///< CAN master status register 0x004 41 | volatile uint32_t TSR; ///< CAN transmit status register 0x008 42 | volatile uint32_t RF0R; ///< CAN receive FIFO 0 register 0x00C 43 | volatile uint32_t RF1R; ///< CAN receive FIFO 1 register 0x010 44 | volatile uint32_t IER; ///< CAN interrupt enable register 0x014 45 | volatile uint32_t ESR; ///< CAN error status register 0x018 46 | volatile uint32_t BTR; ///< CAN bit timing register 0x01C 47 | const uint32_t RESERVED0[88]; ///< Reserved 0x020-0x17F 48 | CanardSTM32TxMailboxType TxMailbox[3]; ///< CAN Tx MailBox 0x180-0x1AC 49 | CanardSTM32RxMailboxType RxMailbox[2]; ///< CAN FIFO MailBox 0x1B0-0x1CC 50 | const uint32_t RESERVED1[12]; ///< Reserved 0x1D0-0x1FF 51 | volatile uint32_t FMR; ///< CAN filter master register 0x200 52 | volatile uint32_t FM1R; ///< CAN filter mode register 0x204 53 | const uint32_t RESERVED2; ///< Reserved 0x208 54 | volatile uint32_t FS1R; ///< CAN filter scale register 0x20C 55 | const uint32_t RESERVED3; ///< Reserved 0x210 56 | volatile uint32_t FFA1R; ///< CAN filter FIFO assignment register 0x214 57 | const uint32_t RESERVED4; ///< Reserved 0x218 58 | volatile uint32_t FA1R; ///< CAN filter activation register 0x21C 59 | const uint32_t RESERVED5[8]; ///< Reserved 0x220-0x23F 60 | CanardSTM32FilterRegisterType FilterRegister[28]; ///< CAN Filter Register 0x240-0x31C 61 | } CanardSTM32CANType; 62 | 63 | /** 64 | * CANx instances 65 | */ 66 | #define CANARD_STM32_CAN1 ((volatile CanardSTM32CANType*)0x40006400U) 67 | #define CANARD_STM32_CAN2 ((volatile CanardSTM32CANType*)0x40006800U) 68 | 69 | // CAN master control register 70 | 71 | #define CANARD_STM32_CAN_MCR_INRQ (1U << 0U) // Bit 0: Initialization Request 72 | #define CANARD_STM32_CAN_MCR_SLEEP (1U << 1U) // Bit 1: Sleep Mode Request 73 | #define CANARD_STM32_CAN_MCR_TXFP (1U << 2U) // Bit 2: Transmit FIFO Priority 74 | #define CANARD_STM32_CAN_MCR_RFLM (1U << 3U) // Bit 3: Receive FIFO Locked Mode 75 | #define CANARD_STM32_CAN_MCR_NART (1U << 4U) // Bit 4: No Automatic Retransmission 76 | #define CANARD_STM32_CAN_MCR_AWUM (1U << 5U) // Bit 5: Automatic Wakeup Mode 77 | #define CANARD_STM32_CAN_MCR_ABOM (1U << 6U) // Bit 6: Automatic Bus-Off Management 78 | #define CANARD_STM32_CAN_MCR_TTCM (1U << 7U) // Bit 7: Time Triggered Communication Mode Enable 79 | #define CANARD_STM32_CAN_MCR_RESET (1U << 15U) // Bit 15: bxCAN software master reset 80 | #define CANARD_STM32_CAN_MCR_DBF (1U << 16U) // Bit 16: Debug freeze 81 | 82 | // CAN master status register 83 | 84 | #define CANARD_STM32_CAN_MSR_INAK (1U << 0U) // Bit 0: Initialization Acknowledge 85 | #define CANARD_STM32_CAN_MSR_SLAK (1U << 1U) // Bit 1: Sleep Acknowledge 86 | #define CANARD_STM32_CAN_MSR_ERRI (1U << 2U) // Bit 2: Error Interrupt 87 | #define CANARD_STM32_CAN_MSR_WKUI (1U << 3U) // Bit 3: Wakeup Interrupt 88 | #define CANARD_STM32_CAN_MSR_SLAKI (1U << 4U) // Bit 4: Sleep acknowledge interrupt 89 | #define CANARD_STM32_CAN_MSR_TXM (1U << 8U) // Bit 8: Transmit Mode 90 | #define CANARD_STM32_CAN_MSR_RXM (1U << 9U) // Bit 9: Receive Mode 91 | #define CANARD_STM32_CAN_MSR_SAMP (1U << 10U) // Bit 10: Last Sample Point 92 | #define CANARD_STM32_CAN_MSR_RX (1U << 11U) // Bit 11: CAN Rx Signal 93 | 94 | // CAN transmit status register 95 | 96 | #define CANARD_STM32_CAN_TSR_RQCP0 (1U << 0U) // Bit 0: Request Completed Mailbox 0 97 | #define CANARD_STM32_CAN_TSR_TXOK0 (1U << 1U) // Bit 1 : Transmission OK of Mailbox 0 98 | #define CANARD_STM32_CAN_TSR_ALST0 (1U << 2U) // Bit 2 : Arbitration Lost for Mailbox 0 99 | #define CANARD_STM32_CAN_TSR_TERR0 (1U << 3U) // Bit 3 : Transmission Error of Mailbox 0 100 | #define CANARD_STM32_CAN_TSR_ABRQ0 (1U << 7U) // Bit 7 : Abort Request for Mailbox 0 101 | #define CANARD_STM32_CAN_TSR_RQCP1 (1U << 8U) // Bit 8 : Request Completed Mailbox 1 102 | #define CANARD_STM32_CAN_TSR_TXOK1 (1U << 9U) // Bit 9 : Transmission OK of Mailbox 1 103 | #define CANARD_STM32_CAN_TSR_ALST1 (1U << 10U) // Bit 10 : Arbitration Lost for Mailbox 1 104 | #define CANARD_STM32_CAN_TSR_TERR1 (1U << 11U) // Bit 11 : Transmission Error of Mailbox 1 105 | #define CANARD_STM32_CAN_TSR_ABRQ1 (1U << 15U) // Bit 15 : Abort Request for Mailbox 1 106 | #define CANARD_STM32_CAN_TSR_RQCP2 (1U << 16U) // Bit 16 : Request Completed Mailbox 2 107 | #define CANARD_STM32_CAN_TSR_TXOK2 (1U << 17U) // Bit 17 : Transmission OK of Mailbox 2 108 | #define CANARD_STM32_CAN_TSR_ALST2 (1U << 18U) // Bit 18: Arbitration Lost for Mailbox 2 109 | #define CANARD_STM32_CAN_TSR_TERR2 (1U << 19U) // Bit 19: Transmission Error of Mailbox 2 110 | #define CANARD_STM32_CAN_TSR_ABRQ2 (1U << 23U) // Bit 23: Abort Request for Mailbox 2 111 | #define CANARD_STM32_CAN_TSR_CODE_SHIFT (24U) // Bits 25-24: Mailbox Code 112 | #define CANARD_STM32_CAN_TSR_CODE_MASK (3U << CANARD_STM32_CAN_TSR_CODE_SHIFT) 113 | #define CANARD_STM32_CAN_TSR_TME0 (1U << 26U) // Bit 26: Transmit Mailbox 0 Empty 114 | #define CANARD_STM32_CAN_TSR_TME1 (1U << 27U) // Bit 27: Transmit Mailbox 1 Empty 115 | #define CANARD_STM32_CAN_TSR_TME2 (1U << 28U) // Bit 28: Transmit Mailbox 2 Empty 116 | #define CANARD_STM32_CAN_TSR_LOW0 (1U << 29U) // Bit 29: Lowest Priority Flag for Mailbox 0 117 | #define CANARD_STM32_CAN_TSR_LOW1 (1U << 30U) // Bit 30: Lowest Priority Flag for Mailbox 1 118 | #define CANARD_STM32_CAN_TSR_LOW2 (1U << 31U) // Bit 31: Lowest Priority Flag for Mailbox 2 119 | 120 | // CAN receive FIFO 0/1 registers 121 | 122 | #define CANARD_STM32_CAN_RFR_FMP_SHIFT (0U) // Bits 1-0: FIFO Message Pending 123 | #define CANARD_STM32_CAN_RFR_FMP_MASK (3U << CANARD_STM32_CAN_RFR_FMP_SHIFT) 124 | #define CANARD_STM32_CAN_RFR_FULL (1U << 3U) // Bit 3: FIFO 0 Full 125 | #define CANARD_STM32_CAN_RFR_FOVR (1U << 4U) // Bit 4: FIFO 0 Overrun 126 | #define CANARD_STM32_CAN_RFR_RFOM (1U << 5U) // Bit 5: Release FIFO 0 Output Mailbox 127 | 128 | // CAN interrupt enable register 129 | 130 | #define CANARD_STM32_CAN_IER_TMEIE (1U << 0U) // Bit 0: Transmit Mailbox Empty Interrupt Enable 131 | #define CANARD_STM32_CAN_IER_FMPIE0 (1U << 1U) // Bit 1: FIFO Message Pending Interrupt Enable 132 | #define CANARD_STM32_CAN_IER_FFIE0 (1U << 2U) // Bit 2: FIFO Full Interrupt Enable 133 | #define CANARD_STM32_CAN_IER_FOVIE0 (1U << 3U) // Bit 3: FIFO Overrun Interrupt Enable 134 | #define CANARD_STM32_CAN_IER_FMPIE1 (1U << 4U) // Bit 4: FIFO Message Pending Interrupt Enable 135 | #define CANARD_STM32_CAN_IER_FFIE1 (1U << 5U) // Bit 5: FIFO Full Interrupt Enable 136 | #define CANARD_STM32_CAN_IER_FOVIE1 (1U << 6U) // Bit 6: FIFO Overrun Interrupt Enable 137 | #define CANARD_STM32_CAN_IER_EWGIE (1U << 8U) // Bit 8: Error Warning Interrupt Enable 138 | #define CANARD_STM32_CAN_IER_EPVIE (1U << 9U) // Bit 9: Error Passive Interrupt Enable 139 | #define CANARD_STM32_CAN_IER_BOFIE (1U << 10U) // Bit 10: Bus-Off Interrupt Enable 140 | #define CANARD_STM32_CAN_IER_LECIE (1U << 11U) // Bit 11: Last Error Code Interrupt Enable 141 | #define CANARD_STM32_CAN_IER_ERRIE (1U << 15U) // Bit 15: Error Interrupt Enable 142 | #define CANARD_STM32_CAN_IER_WKUIE (1U << 16U) // Bit 16: Wakeup Interrupt Enable 143 | #define CANARD_STM32_CAN_IER_SLKIE (1U << 17U) // Bit 17: Sleep Interrupt Enable 144 | 145 | // CAN error status register 146 | 147 | #define CANARD_STM32_CAN_ESR_EWGF (1U << 0U) // Bit 0: Error Warning Flag 148 | #define CANARD_STM32_CAN_ESR_EPVF (1U << 1U) // Bit 1: Error Passive Flag 149 | #define CANARD_STM32_CAN_ESR_BOFF (1U << 2U) // Bit 2: Bus-Off Flag 150 | #define CANARD_STM32_CAN_ESR_LEC_SHIFT (4U) // Bits 6-4: Last Error Code 151 | #define CANARD_STM32_CAN_ESR_LEC_MASK (7U << CANARD_STM32_CAN_ESR_LEC_SHIFT) 152 | #define CANARD_STM32_CAN_ESR_NOERROR (0U << CANARD_STM32_CAN_ESR_LEC_SHIFT) // 000: No Error 153 | #define CANARD_STM32_CAN_ESR_STUFFERROR (1U << CANARD_STM32_CAN_ESR_LEC_SHIFT) // 001: Stuff Error 154 | #define CANARD_STM32_CAN_ESR_FORMERROR (2U << CANARD_STM32_CAN_ESR_LEC_SHIFT) // 010: Form Error 155 | #define CANARD_STM32_CAN_ESR_ACKERROR (3U << CANARD_STM32_CAN_ESR_LEC_SHIFT) // 011: Acknowledgment Error 156 | #define CANARD_STM32_CAN_ESR_BRECERROR (4U << CANARD_STM32_CAN_ESR_LEC_SHIFT) // 100: Bit recessive Error 157 | #define CANARD_STM32_CAN_ESR_BDOMERROR (5U << CANARD_STM32_CAN_ESR_LEC_SHIFT) // 101: Bit dominant Error 158 | #define CANARD_STM32_CAN_ESR_CRCERRPR (6U << CANARD_STM32_CAN_ESR_LEC_SHIFT) // 110: CRC Error 159 | #define CANARD_STM32_CAN_ESR_SWERROR (7U << CANARD_STM32_CAN_ESR_LEC_SHIFT) // 111: Set by software 160 | #define CANARD_STM32_CAN_ESR_TEC_SHIFT (16U) // Bits 23-16: LS byte of the 9-bit Transmit Error Counter 161 | #define CANARD_STM32_CAN_ESR_TEC_MASK (0xFFU << CANARD_STM32_CAN_ESR_TEC_SHIFT) 162 | #define CANARD_STM32_CAN_ESR_REC_SHIFT (24U) // Bits 31-24: Receive Error Counter 163 | #define CANARD_STM32_CAN_ESR_REC_MASK (0xFFU << CANARD_STM32_CAN_ESR_REC_SHIFT) 164 | 165 | // CAN bit timing register 166 | 167 | #define CANARD_STM32_CAN_BTR_BRP_SHIFT (0U) // Bits 9-0: Baud Rate Prescaler 168 | #define CANARD_STM32_CAN_BTR_BRP_MASK (0x03FFU << CANARD_STM32_CAN_BTR_BRP_SHIFT) 169 | #define CANARD_STM32_CAN_BTR_TS1_SHIFT (16U) // Bits 19-16: Time Segment 1 170 | #define CANARD_STM32_CAN_BTR_TS1_MASK (0x0FU << CANARD_STM32_CAN_BTR_TS1_SHIFT) 171 | #define CANARD_STM32_CAN_BTR_TS2_SHIFT (20U) // Bits 22-20: Time Segment 2 172 | #define CANARD_STM32_CAN_BTR_TS2_MASK (7U << CANARD_STM32_CAN_BTR_TS2_SHIFT) 173 | #define CANARD_STM32_CAN_BTR_SJW_SHIFT (24U) // Bits 25-24: Resynchronization Jump Width 174 | #define CANARD_STM32_CAN_BTR_SJW_MASK (3U << CANARD_STM32_CAN_BTR_SJW_SHIFT) 175 | #define CANARD_STM32_CAN_BTR_LBKM (1U << 30U) // Bit 30: Loop Back Mode (Debug); 176 | #define CANARD_STM32_CAN_BTR_SILM (1U << 31U) // Bit 31: Silent Mode (Debug); 177 | 178 | #define CANARD_STM32_CAN_BTR_BRP_MAX (1024U) // Maximum BTR value (without decrement); 179 | #define CANARD_STM32_CAN_BTR_TSEG1_MAX (16U) // Maximum TSEG1 value (without decrement); 180 | #define CANARD_STM32_CAN_BTR_TSEG2_MAX (8U) // Maximum TSEG2 value (without decrement); 181 | 182 | // TX mailbox identifier register 183 | 184 | #define CANARD_STM32_CAN_TIR_TXRQ (1U << 0U) // Bit 0: Transmit Mailbox Request 185 | #define CANARD_STM32_CAN_TIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 186 | #define CANARD_STM32_CAN_TIR_IDE (1U << 2U) // Bit 2: Identifier Extension 187 | #define CANARD_STM32_CAN_TIR_EXID_SHIFT (3U) // Bit 3-31: Extended Identifier 188 | #define CANARD_STM32_CAN_TIR_EXID_MASK (0x1FFFFFFFU << CANARD_STM32_CAN_TIR_EXID_SHIFT) 189 | #define CANARD_STM32_CAN_TIR_STID_SHIFT (21U) // Bits 21-31: Standard Identifier 190 | #define CANARD_STM32_CAN_TIR_STID_MASK (0x07FFU << CANARD_STM32_CAN_TIR_STID_SHIFT) 191 | 192 | // Mailbox data length control and time stamp register 193 | 194 | #define CANARD_STM32_CAN_TDTR_DLC_SHIFT (0U) // Bits 3:0: Data Length Code 195 | #define CANARD_STM32_CAN_TDTR_DLC_MASK (0x0FU << CANARD_STM32_CAN_TDTR_DLC_SHIFT) 196 | #define CANARD_STM32_CAN_TDTR_TGT (1U << 8U) // Bit 8: Transmit Global Time 197 | #define CANARD_STM32_CAN_TDTR_TIME_SHIFT (16U) // Bits 31:16: Message Time Stamp 198 | #define CANARD_STM32_CAN_TDTR_TIME_MASK (0xFFFFU << CANARD_STM32_CAN_TDTR_TIME_SHIFT) 199 | 200 | // Rx FIFO mailbox identifier register 201 | 202 | #define CANARD_STM32_CAN_RIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 203 | #define CANARD_STM32_CAN_RIR_IDE (1U << 2U) // Bit 2: Identifier Extension 204 | #define CANARD_STM32_CAN_RIR_EXID_SHIFT (3U) // Bit 3-31: Extended Identifier 205 | #define CANARD_STM32_CAN_RIR_EXID_MASK (0x1FFFFFFFU << CANARD_STM32_CAN_RIR_EXID_SHIFT) 206 | #define CANARD_STM32_CAN_RIR_STID_SHIFT (21U) // Bits 21-31: Standard Identifier 207 | #define CANARD_STM32_CAN_RIR_STID_MASK (0x07FFU << CANARD_STM32_CAN_RIR_STID_SHIFT) 208 | 209 | // Receive FIFO mailbox data length control and time stamp register 210 | 211 | #define CANARD_STM32_CAN_RDTR_DLC_SHIFT (0U) // Bits 3:0: Data Length Code 212 | #define CANARD_STM32_CAN_RDTR_DLC_MASK (0x0FU << CANARD_STM32_CAN_RDTR_DLC_SHIFT) 213 | #define CANARD_STM32_CAN_RDTR_FM_SHIFT (8U) // Bits 15-8: Filter Match Index 214 | #define CANARD_STM32_CAN_RDTR_FM_MASK (0xFFU << CANARD_STM32_CAN_RDTR_FM_SHIFT) 215 | #define CANARD_STM32_CAN_RDTR_TIME_SHIFT (16U) // Bits 31:16: Message Time Stamp 216 | #define CANARD_STM32_CAN_RDTR_TIME_MASK (0xFFFFU << CANARD_STM32_CAN_RDTR_TIME_SHIFT) 217 | 218 | // CAN filter master register 219 | 220 | #define CANARD_STM32_CAN_FMR_FINIT (1U << 0U) // Bit 0: Filter Init Mode 221 | 222 | #endif // CANARD_STM32_BXCAN_H 223 | -------------------------------------------------------------------------------- /canard/example/src/libcanard/drivers/stm32/canard_stm32.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 UAVCAN Team 3 | * 4 | * Distributed under the MIT License, available in the file LICENSE. 5 | * 6 | * Author: Pavel Kirienko 7 | */ 8 | 9 | #include "Arduino.h" 10 | #include "canard_stm32.h" 11 | #include "_internal_bxcan.h" 12 | //#include "src/dwt_stm32_delay/dwt_stm32_delay.h" 13 | 14 | 15 | #if CANARD_STM32_USE_CAN2 16 | # define BXCAN CANARD_STM32_CAN2 17 | #else 18 | # define BXCAN CANARD_STM32_CAN1 19 | #endif 20 | 21 | /* 22 | * State variables 23 | */ 24 | static CanardSTM32Stats g_stats; 25 | 26 | static bool g_abort_tx_on_error = false; 27 | 28 | 29 | static bool isFramePriorityHigher(uint32_t a, uint32_t b) 30 | { 31 | const uint32_t clean_a = a & CANARD_CAN_EXT_ID_MASK; 32 | const uint32_t clean_b = b & CANARD_CAN_EXT_ID_MASK; 33 | 34 | /* 35 | * STD vs EXT - if 11 most significant bits are the same, EXT loses. 36 | */ 37 | const bool ext_a = (a & CANARD_CAN_FRAME_EFF) != 0; 38 | const bool ext_b = (b & CANARD_CAN_FRAME_EFF) != 0; 39 | if (ext_a != ext_b) 40 | { 41 | const uint32_t arb11_a = ext_a ? (clean_a >> 18U) : clean_a; 42 | const uint32_t arb11_b = ext_b ? (clean_b >> 18U) : clean_b; 43 | if (arb11_a != arb11_b) 44 | { 45 | return arb11_a < arb11_b; 46 | } 47 | else 48 | { 49 | return ext_b; 50 | } 51 | } 52 | 53 | /* 54 | * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses. 55 | */ 56 | const bool rtr_a = (a & CANARD_CAN_FRAME_RTR) != 0; 57 | const bool rtr_b = (b & CANARD_CAN_FRAME_RTR) != 0; 58 | if ((clean_a == clean_b) && (rtr_a != rtr_b)) 59 | { 60 | return rtr_b; 61 | } 62 | 63 | /* 64 | * Plain ID arbitration - greater value loses. 65 | */ 66 | return clean_a < clean_b; 67 | } 68 | 69 | /// Converts libcanard ID value into the bxCAN TX ID register format. 70 | static uint32_t convertFrameIDCanardToRegister(const uint32_t id) 71 | { 72 | uint32_t out = 0; 73 | 74 | if (id & CANARD_CAN_FRAME_EFF) 75 | { 76 | out = ((id & CANARD_CAN_EXT_ID_MASK) << 3U) | CANARD_STM32_CAN_TIR_IDE; 77 | } 78 | else 79 | { 80 | out = ((id & CANARD_CAN_STD_ID_MASK) << 21U); 81 | } 82 | 83 | if (id & CANARD_CAN_FRAME_RTR) 84 | { 85 | out |= CANARD_STM32_CAN_TIR_RTR; 86 | } 87 | 88 | return out; 89 | } 90 | 91 | /// Converts bxCAN TX/RX (sic! both RX/TX are supported) ID register value into the libcanard ID format. 92 | static uint32_t convertFrameIDRegisterToCanard(const uint32_t id) 93 | { 94 | #if (CANARD_STM32_CAN_TIR_RTR != CANARD_STM32_CAN_RIR_RTR) ||\ 95 | (CANARD_STM32_CAN_TIR_IDE != CANARD_STM32_CAN_RIR_IDE) 96 | # error "RIR bits do not match TIR bits, TIR --> libcanard conversion is not possible" 97 | #endif 98 | 99 | uint32_t out = 0; 100 | 101 | if ((id & CANARD_STM32_CAN_RIR_IDE) == 0) 102 | { 103 | out = (CANARD_CAN_STD_ID_MASK & (id >> 21U)); 104 | } 105 | else 106 | { 107 | //out = (CANARD_CAN_EXT_ID_MASK & (id >> 3U)) | CANARD_CAN_FRAME_EFF; 108 | out = (CANARD_CAN_EXT_ID_MASK & (id >> 3U)); 109 | } 110 | 111 | if ((id & CANARD_STM32_CAN_RIR_RTR) != 0) 112 | { 113 | out |= CANARD_CAN_FRAME_RTR; 114 | } 115 | 116 | return out; 117 | } 118 | 119 | 120 | static bool waitMSRINAKBitStateChange(volatile const CanardSTM32CANType* const bxcan, const bool target_state) 121 | { 122 | /** 123 | * A properly functioning bus will exhibit 11 consecutive recessive bits at the end of every correct transmission, 124 | * or while the bus is idle. The 11 consecutive recessive bits are made up of: 125 | * 1 bit - acknowledgement delimiter 126 | * 7 bit - end of frame bits 127 | * 3 bit - inter frame space 128 | * This adds up to 11; therefore, it is not really necessary to wait longer than a few frame TX intervals. 129 | */ 130 | static const uint16_t TimeoutMilliseconds = 1000; 131 | 132 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) 133 | { 134 | const bool state = (bxcan->MSR & CANARD_STM32_CAN_MSR_INAK) != 0; 135 | if (state == target_state) 136 | { 137 | return true; 138 | } 139 | 140 | // Sleep 1 millisecond 141 | // DWT_Delay_us(1000); 142 | delayMicroseconds(1000); 143 | } 144 | 145 | return false; 146 | } 147 | 148 | 149 | static void processErrorStatus(void) 150 | { 151 | /* 152 | * Aborting TX transmissions if abort on error was requested 153 | * Updating error counter 154 | */ 155 | const uint8_t lec = (uint8_t)((BXCAN->ESR & CANARD_STM32_CAN_ESR_LEC_MASK) >> CANARD_STM32_CAN_ESR_LEC_SHIFT); 156 | 157 | if (lec != 0) 158 | { 159 | BXCAN->ESR = 0; // This action does only affect the LEC bits, other bits are read only! 160 | g_stats.error_count++; 161 | 162 | // Abort pending transmissions if auto abort on error is enabled, or if we're in bus off mode 163 | if (g_abort_tx_on_error || (BXCAN->ESR & CANARD_STM32_CAN_ESR_BOFF)) 164 | { 165 | BXCAN->TSR = CANARD_STM32_CAN_TSR_ABRQ0 | CANARD_STM32_CAN_TSR_ABRQ1 | CANARD_STM32_CAN_TSR_ABRQ2; 166 | } 167 | } 168 | } 169 | 170 | 171 | int16_t canardSTM32Init(const CanardSTM32CANTimings* const timings, 172 | const CanardSTM32IfaceMode iface_mode) 173 | { 174 | /* 175 | * Paranoia time. 176 | */ 177 | if ((iface_mode != CanardSTM32IfaceModeNormal) && 178 | (iface_mode != CanardSTM32IfaceModeSilent) && 179 | (iface_mode != CanardSTM32IfaceModeAutomaticTxAbortOnError)) 180 | { 181 | return -CANARD_ERROR_INVALID_ARGUMENT; 182 | } 183 | 184 | if ((timings == NULL) || 185 | (timings->bit_rate_prescaler < 1) || (timings->bit_rate_prescaler > 1024) || 186 | (timings->max_resynchronization_jump_width < 1) || (timings->max_resynchronization_jump_width > 4) || 187 | (timings->bit_segment_1 < 1) || (timings->bit_segment_1 > 16) || 188 | (timings->bit_segment_2 < 1) || (timings->bit_segment_2 > 8)) 189 | { 190 | return -CANARD_ERROR_INVALID_ARGUMENT; 191 | } 192 | 193 | /* 194 | * Initial setup 195 | */ 196 | memset(&g_stats, 0, sizeof(g_stats)); 197 | 198 | g_abort_tx_on_error = (iface_mode == CanardSTM32IfaceModeAutomaticTxAbortOnError); 199 | 200 | #if CANARD_STM32_USE_CAN2 201 | // If we're using CAN2, we MUST initialize CAN1 first, because CAN2 is a slave to CAN1. 202 | CANARD_STM32_CAN1->IER = 0; // We need no interrupts 203 | CANARD_STM32_CAN1->MCR &= ~CANARD_STM32_CAN_MCR_SLEEP; // Exit sleep mode 204 | CANARD_STM32_CAN1->MCR |= CANARD_STM32_CAN_MCR_INRQ; // Request init 205 | 206 | if (!waitMSRINAKBitStateChange(CANARD_STM32_CAN1, true)) // Wait for synchronization 207 | { 208 | CANARD_STM32_CAN1->MCR = CANARD_STM32_CAN_MCR_RESET; 209 | return -CANARD_STM32_ERROR_MSR_INAK_NOT_SET; 210 | } 211 | // CAN1 will be left in the initialization mode forever, in this mode it does not affect the bus at all. 212 | #endif 213 | 214 | BXCAN->IER = 0; // We need no interrupts 215 | BXCAN->MCR &= ~CANARD_STM32_CAN_MCR_SLEEP; // Exit sleep mode 216 | BXCAN->MCR |= CANARD_STM32_CAN_MCR_INRQ; // Request init 217 | 218 | if (!waitMSRINAKBitStateChange(BXCAN, true)) // Wait for synchronization 219 | { 220 | BXCAN->MCR = CANARD_STM32_CAN_MCR_RESET; 221 | return -CANARD_STM32_ERROR_MSR_INAK_NOT_SET; 222 | } 223 | 224 | /* 225 | * Hardware initialization (the hardware has already confirmed initialization mode, see above) 226 | */ 227 | BXCAN->MCR = CANARD_STM32_CAN_MCR_ABOM | CANARD_STM32_CAN_MCR_AWUM | CANARD_STM32_CAN_MCR_INRQ; // RM page 648 228 | 229 | BXCAN->BTR = (((timings->max_resynchronization_jump_width - 1U) & 3U) << 24U) | 230 | (((timings->bit_segment_1 - 1U) & 15U) << 16U) | 231 | (((timings->bit_segment_2 - 1U) & 7U) << 20U) | 232 | ((timings->bit_rate_prescaler - 1U) & 1023U) | 233 | ((iface_mode == CanardSTM32IfaceModeSilent) ? CANARD_STM32_CAN_BTR_SILM : 0); 234 | 235 | CANARD_ASSERT(0 == BXCAN->IER); // Making sure the iterrupts are indeed disabled 236 | 237 | BXCAN->MCR &= ~CANARD_STM32_CAN_MCR_INRQ; // Leave init mode 238 | 239 | if (!waitMSRINAKBitStateChange(BXCAN, false)) 240 | { 241 | BXCAN->MCR = CANARD_STM32_CAN_MCR_RESET; 242 | return -CANARD_STM32_ERROR_MSR_INAK_NOT_CLEARED; 243 | } 244 | 245 | /* 246 | * Default filter configuration. Note that ALL filters are available ONLY via CAN1! 247 | * CAN2 filters are offset by 14. 248 | * We use 14 filters at most always which simplifies the code and ensures compatibility with all 249 | * MCU within the STM32 family. 250 | */ 251 | { 252 | uint32_t fmr = CANARD_STM32_CAN1->FMR & 0xFFFFC0F1U; 253 | fmr |= CANARD_STM32_NUM_ACCEPTANCE_FILTERS << 8U; // CAN2 start bank = 14 (if CAN2 is present) 254 | CANARD_STM32_CAN1->FMR = fmr | CANARD_STM32_CAN_FMR_FINIT; 255 | } 256 | 257 | CANARD_ASSERT(((CANARD_STM32_CAN1->FMR >> 8U) & 0x3FU) == CANARD_STM32_NUM_ACCEPTANCE_FILTERS); 258 | 259 | CANARD_STM32_CAN1->FM1R = 0; // Indentifier Mask mode 260 | CANARD_STM32_CAN1->FS1R = 0x0FFFFFFF; // All 32-bit 261 | 262 | // Filters are alternating between FIFO0 and FIFO1 in order to equalize the load. 263 | // This will cause occasional priority inversion and frame reordering on reception, 264 | // but that is acceptable for UAVCAN, and a majority of other protocols will tolerate 265 | // this too, since there will be no reordering within the same CAN ID. 266 | CANARD_STM32_CAN1->FFA1R = 0x0AAAAAAA; 267 | 268 | #if CANARD_STM32_USE_CAN2 269 | CANARD_STM32_CAN1->FilterRegister[CANARD_STM32_NUM_ACCEPTANCE_FILTERS].FR1 = 0; 270 | CANARD_STM32_CAN1->FilterRegister[CANARD_STM32_NUM_ACCEPTANCE_FILTERS].FR2 = 0; 271 | CANARD_STM32_CAN1->FA1R = (1 << CANARD_STM32_NUM_ACCEPTANCE_FILTERS); // One filter enabled 272 | #else 273 | CANARD_STM32_CAN1->FilterRegister[0].FR1 = 0; 274 | CANARD_STM32_CAN1->FilterRegister[0].FR2 = 0; 275 | CANARD_STM32_CAN1->FA1R = 1; // One filter enabled 276 | #endif 277 | 278 | CANARD_STM32_CAN1->FMR &= ~CANARD_STM32_CAN_FMR_FINIT; // Leave initialization mode 279 | 280 | return 0; 281 | } 282 | 283 | 284 | int16_t canardSTM32Transmit(const CanardCANFrame* const frame) 285 | { 286 | if (frame == NULL) 287 | { 288 | return -CANARD_ERROR_INVALID_ARGUMENT; 289 | } 290 | 291 | if (frame->id & CANARD_CAN_FRAME_ERR) 292 | { 293 | return -CANARD_STM32_ERROR_UNSUPPORTED_FRAME_FORMAT; 294 | } 295 | 296 | /* 297 | * Handling error status might free up some slots through aborts 298 | */ 299 | processErrorStatus(); 300 | 301 | /* 302 | * Seeking an empty slot, checking if priority inversion would occur if we enqueued now. 303 | * We can always enqueue safely if all TX mailboxes are free and no transmissions are pending. 304 | */ 305 | uint8_t tx_mailbox = 0xFF; 306 | 307 | static const uint32_t AllTME = CANARD_STM32_CAN_TSR_TME0 | CANARD_STM32_CAN_TSR_TME1 | CANARD_STM32_CAN_TSR_TME2; 308 | 309 | if ((BXCAN->TSR & AllTME) != AllTME) // At least one TX mailbox is used, detailed check is needed 310 | { 311 | const bool tme[3] = 312 | { 313 | (BXCAN->TSR & CANARD_STM32_CAN_TSR_TME0) != 0, 314 | (BXCAN->TSR & CANARD_STM32_CAN_TSR_TME1) != 0, 315 | (BXCAN->TSR & CANARD_STM32_CAN_TSR_TME2) != 0 316 | }; 317 | 318 | for (uint8_t i = 0; i < 3; i++) 319 | { 320 | if (tme[i]) // This TX mailbox is free, we can use it 321 | { 322 | tx_mailbox = i; 323 | } 324 | else // This TX mailbox is pending, check for priority inversion 325 | { 326 | if (!isFramePriorityHigher(frame->id, convertFrameIDRegisterToCanard(BXCAN->TxMailbox[i].TIR))) 327 | { 328 | // There's a mailbox whose priority is higher or equal the priority of the new frame. 329 | return 0; // Priority inversion would occur! Reject transmission. 330 | } 331 | } 332 | } 333 | 334 | if (tx_mailbox == 0xFF) 335 | { 336 | /* 337 | * All TX mailboxes are busy (this is highly unlikely); at the same time we know that there is no 338 | * higher or equal priority frame that is currently pending. Therefore, priority inversion has 339 | * just happend (sic!), because we can't enqueue the higher priority frame due to all TX mailboxes 340 | * being busy. This scenario is extremely unlikely, because in order for it to happen, the application 341 | * would need to transmit 4 (four) or more CAN frames with different CAN ID ordered from high ID to 342 | * low ID nearly at the same time. For example: 343 | * 1. 0x123 <-- Takes mailbox 0 (or any other) 344 | * 2. 0x122 <-- Takes mailbox 2 (or any other) 345 | * 3. 0x121 <-- Takes mailbox 1 (or any other) 346 | * 4. 0x120 <-- INNER PRIORITY INVERSION HERE (only if all three TX mailboxes are still busy) 347 | * This situation is even less likely to cause any noticeable disruptions on the CAN bus. Despite that, 348 | * it is better to warn the developer about that during debugging, so we fire an assertion failure here. 349 | * It is perfectly safe to remove it. 350 | */ 351 | #if CANARD_STM32_DEBUG_INNER_PRIORITY_INVERSION 352 | CANARD_ASSERT(!"CAN PRIO INV"); 353 | #endif 354 | return 0; 355 | } 356 | } 357 | else // All TX mailboxes are free, use first 358 | { 359 | tx_mailbox = 0; 360 | } 361 | 362 | CANARD_ASSERT(tx_mailbox < 3); // Index check - the value must be correct here 363 | 364 | /* 365 | * By this time we've proved that a priority inversion would not occur, and we've also found a free TX mailbox. 366 | * Therefore it is safe to enqueue the frame now. 367 | */ 368 | volatile CanardSTM32TxMailboxType* const mb = &BXCAN->TxMailbox[tx_mailbox]; 369 | 370 | mb->TDTR = frame->data_len; // DLC equals data length except in CAN FD 371 | 372 | mb->TDHR = (((uint32_t)frame->data[7]) << 24U) | 373 | (((uint32_t)frame->data[6]) << 16U) | 374 | (((uint32_t)frame->data[5]) << 8U) | 375 | (((uint32_t)frame->data[4]) << 0U); 376 | mb->TDLR = (((uint32_t)frame->data[3]) << 24U) | 377 | (((uint32_t)frame->data[2]) << 16U) | 378 | (((uint32_t)frame->data[1]) << 8U) | 379 | (((uint32_t)frame->data[0]) << 0U); 380 | 381 | mb->TIR = convertFrameIDCanardToRegister(frame->id) | CANARD_STM32_CAN_TIR_TXRQ; // Go. 382 | 383 | /* 384 | * The frame is now enqueued and pending transmission. 385 | */ 386 | return 1; 387 | } 388 | 389 | 390 | int16_t canardSTM32Receive(CanardCANFrame* const out_frame) 391 | { 392 | if (out_frame == NULL) 393 | { 394 | return -CANARD_ERROR_INVALID_ARGUMENT; 395 | } 396 | 397 | static volatile uint32_t* const RFxR[2] = 398 | { 399 | &BXCAN->RF0R, 400 | &BXCAN->RF1R 401 | }; 402 | 403 | /* 404 | * This function must be polled periodically, so we use this opportunity to do it. 405 | */ 406 | processErrorStatus(); 407 | 408 | /* 409 | * Reading the TX FIFO 410 | */ 411 | for (uint_fast8_t i = 0; i < 2; i++) 412 | { 413 | volatile CanardSTM32RxMailboxType* const mb = &BXCAN->RxMailbox[i]; 414 | //printint(*RFxR[i]); 415 | //println(""); 416 | 417 | if (((*RFxR[i]) & CANARD_STM32_CAN_RFR_FMP_MASK) != 0) 418 | { 419 | if (*RFxR[i] & CANARD_STM32_CAN_RFR_FOVR) 420 | { 421 | g_stats.rx_overflow_count++; 422 | } 423 | 424 | //printstr("mb->RIR=0x"); 425 | //printint(mb->RIR); 426 | //println(); 427 | out_frame->id = convertFrameIDRegisterToCanard(mb->RIR); 428 | //printstr("out_frame->id=0x"); 429 | //printint(out_frame->id); 430 | //println(); 431 | 432 | out_frame->data_len = (uint8_t)(mb->RDTR & CANARD_STM32_CAN_RDTR_DLC_MASK); 433 | 434 | // Caching to regular (non volatile) memory for faster reads 435 | const uint32_t rdlr = mb->RDLR; 436 | const uint32_t rdhr = mb->RDHR; 437 | 438 | out_frame->data[0] = (uint8_t)(0xFFU & (rdlr >> 0U)); 439 | out_frame->data[1] = (uint8_t)(0xFFU & (rdlr >> 8U)); 440 | out_frame->data[2] = (uint8_t)(0xFFU & (rdlr >> 16U)); 441 | out_frame->data[3] = (uint8_t)(0xFFU & (rdlr >> 24U)); 442 | out_frame->data[4] = (uint8_t)(0xFFU & (rdhr >> 0U)); 443 | out_frame->data[5] = (uint8_t)(0xFFU & (rdhr >> 8U)); 444 | out_frame->data[6] = (uint8_t)(0xFFU & (rdhr >> 16U)); 445 | out_frame->data[7] = (uint8_t)(0xFFU & (rdhr >> 24U)); 446 | 447 | // Release FIFO entry we just read 448 | *RFxR[i] = CANARD_STM32_CAN_RFR_RFOM | CANARD_STM32_CAN_RFR_FOVR | CANARD_STM32_CAN_RFR_FULL; 449 | 450 | // Reading successful 451 | return 1; 452 | } 453 | } 454 | 455 | // No frames to read 456 | return 0; 457 | } 458 | 459 | 460 | int16_t canardSTM32ConfigureAcceptanceFilters(const CanardSTM32AcceptanceFilterConfiguration* const filter_configs, 461 | const uint8_t num_filter_configs) 462 | { 463 | // Note that num_filter_configs = 0 is a valid configuration, which rejects all frames. 464 | if ((filter_configs == NULL) || 465 | (num_filter_configs > CANARD_STM32_NUM_ACCEPTANCE_FILTERS)) 466 | { 467 | return -CANARD_ERROR_INVALID_ARGUMENT; 468 | } 469 | 470 | /* 471 | * First we disable all filters. This may cause momentary RX frame losses, but the application 472 | * should be able to tolerate that. 473 | */ 474 | CANARD_STM32_CAN1->FA1R = 0; 475 | 476 | /* 477 | * Having filters disabled we can update the configuration. 478 | * Register mapping: FR1 - ID, FR2 - Mask 479 | */ 480 | for (uint8_t i = 0; i < num_filter_configs; i++) 481 | { 482 | /* 483 | * Converting the ID and the Mask into the representation that can be chewed by the hardware. 484 | * If Mask asks us to accept both STDID and EXTID, we need to use EXT mode on the filter, 485 | * otherwise it will reject all EXTID frames. This logic is not documented in the RM. 486 | * 487 | * The logic of the hardware acceptance filters can be described as follows: 488 | * 489 | * accepted = (received_id & filter_mask) == (filter_id & filter_mask) 490 | * 491 | * Where: 492 | * - accepted - if true, the frame will be accepted by the filter. 493 | * - received_id - the CAN ID of the received frame, either 11-bit or 29-bit, with extension bits 494 | * marking extended frames, error frames, etc. 495 | * - filter_id - the value of the filter ID register. 496 | * - filter_mask - the value of the filter mask register. 497 | * 498 | * There are special bits that are not members of the CAN ID field: 499 | * - EFF - set for extended frames (29-bit), cleared for standard frames (11-bit) 500 | * - RTR - like above, indicates Remote Transmission Request frames. 501 | * 502 | * The following truth table summarizes the logic (where: FM - filter mask, FID - filter ID, RID - received 503 | * frame ID, A - true if accepted, X - any state): 504 | * 505 | * FM FID RID A 506 | * 0 X X 1 507 | * 1 0 0 1 508 | * 1 1 0 0 509 | * 1 0 1 0 510 | * 1 1 1 1 511 | * 512 | * One would expect that for the purposes of hardware filtering, the special bits should be treated 513 | * in the same way as the real ID bits. However, this is not the case with bxCAN. The following truth 514 | * table has been determined empirically (this behavior was not documented as of 2017): 515 | * 516 | * FM FID RID A 517 | * 0 0 0 1 518 | * 0 0 1 0 <-- frame rejected! 519 | * 0 1 X 1 520 | * 1 0 0 1 521 | * 1 1 0 0 522 | * 1 0 1 0 523 | * 1 1 1 1 524 | */ 525 | uint32_t id = 0; 526 | uint32_t mask = 0; 527 | 528 | const CanardSTM32AcceptanceFilterConfiguration* const cfg = filter_configs + i; 529 | 530 | if ((cfg->id & CANARD_CAN_FRAME_EFF) || !(cfg->mask & CANARD_CAN_FRAME_EFF)) 531 | { 532 | id = (cfg->id & CANARD_CAN_EXT_ID_MASK) << 3U; 533 | mask = (cfg->mask & CANARD_CAN_EXT_ID_MASK) << 3U; 534 | id |= CANARD_STM32_CAN_RIR_IDE; 535 | } 536 | else 537 | { 538 | id = (cfg->id & CANARD_CAN_STD_ID_MASK) << 21U; 539 | mask = (cfg->mask & CANARD_CAN_STD_ID_MASK) << 21U; 540 | } 541 | 542 | if (cfg->id & CANARD_CAN_FRAME_RTR) 543 | { 544 | id |= CANARD_STM32_CAN_RIR_RTR; 545 | } 546 | 547 | if (cfg->mask & CANARD_CAN_FRAME_EFF) 548 | { 549 | mask |= CANARD_STM32_CAN_RIR_IDE; 550 | } 551 | 552 | if (cfg->mask & CANARD_CAN_FRAME_RTR) 553 | { 554 | mask |= CANARD_STM32_CAN_RIR_RTR; 555 | } 556 | 557 | /* 558 | * Applying the converted representation to the registers. 559 | */ 560 | const uint8_t filter_index = 561 | #if CANARD_STM32_USE_CAN2 562 | (uint8_t)(i + CANARD_STM32_NUM_ACCEPTANCE_FILTERS); 563 | #else 564 | i; 565 | #endif 566 | CANARD_STM32_CAN1->FilterRegister[filter_index].FR1 = id; 567 | CANARD_STM32_CAN1->FilterRegister[filter_index].FR2 = mask; 568 | 569 | CANARD_STM32_CAN1->FA1R |= 1U << filter_index; // Enable 570 | } 571 | 572 | return 0; 573 | } 574 | 575 | 576 | CanardSTM32Stats canardSTM32GetStats(void) 577 | { 578 | return g_stats; 579 | } 580 | -------------------------------------------------------------------------------- /canard/example/src/libcanard/drivers/stm32/canard_stm32.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 UAVCAN Team 3 | * 4 | * Distributed under the MIT License, available in the file LICENSE. 5 | * 6 | * Author: Pavel Kirienko 7 | */ 8 | 9 | #ifndef CANARD_STM32_H 10 | #define CANARD_STM32_H 11 | 12 | #include "src/libcanard/canard.h" 13 | #include // NOLINT 14 | 15 | 16 | #ifdef __cplusplus 17 | extern "C" 18 | { 19 | #endif 20 | 21 | /** 22 | * Set this build config macro to 1 to use CAN2 instead of CAN1, if available. 23 | * Setting this parameter when CAN2 is not available may not be detected at compile time! 24 | */ 25 | #if !defined(CANARD_STM32_USE_CAN2) 26 | # define CANARD_STM32_USE_CAN2 0 27 | #endif 28 | 29 | /** 30 | * Trigger an assertion failure if inner priority inversion is detected at run time. 31 | * This setting has no effect in release builds, where NDEBUG is defined. 32 | */ 33 | #if !defined(CANARD_STM32_DEBUG_INNER_PRIORITY_INVERSION) 34 | # define CANARD_STM32_DEBUG_INNER_PRIORITY_INVERSION 1 35 | #endif 36 | 37 | /** 38 | * Driver error codes. 39 | * These values are returned negated from API functions that return int. 40 | */ 41 | #define CANARD_STM32_ERROR_UNSUPPORTED_BIT_RATE 1000 42 | #define CANARD_STM32_ERROR_MSR_INAK_NOT_SET 1001 43 | #define CANARD_STM32_ERROR_MSR_INAK_NOT_CLEARED 1002 44 | #define CANARD_STM32_ERROR_UNSUPPORTED_FRAME_FORMAT 1003 45 | 46 | /** 47 | * This is defined by the bxCAN hardware. 48 | * Devices with only one CAN interface have 14 filters (e.g. F103). 49 | * Devices with two CAN interfaces have 28 filters, which are shared between two interfaces (e.g. F105, F446). 50 | * The filters are distributed between CAN1 and CAN2 by means of the CAN2 start filter bank selection, 51 | * which is a number from 1 to 27 inclusive. Seeing as the start bank cannot be set to 0, CAN2 has one filter less 52 | * to use. 53 | */ 54 | #define CANARD_STM32_NUM_ACCEPTANCE_FILTERS 14U 55 | 56 | /** 57 | * The interface can be initialized in either of these modes. 58 | * 59 | * The Silent mode is useful for automatic CAN bit rate detection, where the interface is initialized at an 60 | * arbitrarily guessed CAN bit rate (typically either 1 Mbps, 500 Kbps, 250 Kbps, or 125 Kbps, these are the 61 | * standard values defined by the UAVCAN specification), and the bus is then listened for 1 second in order to 62 | * determine whether the bit rate was guessed correctly. It is paramount to use the silent mode in this case so 63 | * as to not interfere with ongoing communications on the bus if the guess was incorrect. 64 | * 65 | * The automatic TX abort on error mode should be used during dynamic node ID allocation. The reason for that 66 | * is well explained in the UAVCAN specification, please read it. 67 | * 68 | * The normal mode should be used for all other use cases, particularly for the normal operation of the node, 69 | * hence the name. 70 | */ 71 | typedef enum 72 | { 73 | CanardSTM32IfaceModeNormal, //!< Normal mode 74 | CanardSTM32IfaceModeSilent, //!< Do not affect the bus, only listen 75 | CanardSTM32IfaceModeAutomaticTxAbortOnError //!< Abort pending TX if a bus error has occurred 76 | } CanardSTM32IfaceMode; 77 | 78 | /** 79 | * Interface statistics; these values can be queried using a dedicated API call. 80 | */ 81 | typedef struct 82 | { 83 | uint64_t rx_overflow_count; 84 | uint64_t error_count; 85 | } CanardSTM32Stats; 86 | 87 | /** 88 | * ID and Mask of a hardware acceptance filter. 89 | * The ID and Mask fields support flags @ref CANARD_CAN_FRAME_EFF and @ref CANARD_CAN_FRAME_RTR. 90 | */ 91 | typedef struct 92 | { 93 | uint32_t id; 94 | uint32_t mask; 95 | } CanardSTM32AcceptanceFilterConfiguration; 96 | 97 | /** 98 | * These parameters define the timings of the CAN controller. 99 | * Please refer to the documentation of the bxCAN macrocell for explanation. 100 | * These values can be computed by the developed beforehand if ROM size is of a concern, 101 | * or they can be computed at run time using the function defined below. 102 | */ 103 | typedef struct 104 | { 105 | uint16_t bit_rate_prescaler; /// [1, 1024] 106 | uint8_t bit_segment_1; /// [1, 16] 107 | uint8_t bit_segment_2; /// [1, 8] 108 | uint8_t max_resynchronization_jump_width; /// [1, 4] (recommended value is 1) 109 | } CanardSTM32CANTimings; 110 | 111 | /** 112 | * Initializes the CAN controller at the specified bit rate. 113 | * The mode can be either normal, silent, or auto-abort on error; 114 | * in silent mode the controller will be only listening, not affecting the state of the bus; 115 | * in the auto abort mode the controller will cancel the pending transmissions if a bus error is encountered. 116 | * The auto abort mode is needed for dynamic node ID allocation procedure; please refer to the UAVCAN specification 117 | * for more information about this topic. 118 | * 119 | * This function can be invoked any number of times; every invocation re-initializes everything from scratch. 120 | * 121 | * WARNING: The clock of the CAN module must be enabled before this function is invoked! 122 | * If CAN2 is used, CAN1 must be also enabled! 123 | * 124 | * WARNING: The driver is not thread-safe! 125 | * It does not use IRQ or critical sections though, so it is safe to invoke its API functions from the 126 | * IRQ context from the application. 127 | * 128 | * @retval 0 Success 129 | * @retval negative Error 130 | */ 131 | int16_t canardSTM32Init(const CanardSTM32CANTimings* const timings, 132 | const CanardSTM32IfaceMode iface_mode); 133 | 134 | /** 135 | * Pushes one frame into the TX buffer, if there is space. 136 | * Note that proper care is taken to ensure that no inner priority inversion is taking place. 137 | * This function does never block. 138 | * 139 | * @retval 1 Transmitted successfully 140 | * @retval 0 No space in the buffer 141 | * @retval negative Error 142 | */ 143 | int16_t canardSTM32Transmit(const CanardCANFrame* const frame); 144 | 145 | /** 146 | * Reads one frame from the hardware RX FIFO, unless all FIFO are empty. 147 | * This function does never block. 148 | * 149 | * @retval 1 Read successfully 150 | * @retval 0 The buffer is empty 151 | * @retval negative Error 152 | */ 153 | int16_t canardSTM32Receive(CanardCANFrame* const out_frame); 154 | 155 | /** 156 | * Sets up acceptance filters according to the provided list of ID and masks. 157 | * Note that when the interface is reinitialized, hardware acceptance filters are reset. 158 | * Also note that during filter reconfiguration, some RX frames may be lost. 159 | * 160 | * Setting zero filters will result in rejection of all frames. 161 | * In order to accept all frames, set one filter with ID = Mask = 0, which is also the default configuration. 162 | * 163 | * @retval 0 Success 164 | * @retval negative Error 165 | */ 166 | int16_t canardSTM32ConfigureAcceptanceFilters(const CanardSTM32AcceptanceFilterConfiguration* const filter_configs, 167 | const uint8_t num_filter_configs); 168 | 169 | /** 170 | * Returns the running interface statistics. 171 | */ 172 | CanardSTM32Stats canardSTM32GetStats(void); 173 | 174 | /** 175 | * Given the rate of the clock supplied to the bxCAN macrocell (typically PCLK1) and the desired bit rate, 176 | * this function iteratively solves for the best possible timing settings. The CAN bus timing parameters, 177 | * such as the sample point location, the number of time quantas per bit, etc., are optimized according to the 178 | * recommendations provided in the specifications of UAVCAN, DeviceNet, and CANOpen. 179 | * 180 | * Unless noted otherwise, all units are SI units; particularly, frequency is specified in hertz. 181 | * 182 | * The implementation is adapted from libuavcan. 183 | * 184 | * This function is defined in the header in order to encourage the linker to discard it if it is not used. 185 | * 186 | * @retval 0 Success 187 | * @retval negative Solution could not be found for the provided inputs. 188 | */ 189 | static inline 190 | int16_t canardSTM32ComputeCANTimings(const uint32_t peripheral_clock_rate, 191 | const uint32_t target_bitrate, 192 | CanardSTM32CANTimings* const out_timings) 193 | { 194 | if (target_bitrate < 1000) 195 | { 196 | return -CANARD_STM32_ERROR_UNSUPPORTED_BIT_RATE; 197 | } 198 | 199 | CANARD_ASSERT(out_timings != NULL); // NOLINT 200 | memset(out_timings, 0, sizeof(*out_timings)); 201 | 202 | /* 203 | * Hardware configuration 204 | */ 205 | static const uint8_t MaxBS1 = 16; 206 | static const uint8_t MaxBS2 = 8; 207 | 208 | /* 209 | * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG 210 | * CAN in Automation, 2003 211 | * 212 | * According to the source, optimal quanta per bit are: 213 | * Bitrate Optimal Maximum 214 | * 1000 kbps 8 10 215 | * 500 kbps 16 17 216 | * 250 kbps 16 17 217 | * 125 kbps 16 17 218 | */ 219 | const uint8_t max_quanta_per_bit = (uint8_t)((target_bitrate >= 1000000) ? 10 : 17); // NOLINT 220 | CANARD_ASSERT(max_quanta_per_bit <= (MaxBS1 + MaxBS2)); 221 | 222 | static const uint16_t MaxSamplePointLocationPermill = 900; 223 | 224 | /* 225 | * Computing (prescaler * BS): 226 | * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual 227 | * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified 228 | * let: 229 | * BS = 1 + BS1 + BS2 -- Number of time quanta per bit 230 | * PRESCALER_BS = PRESCALER * BS 231 | * ==> 232 | * PRESCALER_BS = PCLK / BITRATE 233 | */ 234 | const uint32_t prescaler_bs = peripheral_clock_rate / target_bitrate; 235 | 236 | /* 237 | * Searching for such prescaler value so that the number of quanta per bit is highest. 238 | */ 239 | uint8_t bs1_bs2_sum = (uint8_t)(max_quanta_per_bit - 1); // NOLINT 240 | 241 | while ((prescaler_bs % (1U + bs1_bs2_sum)) != 0) 242 | { 243 | if (bs1_bs2_sum <= 2) 244 | { 245 | return -CANARD_STM32_ERROR_UNSUPPORTED_BIT_RATE; // No solution 246 | } 247 | bs1_bs2_sum--; 248 | } 249 | 250 | const uint32_t prescaler = prescaler_bs / (1U + bs1_bs2_sum); 251 | if ((prescaler < 1U) || (prescaler > 1024U)) 252 | { 253 | return -CANARD_STM32_ERROR_UNSUPPORTED_BIT_RATE; // No solution 254 | } 255 | 256 | /* 257 | * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. 258 | * We need to find such values so that the sample point is as close as possible to the optimal value, 259 | * which is 87.5%, which is 7/8. 260 | * 261 | * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) 262 | * {{bs2 -> (1 + bs1)/7}} 263 | * 264 | * Hence: 265 | * bs2 = (1 + bs1) / 7 266 | * bs1 = (7 * bs1_bs2_sum - 1) / 8 267 | * 268 | * Sample point location can be computed as follows: 269 | * Sample point location = (1 + bs1) / (1 + bs1 + bs2) 270 | * 271 | * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: 272 | * - With rounding to nearest 273 | * - With rounding to zero 274 | */ 275 | uint8_t bs1 = (uint8_t)(((7 * bs1_bs2_sum - 1) + 4) / 8); // Trying rounding to nearest first // NOLINT 276 | uint8_t bs2 = (uint8_t)(bs1_bs2_sum - bs1); // NOLINT 277 | CANARD_ASSERT(bs1_bs2_sum > bs1); 278 | 279 | { 280 | const uint16_t sample_point_permill = (uint16_t)(1000U * (1U + bs1) / (1U + bs1 + bs2)); // NOLINT 281 | 282 | if (sample_point_permill > MaxSamplePointLocationPermill) // Strictly more! 283 | { 284 | bs1 = (uint8_t)((7 * bs1_bs2_sum - 1) / 8); // Nope, too far; now rounding to zero 285 | bs2 = (uint8_t)(bs1_bs2_sum - bs1); 286 | } 287 | } 288 | 289 | const bool valid = (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); 290 | 291 | /* 292 | * Final validation 293 | * Helpful Python: 294 | * def sample_point_from_btr(x): 295 | * assert 0b0011110010000000111111000000000 & x == 0 296 | * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 297 | * return (1+ts1+1)/(1+ts1+1+ts2+1) 298 | */ 299 | if ((target_bitrate != (peripheral_clock_rate / (prescaler * (1U + bs1 + bs2)))) || 300 | !valid) 301 | { 302 | // This actually means that the algorithm has a logic error, hence assert(0). 303 | CANARD_ASSERT(0); // NOLINT 304 | return -CANARD_STM32_ERROR_UNSUPPORTED_BIT_RATE; 305 | } 306 | 307 | out_timings->bit_rate_prescaler = (uint16_t) prescaler; 308 | out_timings->max_resynchronization_jump_width = 1; // One is recommended by UAVCAN, CANOpen, and DeviceNet 309 | out_timings->bit_segment_1 = bs1; 310 | out_timings->bit_segment_2 = bs2; 311 | 312 | return 0; 313 | } 314 | 315 | #ifdef __cplusplus 316 | } 317 | #endif 318 | #endif 319 | -------------------------------------------------------------------------------- /canard/example/uavcan.c: -------------------------------------------------------------------------------- 1 | #include "src/libcanard/canard.h" 2 | #include "src/libcanard/drivers/stm32/canard_stm32.h" 3 | #include "uavcan.h" 4 | 5 | static CanardInstance g_canard; //The library instance 6 | static uint8_t g_canard_memory_pool[1024]; //Arena for memory allocation, used by the library 7 | 8 | ////////////////////////////////////////////////////////////////////////////////////// 9 | 10 | void initCanard(uint32_t speed) 11 | { 12 | 13 | CanardSTM32CANTimings timings; 14 | //int result = canardSTM32ComputeCANTimings(HAL_RCC_GetPCLK1Freq(), 1000000, &timings); 15 | int result = canardSTM32ComputeCANTimings(HAL_RCC_GetPCLK1Freq(), speed, &timings); 16 | if (result) { 17 | __ASM volatile("BKPT #01"); 18 | } 19 | result = canardSTM32Init(&timings, CanardSTM32IfaceModeNormal); 20 | if (result) { 21 | __ASM volatile("BKPT #01"); 22 | } 23 | 24 | canardInit(&g_canard, // Uninitialized library instance 25 | g_canard_memory_pool, // Raw memory chunk used for dynamic allocation 26 | sizeof(g_canard_memory_pool), // Size of the above, in bytes 27 | NULL, // Callback, Not used 28 | NULL, // Callback, Not used 29 | NULL); 30 | 31 | canardSetLocalNodeID(&g_canard, 10); 32 | 33 | } 34 | 35 | 36 | void sendCanard(void) 37 | { 38 | 39 | const CanardCANFrame* txf = canardPeekTxQueue(&g_canard); 40 | 41 | while(txf) { 42 | const int tx_res = canardSTM32Transmit(txf); 43 | if (tx_res < 0) { // Failure - drop the frame and report 44 | __ASM volatile("BKPT #01"); // TODO: handle the error properly 45 | } 46 | if(tx_res > 0) { 47 | canardPopTxQueue(&g_canard); 48 | } 49 | txf = canardPeekTxQueue(&g_canard); 50 | } 51 | 52 | } 53 | 54 | 55 | bool receiveCanard(uint32_t * can_id, uint8_t * payload, uint8_t * payload_size) 56 | { 57 | 58 | CanardCANFrame rx_frame; 59 | int res = canardSTM32Receive(&rx_frame); 60 | if(res) { 61 | *can_id = rx_frame.id; 62 | *payload_size = rx_frame.data_len; 63 | memcpy(payload, rx_frame.data, rx_frame.data_len); 64 | //printframe(rx_frame.id, rx_frame.data_len, rx_frame.data); 65 | //canardHandleRxFrame(&g_canard, &rx_frame, HAL_GetTick() * 1000); 66 | return true; 67 | } 68 | return false; 69 | } 70 | 71 | void queueCanard(uint32_t can_id, uint8_t * payload, uint8_t payload_size) 72 | { 73 | canardBroadcastSimple(&g_canard, 74 | can_id, 75 | payload, 76 | payload_size); 77 | 78 | } 79 | -------------------------------------------------------------------------------- /canard/example/uavcan.h: -------------------------------------------------------------------------------- 1 | #include "stm32_def.h" 2 | 3 | void initCanard(uint32_t speed); 4 | void sendCanard(void); 5 | bool receiveCanard(uint32_t * can_id, uint8_t * payload, uint8_t * payload_size); 6 | void queueCanard(uint32_t can_id, uint8_t * payload, uint8_t payload_size); 7 | -------------------------------------------------------------------------------- /stm32f072/README.md: -------------------------------------------------------------------------------- 1 | # stm32f072 2 | Any 48MHz STM32F0 series will work. 3 | - STM32F042 4 | - STM32F072 5 | - STM32F091 6 | - STM32F098 7 | 8 | I used [this](https://github.com/seeers/CAN-Bus-Arduino_Core_STM32) as a reference. 9 | 10 | # Changes from the original 11 | 12 | - frame format 13 | This example support Extended frame format: with 29 identifier bits. 14 | 15 | - CAN port 16 | In this example, you can select the CAN port. 17 | 18 | # Remap CAN port 19 | 20 | STM32F042 have one CAN ports. 21 | These GPIOs can be used as CAN. 22 | |CAN|RX|TX| 23 | |:-:|:-:|:-:| 24 | |CAN1|PA11|PA12| 25 | |CAN1|PB8|PB9| 26 | 27 | 28 | STM32F072/091/098 have one CAN ports. 29 | These GPIOs can be used as CAN. 30 | |CAN|RX|TX| 31 | |:-:|:-:|:-:| 32 | |CAN1|PA11|PA12| 33 | |CAN1|PB8|PB9| 34 | |CAN1|PD0|PD1| 35 | 36 | 37 | CAN_RX mapped to PA11, CAN_TX mapped to PA12 38 | ``` 39 | CANInit(CAN_1000KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 40 | ``` 41 | 42 | CAN_RX mapped to PB8, CAN_TX mapped to PB9 (not available on 36-pin package) 43 | ``` 44 | CANInit(CAN_1000KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 45 | ``` 46 | 47 | CAN_RX mapped to PD0, CAN_TX mapped to PD1 (available on 100-pin and 144-pin package) 48 | ``` 49 | CANInit(CAN_1000KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 50 | ``` 51 | -------------------------------------------------------------------------------- /stm32f072/stm32f072.ino: -------------------------------------------------------------------------------- 1 | #define DEBUG 1 2 | #define AF4 0x04 3 | #define AF0 0x00 4 | 5 | /* Symbolic names for bit rate of CAN message */ 6 | typedef enum {CAN_50KBPS, CAN_100KBPS, CAN_125KBPS, CAN_250KBPS, CAN_500KBPS, CAN_1000KBPS} BITRATE; 7 | 8 | /* Symbolic names for formats of CAN message */ 9 | typedef enum {STANDARD_FORMAT = 0, EXTENDED_FORMAT} CAN_FORMAT; 10 | 11 | /* Symbolic names for type of CAN message */ 12 | typedef enum {DATA_FRAME = 0, REMOTE_FRAME} CAN_FRAME; 13 | 14 | typedef struct 15 | { 16 | uint32_t id; /* 29 bit identifier */ 17 | uint8_t data[8]; /* Data field */ 18 | uint8_t len; /* Length of data field in bytes */ 19 | uint8_t ch; /* Object channel(Not use) */ 20 | uint8_t format; /* 0 - STANDARD, 1- EXTENDED IDENTIFIER */ 21 | uint8_t type; /* 0 - DATA FRAME, 1 - REMOTE FRAME */ 22 | } CAN_msg_t; 23 | 24 | typedef const struct 25 | { 26 | uint8_t TS2; 27 | uint8_t TS1; 28 | uint8_t BRP; 29 | } CAN_bit_timing_config_t; 30 | 31 | CAN_bit_timing_config_t can_configs[6] = {{2, 13, 60}, {2, 13, 30}, {2, 13, 24}, {2, 13, 12}, {2, 13, 6}, {2, 13, 3}}; 32 | 33 | /** 34 | * Print registers. 35 | */ 36 | void printRegister(char * buf, uint32_t reg) { 37 | if (DEBUG == 0) return; 38 | Serial.print(buf); 39 | Serial.print("0x"); 40 | Serial.print(reg, HEX); 41 | Serial.println(); 42 | } 43 | 44 | /** 45 | * Initializes the CAN GPIO registers. 46 | * 47 | * @params: addr - Specified GPIO register address. 48 | * @params: index - Specified GPIO index. 49 | * @params: afry - Specified Alternative function selection AF0-AF15. 50 | * @params: speed - Specified OSPEEDR register value.(Optional) 51 | * 52 | */ 53 | void CANSetGpio(GPIO_TypeDef * addr, uint8_t index, uint8_t afry, uint8_t speed = 3) { 54 | uint8_t _index2 = index * 2; 55 | uint8_t _index4 = index * 4; 56 | uint8_t ofs = 0; 57 | uint8_t setting; 58 | 59 | if (index > 7) { 60 | _index4 = (index - 8) * 4; 61 | ofs = 1; 62 | } 63 | 64 | uint32_t mask; 65 | printRegister("GPIO_AFR(b)=", addr->AFR[1]); 66 | mask = 0xF << _index4; 67 | addr->AFR[ofs] &= ~mask; // Reset alternate function 68 | //setting = 0x9; // AF9 69 | setting = afry; // Alternative function selection 70 | mask = setting << _index4; 71 | addr->AFR[ofs] |= mask; // Set alternate function 72 | printRegister("GPIO_AFR(a)=", addr->AFR[1]); 73 | 74 | printRegister("GPIO_MODER(b)=", addr->MODER); 75 | mask = 0x3 << _index2; 76 | addr->MODER &= ~mask; // Reset mode 77 | setting = 0x2; // Alternate function mode 78 | mask = setting << _index2; 79 | addr->MODER |= mask; // Set mode 80 | printRegister("GPIO_MODER(a)=", addr->MODER); 81 | 82 | printRegister("GPIO_OSPEEDR(b)=", addr->OSPEEDR); 83 | mask = 0x3 << _index2; 84 | addr->OSPEEDR &= ~mask; // Reset speed 85 | setting = speed; 86 | mask = setting << _index2; 87 | addr->OSPEEDR |= mask; // Set speed 88 | printRegister("GPIO_OSPEEDR(a)=", addr->OSPEEDR); 89 | 90 | printRegister("GPIO_OTYPER(b)=", addr->OTYPER); 91 | mask = 0x1 << index; 92 | addr->OTYPER &= ~mask; // Reset Output push-pull 93 | printRegister("GPIO_OTYPER(a)=", addr->OTYPER); 94 | 95 | printRegister("GPIO_PUPDR(b)=", addr->PUPDR); 96 | mask = 0x3 << _index2; 97 | addr->PUPDR &= ~mask; // Reset port pull-up/pull-down 98 | printRegister("GPIO_PUPDR(a)=", addr->PUPDR); 99 | } 100 | 101 | 102 | /** 103 | * Initializes the CAN filter registers. 104 | * 105 | * The bxCAN provides up to 14 scalable/configurable identifier filter banks for selecting the incoming messages the software needs and discarding the others. 106 | * 107 | * @preconditions - This register can be written only when the filter initialization mode is set (FINIT=1) in the CAN_FMR register. 108 | * @params: index - Specified filter index. index 27:14 are available in connectivity line devices only. 109 | * @params: scale - Select filter scale. 110 | * 0: Dual 16-bit scale configuration 111 | * 1: Single 32-bit scale configuration 112 | * @params: mode - Select filter mode. 113 | * 0: Two 32-bit registers of filter bank x are in Identifier Mask mode 114 | * 1: Two 32-bit registers of filter bank x are in Identifier List mode 115 | * @params: fifo - Select filter assigned. 116 | * 0: Filter assigned to FIFO 0 117 | * 1: Filter assigned to FIFO 1 118 | * @params: bank1 - Filter bank register 1 119 | * @params: bank2 - Filter bank register 2 120 | * 121 | */ 122 | void CANSetFilter(uint8_t index, uint8_t scale, uint8_t mode, uint8_t fifo, uint32_t bank1, uint32_t bank2) { 123 | if (index > 13) return; 124 | 125 | CAN->FA1R &= ~(0x1UL<FS1R &= ~(0x1UL<FS1R |= (0x1UL<FM1R &= ~(0x1UL<FM1R |= (0x1UL<FFA1R &= ~(0x1UL<FFA1R |= (0x1UL<sFilterRegister[index].FR1 = bank1; // Set filter bank registers1 145 | CAN->sFilterRegister[index].FR2 = bank2; // Set filter bank registers2 146 | 147 | CAN->FA1R |= (0x1UL<APB1ENR |= 0x2000000UL; // Enable CAN clock 169 | 170 | if (remap == 0) { 171 | RCC->AHBENR |= 0x20000UL; // Enable GPIOA clock 172 | CANSetGpio(GPIOA, 11, AF4); // Set PA11 to AF4 173 | CANSetGpio(GPIOA, 12, AF4); // Set PA12 to AF4 174 | } 175 | 176 | if (remap == 2) { 177 | RCC->AHBENR |= 0x40000UL; // Enable GPIOB clock 178 | CANSetGpio(GPIOB, 8, AF4); // Set PB8 to AF4 179 | CANSetGpio(GPIOB, 9, AF4); // Set PB9 to AF4 180 | } 181 | 182 | if (remap == 3) { 183 | RCC->AHBENR |= 0x100000UL; // Enable GPIOD clock 184 | CANSetGpio(GPIOD, 0, AF0); // Set PD0 to AF0 185 | CANSetGpio(GPIOD, 1, AF0); // Set PD1 to AF0 186 | } 187 | 188 | CAN->MCR |= 0x1UL; // Set CAN to Initialization mode 189 | while (!(CAN->MSR & 0x1UL)); // Wait for Initialization mode 190 | 191 | //CAN->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) 192 | CAN->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) 193 | 194 | // Set bit rates 195 | //CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x1FF)); 196 | //CAN1->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x1FF); 197 | CAN->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x3FF)); 198 | CAN->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x3FF); 199 | printRegister("CAN->BTR=", CAN->BTR); 200 | 201 | // Configure Filters to default values 202 | CAN->FMR |= 0x1UL; // Set to filter initialization mode 203 | 204 | // Set fileter 0 205 | // Single 32-bit scale configuration 206 | // Two 32-bit registers of filter bank x are in Identifier Mask mode 207 | // Filter assigned to FIFO 0 208 | // Filter bank register to all 0 209 | CANSetFilter(0, 1, 0, 0, 0x0UL, 0x0UL); 210 | 211 | CAN->FMR &= ~(0x1UL); // Deactivate initialization mode 212 | 213 | uint16_t TimeoutMilliseconds = 1000; 214 | bool can1 = false; 215 | CAN->MCR &= ~(0x1UL); // Require CAN1 to normal mode 216 | 217 | // Wait for normal mode 218 | // If the connection is not correct, it will not return to normal mode. 219 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) { 220 | if ((CAN->MSR & 0x1UL) == 0) { 221 | can1 = true; 222 | break; 223 | } 224 | delayMicroseconds(1000); 225 | } 226 | //Serial.print("can1="); 227 | //Serial.println(can1); 228 | if (can1) { 229 | Serial.println("CAN1 initialize ok"); 230 | } else { 231 | Serial.println("CAN1 initialize fail!!"); 232 | return false; 233 | } 234 | return true; 235 | } 236 | 237 | 238 | #define STM32_CAN_TIR_TXRQ (1U << 0U) // Bit 0: Transmit Mailbox Request 239 | #define STM32_CAN_RIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 240 | #define STM32_CAN_RIR_IDE (1U << 2U) // Bit 2: Identifier Extension 241 | #define STM32_CAN_TIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 242 | #define STM32_CAN_TIR_IDE (1U << 2U) // Bit 2: Identifier Extension 243 | 244 | #define CAN_EXT_ID_MASK 0x1FFFFFFFU 245 | #define CAN_STD_ID_MASK 0x000007FFU 246 | 247 | /** 248 | * Decodes CAN messages from the data registers and populates a 249 | * CAN message struct with the data fields. 250 | * 251 | * @preconditions - A valid CAN message is received 252 | * @params CAN_rx_msg - CAN message structure for reception 253 | * 254 | */ 255 | void CANReceive(CAN_msg_t* CAN_rx_msg) 256 | { 257 | uint32_t id = CAN->sFIFOMailBox[0].RIR; 258 | if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format 259 | CAN_rx_msg->format = STANDARD_FORMAT;; 260 | CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U)); 261 | } 262 | else { // Extended frame format 263 | CAN_rx_msg->format = EXTENDED_FORMAT;; 264 | CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U)); 265 | } 266 | 267 | if ((id & STM32_CAN_RIR_RTR) == 0) { // Data frame 268 | CAN_rx_msg->type = DATA_FRAME; 269 | } 270 | else { // Remote frame 271 | CAN_rx_msg->type = REMOTE_FRAME; 272 | } 273 | 274 | 275 | CAN_rx_msg->len = (CAN->sFIFOMailBox[0].RDTR) & 0xFUL; 276 | 277 | CAN_rx_msg->data[0] = 0xFFUL & CAN->sFIFOMailBox[0].RDLR; 278 | CAN_rx_msg->data[1] = 0xFFUL & (CAN->sFIFOMailBox[0].RDLR >> 8); 279 | CAN_rx_msg->data[2] = 0xFFUL & (CAN->sFIFOMailBox[0].RDLR >> 16); 280 | CAN_rx_msg->data[3] = 0xFFUL & (CAN->sFIFOMailBox[0].RDLR >> 24); 281 | CAN_rx_msg->data[4] = 0xFFUL & CAN->sFIFOMailBox[0].RDHR; 282 | CAN_rx_msg->data[5] = 0xFFUL & (CAN->sFIFOMailBox[0].RDHR >> 8); 283 | CAN_rx_msg->data[6] = 0xFFUL & (CAN->sFIFOMailBox[0].RDHR >> 16); 284 | CAN_rx_msg->data[7] = 0xFFUL & (CAN->sFIFOMailBox[0].RDHR >> 24); 285 | 286 | // Release FIFO 0 output mailbox. 287 | // Make the next incoming message available. 288 | CAN->RF0R |= 0x20UL; 289 | } 290 | 291 | /** 292 | * Encodes CAN messages using the CAN message struct and populates the 293 | * data registers with the sent. 294 | * 295 | * @params CAN_tx_msg - CAN message structure for transmission 296 | * 297 | */ 298 | void CANSend(CAN_msg_t* CAN_tx_msg) 299 | { 300 | volatile int count = 0; 301 | 302 | uint32_t out = 0; 303 | if (CAN_tx_msg->format == EXTENDED_FORMAT) { // Extended frame format 304 | out = ((CAN_tx_msg->id & CAN_EXT_ID_MASK) << 3U) | STM32_CAN_TIR_IDE; 305 | } 306 | else { // Standard frame format 307 | out = ((CAN_tx_msg->id & CAN_STD_ID_MASK) << 21U); 308 | } 309 | 310 | // Remote frame 311 | if (CAN_tx_msg->type == REMOTE_FRAME) { 312 | out |= STM32_CAN_TIR_RTR; 313 | } 314 | 315 | CAN->sTxMailBox[0].TDTR &= ~(0xF); 316 | CAN->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL; 317 | 318 | CAN->sTxMailBox[0].TDLR = (((uint32_t) CAN_tx_msg->data[3] << 24) | 319 | ((uint32_t) CAN_tx_msg->data[2] << 16) | 320 | ((uint32_t) CAN_tx_msg->data[1] << 8) | 321 | ((uint32_t) CAN_tx_msg->data[0] )); 322 | CAN->sTxMailBox[0].TDHR = (((uint32_t) CAN_tx_msg->data[7] << 24) | 323 | ((uint32_t) CAN_tx_msg->data[6] << 16) | 324 | ((uint32_t) CAN_tx_msg->data[5] << 8) | 325 | ((uint32_t) CAN_tx_msg->data[4] )); 326 | 327 | // Send Go 328 | CAN->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; 329 | 330 | // Wait until the mailbox is empty 331 | while(CAN->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000); 332 | 333 | // The mailbox don't becomes empty while loop 334 | if (CAN->sTxMailBox[0].TIR & 0x1UL) { 335 | Serial.println("Send Fail"); 336 | Serial.println(CAN->ESR); 337 | Serial.println(CAN->MSR); 338 | Serial.println(CAN->TSR); 339 | } 340 | } 341 | 342 | /** 343 | * Returns whether there are CAN messages available. 344 | * 345 | * @returns If pending CAN messages are in the CAN controller 346 | * 347 | */ 348 | uint8_t CANMsgAvail(void) 349 | { 350 | // Check for pending FIFO 0 messages 351 | return CAN->RF0R & 0x3UL; 352 | } 353 | 354 | 355 | uint8_t counter = 0; 356 | uint8_t frameLength = 0; 357 | unsigned long previousMillis = 0; // stores last time output was updated 358 | const long interval = 1000; // transmission interval (milliseconds) 359 | 360 | void setup() { 361 | Serial.begin(115200); 362 | bool ret = CANInit(CAN_500KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 363 | //bool ret = CANInit(CAN_500KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 364 | //bool ret = CANInit(CAN_500KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 365 | //bool ret = CANInit(CAN_1000KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 366 | //bool ret = CANInit(CAN_1000KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 367 | //bool ret = CANInit(CAN_1000KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 368 | if (!ret) while(true); 369 | } 370 | 371 | void loop() { 372 | CAN_msg_t CAN_TX_msg; 373 | CAN_msg_t CAN_RX_msg; 374 | 375 | CAN_TX_msg.data[0] = 0x00; 376 | CAN_TX_msg.data[1] = 0x01; 377 | CAN_TX_msg.data[2] = 0x02; 378 | CAN_TX_msg.data[3] = 0x03; 379 | CAN_TX_msg.data[4] = 0x04; 380 | CAN_TX_msg.data[5] = 0x05; 381 | CAN_TX_msg.data[6] = 0x06; 382 | CAN_TX_msg.data[7] = 0x07; 383 | CAN_TX_msg.len = frameLength; 384 | 385 | unsigned long currentMillis = millis(); 386 | if (currentMillis - previousMillis >= interval) { 387 | previousMillis = currentMillis; 388 | if ( ( counter % 2) == 0) { 389 | CAN_TX_msg.type = DATA_FRAME; 390 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 391 | CAN_TX_msg.format = EXTENDED_FORMAT; 392 | CAN_TX_msg.id = 0x32F072; 393 | } else { 394 | CAN_TX_msg.type = DATA_FRAME; 395 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 396 | CAN_TX_msg.format = STANDARD_FORMAT; 397 | CAN_TX_msg.id = 0x072; 398 | } 399 | CANSend(&CAN_TX_msg); 400 | frameLength++; 401 | if (frameLength == 9) frameLength = 0; 402 | counter++; 403 | } 404 | 405 | if(CANMsgAvail()) { 406 | CANReceive(&CAN_RX_msg); 407 | 408 | if (CAN_RX_msg.format == EXTENDED_FORMAT) { 409 | Serial.print("Extended ID: 0x"); 410 | if (CAN_RX_msg.id < 0x10000000) Serial.print("0"); 411 | if (CAN_RX_msg.id < 0x1000000) Serial.print("0"); 412 | if (CAN_RX_msg.id < 0x100000) Serial.print("0"); 413 | if (CAN_RX_msg.id < 0x10000) Serial.print("0"); 414 | if (CAN_RX_msg.id < 0x1000) Serial.print("0"); 415 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 416 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 417 | Serial.print(CAN_RX_msg.id, HEX); 418 | } else { 419 | Serial.print("Standard ID: 0x"); 420 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 421 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 422 | Serial.print(CAN_RX_msg.id, HEX); 423 | Serial.print(" "); 424 | } 425 | 426 | Serial.print(" DLC: "); 427 | Serial.print(CAN_RX_msg.len); 428 | if (CAN_RX_msg.type == DATA_FRAME) { 429 | Serial.print(" Data: "); 430 | for(int i=0; i 7) { 60 | _index4 = (index - 8) * 4; 61 | ofs = 1; 62 | } 63 | 64 | uint32_t mask; 65 | printRegister("GPIO_AFR(b)=", addr->AFR[1]); 66 | mask = 0xF << _index4; 67 | addr->AFR[ofs] &= ~mask; // Reset alternate function 68 | //setting = 0x9; // AF9 69 | setting = afry; // Alternative function selection 70 | mask = setting << _index4; 71 | addr->AFR[ofs] |= mask; // Set alternate function 72 | printRegister("GPIO_AFR(a)=", addr->AFR[1]); 73 | 74 | printRegister("GPIO_MODER(b)=", addr->MODER); 75 | mask = 0x3 << _index2; 76 | addr->MODER &= ~mask; // Reset mode 77 | setting = 0x2; // Alternate function mode 78 | mask = setting << _index2; 79 | addr->MODER |= mask; // Set mode 80 | printRegister("GPIO_MODER(a)=", addr->MODER); 81 | 82 | printRegister("GPIO_OSPEEDR(b)=", addr->OSPEEDR); 83 | mask = 0x3 << _index2; 84 | addr->OSPEEDR &= ~mask; // Reset speed 85 | setting = speed; 86 | mask = setting << _index2; 87 | addr->OSPEEDR |= mask; // Set speed 88 | printRegister("GPIO_OSPEEDR(a)=", addr->OSPEEDR); 89 | 90 | printRegister("GPIO_OTYPER(b)=", addr->OTYPER); 91 | mask = 0x1 << index; 92 | addr->OTYPER &= ~mask; // Reset Output push-pull 93 | printRegister("GPIO_OTYPER(a)=", addr->OTYPER); 94 | 95 | printRegister("GPIO_PUPDR(b)=", addr->PUPDR); 96 | mask = 0x3 << _index2; 97 | addr->PUPDR &= ~mask; // Reset port pull-up/pull-down 98 | printRegister("GPIO_PUPDR(a)=", addr->PUPDR); 99 | } 100 | 101 | 102 | /** 103 | * Initializes the CAN filter registers. 104 | * 105 | * The bxCAN provides up to 14 scalable/configurable identifier filter banks, for selecting the incoming messages, that the software needs and discarding the others. 106 | * 107 | * @preconditions - This register can be written only when the filter initialization mode is set (FINIT=1) in the CAN_FMR register. 108 | * @params: index - Specified filter index. index 27:14 are available in connectivity line devices only. 109 | * @params: scale - Select filter scale. 110 | * 0: Dual 16-bit scale configuration 111 | * 1: Single 32-bit scale configuration 112 | * @params: mode - Select filter mode. 113 | * 0: Two 32-bit registers of filter bank x are in Identifier Mask mode 114 | * 1: Two 32-bit registers of filter bank x are in Identifier List mode 115 | * @params: fifo - Select filter assigned. 116 | * 0: Filter assigned to FIFO 0 117 | * 1: Filter assigned to FIFO 1 118 | * @params: bank1 - Filter bank register 1 119 | * @params: bank2 - Filter bank register 2 120 | * 121 | */ 122 | void CANSetFilter(uint8_t index, uint8_t scale, uint8_t mode, uint8_t fifo, uint32_t bank1, uint32_t bank2) { 123 | if (index > 13) return; 124 | 125 | CAN1->FA1R &= ~(0x1UL<FS1R &= ~(0x1UL<FS1R |= (0x1UL<FM1R &= ~(0x1UL<FM1R |= (0x1UL<FFA1R &= ~(0x1UL<FFA1R |= (0x1UL<sFilterRegister[index].FR1 = bank1; // Set filter bank registers1 145 | CAN1->sFilterRegister[index].FR2 = bank2; // Set filter bank registers2 146 | 147 | CAN1->FA1R |= (0x1UL<APB1ENR |= 0x2000000UL; // Enable CAN clock 169 | 170 | if (remap == 0) { 171 | RCC->AHBENR |= 0x20000UL; // Enable GPIOA clock 172 | CANSetGpio(GPIOA, 11, AF9); // Set PA11 to AF9 173 | CANSetGpio(GPIOA, 12, AF9); // Set PA12 to AF9 174 | } 175 | 176 | if (remap == 2) { 177 | RCC->AHBENR |= 0x40000UL; // Enable GPIOB clock 178 | CANSetGpio(GPIOB, 8, AF9); // Set PB8 to AF9 179 | CANSetGpio(GPIOB, 9, AF9); // Set PB9 to AF9 180 | } 181 | 182 | if (remap == 3) { 183 | RCC->AHBENR |= 0x100000UL; // Enable GPIOD clock 184 | CANSetGpio(GPIOD, 0, AF7); // Set PD0 to AF7 185 | CANSetGpio(GPIOD, 1, AF7); // Set PD1 to AF7 186 | } 187 | 188 | CAN1->MCR |= 0x1UL; // Set CAN to Initialization mode 189 | while (!(CAN1->MSR & 0x1UL)); // Wait for Initialization mode 190 | 191 | //CAN1->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) 192 | CAN1->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) 193 | 194 | // Set bit rates 195 | //CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x1FF)); 196 | //CAN1->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x1FF); 197 | CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x3FF)); 198 | CAN1->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x3FF); 199 | printRegister("CAN1->BTR=", CAN1->BTR); 200 | 201 | // Configure Filters to default values 202 | CAN1->FMR |= 0x1UL; // Set to filter initialization mode 203 | 204 | // Set fileter 0 205 | // Single 32-bit scale configuration 206 | // Two 32-bit registers of filter bank x are in Identifier Mask mode 207 | // Filter assigned to FIFO 0 208 | // Filter bank register to all 0 209 | CANSetFilter(0, 1, 0, 0, 0x0UL, 0x0UL); 210 | 211 | CAN1->FMR &= ~(0x1UL); // Deactivate initialization mode 212 | 213 | uint16_t TimeoutMilliseconds = 1000; 214 | bool can1 = false; 215 | CAN1->MCR &= ~(0x1UL); // Require CAN1 to normal mode 216 | 217 | // Wait for normal mode 218 | // If the connection is not correct, it will not return to normal mode. 219 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) { 220 | if ((CAN1->MSR & 0x1UL) == 0) { 221 | can1 = true; 222 | break; 223 | } 224 | delayMicroseconds(1000); 225 | } 226 | //Serial.print("can1="); 227 | //Serial.println(can1); 228 | if (can1) { 229 | Serial.println("CAN1 initialize ok"); 230 | } else { 231 | Serial.println("CAN1 initialize fail!!"); 232 | return false; 233 | } 234 | return true; 235 | } 236 | 237 | 238 | #define STM32_CAN_TIR_TXRQ (1U << 0U) // Bit 0: Transmit Mailbox Request 239 | #define STM32_CAN_RIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 240 | #define STM32_CAN_RIR_IDE (1U << 2U) // Bit 2: Identifier Extension 241 | #define STM32_CAN_TIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 242 | #define STM32_CAN_TIR_IDE (1U << 2U) // Bit 2: Identifier Extension 243 | 244 | #define CAN_EXT_ID_MASK 0x1FFFFFFFU 245 | #define CAN_STD_ID_MASK 0x000007FFU 246 | 247 | /** 248 | * Decodes CAN messages from the data registers and populates a 249 | * CAN message struct with the data fields. 250 | * 251 | * @preconditions - A valid CAN message is received 252 | * @params CAN_rx_msg - CAN message structure for reception 253 | * 254 | */ 255 | void CANReceive(CAN_msg_t* CAN_rx_msg) 256 | { 257 | uint32_t id = CAN1->sFIFOMailBox[0].RIR; 258 | if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format 259 | CAN_rx_msg->format = STANDARD_FORMAT;; 260 | CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U)); 261 | } 262 | else { // Extended frame format 263 | CAN_rx_msg->format = EXTENDED_FORMAT;; 264 | CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U)); 265 | } 266 | 267 | if ((id & STM32_CAN_RIR_RTR) == 0) { // Data frame 268 | CAN_rx_msg->type = DATA_FRAME; 269 | } 270 | else { // Remote frame 271 | CAN_rx_msg->type = REMOTE_FRAME; 272 | } 273 | 274 | 275 | CAN_rx_msg->len = (CAN1->sFIFOMailBox[0].RDTR) & 0xFUL; 276 | 277 | CAN_rx_msg->data[0] = 0xFFUL & CAN1->sFIFOMailBox[0].RDLR; 278 | CAN_rx_msg->data[1] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 8); 279 | CAN_rx_msg->data[2] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 16); 280 | CAN_rx_msg->data[3] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 24); 281 | CAN_rx_msg->data[4] = 0xFFUL & CAN1->sFIFOMailBox[0].RDHR; 282 | CAN_rx_msg->data[5] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 8); 283 | CAN_rx_msg->data[6] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 16); 284 | CAN_rx_msg->data[7] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 24); 285 | 286 | // Release FIFO 0 output mailbox. 287 | // Make the next incoming message available. 288 | CAN1->RF0R |= 0x20UL; 289 | } 290 | 291 | /** 292 | * Encodes CAN messages using the CAN message struct and populates the 293 | * data registers with the sent. 294 | * 295 | * @params CAN_tx_msg - CAN message structure for transmission 296 | * 297 | */ 298 | void CANSend(CAN_msg_t* CAN_tx_msg) 299 | { 300 | volatile int count = 0; 301 | 302 | uint32_t out = 0; 303 | if (CAN_tx_msg->format == EXTENDED_FORMAT) { // Extended frame format 304 | out = ((CAN_tx_msg->id & CAN_EXT_ID_MASK) << 3U) | STM32_CAN_TIR_IDE; 305 | } 306 | else { // Standard frame format 307 | out = ((CAN_tx_msg->id & CAN_STD_ID_MASK) << 21U); 308 | } 309 | 310 | // Remote frame 311 | if (CAN_tx_msg->type == REMOTE_FRAME) { 312 | out |= STM32_CAN_TIR_RTR; 313 | } 314 | 315 | CAN1->sTxMailBox[0].TDTR &= ~(0xF); 316 | CAN1->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL; 317 | 318 | CAN1->sTxMailBox[0].TDLR = (((uint32_t) CAN_tx_msg->data[3] << 24) | 319 | ((uint32_t) CAN_tx_msg->data[2] << 16) | 320 | ((uint32_t) CAN_tx_msg->data[1] << 8) | 321 | ((uint32_t) CAN_tx_msg->data[0] )); 322 | CAN1->sTxMailBox[0].TDHR = (((uint32_t) CAN_tx_msg->data[7] << 24) | 323 | ((uint32_t) CAN_tx_msg->data[6] << 16) | 324 | ((uint32_t) CAN_tx_msg->data[5] << 8) | 325 | ((uint32_t) CAN_tx_msg->data[4] )); 326 | 327 | // Send Go 328 | CAN1->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; 329 | 330 | // Wait until the mailbox is empty 331 | while(CAN1->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000); 332 | 333 | // The mailbox don't becomes empty while loop 334 | if (CAN1->sTxMailBox[0].TIR & 0x1UL) { 335 | Serial.println("Send Fail"); 336 | Serial.println(CAN1->ESR); 337 | Serial.println(CAN1->MSR); 338 | Serial.println(CAN1->TSR); 339 | } 340 | } 341 | 342 | /** 343 | * Returns whether there are CAN messages available. 344 | * 345 | * @returns If pending CAN messages are in the CAN controller 346 | * 347 | */ 348 | uint8_t CANMsgAvail(void) 349 | { 350 | // Check for pending FIFO 0 messages 351 | return CAN1->RF0R & 0x3UL; 352 | } 353 | 354 | 355 | uint8_t counter = 0; 356 | uint8_t frameLength = 0; 357 | unsigned long previousMillis = 0; // stores last time output was updated 358 | const long interval = 1000; // transmission interval (milliseconds) 359 | 360 | void setup() { 361 | Serial.begin(115200); 362 | bool ret = CANInit(CAN_500KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 363 | //bool ret = CANInit(CAN_500KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 364 | //bool ret = CANInit(CAN_500KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 365 | //bool ret = CANInit(CAN_1000KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 366 | //bool ret = CANInit(CAN_1000KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 367 | //bool ret = CANInit(CAN_1000KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 368 | if (!ret) while(true); 369 | } 370 | 371 | void loop() { 372 | CAN_msg_t CAN_TX_msg; 373 | CAN_msg_t CAN_RX_msg; 374 | 375 | CAN_TX_msg.data[0] = 0x00; 376 | CAN_TX_msg.data[1] = 0x01; 377 | CAN_TX_msg.data[2] = 0x02; 378 | CAN_TX_msg.data[3] = 0x03; 379 | CAN_TX_msg.data[4] = 0x04; 380 | CAN_TX_msg.data[5] = 0x05; 381 | CAN_TX_msg.data[6] = 0x06; 382 | CAN_TX_msg.data[7] = 0x07; 383 | CAN_TX_msg.len = frameLength; 384 | 385 | unsigned long currentMillis = millis(); 386 | if (currentMillis - previousMillis >= interval) { 387 | previousMillis = currentMillis; 388 | if ( ( counter % 2) == 0) { 389 | CAN_TX_msg.type = DATA_FRAME; 390 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 391 | CAN_TX_msg.format = EXTENDED_FORMAT; 392 | CAN_TX_msg.id = 0x32F303; 393 | } else { 394 | CAN_TX_msg.type = DATA_FRAME; 395 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 396 | CAN_TX_msg.format = STANDARD_FORMAT; 397 | CAN_TX_msg.id = 0x303; 398 | } 399 | CANSend(&CAN_TX_msg); 400 | frameLength++; 401 | if (frameLength == 9) frameLength = 0; 402 | counter++; 403 | } 404 | 405 | if(CANMsgAvail()) { 406 | CANReceive(&CAN_RX_msg); 407 | 408 | if (CAN_RX_msg.format == EXTENDED_FORMAT) { 409 | Serial.print("Extended ID: 0x"); 410 | if (CAN_RX_msg.id < 0x10000000) Serial.print("0"); 411 | if (CAN_RX_msg.id < 0x1000000) Serial.print("0"); 412 | if (CAN_RX_msg.id < 0x100000) Serial.print("0"); 413 | if (CAN_RX_msg.id < 0x10000) Serial.print("0"); 414 | if (CAN_RX_msg.id < 0x1000) Serial.print("0"); 415 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 416 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 417 | Serial.print(CAN_RX_msg.id, HEX); 418 | } else { 419 | Serial.print("Standard ID: 0x"); 420 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 421 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 422 | Serial.print(CAN_RX_msg.id, HEX); 423 | Serial.print(" "); 424 | } 425 | 426 | Serial.print(" DLC: "); 427 | Serial.print(CAN_RX_msg.len); 428 | if (CAN_RX_msg.type == DATA_FRAME) { 429 | Serial.print(" Data: "); 430 | for(int i=0; i 7) { 56 | _index4 = (index - 8) * 4; 57 | ofs = 1; 58 | } 59 | 60 | uint32_t mask; 61 | printRegister("GPIO_AFR(b)=", addr->AFR[1]); 62 | mask = 0xF << _index4; 63 | addr->AFR[ofs] &= ~mask; // Reset alternate function 64 | setting = 0x9; // AF9 65 | mask = setting << _index4; 66 | addr->AFR[ofs] |= mask; // Set alternate function 67 | printRegister("GPIO_AFR(a)=", addr->AFR[1]); 68 | 69 | printRegister("GPIO_MODER(b)=", addr->MODER); 70 | mask = 0x3 << _index2; 71 | addr->MODER &= ~mask; // Reset mode 72 | setting = 0x2; // Alternate function mode 73 | mask = setting << _index2; 74 | addr->MODER |= mask; // Set mode 75 | printRegister("GPIO_MODER(a)=", addr->MODER); 76 | 77 | printRegister("GPIO_OSPEEDR(b)=", addr->OSPEEDR); 78 | mask = 0x3 << _index2; 79 | addr->OSPEEDR &= ~mask; // Reset speed 80 | setting = speed; 81 | mask = setting << _index2; 82 | addr->OSPEEDR |= mask; // Set speed 83 | printRegister("GPIO_OSPEEDR(a)=", addr->OSPEEDR); 84 | 85 | printRegister("GPIO_OTYPER(b)=", addr->OTYPER); 86 | mask = 0x1 << index; 87 | addr->OTYPER &= ~mask; // Reset Output push-pull 88 | printRegister("GPIO_OTYPER(a)=", addr->OTYPER); 89 | 90 | printRegister("GPIO_PUPDR(b)=", addr->PUPDR); 91 | mask = 0x3 << _index2; 92 | addr->PUPDR &= ~mask; // Reset port pull-up/pull-down 93 | printRegister("GPIO_PUPDR(a)=", addr->PUPDR); 94 | } 95 | 96 | 97 | 98 | /** 99 | * Initializes the CAN filter registers. 100 | * 101 | * The bxCAN provides up to 28 scalable/configurable identifier filter banks for selecting the incoming messages the software needs and discarding the others. 102 | * 103 | * @preconditions - This register can be written only when the filter initialization mode is set (FINIT=1) in the CAN_FMR register. 104 | * @params: index - Specified filter index. index 27:14 are available in connectivity line devices only. 105 | * @params: scale - Select filter scale. 106 | * 0: Dual 16-bit scale configuration 107 | * 1: Single 32-bit scale configuration 108 | * @params: mode - Select filter mode. 109 | * 0: Two 32-bit registers of filter bank x are in Identifier Mask mode 110 | * 1: Two 32-bit registers of filter bank x are in Identifier List mode 111 | * @params: fifo - Select filter assigned. 112 | * 0: Filter assigned to FIFO 0 113 | * 1: Filter assigned to FIFO 1 114 | * @params: bank1 - Filter bank register 1 115 | * @params: bank2 - Filter bank register 2 116 | * 117 | */ 118 | void CANSetFilter(uint8_t index, uint8_t scale, uint8_t mode, uint8_t fifo, uint32_t bank1, uint32_t bank2) { 119 | if (index > 27) return; 120 | 121 | CAN1->FA1R &= ~(0x1UL<FS1R &= ~(0x1UL<FS1R |= (0x1UL<FM1R &= ~(0x1UL<FM1R |= (0x1UL<FFA1R &= ~(0x1UL<FFA1R |= (0x1UL<sFilterRegister[index].FR1 = bank1; // Set filter bank registers1 141 | CAN1->sFilterRegister[index].FR2 = bank2; // Set filter bank registers2 142 | 143 | CAN1->FA1R |= (0x1UL<APB1ENR |= 0x2000000UL; // Enable CAN1 clock 167 | RCC->APB1ENR |= 0x4000000UL; // Enable CAN2 clock 168 | 169 | if (remap == 0) { 170 | // CAN1 171 | RCC->AHB1ENR |= 0x1; // Enable GPIOA clock 172 | CANSetGpio(GPIOA, 11); // Set PA11 173 | CANSetGpio(GPIOA, 12); // Set PA12 174 | 175 | // CAN2 176 | RCC->AHB1ENR |= 0x2; // Enable GPIOB clock 177 | CANSetGpio(GPIOB, 5); // Set PB5 178 | CANSetGpio(GPIOB, 6); // Set PB6 179 | } 180 | 181 | if (remap == 2) { 182 | // CAN1 183 | RCC->AHB1ENR |= 0x2; // Enable GPIOB clock 184 | CANSetGpio(GPIOB, 8); // Set PB8 185 | CANSetGpio(GPIOB, 9); // Set PB9 186 | 187 | // CAN2 188 | RCC->AHB1ENR |= 0x2; // Enable GPIOB clock 189 | CANSetGpio(GPIOB, 12); // Set PB12 190 | CANSetGpio(GPIOB, 13); // Set PB13 191 | } 192 | 193 | if (remap == 3) { 194 | // CAN1 195 | RCC->AHB1ENR |= 0x8; // Enable GPIOD clock 196 | CANSetGpio(GPIOD, 0); // Set PD0 197 | CANSetGpio(GPIOD, 1); // Set PD1 198 | 199 | // CAN2 200 | RCC->AHB1ENR |= 0x2; // Enable GPIOB clock 201 | CANSetGpio(GPIOB, 12); // Set PB12 202 | CANSetGpio(GPIOB, 13); // Set PB13 203 | } 204 | 205 | CAN1->MCR |= 0x1UL; // Require CAN1 to Initialization mode 206 | while (!(CAN1->MSR & 0x1UL)); // Wait for Initialization mode 207 | printRegister("CAN1->MCR=", CAN1->MCR); 208 | 209 | CAN2->MCR |= 0x1UL; // Require CAN2 to Initialization mode 210 | while (!(CAN2->MSR & 0x1UL)); // Wait for Initialization mode 211 | printRegister("CAN2->MCR=", CAN2->MCR); 212 | 213 | //CAN1->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) 214 | CAN1->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) 215 | 216 | //CAN2->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) 217 | CAN2->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) 218 | 219 | 220 | // Set bit rates 221 | CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x3FF)); 222 | CAN1->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x3FF); 223 | printRegister("CAN1->BTR=", CAN1->BTR); 224 | 225 | CAN2->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x3FF)); 226 | CAN2->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x3FF); 227 | printRegister("CAN2->BTR=", CAN2->BTR); 228 | 229 | // Configure Filters to default values 230 | CAN1->FMR |= 0x1UL; // Set to filter initialization mode 231 | CAN1->FMR &= 0xFFFFC0FF; // Clear CAN2 start bank 232 | printRegister("CAN1->FMR=", CAN1->FMR); 233 | 234 | // bxCAN has 28 filters. 235 | // These filters are used for both CAN1 and CAN2. 236 | // STM32F405 has CAN1 and CAN2, so CAN2 filters are offset by 14 237 | CAN1->FMR |= 0xE00; // Start bank for the CAN2 interface 238 | 239 | // Set fileter 0 240 | // Single 32-bit scale configuration 241 | // Two 32-bit registers of filter bank x are in Identifier Mask mode 242 | // Filter assigned to FIFO 0 243 | // Filter bank register to all 0 244 | CANSetFilter(0, 1, 0, 0, 0x0UL, 0x0UL); 245 | 246 | // Set fileter 14 247 | // Single 32-bit scale configuration 248 | // Two 32-bit registers of filter bank x are in Identifier Mask mode 249 | // Filter assigned to FIFO 0 250 | // Filter bank register to all 0 251 | CANSetFilter(14, 1, 0, 0, 0x0UL, 0x0UL); 252 | 253 | CAN1->FMR &= ~(0x1UL); // Deactivate initialization mode 254 | 255 | uint16_t TimeoutMilliseconds = 1000; 256 | bool can2 = false; 257 | CAN2->MCR &= ~(0x1UL); // Require CAN2 to normal mode 258 | 259 | // Wait for normal mode 260 | // If the connection is not correct, it will not return to normal mode. 261 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) { 262 | if ((CAN2->MSR & 0x1UL) == 0) { 263 | can2 = true; 264 | break; 265 | } 266 | delayMicroseconds(1000); 267 | } 268 | //Serial.print("can2="); 269 | //Serial.println(can2); 270 | if (can2) { 271 | Serial.println("CAN2 initialize ok"); 272 | } else { 273 | Serial.println("CAN2 initialize fail!!"); 274 | } 275 | 276 | bool can1 = false; 277 | CAN1->MCR &= ~(0x1UL); // Require CAN1 to normal mode 278 | 279 | // Wait for normal mode 280 | // If the connection is not correct, it will not return to normal mode. 281 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) { 282 | if ((CAN1->MSR & 0x1UL) == 0) { 283 | can1 = true; 284 | break; 285 | } 286 | delayMicroseconds(1000); 287 | } 288 | //Serial.print("can1="); 289 | //Serial.println(can1); 290 | if (can1) { 291 | Serial.println("CAN1 initialize ok"); 292 | } else { 293 | Serial.println("CAN1 initialize fail!!"); 294 | return false; 295 | } 296 | return true; 297 | } 298 | 299 | 300 | #define STM32_CAN_TIR_TXRQ (1U << 0U) // Bit 0: Transmit Mailbox Request 301 | #define STM32_CAN_RIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 302 | #define STM32_CAN_RIR_IDE (1U << 2U) // Bit 2: Identifier Extension 303 | #define STM32_CAN_TIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 304 | #define STM32_CAN_TIR_IDE (1U << 2U) // Bit 2: Identifier Extension 305 | 306 | #define CAN_EXT_ID_MASK 0x1FFFFFFFU 307 | #define CAN_STD_ID_MASK 0x000007FFU 308 | 309 | /** 310 | * Decodes CAN messages from the data registers and populates a 311 | * CAN message struct with the data fields. 312 | * 313 | * @preconditions - A valid CAN message is received 314 | * @params CAN_rx_msg - CAN message structure for reception 315 | * 316 | */ 317 | void CANReceive(uint8_t ch, CAN_msg_t* CAN_rx_msg) 318 | { 319 | if(ch == 1) { 320 | uint32_t id = CAN1->sFIFOMailBox[0].RIR; 321 | if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format 322 | CAN_rx_msg->format = STANDARD_FORMAT;; 323 | CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U)); 324 | } 325 | else { // Extended frame format 326 | CAN_rx_msg->format = EXTENDED_FORMAT;; 327 | CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U)); 328 | } 329 | 330 | if ((id & STM32_CAN_RIR_RTR) == 0) { // Data frame 331 | CAN_rx_msg->type = DATA_FRAME; 332 | } 333 | else { // Remote frame 334 | CAN_rx_msg->type = REMOTE_FRAME; 335 | } 336 | 337 | 338 | CAN_rx_msg->len = (CAN1->sFIFOMailBox[0].RDTR) & 0xFUL; 339 | 340 | CAN_rx_msg->data[0] = 0xFFUL & CAN1->sFIFOMailBox[0].RDLR; 341 | CAN_rx_msg->data[1] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 8); 342 | CAN_rx_msg->data[2] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 16); 343 | CAN_rx_msg->data[3] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 24); 344 | CAN_rx_msg->data[4] = 0xFFUL & CAN1->sFIFOMailBox[0].RDHR; 345 | CAN_rx_msg->data[5] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 8); 346 | CAN_rx_msg->data[6] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 16); 347 | CAN_rx_msg->data[7] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 24); 348 | 349 | CAN1->RF0R |= 0x20UL; 350 | } // end CAN1 351 | 352 | if(ch == 2) { 353 | uint32_t id = CAN2->sFIFOMailBox[0].RIR; 354 | if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format 355 | CAN_rx_msg->format = STANDARD_FORMAT;; 356 | CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U)); 357 | } 358 | else { // Extended frame format 359 | CAN_rx_msg->format = EXTENDED_FORMAT;; 360 | CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U)); 361 | } 362 | 363 | if ((id & STM32_CAN_RIR_RTR) == 0) { // Data frame 364 | CAN_rx_msg->type = DATA_FRAME; 365 | } 366 | else { // Remote frame 367 | CAN_rx_msg->type = REMOTE_FRAME; 368 | } 369 | 370 | 371 | CAN_rx_msg->len = (CAN2->sFIFOMailBox[0].RDTR) & 0xFUL; 372 | 373 | CAN_rx_msg->data[0] = 0xFFUL & CAN2->sFIFOMailBox[0].RDLR; 374 | CAN_rx_msg->data[1] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDLR >> 8); 375 | CAN_rx_msg->data[2] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDLR >> 16); 376 | CAN_rx_msg->data[3] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDLR >> 24); 377 | CAN_rx_msg->data[4] = 0xFFUL & CAN2->sFIFOMailBox[0].RDHR; 378 | CAN_rx_msg->data[5] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDHR >> 8); 379 | CAN_rx_msg->data[6] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDHR >> 16); 380 | CAN_rx_msg->data[7] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDHR >> 24); 381 | 382 | CAN2->RF0R |= 0x20UL; 383 | } // END CAN2 384 | 385 | } 386 | 387 | /** 388 | * Encodes CAN messages using the CAN message struct and populates the 389 | * data registers with the sent. 390 | * 391 | * @params CAN_tx_msg - CAN message structure for transmission 392 | * 393 | */ 394 | void CANSend(uint8_t ch, CAN_msg_t* CAN_tx_msg) 395 | { 396 | volatile int count = 0; 397 | 398 | uint32_t out = 0; 399 | if (CAN_tx_msg->format == EXTENDED_FORMAT) { // Extended frame format 400 | out = ((CAN_tx_msg->id & CAN_EXT_ID_MASK) << 3U) | STM32_CAN_TIR_IDE; 401 | } 402 | else { // Standard frame format 403 | out = ((CAN_tx_msg->id & CAN_STD_ID_MASK) << 21U); 404 | } 405 | 406 | // Remote frame 407 | if (CAN_tx_msg->type == REMOTE_FRAME) { 408 | out |= STM32_CAN_TIR_RTR; 409 | } 410 | 411 | if (ch == 1) { 412 | CAN1->sTxMailBox[0].TDTR &= ~(0xF); 413 | CAN1->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL; 414 | 415 | CAN1->sTxMailBox[0].TDLR = (((uint32_t) CAN_tx_msg->data[3] << 24) | 416 | ((uint32_t) CAN_tx_msg->data[2] << 16) | 417 | ((uint32_t) CAN_tx_msg->data[1] << 8) | 418 | ((uint32_t) CAN_tx_msg->data[0] )); 419 | CAN1->sTxMailBox[0].TDHR = (((uint32_t) CAN_tx_msg->data[7] << 24) | 420 | ((uint32_t) CAN_tx_msg->data[6] << 16) | 421 | ((uint32_t) CAN_tx_msg->data[5] << 8) | 422 | ((uint32_t) CAN_tx_msg->data[4] )); 423 | 424 | // Send Go 425 | CAN1->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; 426 | 427 | // Wait until the mailbox is empty 428 | while(CAN1->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000); 429 | 430 | // The mailbox don't becomes empty while loop 431 | if (CAN1->sTxMailBox[0].TIR & 0x1UL) { 432 | Serial.println("Send Fail"); 433 | Serial.println(CAN1->ESR); 434 | Serial.println(CAN1->MSR); 435 | Serial.println(CAN1->TSR); 436 | } 437 | } // end CAN1 438 | 439 | if (ch == 2) { 440 | CAN2->sTxMailBox[0].TDTR &= ~(0xF); 441 | CAN2->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL; 442 | 443 | CAN2->sTxMailBox[0].TDLR = (((uint32_t) CAN_tx_msg->data[3] << 24) | 444 | ((uint32_t) CAN_tx_msg->data[2] << 16) | 445 | ((uint32_t) CAN_tx_msg->data[1] << 8) | 446 | ((uint32_t) CAN_tx_msg->data[0] )); 447 | CAN2->sTxMailBox[0].TDHR = (((uint32_t) CAN_tx_msg->data[7] << 24) | 448 | ((uint32_t) CAN_tx_msg->data[6] << 16) | 449 | ((uint32_t) CAN_tx_msg->data[5] << 8) | 450 | ((uint32_t) CAN_tx_msg->data[4] )); 451 | 452 | // Send Go 453 | CAN2->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; 454 | 455 | // Wait until the mailbox is empty 456 | while(CAN2->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000); 457 | 458 | // The mailbox don't becomes empty while loop 459 | if (CAN2->sTxMailBox[0].TIR & 0x1UL) { 460 | Serial.println("Send Fail"); 461 | Serial.println(CAN1->ESR); 462 | Serial.println(CAN1->MSR); 463 | Serial.println(CAN1->TSR); 464 | } 465 | } // end CAN2 466 | 467 | } 468 | 469 | /** 470 | * Returns whether there are CAN messages available. 471 | * 472 | * @returns If pending CAN messages are in the CAN controller 473 | * 474 | */ 475 | uint8_t CANMsgAvail(uint8_t ch) 476 | { 477 | if (ch == 1) { 478 | // Check for pending FIFO 0 messages 479 | return CAN1->RF0R & 0x3UL; 480 | } // end CAN1 481 | 482 | if (ch == 2) { 483 | // Check for pending FIFO 0 messages 484 | return CAN2->RF0R & 0x3UL; 485 | } // end CAN2 486 | 487 | return 0; 488 | } 489 | 490 | 491 | uint8_t counter = 0; 492 | uint8_t frameLength = 0; 493 | unsigned long previousMillis = 0; // stores last time output was updated 494 | const long interval = 1000; // transmission interval (milliseconds) 495 | 496 | void setup() { 497 | Serial.begin(115200); 498 | bool ret = CANInit(CAN_500KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 499 | //bool ret = CANInit(CAN_500KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 500 | //bool ret = CANInit(CAN_500KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 501 | //bool ret = CANInit(CAN_1000KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 502 | //bool ret = CANInit(CAN_1000KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 503 | //bool ret = CANInit(CAN_1000KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 504 | if (!ret) while(true); 505 | } 506 | 507 | void loop() { 508 | CAN_msg_t CAN_TX_msg; 509 | CAN_msg_t CAN_RX_msg; 510 | 511 | CAN_TX_msg.data[0] = 0x00; 512 | CAN_TX_msg.data[1] = 0x01; 513 | CAN_TX_msg.data[2] = 0x02; 514 | CAN_TX_msg.data[3] = 0x03; 515 | CAN_TX_msg.data[4] = 0x04; 516 | CAN_TX_msg.data[5] = 0x05; 517 | CAN_TX_msg.data[6] = 0x06; 518 | CAN_TX_msg.data[7] = 0x07; 519 | CAN_TX_msg.len = frameLength; 520 | 521 | uint8_t send_ch = 1; 522 | unsigned long currentMillis = millis(); 523 | if (currentMillis - previousMillis >= interval) { 524 | previousMillis = currentMillis; 525 | if ( ( counter % 2) == 0) { 526 | CAN_TX_msg.type = DATA_FRAME; 527 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 528 | CAN_TX_msg.format = EXTENDED_FORMAT; 529 | CAN_TX_msg.id = 0x32F407; 530 | } else { 531 | CAN_TX_msg.type = DATA_FRAME; 532 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 533 | CAN_TX_msg.format = STANDARD_FORMAT; 534 | CAN_TX_msg.id = 0x407; 535 | } 536 | CANSend(send_ch, &CAN_TX_msg); 537 | frameLength++; 538 | if (frameLength == 9) frameLength = 0; 539 | counter++; 540 | } 541 | 542 | uint8_t recv_ch = 1; 543 | if(CANMsgAvail(recv_ch)) { 544 | CANReceive(recv_ch, &CAN_RX_msg); 545 | Serial.print("Channel:"); 546 | Serial.print(recv_ch); 547 | 548 | if (CAN_RX_msg.format == EXTENDED_FORMAT) { 549 | Serial.print(" Extended ID: 0x"); 550 | if (CAN_RX_msg.id < 0x10000000) Serial.print("0"); 551 | if (CAN_RX_msg.id < 0x1000000) Serial.print("0"); 552 | if (CAN_RX_msg.id < 0x100000) Serial.print("0"); 553 | if (CAN_RX_msg.id < 0x10000) Serial.print("0"); 554 | if (CAN_RX_msg.id < 0x1000) Serial.print("0"); 555 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 556 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 557 | Serial.print(CAN_RX_msg.id, HEX); 558 | } else { 559 | Serial.print(" Standard ID: 0x"); 560 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 561 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 562 | Serial.print(CAN_RX_msg.id, HEX); 563 | Serial.print(" "); 564 | } 565 | 566 | Serial.print(" DLC: "); 567 | Serial.print(CAN_RX_msg.len); 568 | if (CAN_RX_msg.type == DATA_FRAME) { 569 | Serial.print(" Data: "); 570 | for(int i=0; i 7) { 57 | _index4 = (index - 8) * 4; 58 | ofs = 1; 59 | } 60 | 61 | uint32_t mask; 62 | printRegister("GPIO_AFR(b)=", addr->AFR[1]); 63 | mask = 0xF << _index4; 64 | addr->AFR[ofs] &= ~mask; // Reset alternate function 65 | setting = 0x9; // AF9 66 | mask = setting << _index4; 67 | addr->AFR[ofs] |= mask; // Set alternate function 68 | printRegister("GPIO_AFR(a)=", addr->AFR[1]); 69 | 70 | printRegister("GPIO_MODER(b)=", addr->MODER); 71 | mask = 0x3 << _index2; 72 | addr->MODER &= ~mask; // Reset mode 73 | setting = 0x2; // Alternate function mode 74 | mask = setting << _index2; 75 | addr->MODER |= mask; // Set mode 76 | printRegister("GPIO_MODER(a)=", addr->MODER); 77 | 78 | printRegister("GPIO_OSPEEDR(b)=", addr->OSPEEDR); 79 | mask = 0x3 << _index2; 80 | addr->OSPEEDR &= ~mask; // Reset speed 81 | setting = speed; 82 | mask = setting << _index2; 83 | addr->OSPEEDR |= mask; // Set speed 84 | printRegister("GPIO_OSPEEDR(a)=", addr->OSPEEDR); 85 | 86 | printRegister("GPIO_OTYPER(b)=", addr->OTYPER); 87 | mask = 0x1 << index; 88 | addr->OTYPER &= ~mask; // Reset Output push-pull 89 | printRegister("GPIO_OTYPER(a)=", addr->OTYPER); 90 | 91 | printRegister("GPIO_PUPDR(b)=", addr->PUPDR); 92 | mask = 0x3 << _index2; 93 | addr->PUPDR &= ~mask; // Reset port pull-up/pull-down 94 | printRegister("GPIO_PUPDR(a)=", addr->PUPDR); 95 | } 96 | 97 | /** 98 | * Initializes the CAN filter registers. 99 | * 100 | * The bxCAN provides up to 28 scalable/configurable identifier filter banks for selecting the incoming messages the software needs and discarding the others. 101 | * 102 | * @preconditions - This register can be written only when the filter initialization mode is set (FINIT=1) in the CAN_FMR register. 103 | * @params: index - Specified filter index. index 27:14 are available in connectivity line devices only. 104 | * @params: scale - Select filter scale. 105 | * 0: Dual 16-bit scale configuration 106 | * 1: Single 32-bit scale configuration 107 | * @params: mode - Select filter mode. 108 | * 0: Two 32-bit registers of filter bank x are in Identifier Mask mode 109 | * 1: Two 32-bit registers of filter bank x are in Identifier List mode 110 | * @params: fifo - Select filter assigned. 111 | * 0: Filter assigned to FIFO 0 112 | * 1: Filter assigned to FIFO 1 113 | * @params: bank1 - Filter bank register 1 114 | * @params: bank2 - Filter bank register 2 115 | * 116 | */ 117 | void CANSetFilter(uint8_t index, uint8_t scale, uint8_t mode, uint8_t fifo, uint32_t bank1, uint32_t bank2) { 118 | if (index > 27) return; 119 | 120 | CAN1->FA1R &= ~(0x1UL<FS1R &= ~(0x1UL<FS1R |= (0x1UL<FM1R &= ~(0x1UL<FM1R |= (0x1UL<FFA1R &= ~(0x1UL<FFA1R |= (0x1UL<sFilterRegister[index].FR1 = bank1; // Set filter bank registers1 140 | CAN1->sFilterRegister[index].FR2 = bank2; // Set filter bank registers2 141 | 142 | CAN1->FA1R |= (0x1UL<APB1ENR |= 0x2000000UL; // Enable CAN1 clock 167 | RCC->APB1ENR |= 0x4000000UL; // Enable CAN2 clock 168 | 169 | if (remap == 0) { 170 | // CAN1 171 | RCC->AHB1ENR |= 0x1; // Enable GPIOA clock 172 | CANSetGpio(GPIOA, 11); // Set PA11 173 | CANSetGpio(GPIOA, 12); // Set PA12 174 | 175 | // CAN2 176 | RCC->AHB1ENR |= 0x2; // Enable GPIOB clock 177 | CANSetGpio(GPIOB, 5); // Set PB5 178 | CANSetGpio(GPIOB, 6); // Set PB6 179 | } 180 | 181 | if (remap == 2) { 182 | // CAN1 183 | RCC->AHB1ENR |= 0x2; // Enable GPIOB clock 184 | CANSetGpio(GPIOB, 8); // Set PB8 185 | CANSetGpio(GPIOB, 9); // Set PB9 186 | 187 | // CAN2 188 | RCC->AHB1ENR |= 0x2; // Enable GPIOB clock 189 | CANSetGpio(GPIOB, 12); // Set PB12 190 | CANSetGpio(GPIOB, 13); // Set PB13 191 | } 192 | 193 | if (remap == 3) { 194 | // CAN1 195 | RCC->AHB1ENR |= 0x8; // Enable GPIOD clock 196 | CANSetGpio(GPIOD, 0); // Set PD0 197 | CANSetGpio(GPIOD, 1); // Set PD1 198 | 199 | // CAN2 200 | RCC->AHB1ENR |= 0x2; // Enable GPIOB clock 201 | CANSetGpio(GPIOB, 12); // Set PB12 202 | CANSetGpio(GPIOB, 13); // Set PB13 203 | } 204 | 205 | CAN1->MCR |= 0x1UL; // Require CAN1 to Initialization mode 206 | while (!(CAN1->MSR & 0x1UL)); // Wait for Initialization mode 207 | printRegister("CAN1->MCR=", CAN1->MCR); 208 | 209 | CAN2->MCR |= 0x1UL; // Require CAN2 to Initialization mode 210 | while (!(CAN2->MSR & 0x1UL)); // Wait for Initialization mode 211 | printRegister("CAN2->MCR=", CAN2->MCR); 212 | 213 | //CAN1->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) 214 | CAN1->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) 215 | 216 | //CAN2->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) 217 | CAN2->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) 218 | 219 | 220 | // Set bit rates 221 | CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x3FF)); 222 | CAN1->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x3FF); 223 | printRegister("CAN1->BTR=", CAN1->BTR); 224 | 225 | CAN2->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x3FF)); 226 | CAN2->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x3FF); 227 | printRegister("CAN2->BTR=", CAN2->BTR); 228 | 229 | // Configure Filters to default values 230 | CAN1->FMR |= 0x1UL; // Set to filter initialization mode 231 | CAN1->FMR &= 0xFFFFC0FF; // Clear CAN2 start bank 232 | printRegister("CAN1->FMR=", CAN1->FMR); 233 | 234 | // bxCAN has 28 filters. 235 | // These filters are used for both CAN1 and CAN2. 236 | // STM32F405 has CAN1 and CAN2, so CAN2 filters are offset by 14 237 | CAN1->FMR |= 0xE00; // Start bank for the CAN2 interface 238 | 239 | // Set fileter 0 240 | // Single 32-bit scale configuration 241 | // Two 32-bit registers of filter bank x are in Identifier Mask mode 242 | // Filter assigned to FIFO 0 243 | // Filter bank register to all 0 244 | CANSetFilter(0, 1, 0, 0, 0x0UL, 0x0UL); 245 | 246 | // Set fileter 14 247 | // Single 32-bit scale configuration 248 | // Two 32-bit registers of filter bank x are in Identifier Mask mode 249 | // Filter assigned to FIFO 0 250 | // Filter bank register to all 0 251 | CANSetFilter(14, 1, 0, 0, 0x0UL, 0x0UL); 252 | 253 | CAN1->FMR &= ~(0x1UL); // Deactivate initialization mode 254 | 255 | uint16_t TimeoutMilliseconds = 1000; 256 | bool can2 = false; 257 | CAN2->MCR &= ~(0x1UL); // Require CAN2 to normal mode 258 | 259 | // Wait for normal mode 260 | // If the connection is not correct, it will not return to normal mode. 261 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) { 262 | if ((CAN2->MSR & 0x1UL) == 0) { 263 | can2 = true; 264 | break; 265 | } 266 | delayMicroseconds(1000); 267 | } 268 | //Serial.print("can2="); 269 | //Serial.println(can2); 270 | if (can2) { 271 | Serial.println("CAN2 initialize ok"); 272 | } else { 273 | Serial.println("CAN2 initialize fail!!"); 274 | } 275 | 276 | bool can1 = false; 277 | CAN1->MCR &= ~(0x1UL); // Require CAN1 to normal mode 278 | 279 | // Wait for normal mode 280 | // If the connection is not correct, it will not return to normal mode. 281 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) { 282 | if ((CAN1->MSR & 0x1UL) == 0) { 283 | can1 = true; 284 | break; 285 | } 286 | delayMicroseconds(1000); 287 | } 288 | //Serial.print("can1="); 289 | //Serial.println(can1); 290 | if (can1) { 291 | Serial.println("CAN1 initialize ok"); 292 | } else { 293 | Serial.println("CAN1 initialize fail!!"); 294 | return false; 295 | } 296 | return true; 297 | } 298 | 299 | 300 | #define STM32_CAN_TIR_TXRQ (1U << 0U) // Bit 0: Transmit Mailbox Request 301 | #define STM32_CAN_RIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 302 | #define STM32_CAN_RIR_IDE (1U << 2U) // Bit 2: Identifier Extension 303 | #define STM32_CAN_TIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 304 | #define STM32_CAN_TIR_IDE (1U << 2U) // Bit 2: Identifier Extension 305 | 306 | #define CAN_EXT_ID_MASK 0x1FFFFFFFU 307 | #define CAN_STD_ID_MASK 0x000007FFU 308 | 309 | /** 310 | * Decodes CAN messages from the data registers and populates a 311 | * CAN message struct with the data fields. 312 | * 313 | * @preconditions - A valid CAN message is received 314 | * @params CAN_rx_msg - CAN message structure for reception 315 | * 316 | */ 317 | void CANReceive(uint8_t ch, CAN_msg_t* CAN_rx_msg) 318 | { 319 | if(ch == 1) { 320 | uint32_t id = CAN1->sFIFOMailBox[0].RIR; 321 | if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format 322 | CAN_rx_msg->format = STANDARD_FORMAT;; 323 | CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U)); 324 | } 325 | else { // Extended frame format 326 | CAN_rx_msg->format = EXTENDED_FORMAT;; 327 | CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U)); 328 | } 329 | 330 | if ((id & STM32_CAN_RIR_RTR) == 0) { // Data frame 331 | CAN_rx_msg->type = DATA_FRAME; 332 | } 333 | else { // Remote frame 334 | CAN_rx_msg->type = REMOTE_FRAME; 335 | } 336 | 337 | 338 | CAN_rx_msg->len = (CAN1->sFIFOMailBox[0].RDTR) & 0xFUL; 339 | 340 | CAN_rx_msg->data[0] = 0xFFUL & CAN1->sFIFOMailBox[0].RDLR; 341 | CAN_rx_msg->data[1] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 8); 342 | CAN_rx_msg->data[2] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 16); 343 | CAN_rx_msg->data[3] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 24); 344 | CAN_rx_msg->data[4] = 0xFFUL & CAN1->sFIFOMailBox[0].RDHR; 345 | CAN_rx_msg->data[5] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 8); 346 | CAN_rx_msg->data[6] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 16); 347 | CAN_rx_msg->data[7] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 24); 348 | 349 | CAN1->RF0R |= 0x20UL; 350 | } // end CAN1 351 | 352 | if(ch == 2) { 353 | uint32_t id = CAN2->sFIFOMailBox[0].RIR; 354 | if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format 355 | CAN_rx_msg->format = STANDARD_FORMAT;; 356 | CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U)); 357 | } 358 | else { // Extended frame format 359 | CAN_rx_msg->format = EXTENDED_FORMAT;; 360 | CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U)); 361 | } 362 | 363 | if ((id & STM32_CAN_RIR_RTR) == 0) { // Data frame 364 | CAN_rx_msg->type = DATA_FRAME; 365 | } 366 | else { // Remote frame 367 | CAN_rx_msg->type = REMOTE_FRAME; 368 | } 369 | 370 | 371 | CAN_rx_msg->len = (CAN2->sFIFOMailBox[0].RDTR) & 0xFUL; 372 | 373 | CAN_rx_msg->data[0] = 0xFFUL & CAN2->sFIFOMailBox[0].RDLR; 374 | CAN_rx_msg->data[1] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDLR >> 8); 375 | CAN_rx_msg->data[2] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDLR >> 16); 376 | CAN_rx_msg->data[3] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDLR >> 24); 377 | CAN_rx_msg->data[4] = 0xFFUL & CAN2->sFIFOMailBox[0].RDHR; 378 | CAN_rx_msg->data[5] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDHR >> 8); 379 | CAN_rx_msg->data[6] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDHR >> 16); 380 | CAN_rx_msg->data[7] = 0xFFUL & (CAN2->sFIFOMailBox[0].RDHR >> 24); 381 | 382 | CAN2->RF0R |= 0x20UL; 383 | } // END CAN2 384 | 385 | } 386 | 387 | /** 388 | * Encodes CAN messages using the CAN message struct and populates the 389 | * data registers with the sent. 390 | * 391 | * @params CAN_tx_msg - CAN message structure for transmission 392 | * 393 | */ 394 | void CANSend(uint8_t ch, CAN_msg_t* CAN_tx_msg) 395 | { 396 | volatile int count = 0; 397 | 398 | uint32_t out = 0; 399 | if (CAN_tx_msg->format == EXTENDED_FORMAT) { // Extended frame format 400 | out = ((CAN_tx_msg->id & CAN_EXT_ID_MASK) << 3U) | STM32_CAN_TIR_IDE; 401 | } 402 | else { // Standard frame format 403 | out = ((CAN_tx_msg->id & CAN_STD_ID_MASK) << 21U); 404 | } 405 | 406 | // Remote frame 407 | if (CAN_tx_msg->type == REMOTE_FRAME) { 408 | out |= STM32_CAN_TIR_RTR; 409 | } 410 | 411 | if (ch == 1) { 412 | CAN1->sTxMailBox[0].TDTR &= ~(0xF); 413 | CAN1->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL; 414 | 415 | CAN1->sTxMailBox[0].TDLR = (((uint32_t) CAN_tx_msg->data[3] << 24) | 416 | ((uint32_t) CAN_tx_msg->data[2] << 16) | 417 | ((uint32_t) CAN_tx_msg->data[1] << 8) | 418 | ((uint32_t) CAN_tx_msg->data[0] )); 419 | CAN1->sTxMailBox[0].TDHR = (((uint32_t) CAN_tx_msg->data[7] << 24) | 420 | ((uint32_t) CAN_tx_msg->data[6] << 16) | 421 | ((uint32_t) CAN_tx_msg->data[5] << 8) | 422 | ((uint32_t) CAN_tx_msg->data[4] )); 423 | 424 | // Send Go 425 | CAN1->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; 426 | 427 | // Wait until the mailbox is empty 428 | while(CAN1->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000); 429 | 430 | // The mailbox don't becomes empty while loop 431 | if (CAN1->sTxMailBox[0].TIR & 0x1UL) { 432 | Serial.println("Send Fail"); 433 | Serial.println(CAN1->ESR); 434 | Serial.println(CAN1->MSR); 435 | Serial.println(CAN1->TSR); 436 | } 437 | } // end CAN1 438 | 439 | if (ch == 2) { 440 | CAN2->sTxMailBox[0].TDTR &= ~(0xF); 441 | CAN2->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL; 442 | 443 | CAN2->sTxMailBox[0].TDLR = (((uint32_t) CAN_tx_msg->data[3] << 24) | 444 | ((uint32_t) CAN_tx_msg->data[2] << 16) | 445 | ((uint32_t) CAN_tx_msg->data[1] << 8) | 446 | ((uint32_t) CAN_tx_msg->data[0] )); 447 | CAN2->sTxMailBox[0].TDHR = (((uint32_t) CAN_tx_msg->data[7] << 24) | 448 | ((uint32_t) CAN_tx_msg->data[6] << 16) | 449 | ((uint32_t) CAN_tx_msg->data[5] << 8) | 450 | ((uint32_t) CAN_tx_msg->data[4] )); 451 | 452 | // Send Go 453 | CAN2->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; 454 | 455 | // Wait until the mailbox is empty 456 | while(CAN2->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000); 457 | 458 | // The mailbox don't becomes empty while loop 459 | if (CAN2->sTxMailBox[0].TIR & 0x1UL) { 460 | Serial.println("Send Fail"); 461 | Serial.println(CAN1->ESR); 462 | Serial.println(CAN1->MSR); 463 | Serial.println(CAN1->TSR); 464 | } 465 | } // end CAN2 466 | 467 | } 468 | 469 | /** 470 | * Returns whether there are CAN messages available. 471 | * 472 | * @returns If pending CAN messages are in the CAN controller 473 | * 474 | */ 475 | uint8_t CANMsgAvail(uint8_t ch) 476 | { 477 | if (ch == 1) { 478 | // Check for pending FIFO 0 messages 479 | return CAN1->RF0R & 0x3UL; 480 | } // end CAN1 481 | 482 | if (ch == 2) { 483 | // Check for pending FIFO 0 messages 484 | return CAN2->RF0R & 0x3UL; 485 | } // end CAN2 486 | 487 | } 488 | 489 | 490 | uint8_t counter = 0; 491 | uint8_t frameLength = 0; 492 | unsigned long previousMillis = 0; // stores last time output was updated 493 | const long interval = 1000; // transmission interval (milliseconds) 494 | 495 | void setup() { 496 | Serial.begin(115200); 497 | bool ret = CANInit(CAN_500KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 498 | //bool ret = CANInit(CAN_500KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 499 | //bool ret = CANInit(CAN_500KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 500 | //bool ret = CANInit(CAN_1000KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 501 | //bool ret = CANInit(CAN_1000KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 502 | //bool ret = CANInit(CAN_1000KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 503 | if (!ret) while(true); 504 | } 505 | 506 | void loop() { 507 | CAN_msg_t CAN_TX_msg; 508 | CAN_msg_t CAN_RX_msg; 509 | 510 | CAN_TX_msg.data[0] = 0x00; 511 | CAN_TX_msg.data[1] = 0x01; 512 | CAN_TX_msg.data[2] = 0x02; 513 | CAN_TX_msg.data[3] = 0x03; 514 | CAN_TX_msg.data[4] = 0x04; 515 | CAN_TX_msg.data[5] = 0x05; 516 | CAN_TX_msg.data[6] = 0x06; 517 | CAN_TX_msg.data[7] = 0x07; 518 | CAN_TX_msg.len = frameLength; 519 | 520 | uint8_t send_ch = 1; 521 | unsigned long currentMillis = millis(); 522 | if (currentMillis - previousMillis >= interval) { 523 | previousMillis = currentMillis; 524 | if ( ( counter % 2) == 0) { 525 | CAN_TX_msg.type = DATA_FRAME; 526 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 527 | CAN_TX_msg.format = EXTENDED_FORMAT; 528 | CAN_TX_msg.id = 0x32F446; 529 | } else { 530 | CAN_TX_msg.type = DATA_FRAME; 531 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 532 | CAN_TX_msg.format = STANDARD_FORMAT; 533 | CAN_TX_msg.id = 0x446; 534 | } 535 | CANSend(send_ch, &CAN_TX_msg); 536 | frameLength++; 537 | if (frameLength == 9) frameLength = 0; 538 | counter++; 539 | } 540 | 541 | uint8_t recv_ch = 1; 542 | if(CANMsgAvail(recv_ch)) { 543 | CANReceive(recv_ch, &CAN_RX_msg); 544 | Serial.print("Channel:"); 545 | Serial.print(recv_ch); 546 | 547 | if (CAN_RX_msg.format == EXTENDED_FORMAT) { 548 | Serial.print(" Extended ID: 0x"); 549 | if (CAN_RX_msg.id < 0x10000000) Serial.print("0"); 550 | if (CAN_RX_msg.id < 0x1000000) Serial.print("0"); 551 | if (CAN_RX_msg.id < 0x100000) Serial.print("0"); 552 | if (CAN_RX_msg.id < 0x10000) Serial.print("0"); 553 | if (CAN_RX_msg.id < 0x1000) Serial.print("0"); 554 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 555 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 556 | Serial.print(CAN_RX_msg.id, HEX); 557 | } else { 558 | Serial.print(" Standard ID: 0x"); 559 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 560 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 561 | Serial.print(CAN_RX_msg.id, HEX); 562 | Serial.print(" "); 563 | } 564 | 565 | Serial.print(" DLC: "); 566 | Serial.print(CAN_RX_msg.len); 567 | if (CAN_RX_msg.type == DATA_FRAME) { 568 | Serial.print(" Data: "); 569 | for(int i=0; i 7) { 57 | _index4 = (index - 8) * 4; 58 | ofs = 1; 59 | } 60 | 61 | uint32_t mask; 62 | printRegister("GPIO_AFR(b)=", addr->AFR[1]); 63 | mask = 0xF << _index4; 64 | addr->AFR[ofs] &= ~mask; // Reset alternate function 65 | setting = 0x9; // AF9 66 | mask = setting << _index4; 67 | addr->AFR[ofs] |= mask; // Set alternate function 68 | printRegister("GPIO_AFR(a)=", addr->AFR[1]); 69 | 70 | printRegister("GPIO_MODER(b)=", addr->MODER); 71 | mask = 0x3 << _index2; 72 | addr->MODER &= ~mask; // Reset mode 73 | setting = 0x2; // Alternate function mode 74 | mask = setting << _index2; 75 | addr->MODER |= mask; // Set mode 76 | printRegister("GPIO_MODER(a)=", addr->MODER); 77 | 78 | printRegister("GPIO_OSPEEDR(b)=", addr->OSPEEDR); 79 | mask = 0x3 << _index2; 80 | addr->OSPEEDR &= ~mask; // Reset speed 81 | setting = speed; 82 | mask = setting << _index2; 83 | addr->OSPEEDR |= mask; // Set speed 84 | printRegister("GPIO_OSPEEDR(a)=", addr->OSPEEDR); 85 | 86 | printRegister("GPIO_OTYPER(b)=", addr->OTYPER); 87 | mask = 0x1 << index; 88 | addr->OTYPER &= ~mask; // Reset Output push-pull 89 | printRegister("GPIO_OTYPER(a)=", addr->OTYPER); 90 | 91 | printRegister("GPIO_PUPDR(b)=", addr->PUPDR); 92 | mask = 0x3 << _index2; 93 | addr->PUPDR &= ~mask; // Reset port pull-up/pull-down 94 | printRegister("GPIO_PUPDR(a)=", addr->PUPDR); 95 | } 96 | 97 | /** 98 | * Initializes the CAN filter registers. 99 | * 100 | * The bxCAN provides up to 14 scalable/configurable identifier filter banks for selecting the incoming messages the software needs and discarding the others. 101 | * 102 | * @preconditions - This register can be written only when the filter initialization mode is set (FINIT=1) in the CAN_FMR register. 103 | * @params: index - Specified filter index. index 27:14 are available in connectivity line devices only. 104 | * @params: scale - Select filter scale. 105 | * 0: Dual 16-bit scale configuration 106 | * 1: Single 32-bit scale configuration 107 | * @params: mode - Select filter mode. 108 | * 0: Two 32-bit registers of filter bank x are in Identifier Mask mode 109 | * 1: Two 32-bit registers of filter bank x are in Identifier List mode 110 | * @params: fifo - Select filter assigned. 111 | * 0: Filter assigned to FIFO 0 112 | * 1: Filter assigned to FIFO 1 113 | * @params: bank1 - Filter bank register 1 114 | * @params: bank2 - Filter bank register 2 115 | * 116 | */ 117 | void CANSetFilter(uint8_t index, uint8_t scale, uint8_t mode, uint8_t fifo, uint32_t bank1, uint32_t bank2) { 118 | if (index > 13) return; 119 | 120 | CAN1->FA1R &= ~(0x1UL<FS1R &= ~(0x1UL<FS1R |= (0x1UL<FM1R &= ~(0x1UL<FM1R |= (0x1UL<FFA1R &= ~(0x1UL<FFA1R |= (0x1UL<sFilterRegister[index].FR1 = bank1; // Set filter bank registers1 140 | CAN1->sFilterRegister[index].FR2 = bank2; // Set filter bank registers2 141 | 142 | CAN1->FA1R |= (0x1UL<APB1ENR |= 0x2000000UL; // Enable CAN1 clock 164 | 165 | if (remap == 0) { 166 | RCC->AHB1ENR |= 0x1; // Enable GPIOA clock 167 | CANSetGpio(GPIOA, 11); // Set PA11 168 | CANSetGpio(GPIOA, 12); // Set PA12 169 | } 170 | 171 | if (remap == 2) { 172 | RCC->AHB1ENR |= 0x2; // Enable GPIOB clock 173 | CANSetGpio(GPIOB, 8); // Set PB8 174 | CANSetGpio(GPIOB, 9); // Set PB9 175 | } 176 | 177 | if (remap == 3) { 178 | RCC->AHB1ENR |= 0x8; // Enable GPIOD clock 179 | CANSetGpio(GPIOD, 0); // Set PD0 180 | CANSetGpio(GPIOD, 1); // Set PD1 181 | } 182 | 183 | CAN1->MCR |= 0x1UL; // Require CAN1 to Initialization mode 184 | while (!(CAN1->MSR & 0x1UL)); // Wait for Initialization mode 185 | printRegister("CAN1->MCR=", CAN1->MCR); 186 | 187 | //CAN1->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) 188 | CAN1->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) 189 | 190 | // Set bit rates 191 | CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x3FF)); 192 | CAN1->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x3FF); 193 | printRegister("CAN1->BTR=", CAN1->BTR); 194 | 195 | // Configure Filters to default values 196 | CAN1->FMR |= 0x1UL; // Set to filter initialization mode 197 | 198 | // Set fileter 0 199 | // Single 32-bit scale configuration 200 | // Two 32-bit registers of filter bank x are in Identifier Mask mode 201 | // Filter assigned to FIFO 0 202 | // Filter bank register to all 0 203 | CANSetFilter(0, 1, 0, 0, 0x0UL, 0x0UL); 204 | 205 | CAN1->FMR &= ~(0x1UL); // Deactivate initialization mode 206 | 207 | uint16_t TimeoutMilliseconds = 1000; 208 | bool can1 = false; 209 | CAN1->MCR &= ~(0x1UL); // Require CAN1 to normal mode 210 | 211 | // Wait for normal mode 212 | // If the connection is not correct, it will not return to normal mode. 213 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) { 214 | Serial.print("CAN1->MCR=0x"); 215 | Serial.print(CAN1->MCR, HEX); 216 | Serial.print(" CAN1->MSR=0x"); 217 | Serial.println(CAN1->MSR, HEX); 218 | if ((CAN1->MSR & 0x1UL) == 0) { 219 | can1 = true; 220 | break; 221 | } 222 | delayMicroseconds(1000); 223 | } 224 | Serial.print("can1="); 225 | Serial.println(can1); 226 | if (can1) { 227 | Serial.println("CAN1 initialize ok"); 228 | } else { 229 | Serial.println("CAN1 initialize fail!!"); 230 | return false; 231 | } 232 | return true; 233 | } 234 | 235 | 236 | #define STM32_CAN_TIR_TXRQ (1U << 0U) // Bit 0: Transmit Mailbox Request 237 | #define STM32_CAN_RIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 238 | #define STM32_CAN_RIR_IDE (1U << 2U) // Bit 2: Identifier Extension 239 | #define STM32_CAN_TIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 240 | #define STM32_CAN_TIR_IDE (1U << 2U) // Bit 2: Identifier Extension 241 | 242 | #define CAN_EXT_ID_MASK 0x1FFFFFFFU 243 | #define CAN_STD_ID_MASK 0x000007FFU 244 | 245 | /** 246 | * Decodes CAN messages from the data registers and populates a 247 | * CAN message struct with the data fields. 248 | * 249 | * @preconditions - A valid CAN message is received 250 | * @params CAN_rx_msg - CAN message structure for reception 251 | * 252 | */ 253 | void CANReceive(CAN_msg_t* CAN_rx_msg) 254 | { 255 | uint32_t id = CAN1->sFIFOMailBox[0].RIR; 256 | if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format 257 | CAN_rx_msg->format = STANDARD_FORMAT;; 258 | CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U)); 259 | } 260 | else { // Extended frame format 261 | CAN_rx_msg->format = EXTENDED_FORMAT;; 262 | CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U)); 263 | } 264 | 265 | if ((id & STM32_CAN_RIR_RTR) == 0) { // Data frame 266 | CAN_rx_msg->type = DATA_FRAME; 267 | } 268 | else { // Remote frame 269 | CAN_rx_msg->type = REMOTE_FRAME; 270 | } 271 | 272 | 273 | CAN_rx_msg->len = (CAN1->sFIFOMailBox[0].RDTR) & 0xFUL; 274 | 275 | CAN_rx_msg->data[0] = 0xFFUL & CAN1->sFIFOMailBox[0].RDLR; 276 | CAN_rx_msg->data[1] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 8); 277 | CAN_rx_msg->data[2] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 16); 278 | CAN_rx_msg->data[3] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 24); 279 | CAN_rx_msg->data[4] = 0xFFUL & CAN1->sFIFOMailBox[0].RDHR; 280 | CAN_rx_msg->data[5] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 8); 281 | CAN_rx_msg->data[6] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 16); 282 | CAN_rx_msg->data[7] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 24); 283 | 284 | CAN1->RF0R |= 0x20UL; 285 | } 286 | 287 | /** 288 | * Encodes CAN messages using the CAN message struct and populates the 289 | * data registers with the sent. 290 | * 291 | * @params CAN_tx_msg - CAN message structure for transmission 292 | * 293 | */ 294 | void CANSend(CAN_msg_t* CAN_tx_msg) 295 | { 296 | volatile int count = 0; 297 | 298 | uint32_t out = 0; 299 | if (CAN_tx_msg->format == EXTENDED_FORMAT) { // Extended frame format 300 | out = ((CAN_tx_msg->id & CAN_EXT_ID_MASK) << 3U) | STM32_CAN_TIR_IDE; 301 | } 302 | else { // Standard frame format 303 | out = ((CAN_tx_msg->id & CAN_STD_ID_MASK) << 21U); 304 | } 305 | 306 | // Remote frame 307 | if (CAN_tx_msg->type == REMOTE_FRAME) { 308 | out |= STM32_CAN_TIR_RTR; 309 | } 310 | 311 | CAN1->sTxMailBox[0].TDTR &= ~(0xF); 312 | CAN1->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL; 313 | 314 | CAN1->sTxMailBox[0].TDLR = (((uint32_t) CAN_tx_msg->data[3] << 24) | 315 | ((uint32_t) CAN_tx_msg->data[2] << 16) | 316 | ((uint32_t) CAN_tx_msg->data[1] << 8) | 317 | ((uint32_t) CAN_tx_msg->data[0] )); 318 | CAN1->sTxMailBox[0].TDHR = (((uint32_t) CAN_tx_msg->data[7] << 24) | 319 | ((uint32_t) CAN_tx_msg->data[6] << 16) | 320 | ((uint32_t) CAN_tx_msg->data[5] << 8) | 321 | ((uint32_t) CAN_tx_msg->data[4] )); 322 | 323 | // Send Go 324 | CAN1->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; 325 | 326 | // Wait until the mailbox is empty 327 | while(CAN1->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000); 328 | 329 | // The mailbox don't becomes empty while loop 330 | if (CAN1->sTxMailBox[0].TIR & 0x1UL) { 331 | Serial.println("Send Fail"); 332 | Serial.println(CAN1->ESR); 333 | Serial.println(CAN1->MSR); 334 | Serial.println(CAN1->TSR); 335 | } 336 | } 337 | 338 | /** 339 | * Returns whether there are CAN messages available. 340 | * 341 | * @returns If pending CAN messages are in the CAN controller 342 | * 343 | */ 344 | uint8_t CANMsgAvail() 345 | { 346 | // Check for pending FIFO 0 messages 347 | return CAN1->RF0R & 0x3UL; 348 | } 349 | 350 | 351 | uint8_t counter = 0; 352 | uint8_t frameLength = 0; 353 | unsigned long previousMillis = 0; // stores last time output was updated 354 | const long interval = 1000; // transmission interval (milliseconds) 355 | 356 | void setup() { 357 | Serial.begin(115200); 358 | delay(1000); 359 | bool ret = CANInit(CAN_500KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 360 | //bool ret = CANInit(CAN_500KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 361 | //bool ret = CANInit(CAN_500KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 362 | //bool ret = CANInit(CAN_1000KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 363 | //bool ret = CANInit(CAN_1000KBPS, 2); // CAN_RX mapped to PB8, CAN_TX mapped to PB9 364 | //bool ret = CANInit(CAN_1000KBPS, 3); // CAN_RX mapped to PD0, CAN_TX mapped to PD1 365 | if (!ret) while(true); 366 | } 367 | 368 | void loop() { 369 | CAN_msg_t CAN_TX_msg; 370 | CAN_msg_t CAN_RX_msg; 371 | 372 | CAN_TX_msg.data[0] = 0x00; 373 | CAN_TX_msg.data[1] = 0x01; 374 | CAN_TX_msg.data[2] = 0x02; 375 | CAN_TX_msg.data[3] = 0x03; 376 | CAN_TX_msg.data[4] = 0x04; 377 | CAN_TX_msg.data[5] = 0x05; 378 | CAN_TX_msg.data[6] = 0x06; 379 | CAN_TX_msg.data[7] = 0x07; 380 | CAN_TX_msg.len = frameLength; 381 | 382 | unsigned long currentMillis = millis(); 383 | if (currentMillis - previousMillis >= interval) { 384 | previousMillis = currentMillis; 385 | if ( ( counter % 2) == 0) { 386 | CAN_TX_msg.type = DATA_FRAME; 387 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 388 | CAN_TX_msg.format = EXTENDED_FORMAT; 389 | CAN_TX_msg.id = 0x32F722; 390 | } else { 391 | CAN_TX_msg.type = DATA_FRAME; 392 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 393 | CAN_TX_msg.format = STANDARD_FORMAT; 394 | CAN_TX_msg.id = 0x722; 395 | } 396 | CANSend(&CAN_TX_msg); 397 | frameLength++; 398 | if (frameLength == 9) frameLength = 0; 399 | counter++; 400 | } 401 | 402 | if(CANMsgAvail()) { 403 | CANReceive(&CAN_RX_msg); 404 | 405 | if (CAN_RX_msg.format == EXTENDED_FORMAT) { 406 | Serial.print(" Extended ID: 0x"); 407 | if (CAN_RX_msg.id < 0x10000000) Serial.print("0"); 408 | if (CAN_RX_msg.id < 0x1000000) Serial.print("0"); 409 | if (CAN_RX_msg.id < 0x100000) Serial.print("0"); 410 | if (CAN_RX_msg.id < 0x10000) Serial.print("0"); 411 | if (CAN_RX_msg.id < 0x1000) Serial.print("0"); 412 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 413 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 414 | Serial.print(CAN_RX_msg.id, HEX); 415 | } else { 416 | Serial.print(" Standard ID: 0x"); 417 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 418 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 419 | Serial.print(CAN_RX_msg.id, HEX); 420 | Serial.print(" "); 421 | } 422 | 423 | Serial.print(" DLC: "); 424 | Serial.print(CAN_RX_msg.len); 425 | if (CAN_RX_msg.type == DATA_FRAME) { 426 | Serial.print(" Data: "); 427 | for(int i=0; i 7) { 62 | _index4 = (index - 8) * 4; 63 | ofs = 1; 64 | } 65 | 66 | uint32_t mask; 67 | printRegister("GPIO_AFR(b)=", addr->AFR[1]); 68 | mask = 0xF << _index4; 69 | addr->AFR[ofs] &= ~mask; // Reset alternate function 70 | //setting = 0x9; // AF9 71 | setting = afry; // Alternative function selection 72 | mask = setting << _index4; 73 | addr->AFR[ofs] |= mask; // Set alternate function 74 | printRegister("GPIO_AFR(a)=", addr->AFR[1]); 75 | 76 | printRegister("GPIO_MODER(b)=", addr->MODER); 77 | mask = 0x3 << _index2; 78 | addr->MODER &= ~mask; // Reset mode 79 | setting = 0x2; // Alternate function mode 80 | mask = setting << _index2; 81 | addr->MODER |= mask; // Set mode 82 | printRegister("GPIO_MODER(a)=", addr->MODER); 83 | 84 | printRegister("GPIO_OSPEEDR(b)=", addr->OSPEEDR); 85 | mask = 0x3 << _index2; 86 | addr->OSPEEDR &= ~mask; // Reset speed 87 | setting = speed; 88 | mask = setting << _index2; 89 | addr->OSPEEDR |= mask; // Set speed 90 | printRegister("GPIO_OSPEEDR(a)=", addr->OSPEEDR); 91 | 92 | printRegister("GPIO_OTYPER(b)=", addr->OTYPER); 93 | mask = 0x1 << index; 94 | addr->OTYPER &= ~mask; // Reset Output push-pull 95 | printRegister("GPIO_OTYPER(a)=", addr->OTYPER); 96 | 97 | printRegister("GPIO_PUPDR(b)=", addr->PUPDR); 98 | mask = 0x3 << _index2; 99 | addr->PUPDR &= ~mask; // Reset port pull-up/pull-down 100 | printRegister("GPIO_PUPDR(a)=", addr->PUPDR); 101 | } 102 | 103 | 104 | /** 105 | * Initializes the CAN filter registers. 106 | * 107 | * The bxCAN provides up to 14 scalable/configurable identifier filter banks, for selecting the incoming messages, that the software needs and discarding the others. 108 | * 109 | * @preconditions - This register can be written only when the filter initialization mode is set (FINIT=1) in the CAN_FMR register. 110 | * @params: index - Specified filter index. index 27:14 are available in connectivity line devices only. 111 | * @params: scale - Select filter scale. 112 | * 0: Dual 16-bit scale configuration 113 | * 1: Single 32-bit scale configuration 114 | * @params: mode - Select filter mode. 115 | * 0: Two 32-bit registers of filter bank x are in Identifier Mask mode 116 | * 1: Two 32-bit registers of filter bank x are in Identifier List mode 117 | * @params: fifo - Select filter assigned. 118 | * 0: Filter assigned to FIFO 0 119 | * 1: Filter assigned to FIFO 1 120 | * @params: bank1 - Filter bank register 1 121 | * @params: bank2 - Filter bank register 2 122 | * 123 | */ 124 | void CANSetFilter(uint8_t index, uint8_t scale, uint8_t mode, uint8_t fifo, uint32_t bank1, uint32_t bank2) { 125 | if (index > 13) return; 126 | 127 | CAN1->FA1R &= ~(0x1UL<FS1R &= ~(0x1UL<FS1R |= (0x1UL<FM1R &= ~(0x1UL<FM1R |= (0x1UL<FFA1R &= ~(0x1UL<FFA1R |= (0x1UL<sFilterRegister[index].FR1 = bank1; // Set filter bank registers1 147 | CAN1->sFilterRegister[index].FR2 = bank2; // Set filter bank registers2 148 | 149 | CAN1->FA1R |= (0x1UL<APB1ENR1 |= 0x2000000UL; // Enable CAN clock 171 | 172 | if (remap == 0) { 173 | RCC->AHB2ENR |= 0x1UL; // Enable GPIOA clock 174 | CANSetGpio(GPIOA, 11, AF9); // Set PA11 to AF9 175 | CANSetGpio(GPIOA, 12, AF9); // Set PA12 to AF9 176 | } 177 | 178 | if (remap == 1) { 179 | RCC->AHB2ENR |= 0x2UL; // Enable GPIOB clock 180 | CANSetGpio(GPIOB, 5, AF3); // Set PB5 to AF3 181 | CANSetGpio(GPIOB, 6, AF8); // Set PB6 to AF8 182 | } 183 | 184 | if (remap == 2) { 185 | RCC->AHB2ENR |= 0x2UL; // Enable GPIOB clock 186 | CANSetGpio(GPIOB, 8, AF9); // Set PB8 to AF9 187 | CANSetGpio(GPIOB, 9, AF9); // Set PB9 to AF9 188 | } 189 | 190 | if (remap == 3) { 191 | RCC->AHB2ENR |= 0x2UL; // Enable GPIOB clock 192 | CANSetGpio(GPIOB, 12, AF10); // Set PB12 to AF10 193 | CANSetGpio(GPIOB, 13, AF10); // Set PB13 to AF10 194 | } 195 | 196 | CAN1->MCR |= 0x1UL; // Set CAN to Initialization mode 197 | while (!(CAN1->MSR & 0x1UL)); // Wait for Initialization mode 198 | 199 | //CAN1->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) 200 | CAN1->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) 201 | 202 | // Set bit rates 203 | //CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x1FF)); 204 | //CAN1->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x1FF); 205 | CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x3FF)); 206 | CAN1->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x3FF); 207 | printRegister("CAN1->BTR=", CAN1->BTR); 208 | 209 | // Configure Filters to default values 210 | CAN1->FMR |= 0x1UL; // Set to filter initialization mode 211 | 212 | // Set fileter 0 213 | // Single 32-bit scale configuration 214 | // Two 32-bit registers of filter bank x are in Identifier Mask mode 215 | // Filter assigned to FIFO 0 216 | // Filter bank register to all 0 217 | CANSetFilter(0, 1, 0, 0, 0x0UL, 0x0UL); 218 | 219 | CAN1->FMR &= ~(0x1UL); // Deactivate initialization mode 220 | 221 | uint16_t TimeoutMilliseconds = 1000; 222 | bool can1 = false; 223 | CAN1->MCR &= ~(0x1UL); // Require CAN1 to normal mode 224 | 225 | // Wait for normal mode 226 | // If the connection is not correct, it will not return to normal mode. 227 | for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) { 228 | if ((CAN1->MSR & 0x1UL) == 0) { 229 | can1 = true; 230 | break; 231 | } 232 | delayMicroseconds(1000); 233 | } 234 | //Serial.print("can1="); 235 | //Serial.println(can1); 236 | if (can1) { 237 | Serial.println("CAN1 initialize ok"); 238 | } else { 239 | Serial.println("CAN1 initialize fail!!"); 240 | return false; 241 | } 242 | return true; 243 | } 244 | 245 | 246 | #define STM32_CAN_TIR_TXRQ (1U << 0U) // Bit 0: Transmit Mailbox Request 247 | #define STM32_CAN_RIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 248 | #define STM32_CAN_RIR_IDE (1U << 2U) // Bit 2: Identifier Extension 249 | #define STM32_CAN_TIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request 250 | #define STM32_CAN_TIR_IDE (1U << 2U) // Bit 2: Identifier Extension 251 | 252 | #define CAN_EXT_ID_MASK 0x1FFFFFFFU 253 | #define CAN_STD_ID_MASK 0x000007FFU 254 | 255 | /** 256 | * Decodes CAN messages from the data registers and populates a 257 | * CAN message struct with the data fields. 258 | * 259 | * @preconditions - A valid CAN message is received 260 | * @params CAN_rx_msg - CAN message structure for reception 261 | * 262 | */ 263 | void CANReceive(CAN_msg_t* CAN_rx_msg) 264 | { 265 | uint32_t id = CAN1->sFIFOMailBox[0].RIR; 266 | if ((id & STM32_CAN_RIR_IDE) == 0) { // Standard frame format 267 | CAN_rx_msg->format = STANDARD_FORMAT;; 268 | CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21U)); 269 | } 270 | else { // Extended frame format 271 | CAN_rx_msg->format = EXTENDED_FORMAT;; 272 | CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3U)); 273 | } 274 | 275 | if ((id & STM32_CAN_RIR_RTR) == 0) { // Data frame 276 | CAN_rx_msg->type = DATA_FRAME; 277 | } 278 | else { // Remote frame 279 | CAN_rx_msg->type = REMOTE_FRAME; 280 | } 281 | 282 | 283 | CAN_rx_msg->len = (CAN1->sFIFOMailBox[0].RDTR) & 0xFUL; 284 | 285 | CAN_rx_msg->data[0] = 0xFFUL & CAN1->sFIFOMailBox[0].RDLR; 286 | CAN_rx_msg->data[1] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 8); 287 | CAN_rx_msg->data[2] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 16); 288 | CAN_rx_msg->data[3] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 24); 289 | CAN_rx_msg->data[4] = 0xFFUL & CAN1->sFIFOMailBox[0].RDHR; 290 | CAN_rx_msg->data[5] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 8); 291 | CAN_rx_msg->data[6] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 16); 292 | CAN_rx_msg->data[7] = 0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 24); 293 | 294 | // Release FIFO 0 output mailbox. 295 | // Make the next incoming message available. 296 | CAN1->RF0R |= 0x20UL; 297 | } 298 | 299 | /** 300 | * Encodes CAN messages using the CAN message struct and populates the 301 | * data registers with the sent. 302 | * 303 | * @params CAN_tx_msg - CAN message structure for transmission 304 | * 305 | */ 306 | void CANSend(CAN_msg_t* CAN_tx_msg) 307 | { 308 | volatile int count = 0; 309 | 310 | uint32_t out = 0; 311 | if (CAN_tx_msg->format == EXTENDED_FORMAT) { // Extended frame format 312 | out = ((CAN_tx_msg->id & CAN_EXT_ID_MASK) << 3U) | STM32_CAN_TIR_IDE; 313 | } 314 | else { // Standard frame format 315 | out = ((CAN_tx_msg->id & CAN_STD_ID_MASK) << 21U); 316 | } 317 | 318 | // Remote frame 319 | if (CAN_tx_msg->type == REMOTE_FRAME) { 320 | out |= STM32_CAN_TIR_RTR; 321 | } 322 | 323 | CAN1->sTxMailBox[0].TDTR &= ~(0xF); 324 | CAN1->sTxMailBox[0].TDTR |= CAN_tx_msg->len & 0xFUL; 325 | 326 | CAN1->sTxMailBox[0].TDLR = (((uint32_t) CAN_tx_msg->data[3] << 24) | 327 | ((uint32_t) CAN_tx_msg->data[2] << 16) | 328 | ((uint32_t) CAN_tx_msg->data[1] << 8) | 329 | ((uint32_t) CAN_tx_msg->data[0] )); 330 | CAN1->sTxMailBox[0].TDHR = (((uint32_t) CAN_tx_msg->data[7] << 24) | 331 | ((uint32_t) CAN_tx_msg->data[6] << 16) | 332 | ((uint32_t) CAN_tx_msg->data[5] << 8) | 333 | ((uint32_t) CAN_tx_msg->data[4] )); 334 | 335 | // Send Go 336 | CAN1->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; 337 | 338 | // Wait until the mailbox is empty 339 | while(CAN1->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000); 340 | 341 | // The mailbox don't becomes empty while loop 342 | if (CAN1->sTxMailBox[0].TIR & 0x1UL) { 343 | Serial.println("Send Fail"); 344 | Serial.println(CAN1->ESR); 345 | Serial.println(CAN1->MSR); 346 | Serial.println(CAN1->TSR); 347 | } 348 | } 349 | 350 | /** 351 | * Returns whether there are CAN messages available. 352 | * 353 | * @returns If pending CAN messages are in the CAN controller 354 | * 355 | */ 356 | uint8_t CANMsgAvail(void) 357 | { 358 | // Check for pending FIFO 0 messages 359 | return CAN1->RF0R & 0x3UL; 360 | } 361 | 362 | 363 | uint8_t counter = 0; 364 | uint8_t frameLength = 0; 365 | unsigned long previousMillis = 0; // stores last time output was updated 366 | const long interval = 1000; // transmission interval (milliseconds) 367 | 368 | void setup() { 369 | Serial.begin(115200); 370 | bool ret = CANInit(CAN_500KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 371 | //bool ret = CANInit(CAN_500KBPS, 1); // CAN_RX mapped to PB5 , CAN_TX mapped to PB6 372 | //bool ret = CANInit(CAN_500KBPS, 2); // CAN_RX mapped to PB8 , CAN_TX mapped to PB9 373 | //bool ret = CANInit(CAN_500KBPS, 3); // CAN_RX mapped to PB12, CAN_TX mapped to PB13 374 | //bool ret = CANInit(CAN_1000KBPS, 0); // CAN_RX mapped to PA11, CAN_TX mapped to PA12 375 | //bool ret = CANInit(CAN_1000KBPS, 1); // CAN_RX mapped to PB5 , CAN_TX mapped to PB6 376 | //bool ret = CANInit(CAN_1000KBPS, 2); // CAN_RX mapped to PB8 , CAN_TX mapped to PB9 377 | //bool ret = CANInit(CAN_1000KBPS, 3); // CAN_RX mapped to PB12, CAN_TX mapped to PB13 378 | if (!ret) while(true); 379 | } 380 | 381 | void loop() { 382 | CAN_msg_t CAN_TX_msg; 383 | CAN_msg_t CAN_RX_msg; 384 | 385 | CAN_TX_msg.data[0] = 0x00; 386 | CAN_TX_msg.data[1] = 0x01; 387 | CAN_TX_msg.data[2] = 0x02; 388 | CAN_TX_msg.data[3] = 0x03; 389 | CAN_TX_msg.data[4] = 0x04; 390 | CAN_TX_msg.data[5] = 0x05; 391 | CAN_TX_msg.data[6] = 0x06; 392 | CAN_TX_msg.data[7] = 0x07; 393 | CAN_TX_msg.len = frameLength; 394 | 395 | unsigned long currentMillis = millis(); 396 | if (currentMillis - previousMillis >= interval) { 397 | previousMillis = currentMillis; 398 | if ( ( counter % 2) == 0) { 399 | CAN_TX_msg.type = DATA_FRAME; 400 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 401 | CAN_TX_msg.format = EXTENDED_FORMAT; 402 | CAN_TX_msg.id = 0x321452; 403 | } else { 404 | CAN_TX_msg.type = DATA_FRAME; 405 | if (CAN_TX_msg.len == 0) CAN_TX_msg.type = REMOTE_FRAME; 406 | CAN_TX_msg.format = STANDARD_FORMAT; 407 | CAN_TX_msg.id = 0x452; 408 | } 409 | CANSend(&CAN_TX_msg); 410 | frameLength++; 411 | if (frameLength == 9) frameLength = 0; 412 | counter++; 413 | } 414 | 415 | if(CANMsgAvail()) { 416 | CANReceive(&CAN_RX_msg); 417 | 418 | if (CAN_RX_msg.format == EXTENDED_FORMAT) { 419 | Serial.print("Extended ID: 0x"); 420 | if (CAN_RX_msg.id < 0x10000000) Serial.print("0"); 421 | if (CAN_RX_msg.id < 0x1000000) Serial.print("0"); 422 | if (CAN_RX_msg.id < 0x100000) Serial.print("0"); 423 | if (CAN_RX_msg.id < 0x10000) Serial.print("0"); 424 | if (CAN_RX_msg.id < 0x1000) Serial.print("0"); 425 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 426 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 427 | Serial.print(CAN_RX_msg.id, HEX); 428 | } else { 429 | Serial.print("Standard ID: 0x"); 430 | if (CAN_RX_msg.id < 0x100) Serial.print("0"); 431 | if (CAN_RX_msg.id < 0x10) Serial.print("0"); 432 | Serial.print(CAN_RX_msg.id, HEX); 433 | Serial.print(" "); 434 | } 435 | 436 | Serial.print(" DLC: "); 437 | Serial.print(CAN_RX_msg.len); 438 | if (CAN_RX_msg.type == DATA_FRAME) { 439 | Serial.print(" Data: "); 440 | for(int i=0; i