diff --git a/.gitmodules b/.gitmodules index 77f7d11..91fc26b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,12 +7,6 @@ [submodule "dronecan/DSDL"] path = dronecan/DSDL url = https://github.com/dronecan/DSDL.git -[submodule "lib/ArduinoDroneCANlib"] - path = lib/ArduinoDroneCANlib - url = https://github.com/BeyondRobotix/ArduinoDroneCANlib.git [submodule "dronecan/libcanard_repo"] path = dronecan/libcanard_repo - url = https://github.com/dronecan/libcanard.git -[submodule "variants/BRMicroNode"] - path = variants/BRMicroNode - url = https://github.com/BeyondRobotix/BRMicroNodeVariant.git + url = https://github.com/dronecan/libcanard.git \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index ea18208..bd3a8db 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -41,6 +41,7 @@ "stdexcept": "cpp", "streambuf": "cpp", "cinttypes": "cpp", - "typeinfo": "cpp" + "typeinfo": "cpp", + "stm32h7xx_hal_fdcan.h": "c" } } \ No newline at end of file diff --git a/CANFDIface.cpp b/CANFDIface.cpp new file mode 100644 index 0000000..f4b7889 --- /dev/null +++ b/CANFDIface.cpp @@ -0,0 +1,1251 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Siddharth Bharat Purohit + */ + +#include +#include "AP_HAL_ChibiOS.h" + +#define HAL_NUM_CAN_IFACES 3 +#define STM32H7XX + + +#if HAL_NUM_CAN_IFACES +#include +#include +#include +# include +#include +#include + +# if defined(STM32H7XX) || defined(STM32G4) +#include "CANFDIface.h" + +#define FDCAN1_IT0_IRQHandler STM32_FDCAN1_IT0_HANDLER +#define FDCAN1_IT1_IRQHandler STM32_FDCAN1_IT1_HANDLER +#define FDCAN2_IT0_IRQHandler STM32_FDCAN2_IT0_HANDLER +#define FDCAN2_IT1_IRQHandler STM32_FDCAN2_IT1_HANDLER + +// FIFO elements are spaced at 18 words +#define FDCAN_FRAME_BUFFER_SIZE 18 + + +//Message RAM Allocations in Word lengths + +#if defined(STM32H7) +#define MAX_FILTER_LIST_SIZE 78U //78 element Standard Filter List elements or 40 element Extended Filter List +#define FDCAN_NUM_RXFIFO0_SIZE 108U //6 Frames +#define FDCAN_TX_FIFO_BUFFER_SIZE 126U //7 Frames +#define MESSAGE_RAM_END_ADDR 0x4000B5FC + +#elif defined(STM32G4) +#define MAX_FILTER_LIST_SIZE 80U //80 element Standard Filter List elements or 40 element Extended Filter List +#define FDCAN_NUM_RXFIFO0_SIZE 104U //26 Frames +#define FDCAN_TX_FIFO_BUFFER_SIZE 128U //32 Frames +#define FDCAN_MESSAGERAM_STRIDE 0x350 // separation of messageram areas +#define FDCAN_EXFILTER_OFFSET 0x70 +#define FDCAN_RXFIFO0_OFFSET 0xB0 +#define FDCAN_RXFIFO1_OFFSET 0x188 +#define FDCAN_TXFIFO_OFFSET 0x278 + +#define MESSAGE_RAM_END_ADDR 0x4000B5FC + +#else +#error "Unsupported MCU for FDCAN" +#endif + +extern const AP_HAL::HAL& hal; + +#define STR(x) #x +#define XSTR(x) STR(x) +#if !defined(HAL_LLD_USE_CLOCK_MANAGEMENT) +static_assert(STM32_FDCANCLK == 80U*1000U*1000U, "FDCAN clock must be 80MHz, got " XSTR(STM32_FDCANCLK)); +#endif + +using namespace ChibiOS; + +#if HAL_CANMANAGER_ENABLED +#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANFDIface", fmt, ##args); } while (0) +#else +#define Debug(fmt, args...) +#endif + +constexpr CANIface::CanType* const CANIface::Can[]; +static ChibiOS::CANIface* can_ifaces[HAL_NUM_CAN_IFACES]; + +uint8_t CANIface::next_interface; + +// mapping from logical interface to physical. First physical is 0, first logical is 0 +static constexpr uint8_t can_interfaces[HAL_NUM_CAN_IFACES] = { HAL_CAN_INTERFACE_LIST }; + +// mapping from physical interface back to logical. First physical is 0, first logical is 0 +static constexpr int8_t can_iface_to_idx[3] = { HAL_CAN_INTERFACE_REV_LIST }; + +#define REG_SET_TIMEOUT 250 // if it takes longer than 250ms for setting a register we have failed + +static inline bool driver_initialised(uint8_t iface_index) +{ + if (can_ifaces[iface_index] == nullptr) { + return false; + } + return true; +} + +static inline void handleCANInterrupt(uint8_t phys_index, uint8_t line_index) +{ + const int8_t iface_index = can_iface_to_idx[phys_index]; + if (iface_index < 0 || iface_index >= HAL_NUM_CAN_IFACES) { + return; + } + if (!driver_initialised(iface_index)) { + //Just reset all the interrupts and return + CANIface::Can[iface_index]->IR = FDCAN_IR_RF0N; + CANIface::Can[iface_index]->IR = FDCAN_IR_RF1N; + CANIface::Can[iface_index]->IR = FDCAN_IR_TEFN; + return; + } + if (line_index == 0) { + if ((CANIface::Can[iface_index]->IR & FDCAN_IR_RF0N) || + (CANIface::Can[iface_index]->IR & FDCAN_IR_RF0F)) { + CANIface::Can[iface_index]->IR = FDCAN_IR_RF0N | FDCAN_IR_RF0F; + can_ifaces[iface_index]->handleRxInterrupt(0); + } + if ((CANIface::Can[iface_index]->IR & FDCAN_IR_RF1N) || + (CANIface::Can[iface_index]->IR & FDCAN_IR_RF1F)) { + CANIface::Can[iface_index]->IR = FDCAN_IR_RF1N | FDCAN_IR_RF1F; + can_ifaces[iface_index]->handleRxInterrupt(1); + } + } else { + if (CANIface::Can[iface_index]->IR & FDCAN_IR_TC) { + CANIface::Can[iface_index]->IR = FDCAN_IR_TC; + uint64_t timestamp_us = AP_HAL::micros64(); + if (timestamp_us > 0) { + timestamp_us--; + } + can_ifaces[iface_index]->handleTxCompleteInterrupt(timestamp_us); + } + + if ((CANIface::Can[iface_index]->IR & FDCAN_IR_BO)) { + CANIface::Can[iface_index]->IR = FDCAN_IR_BO; + can_ifaces[iface_index]->handleBusOffInterrupt(); + } + } + can_ifaces[iface_index]->pollErrorFlagsFromISR(); +} + +uint32_t CANIface::FDCANMessageRAMOffset_ = 0; + +CANIface::CANIface(uint8_t index) : + rx_bytebuffer_((uint8_t*)rx_buffer, sizeof(rx_buffer)), + rx_queue_(&rx_bytebuffer_), + self_index_(index) +{ + if (index >= HAL_NUM_CAN_IFACES) { + AP_HAL::panic("Bad CANIface index."); + } else { + can_ = Can[index]; + } +} + +// constructor suitable for array +CANIface::CANIface() : + CANIface(next_interface++) +{} + +void CANIface::handleBusOffInterrupt() +{ + _detected_bus_off = true; +} + +bool CANIface::computeTimings(const uint32_t target_bitrate, Timings& out_timings) const +{ + if (target_bitrate < 1) { + return false; + } + + /* + * Hardware configuration + */ + const uint32_t pclk = STM32_FDCANCLK; + + static const int MaxBS1 = 16; + static const int MaxBS2 = 8; + + /* + * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG + * CAN in Automation, 2003 + * + * According to the source, optimal quanta per bit are: + * Bitrate Optimal Maximum + * 1000 kbps 8 10 + * 500 kbps 16 17 + * 250 kbps 16 17 + * 125 kbps 16 17 + */ + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; + + static const int MaxSamplePointLocation = 900; + + /* + * Computing (prescaler * BS): + * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual + * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified + * let: + * BS = 1 + BS1 + BS2 -- Number of time quanta per bit + * PRESCALER_BS = PRESCALER * BS + * ==> + * PRESCALER_BS = PCLK / BITRATE + */ + const uint32_t prescaler_bs = pclk / target_bitrate; + + /* + * Searching for such prescaler value so that the number of quanta per bit is highest. + */ + uint8_t bs1_bs2_sum = uint8_t(max_quanta_per_bit - 1); + + while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) { + if (bs1_bs2_sum <= 2) { + return false; // No solution + } + bs1_bs2_sum--; + } + + const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); + if ((prescaler < 1U) || (prescaler > 1024U)) { + Debug("Timings: No Solution found\n"); + return false; // No solution + } + + /* + * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. + * We need to find the values so that the sample point is as close as possible to the optimal value. + * + * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) + * {{bs2 -> (1 + bs1)/7}} + * + * Hence: + * bs2 = (1 + bs1) / 7 + * bs1 = (7 * bs1_bs2_sum - 1) / 8 + * + * Sample point location can be computed as follows: + * Sample point location = (1 + bs1) / (1 + bs1 + bs2) + * + * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: + * - With rounding to nearest + * - With rounding to zero + */ + struct BsPair { + uint8_t bs1; + uint8_t bs2; + uint16_t sample_point_permill; + + BsPair() : + bs1(0), + bs2(0), + sample_point_permill(0) + { } + + BsPair(uint8_t bs1_bs2_sum, uint8_t arg_bs1) : + bs1(arg_bs1), + bs2(uint8_t(bs1_bs2_sum - bs1)), + sample_point_permill(uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2))) + {} + + bool isValid() const + { + return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); + } + }; + + // First attempt with rounding to nearest + BsPair solution(bs1_bs2_sum, uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8)); + + if (solution.sample_point_permill > MaxSamplePointLocation) { + // Second attempt with rounding to zero + solution = BsPair(bs1_bs2_sum, uint8_t((7 * bs1_bs2_sum - 1) / 8)); + } + + Debug("Timings: quanta/bit: %d, sample point location: %.1f%%\n", + int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F); + + /* + * Final validation + * Helpful Python: + * def sample_point_from_btr(x): + * assert 0b0011110010000000111111000000000 & x == 0 + * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 + * return (1+ts1+1)/(1+ts1+1+ts2+1) + * + */ + if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) { + Debug("Timings: Invalid Solution %lu %lu %d %d %lu \n", pclk, prescaler, int(solution.bs1), int(solution.bs2), (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))); + return false; + } + + out_timings.sample_point_permill = solution.sample_point_permill; + out_timings.prescaler = uint16_t(prescaler); + out_timings.sjw = 1; + out_timings.bs1 = uint8_t(solution.bs1); + out_timings.bs2 = uint8_t(solution.bs2); + return true; +} + +/* + table driven timings for CANFD + These timings are from https://www.kvaser.com/support/calculators/can-fd-bit-timing-calculator + */ +bool CANIface::computeFDTimings(const uint32_t target_bitrate, Timings& out_timings) const +{ + static const struct { + uint8_t bitrate_mbaud; + uint8_t prescaler; + uint8_t bs1; + uint8_t bs2; + uint8_t sjw; + uint8_t sample_point_pct; + } CANFD_timings[] { + { 1, 4, 14, 5, 5, 75}, + { 2, 2, 14, 5, 5, 75}, + { 4, 1, 14, 5, 5, 75}, + { 5, 1, 11, 4, 4, 75}, + { 8, 1, 6, 3, 3, 70}, + }; + for (const auto &t : CANFD_timings) { + if (t.bitrate_mbaud*1000U*1000U == target_bitrate) { + // out_timings has the register bits, which are the actual value minus 1 + out_timings.prescaler = t.prescaler; + out_timings.bs1 = t.bs1; + out_timings.bs2 = t.bs2; + out_timings.sjw = t.sjw; + out_timings.sample_point_permill = t.sample_point_pct*10; + return true; + } + } + return false; +} + +int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, + CanIOFlags flags) +{ + if (!initialised_) { + return -1; + } + + stats.tx_requests++; + if (frame.isErrorFrame() || (frame.dlc > 8 && !frame.isCanFDFrame()) || + frame.dlc > 15) { + stats.tx_rejected++; + return -1; + } + + { + CriticalSectionLocker lock; + + /* + * Seeking for an empty slot + */ + uint8_t index; + + if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) { + stats.tx_overflow++; + if (stats.tx_success == 0) { + /* + if we have never successfully transmitted a frame + then we may be operating with just MAVCAN or UDP + MCAST. Consider the frame sent if the send + succeeds. This allows for UDP MCAST and MAVCAN to + operate fully when the CAN bus has no cable plugged + in + */ + return AP_HAL::CANIface::send(frame, tx_deadline, flags); + } + return 0; //we don't have free space + } + index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos); + + // Copy Frame to RAM + // Calculate Tx element address + uint32_t* buffer = (uint32_t *)(MessageRam_.TxFIFOQSA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); + + //Setup Frame ID + if (frame.isExtended()) { + buffer[0] = (IDE | frame.id); + } else { + buffer[0] = (frame.id << 18); + } + if (frame.isRemoteTransmissionRequest()) { + buffer[0] |= RTR; + } + //Write Data Length Code, and Message Marker + buffer[1] = frame.dlc << 16 | index << 24; + + if (frame.isCanFDFrame()) { + buffer[1] |= FDF | BRS; // do CAN FD transfer and bit rate switching + stats.fdf_tx_requests++; + pending_tx_[index].canfd_frame = true; + } else { + pending_tx_[index].canfd_frame = false; + } + + // Write Frame to the message RAM + const uint8_t data_length = AP_HAL::CANFrame::dlcToDataLength(frame.dlc); + uint32_t *data_ptr = &buffer[2]; + for (uint8_t i = 0; i < (data_length+3)/4; i++) { + data_ptr[i] = frame.data_32[i]; + } + + //Set Add Request + can_->TXBAR = (1 << index); + + //Registering the pending transmission so we can track its deadline and loopback it as needed + pending_tx_[index].deadline = tx_deadline; + pending_tx_[index].frame = frame; + pending_tx_[index].loopback = (flags & AP_HAL::CANIface::Loopback) != 0; + pending_tx_[index].abort_on_error = (flags & AP_HAL::CANIface::AbortOnError) != 0; + pending_tx_[index].index = index; + // setup frame initial state + pending_tx_[index].aborted = false; + pending_tx_[index].setup = true; + pending_tx_[index].pushed = false; + } + + // also send on MAVCAN, but don't consider it an error if we can't get the MAVCAN out + AP_HAL::CANIface::send(frame, tx_deadline, flags); + + return 1; +} + +int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags) +{ + { + CriticalSectionLocker lock; + CanRxItem rx_item; + if (!rx_queue_.pop(rx_item) || !initialised_) { + return 0; + } + out_frame = rx_item.frame; + out_timestamp_us = rx_item.timestamp_us; + out_flags = rx_item.flags; + } + + return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags); +} + +bool CANIface::configureFilters(const CanFilterConfig* filter_configs, + uint16_t num_configs) +{ + // only enable filters in AP_Periph. It makes no sense on the flight controller +#if !defined(HAL_BUILD_AP_PERIPH) || defined(STM32G4) + // no filtering + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + initialised_ = true; + return true; +#else + uint32_t num_extid = 0, num_stdid = 0; + uint32_t total_available_list_size = MAX_FILTER_LIST_SIZE; + uint32_t* filter_ptr; + if (initialised_ || mode_ != FilteredMode) { + // we are already initialised can't do anything here + return false; + } + //count number of frames of each type + for (uint8_t i = 0; i < num_configs; i++) { + const CanFilterConfig* const cfg = filter_configs + i; + if ((cfg->id & AP_HAL::CANFrame::FlagEFF) || !(cfg->mask & AP_HAL::CANFrame::FlagEFF)) { + num_extid++; + } else { + num_stdid++; + } + } + CriticalSectionLocker lock; + //Allocate Message RAM for Standard ID Filter List + if (num_stdid == 0) { //No Frame with Standard ID is to be accepted +#if defined(STM32G4) + can_->RXGFC |= 0x2; //Reject All Standard ID Frames +#else + can_->GFC |= 0x2; //Reject All Standard ID Frames +#endif + } else if ((num_stdid < total_available_list_size) && (num_stdid <= 128)) { + can_->SIDFC = (FDCANMessageRAMOffset_ << 2) | (num_stdid << 16); + MessageRam_.StandardFilterSA = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + FDCANMessageRAMOffset_ += num_stdid; + total_available_list_size -= num_stdid; + can_->GFC |= (0x3U << 4); //Reject non matching Standard frames + } else { //The List is too big, return fail + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + return false; + } + + if (num_stdid) { + num_stdid = 0; //reset list count + filter_ptr = (uint32_t*)MessageRam_.StandardFilterSA; + //Run through the filter list and setup standard id filter list + for (uint8_t i = 0; i < num_configs; i++) { + uint32_t id = 0; + uint32_t mask = 0; + const CanFilterConfig* const cfg = filter_configs + i; + if (!((cfg->id & AP_HAL::CANFrame::FlagEFF) || !(cfg->mask & AP_HAL::CANFrame::FlagEFF))) { + id = (cfg->id & AP_HAL::CANFrame::MaskStdID); // Regular std frames, nothing fancy. + mask = (cfg->mask & 0x7F); + filter_ptr[num_stdid] = 0x2U << 30 | //Classic CAN Filter + 0x1U << 27 | //Store in Rx FIFO0 if filter matches + id << 16 | + mask; + num_stdid++; + } + } + } + + //Allocate Message RAM for Extended ID Filter List + if (num_extid == 0) { //No Frame with Extended ID is to be accepted + can_->GFC |= 0x1; //Reject All Extended ID Frames + } else if ((num_extid < (total_available_list_size/2)) && (num_extid <= 64)) { + can_->XIDFC = (FDCANMessageRAMOffset_ << 2) | (num_extid << 16); + MessageRam_.ExtendedFilterSA = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + FDCANMessageRAMOffset_ += num_extid*2; + can_->GFC |= (0x3U << 2); // Reject non matching Extended frames + } else { //The List is too big, return fail + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + return false; + } + + if (num_extid) { + num_extid = 0; + filter_ptr = (uint32_t*)MessageRam_.ExtendedFilterSA; + //Run through the filter list and setup extended id filter list + for (uint8_t i = 0; i < num_configs; i++) { + uint32_t id = 0; + uint32_t mask = 0; + const CanFilterConfig* const cfg = filter_configs + i; + if ((cfg->id & AP_HAL::CANFrame::FlagEFF) || !(cfg->mask & AP_HAL::CANFrame::FlagEFF)) { + id = (cfg->id & AP_HAL::CANFrame::MaskExtID); + mask = (cfg->mask & AP_HAL::CANFrame::MaskExtID); + filter_ptr[num_extid*2] = 0x1U << 29 | id; //Store in Rx FIFO0 if filter matches + filter_ptr[num_extid*2 + 1] = 0x2U << 30 | mask; // Classic CAN Filter + num_extid++; + } + } + } + + MessageRam_.EndAddress = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + if (MessageRam_.EndAddress > MESSAGE_RAM_END_ADDR) { + //We are overflowing the limit of Allocated Message RAM + AP_HAL::panic("CANFDIface: Message RAM Overflow!"); + } + + // Finally get out of Config Mode + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + initialised_ = true; + return true; +#endif // AP_Periph, STM32G4 +} + +uint16_t CANIface::getNumFilters() const +{ + return MAX_FILTER_LIST_SIZE; +} + +bool CANIface::clock_init_ = false; +bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const OperatingMode mode) +{ + Debug("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); + if (self_index_ > HAL_NUM_CAN_IFACES) { + Debug("CAN drv init failed"); + return false; + } + if (can_ifaces[self_index_] == nullptr) { + can_ifaces[self_index_] = this; +#if !defined(HAL_BOOTLOADER_BUILD) + AP_HAL::get_HAL_mutable().can[self_index_] = this; +#endif + } + + bitrate_ = bitrate; + mode_ = mode; + //Only do it once + //Doing it second time will reset the previously initialised bus + if (!clock_init_) { + CriticalSectionLocker lock; +#if defined(STM32G4) + RCC->APB1ENR1 |= RCC_APB1ENR1_FDCANEN; + RCC->APB1RSTR1 |= RCC_APB1RSTR1_FDCANRST; + RCC->APB1RSTR1 &= ~RCC_APB1RSTR1_FDCANRST; +#else + RCC->APB1HENR |= RCC_APB1HENR_FDCANEN; + RCC->APB1HRSTR |= RCC_APB1HRSTR_FDCANRST; + RCC->APB1HRSTR &= ~RCC_APB1HRSTR_FDCANRST; +#endif + clock_init_ = true; + } + + /* + * IRQ + */ + if (!irq_init_) { + CriticalSectionLocker lock; + switch (can_interfaces[self_index_]) { + case 0: + nvicEnableVector(FDCAN1_IT0_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + nvicEnableVector(FDCAN1_IT1_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + break; +#ifdef FDCAN2 + case 1: + nvicEnableVector(FDCAN2_IT0_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + nvicEnableVector(FDCAN2_IT1_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + break; +#endif +#ifdef FDCAN3 + case 2: + nvicEnableVector(FDCAN3_IT0_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + nvicEnableVector(FDCAN3_IT1_IRQn, CORTEX_MAX_KERNEL_PRIORITY); + break; +#endif + } + irq_init_ = true; + } + + // Setup FDCAN for configuration mode and disable all interrupts + { + CriticalSectionLocker lock; + + can_->CCCR &= ~FDCAN_CCCR_CSR; // Exit sleep mode + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_CSA) == FDCAN_CCCR_CSA) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } //Wait for wake up ack + can_->CCCR |= FDCAN_CCCR_INIT; // Request init + while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + can_->CCCR |= FDCAN_CCCR_CCE; //Enable Config change + can_->IE = 0; // Disable interrupts while initialization is in progress + } + + /* + * Object state - interrupts are disabled, so it's safe to modify it now + */ + rx_queue_.clear(); + for (uint32_t i=0; i < NumTxMailboxes; i++) { + pending_tx_[i] = CanTxItem(); + } + peak_tx_mailbox_index_ = 0; + had_activity_ = false; + + /* + * CAN timings for this bitrate + */ + if (!computeTimings(bitrate, timings)) { + can_->CCCR &= ~FDCAN_CCCR_INIT; + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + return false; + } + _bitrate = bitrate; + Debug("Timings: presc=%u sjw=%u bs1=%u bs2=%u\n", + unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2)); + + //setup timing register + can_->NBTP = (((timings.sjw-1) << FDCAN_NBTP_NSJW_Pos) | + ((timings.bs1-1) << FDCAN_NBTP_NTSEG1_Pos) | + ((timings.bs2-1) << FDCAN_NBTP_NTSEG2_Pos) | + ((timings.prescaler-1) << FDCAN_NBTP_NBRP_Pos)); + + if (fdbitrate) { + if (!computeFDTimings(fdbitrate, fdtimings)) { + can_->CCCR &= ~FDCAN_CCCR_INIT; + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + return false; + } + _fdbitrate = fdbitrate; + Debug("CANFD Timings: presc=%u bs1=%u bs2=%u\n", + unsigned(fdtimings.prescaler), unsigned(fdtimings.bs1), unsigned(fdtimings.bs2)); + can_->DBTP = (((fdtimings.bs1-1) << FDCAN_DBTP_DTSEG1_Pos) | + ((fdtimings.bs2-1) << FDCAN_DBTP_DTSEG2_Pos) | + ((fdtimings.prescaler-1) << FDCAN_DBTP_DBRP_Pos) | + ((fdtimings.sjw-1) << FDCAN_DBTP_DSJW_Pos)) | + FDCAN_DBTP_TDC; + // use a transmitter delay compensation offset of 10, suitable + // for MCP2557FD transceiver with delay of 120ns + can_->TDCR = 10<RXESC = 0; //Set for 8Byte Frames +#endif + + //Setup Message RAM + setupMessageRam(); + // Reset Bus Off + _detected_bus_off = false; + //Clear all Interrupts + can_->IR = 0x3FFFFFFF; + //Enable Interrupts + can_->IE = FDCAN_IE_TCE | // Transmit Complete interrupt enable + FDCAN_IE_BOE | // Bus off Error Interrupt enable + FDCAN_IE_RF0NE | // RX FIFO 0 new message + FDCAN_IE_RF0FE | // Rx FIFO 0 FIFO Full + FDCAN_IE_RF1NE | // RX FIFO 1 new message + FDCAN_IE_RF1FE; // Rx FIFO 1 FIFO Full +#if defined(STM32G4) + can_->ILS = FDCAN_ILS_PERR | FDCAN_ILS_SMSG; +#else + can_->ILS = FDCAN_ILS_TCL | FDCAN_ILS_BOE; //Set Line 1 for Transmit Complete Event Interrupt and Bus Off Interrupt +#endif + // And Busoff error +#if defined(STM32G4) + can_->TXBTIE = 0x7; +#else + can_->TXBTIE = 0xFFFFFFFF; +#endif + can_->ILE = 0x3; + +#if HAL_CANFD_SUPPORTED + can_->CCCR |= FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE; // enable sending CAN FD frames, and Bitrate switching +#endif + + // If mode is Filtered then we finish the initialisation in configureFilter method + // otherwise we finish here + if (mode != FilteredMode) { + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + uint32_t while_start_ms = AP_HAL::millis(); + while ((can_->CCCR & FDCAN_CCCR_INIT) == 1) { + if ((AP_HAL::millis() - while_start_ms) > REG_SET_TIMEOUT) { + return false; + } + } + + //initialised + initialised_ = true; + } + return true; +} + +void CANIface::clear_rx() +{ + CriticalSectionLocker lock; + rx_queue_.clear(); +} + +void CANIface::setupMessageRam() +{ +#if defined(STM32G4) + const uint32_t base = SRAMCAN_BASE + FDCAN_MESSAGERAM_STRIDE * can_interfaces[self_index_]; + memset((void*)base, 0, FDCAN_MESSAGERAM_STRIDE); + MessageRam_.StandardFilterSA = base; + MessageRam_.ExtendedFilterSA = base + FDCAN_EXFILTER_OFFSET; + MessageRam_.RxFIFO0SA = base + FDCAN_RXFIFO0_OFFSET; + MessageRam_.RxFIFO1SA = base + FDCAN_RXFIFO1_OFFSET; + MessageRam_.TxFIFOQSA = base + FDCAN_TXFIFO_OFFSET; + + can_->TXBC = 0; // fifo mode +#else + uint32_t num_elements = 0; + + can_->RXESC = 0x777; //Support upto 64byte long frames + can_->TXESC = 0x7; //Support upto 64byte long frames + // Rx FIFO 0 start address and element count + num_elements = MIN((FDCAN_NUM_RXFIFO0_SIZE/FDCAN_FRAME_BUFFER_SIZE), 64U); + if (num_elements) { + can_->RXF0C = (FDCANMessageRAMOffset_ << 2) | (num_elements << 16); + MessageRam_.RxFIFO0SA = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + FDCANMessageRAMOffset_ += num_elements*FDCAN_FRAME_BUFFER_SIZE; + } + + // Tx FIFO/queue start address and element count + num_elements = MIN((FDCAN_TX_FIFO_BUFFER_SIZE/FDCAN_FRAME_BUFFER_SIZE), 32U); + if (num_elements) { + can_->TXBC = (FDCANMessageRAMOffset_ << 2) | (num_elements << 24); + MessageRam_.TxFIFOQSA = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + FDCANMessageRAMOffset_ += num_elements*FDCAN_FRAME_BUFFER_SIZE; + } + MessageRam_.EndAddress = SRAMCAN_BASE + (FDCANMessageRAMOffset_ * 4U); + if (MessageRam_.EndAddress > MESSAGE_RAM_END_ADDR) { + //We are overflowing the limit of Allocated Message RAM + AP_HAL::panic("CANFDIface: Message RAM Overflow!"); + return; + } +#endif +} + +void CANIface::handleTxCompleteInterrupt(const uint64_t timestamp_us) +{ + for (uint8_t i = 0; i < NumTxMailboxes; i++) { + if ((can_->TXBTO & (1UL << i))) { + + if (!pending_tx_[i].pushed) { + stats.tx_success++; + stats.last_transmit_us = timestamp_us; + if (pending_tx_[i].canfd_frame) { + stats.fdf_tx_success++; + } + pending_tx_[i].pushed = true; + } else { + continue; + } + + if (pending_tx_[i].loopback && had_activity_) { + CanRxItem rx_item; + rx_item.frame = pending_tx_[i].frame; + rx_item.timestamp_us = timestamp_us; + rx_item.flags = AP_HAL::CANIface::Loopback; + add_to_rx_queue(rx_item); + } + stats.num_events++; + if (sem_handle != nullptr) { + sem_handle->signal_ISR(); + } + } + } +} + +bool CANIface::readRxFIFO(uint8_t fifo_index) +{ + uint32_t *frame_ptr; + uint32_t index; + uint64_t timestamp_us = AP_HAL::micros64(); + if (fifo_index == 0) { +#if !defined(STM32G4) + //Check if RAM allocated to RX FIFO + if ((can_->RXF0C & FDCAN_RXF0C_F0S) == 0) { + return false; + } +#endif + //Register Message Lost as a hardware error + if ((can_->RXF0S & FDCAN_RXF0S_RF0L) != 0) { + stats.rx_errors++; + } + + if ((can_->RXF0S & FDCAN_RXF0S_F0FL) == 0) { + return false; //No More messages in FIFO + } else { + index = ((can_->RXF0S & FDCAN_RXF0S_F0GI) >> 8); + frame_ptr = (uint32_t *)(MessageRam_.RxFIFO0SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); + } + } else if (fifo_index == 1) { +#if !defined(STM32G4) + //Check if RAM allocated to RX FIFO + if ((can_->RXF1C & FDCAN_RXF1C_F1S) == 0) { + return false; + } +#endif + //Register Message Lost as a hardware error + if ((can_->RXF1S & FDCAN_RXF1S_RF1L) != 0) { + stats.rx_errors++; + } + + if ((can_->RXF1S & FDCAN_RXF1S_F1FL) == 0) { + return false; + } else { + index = ((can_->RXF1S & FDCAN_RXF1S_F1GI) >> 8); + frame_ptr = (uint32_t *)(MessageRam_.RxFIFO1SA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); + } + } else { + return false; + } + + // Read the frame contents + AP_HAL::CANFrame frame {}; + uint32_t id = frame_ptr[0]; + if ((id & IDE) == 0) { + //Standard ID + frame.id = ((id & STID_MASK) >> 18) & AP_HAL::CANFrame::MaskStdID; + } else { + //Extended ID + frame.id = (id & EXID_MASK) & AP_HAL::CANFrame::MaskExtID; + frame.id |= AP_HAL::CANFrame::FlagEFF; + } + + if ((id & RTR) != 0) { + frame.id |= AP_HAL::CANFrame::FlagRTR; + } + + if (frame_ptr[1] & FDF) { + frame.setCanFD(true); + stats.fdf_rx_received++; + } else { + frame.setCanFD(false); + } + + frame.dlc = (frame_ptr[1] & DLC_MASK) >> 16; + uint8_t *data = (uint8_t*)&frame_ptr[2]; + + for (uint8_t i = 0; i < AP_HAL::CANFrame::dlcToDataLength(frame.dlc); i++) { + frame.data[i] = data[i]; + } + + //Acknowledge the FIFO entry we just read + if (fifo_index == 0) { + can_->RXF0A = index; + } else if (fifo_index == 1) { + can_->RXF1A = index; + } + + /* + * Store with timeout into the FIFO buffer + */ + + CanRxItem rx_item; + rx_item.frame = frame; + rx_item.timestamp_us = timestamp_us; + rx_item.flags = 0; + if (add_to_rx_queue(rx_item)) { + stats.rx_received++; + } else { + stats.rx_overflow++; + } + return true; +} + +void CANIface::handleRxInterrupt(uint8_t fifo_index) +{ + while (readRxFIFO(fifo_index)) { + had_activity_ = true; + } + stats.num_events++; + if (sem_handle != nullptr) { + sem_handle->signal_ISR(); + } +} + +/** + * This method is used to count errors and abort transmission on error if necessary. + * This functionality used to be implemented in the SCE interrupt handler, but that approach was + * generating too much processing overhead, especially on disconnected interfaces. + * + * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled. + */ +void CANIface::pollErrorFlagsFromISR() +{ + const uint8_t cel = can_->ECR >> 16; + + if (cel != 0) { + stats.ecr = can_->ECR; + for (int i = 0; i < NumTxMailboxes; i++) { + if (!pending_tx_[i].abort_on_error || pending_tx_[i].aborted) { + continue; + } + if (((1 << pending_tx_[i].index) & can_->TXBRP)) { + can_->TXBCR = 1 << pending_tx_[i].index; // Goodnight sweet transmission + pending_tx_[i].aborted = true; + stats.tx_abort++; + } + } + } +} + +void CANIface::pollErrorFlags() +{ + CriticalSectionLocker cs_locker; + pollErrorFlagsFromISR(); +} + +bool CANIface::canAcceptNewTxFrame() const +{ +#if !defined(STM32G4) + //Check if Tx FIFO is allocated + if ((can_->TXBC & FDCAN_TXBC_TFQS) == 0) { + return false; + } +#endif + if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) { + return false; //we don't have free space + } + + return true; +} + +/** + * Total number of hardware failures and other kinds of errors (e.g. queue overruns). + * May increase continuously if the interface is not connected to the bus. + */ +uint32_t CANIface::getErrorCount() const +{ + CriticalSectionLocker lock; + return stats.num_busoff_err + + stats.rx_errors + + stats.rx_overflow + + stats.tx_rejected + + stats.tx_abort + + stats.tx_timedout; +} + +bool CANIface::set_event_handle(AP_HAL::BinarySemaphore *handle) +{ + sem_handle = handle; + return true; +} + +bool CANIface::isRxBufferEmpty() const +{ + CriticalSectionLocker lock; + return rx_queue_.available() == 0; +} + +void CANIface::clearErrors() +{ + if (_detected_bus_off) { + //Try Recovering from BusOff + //While in Bus off mode the CAN Peripheral is put + //into INIT mode, when we ask Peripheral to get out + //of INIT mode, the bit stream processor (BSP) synchronizes + //itself to the data transfer on the CAN bus by + //waiting for the occurrence of a sequence of 11 consecutive + //recessive bits (Bus_Idle) before it can take part in bus + //activities and start the message transfer + can_->CCCR &= ~FDCAN_CCCR_INIT; // Leave init mode + stats.num_busoff_err++; + _detected_bus_off = false; + } +} + +void CANIface::discardTimedOutTxMailboxes(uint64_t current_time) +{ + CriticalSectionLocker lock; + for (int i = 0; i < NumTxMailboxes; i++) { + if (pending_tx_[i].aborted || !pending_tx_[i].setup) { + continue; + } + if (((1 << pending_tx_[i].index) & can_->TXBRP) && pending_tx_[i].deadline < current_time) { + can_->TXBCR = 1 << pending_tx_[i].index; // Goodnight sweet transmission + pending_tx_[i].aborted = true; + stats.tx_timedout++; + } + } +} + +void CANIface::checkAvailable(bool& read, bool& write, const AP_HAL::CANFrame* pending_tx) const +{ + write = false; + read = !isRxBufferEmpty(); + if (pending_tx != nullptr) { + write = canAcceptNewTxFrame(); + } +} + +bool CANIface::select(bool &read, bool &write, + const AP_HAL::CANFrame* pending_tx, + uint64_t blocking_deadline) +{ + const bool in_read = read; + const bool in_write= write; + uint64_t time = AP_HAL::micros64(); + + if (!read && !write) { + //invalid request + return false; + } + + discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots + pollErrorFlags(); + clearErrors(); + + checkAvailable(read, write, pending_tx); // Check if we already have some of the requested events + if ((read && in_read) || (write && in_write)) { + return true; + } + while (time < blocking_deadline) { + if (sem_handle == nullptr) { + break; + } + IGNORE_RETURN(sem_handle->wait(blocking_deadline - time)); // Block until timeout expires or any iface updates + checkAvailable(read, write, pending_tx); // Check what we got + if ((read && in_read) || (write && in_write)) { + return true; + } + time = AP_HAL::micros64(); + } + return false; +} + +#if !defined(HAL_BOOTLOADER_BUILD) +void CANIface::get_stats(ExpandingString &str) +{ + CriticalSectionLocker lock; + str.printf("------- Clock Config -------\n" + "CAN_CLK_FREQ: %luMHz\n" + "Std Timings: bitrate=%lu presc=%u\n" + "sjw=%u bs1=%u bs2=%u sample_point=%f%%\n" + "FD Timings: bitrate=%lu presc=%u\n" + "sjw=%u bs1=%u bs2=%u sample_point=%f%%\n" + "------- CAN Interface Stats -------\n" + "tx_requests: %lu\n" + "tx_rejected: %lu\n" + "tx_overflow: %lu\n" + "tx_success: %lu\n" + "tx_timedout: %lu\n" + "tx_abort: %lu\n" + "rx_received: %lu\n" + "rx_overflow: %lu\n" + "rx_errors: %lu\n" + "num_busoff_err: %lu\n" + "num_events: %lu\n" + "ECR: %lx\n" + "fdf_rx: %lu\n" + "fdf_tx_req: %lu\n" + "fdf_tx: %lu\n", + STM32_FDCANCLK/1000000UL, + _bitrate, unsigned(timings.prescaler), + unsigned(timings.sjw), unsigned(timings.bs1), + unsigned(timings.bs2), timings.sample_point_permill*0.1f, + _fdbitrate, unsigned(fdtimings.prescaler), + unsigned(fdtimings.sjw), unsigned(fdtimings.bs1), + unsigned(fdtimings.bs2), fdtimings.sample_point_permill*0.1f, + stats.tx_requests, + stats.tx_rejected, + stats.tx_overflow, + stats.tx_success, + stats.tx_timedout, + stats.tx_abort, + stats.rx_received, + stats.rx_overflow, + stats.rx_errors, + stats.num_busoff_err, + stats.num_events, + stats.ecr, + stats.fdf_rx_received, + stats.fdf_tx_requests, + stats.fdf_tx_success); +} +#endif + +/* + * Interrupt handlers + */ +extern "C" +{ +#ifdef HAL_CAN_IFACE1_ENABLE + // FDCAN1 + CH_IRQ_HANDLER(FDCAN1_IT0_IRQHandler); + CH_IRQ_HANDLER(FDCAN1_IT0_IRQHandler) + { + CH_IRQ_PROLOGUE(); + handleCANInterrupt(0, 0); + CH_IRQ_EPILOGUE(); + } + + CH_IRQ_HANDLER(FDCAN1_IT1_IRQHandler); + CH_IRQ_HANDLER(FDCAN1_IT1_IRQHandler) + { + CH_IRQ_PROLOGUE(); + handleCANInterrupt(0, 1); + CH_IRQ_EPILOGUE(); + } +#endif + +#ifdef HAL_CAN_IFACE2_ENABLE + // FDCAN2 + CH_IRQ_HANDLER(FDCAN2_IT0_IRQHandler); + CH_IRQ_HANDLER(FDCAN2_IT0_IRQHandler) + { + CH_IRQ_PROLOGUE(); + handleCANInterrupt(1, 0); + CH_IRQ_EPILOGUE(); + } + + CH_IRQ_HANDLER(FDCAN2_IT1_IRQHandler); + CH_IRQ_HANDLER(FDCAN2_IT1_IRQHandler) + { + CH_IRQ_PROLOGUE(); + handleCANInterrupt(1, 1); + CH_IRQ_EPILOGUE(); + } +#endif + +#ifdef HAL_CAN_IFACE3_ENABLE + // FDCAN3 + CH_IRQ_HANDLER(FDCAN3_IT0_IRQHandler); + CH_IRQ_HANDLER(FDCAN3_IT0_IRQHandler) + { + CH_IRQ_PROLOGUE(); + handleCANInterrupt(2, 0); + CH_IRQ_EPILOGUE(); + } + + CH_IRQ_HANDLER(FDCAN3_IT1_IRQHandler); + CH_IRQ_HANDLER(FDCAN3_IT1_IRQHandler) + { + CH_IRQ_PROLOGUE(); + handleCANInterrupt(2, 1); + CH_IRQ_EPILOGUE(); + } +#endif + +} // extern "C" + +#endif //defined(STM32H7XX) || defined(STM32G4) + +#endif //HAL_NUM_CAN_IFACES diff --git a/boards/CoreNode.json b/boards/CoreNode.json new file mode 100644 index 0000000..2cf7e4d --- /dev/null +++ b/boards/CoreNode.json @@ -0,0 +1,49 @@ +{ + "build": { + "cpu": "cortex-m7", + "extra_flags": "-DSTM32H7xx -DSTM32H743xx", + "f_cpu": "480000000L", + "mcu": "stm32h743xih6", + "product_line": "STM32H743xx", + "variant": "CoreNode" + }, + "connectivity": [ + "can", + "ethernet" + ], + "debug": { + "default_tools": [ + "stlink" + ], + "jlink_device": "STM32H743XI", + "onboard_tools": [ + "stlink" + ], + "openocd_board": "st_nucleo_h743zi", + "openocd_target": "stm32h7x", + "svd_path": "STM32H743.svd" + }, + "frameworks": [ + "cmsis", + "stm32cube", + "libopencm3", + "mbed", + "zephyr", + "arduino" + ], + "name": "Beyond Robotix Core Node", + "upload": { + "maximum_ram_size": 524288, + "maximum_size": 2097152, + "protocol": "stlink", + "protocols": [ + "jlink", + "cmsis-dap", + "stlink", + "blackmagic", + "mbed" + ] + }, + "url": "https://www.beyondrobotix.com", + "vendor": "Beyond Robotix" +} diff --git a/boards/BRMicroNode.json b/boards/MicroNode.json similarity index 96% rename from boards/BRMicroNode.json rename to boards/MicroNode.json index 55dc53e..c655436 100644 --- a/boards/BRMicroNode.json +++ b/boards/MicroNode.json @@ -5,7 +5,7 @@ "f_cpu": "80000000L", "mcu": "stm32l431CCU6", "product_line": "STM32L432xx", - "variant": "BRMicroNode" + "variant": "MicroNode" }, "debug": { "default_tools": [ diff --git a/boards/MicroNodePlus.json b/boards/MicroNodePlus.json new file mode 100644 index 0000000..94d6a98 --- /dev/null +++ b/boards/MicroNodePlus.json @@ -0,0 +1,48 @@ +{ + "build": { + "core": "stm32", + "cpu": "cortex-m7", + "extra_flags": "-DSTM32H7 -DSTM32H7xx -DSTM32H723xx", + "f_cpu": "550000000L", + "mcu": "stm32h723zgt6", + "product_line": "STM32H723xx", + "variant": "MicroNodePlus" + }, + "connectivity": [ + "can", + "ethernet" + ], + "debug": { + "default_tools": [ + "stlink" + ], + "jlink_device": "STM32H723ZG", + "onboard_tools": [ + "stlink" + ], + "openocd_target": "stm32h7x", + "svd_path": "STM32H723.svd" + }, + "frameworks": [ + "arduino", + "stm32cube", + "mbed", + "libopencm3", + "zephyr" + ], + "name": "MicroNode+", + "upload": { + "maximum_ram_size": 327680, + "maximum_size": 1048576, + "protocol": "stlink", + "protocols": [ + "blackmagic", + "cmsis-dap", + "jlink", + "stlink", + "mbed" + ] + }, + "url": "https://www.st.com/en/evaluation-tools/nucleo-h723zg.html", + "vendor": "ST" +} diff --git a/lib/ArduinoDroneCANlib b/lib/ArduinoDroneCANlib deleted file mode 160000 index 502a3d5..0000000 --- a/lib/ArduinoDroneCANlib +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 502a3d505ded8e842efb22879cf6683fc1e20b43 diff --git a/lib/acanfd-stm32/.gitattributes b/lib/acanfd-stm32/.gitattributes new file mode 100644 index 0000000..dfe0770 --- /dev/null +++ b/lib/acanfd-stm32/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/lib/acanfd-stm32/.gitignore b/lib/acanfd-stm32/.gitignore new file mode 100644 index 0000000..259148f --- /dev/null +++ b/lib/acanfd-stm32/.gitignore @@ -0,0 +1,32 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app diff --git a/lib/acanfd-stm32/LICENSE b/lib/acanfd-stm32/LICENSE new file mode 100644 index 0000000..e1ae76d --- /dev/null +++ b/lib/acanfd-stm32/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 pierremolinaro + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/lib/acanfd-stm32/README.md b/lib/acanfd-stm32/README.md new file mode 100644 index 0000000..7833684 --- /dev/null +++ b/lib/acanfd-stm32/README.md @@ -0,0 +1,23 @@ +## CANFD Library for STM32 G4 and H7 + +Handled boards: + +* NUCLEO-G431KB (1 CAN FD), +* NUCLEO-G474RE (3 CAN FD), +* NUCLEO-H743ZI2 (2 CAN FD). + +### Compatibility with other ACAN libraries + +This library is fully compatible with other ACAN libraries, it uses a very similar API and the same `CANFDMessage` and `CANMessage`classes for handling messages. + +### ACANFD_STM32 library description + +The driver supports many bit rates, as standard 62.5 kbit/s, 125 kbit/s, 250 kbit/s, 500 kbit/s, and 1 Mbit/s. An efficient CAN bit timing calculator finds settings for them, but also for exotic bit rates as 833 kbit/s. If the wished bit rate cannot be achieved, the `beginFD` method does not configure the hardware and returns an error code. + +> Driver API is fully described by the PDF file in the `extras` directory. + +### Demo Sketches + +Demo sketches are provided for all handled boards in the `examples` directory. + + diff --git a/lib/acanfd-stm32/examples/G0B1RE-LoopBackDemo/G0B1RE-LoopBackDemo.ino b/lib/acanfd-stm32/examples/G0B1RE-LoopBackDemo/G0B1RE-LoopBackDemo.ino new file mode 100644 index 0000000..e37e1e9 --- /dev/null +++ b/lib/acanfd-stm32/examples/G0B1RE-LoopBackDemo/G0B1RE-LoopBackDemo.ino @@ -0,0 +1,159 @@ +//----------------------------------------------------------------- +// This demo runs on NUCLEO_G0B1RE +// The can clock frequency is 64 MHz. +// The 2 FDCAN modules are configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pins. No external hardware is required. +// Default TxCAN pins are used: +// - fdcan1: PA_12 +// - fdcan2: PB_1 +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_G0B1RE + #error This sketch runs on NUCLEO-G0B1RE Nucleo-64 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once in a sketch, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + + Serial.print ("CPU frequency: ") ; + Serial.print (F_CPU) ; + Serial.println (" Hz") ; + Serial.print ("PCLK1 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK1Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("HCLK frequency: ") ; + Serial.print (HAL_RCC_GetHCLKFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("SysClock frequency: ") ; + Serial.print (HAL_RCC_GetSysClockFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("FDCAN Clock: ") ; + Serial.print (fdcanClock ()) ; + Serial.println (" Hz") ; + + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x2) ; + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + +//--- beginFD is called without any receive filter, all sent frames are received +// by receiveFD0 throught receiveFIFO0 + uint32_t errorCode = fdcan1.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan1 configuration ok") ; + }else{ + Serial.print ("Error fdcan1: 0x") ; + Serial.println (errorCode, HEX) ; + } + errorCode = fdcan2.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan2 configuration ok") ; + }else{ + Serial.print ("Error fdcan2: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t gSendDate = 0 ; +static uint32_t gSentCount1 = 0 ; +static uint32_t gReceivedCount1 = 0 ; +static uint32_t gSentCount2 = 0 ; +static uint32_t gReceivedCount2 = 0 ; + +//----------------------------------------------------------------- + +void loop () { +//--- As the library does not implement FDCAN interrupt handling for +// the STM32G0B1RE, you should poll each module. + fdcan1.poll () ; + fdcan2.poll () ; +//--- + if (gSendDate < millis ()) { + gSendDate += 1000 ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + CANFDMessage message ; + message.id = 0x7FF ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; +//--- tryToSendReturnStatusFD returns 0 if message has been successfully +// appended in transmit buffer. +// A not zero returned value contains an error code (see doc) + uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount1 += 1 ; + Serial.print ("fdcan1 sent: ") ; + Serial.println (gSentCount1) ; + } + sendStatus = fdcan2.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount2 += 1 ; + Serial.print ("fdcan2 sent: ") ; + Serial.println (gSentCount2) ; + } + } + CANFDMessage messageFD ; + if (fdcan1.receiveFD0 (messageFD)) { + gReceivedCount1 += 1 ; + Serial.print ("fdcan1 received: ") ; + Serial.println (gReceivedCount1) ; + } + if (fdcan2.receiveFD0 (messageFD)) { + gReceivedCount2 += 1 ; + Serial.print ("fdcan2 received: ") ; + Serial.println (gReceivedCount2) ; + } +} diff --git a/lib/acanfd-stm32/examples/G431KB-LoopBackDemo-ExtendedFilters/G431KB-LoopBackDemo-ExtendedFilters.ino b/lib/acanfd-stm32/examples/G431KB-LoopBackDemo-ExtendedFilters/G431KB-LoopBackDemo-ExtendedFilters.ino new file mode 100644 index 0000000..f7b0b86 --- /dev/null +++ b/lib/acanfd-stm32/examples/G431KB-LoopBackDemo-ExtendedFilters/G431KB-LoopBackDemo-ExtendedFilters.ino @@ -0,0 +1,144 @@ +// This demo runs on NUCLEO_G431KB +// The FDCAN module is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pin. No external hardware is required. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_G431KB + #error This sketch runs on NUCLEO-G431KB Nucleo-32 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x8) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + ACANFD_STM32_ExtendedFilters extendedFilters ; +//--- Add single filter: identifier (1 matching identifier) + extendedFilters.addSingle (0x5555, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + extendedFilters.addDual (0x3333, 0x4444, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add range filter: low bound, high bound (565 matching identifiers) + extendedFilters.addRange (0x1000, 0x1234, ACANFD_STM32_FilterAction::FIFO1) ; +//--- Add classic filter: identifier and mask (32 matching identifiers) + extendedFilters.addClassic (0x6789, 0x1FFF67BD, ACANFD_STM32_FilterAction::FIFO0) ; + +//--- Reject extended frames that do not match any filter + settings.mNonMatchingExtendedFrameReception = ACANFD_STM32_FilterAction::REJECT ; + + const uint32_t errorCode = fdcan1.beginFD (settings, extendedFilters) ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentIdentifier = 0 ; +static uint32_t gReceiveCountFIFO0 = 0 ; +static uint32_t gReceiveCountFIFO1 = 0 ; +static bool gOk = true ; + +//----------------------------------------------------------------- + +static void printCount (const uint32_t inActualCount, const uint32_t inExpectedCount) { + Serial.print (", ") ; + if (inActualCount == inExpectedCount) { + Serial.print ("ok") ; + }else{ + Serial.print (inActualCount) ; + Serial.print ("/") ; + Serial.print (inExpectedCount) ; + } +} + +//----------------------------------------------------------------- + +void loop () { + if (gOk && (gSentIdentifier <= 0x1FFFFFFF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + frame.ext = true ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- Receive frame + CANFDMessage frame ; + if (gOk && fdcan1.receiveFD0 (frame)) { + gReceiveCountFIFO0 += 1 ; + } + if (gOk && fdcan1.receiveFD1 (frame)) { + gReceiveCountFIFO1 += 1 ; + } +//--- Blink led and display + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent: ") ; + Serial.print (gSentIdentifier) ; + printCount (gReceiveCountFIFO0, 35) ; + printCount (gReceiveCountFIFO1, 565) ; + Serial.println () ; + } +} + +//----------------------------------------------------------------- diff --git a/lib/acanfd-stm32/examples/G431KB-LoopBackDemo-StandardFilters/G431KB-LoopBackDemo-StandardFilters.ino b/lib/acanfd-stm32/examples/G431KB-LoopBackDemo-StandardFilters/G431KB-LoopBackDemo-StandardFilters.ino new file mode 100644 index 0000000..38741e5 --- /dev/null +++ b/lib/acanfd-stm32/examples/G431KB-LoopBackDemo-StandardFilters/G431KB-LoopBackDemo-StandardFilters.ino @@ -0,0 +1,143 @@ +// This demo runs on NUCLEO_G431KB +// The FDCAN module is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pin. No external hardware is required. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_G431KB + #error This sketch runs on NUCLEO-G431KB Nucleo-32 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x2) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + ACANFD_STM32_StandardFilters standardFilters ; +//--- Add classic filter: identifier and mask (8 matching identifiers) + standardFilters.addClassic (0x405, 0x7D5, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add range filter: low bound, high bound (36 matching identifiers) + standardFilters.addRange (0x100, 0x123, ACANFD_STM32_FilterAction::FIFO1) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + standardFilters.addDual (0x033, 0x44, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add single filter: identifier (1 matching identifier) + standardFilters.addSingle (0x055, ACANFD_STM32_FilterAction::FIFO0) ; + +//--- Reject standard frames that do not match any filter + settings.mNonMatchingStandardFrameReception = ACANFD_STM32_FilterAction::REJECT ; + + const uint32_t errorCode = fdcan1.beginFD (settings, standardFilters) ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static void printCount (const uint32_t inActualCount, const uint32_t inExpectedCount) { + Serial.print (", ") ; + if (inActualCount == inExpectedCount) { + Serial.print ("ok") ; + }else{ + Serial.print (inActualCount) ; + Serial.print ("/") ; + Serial.print (inExpectedCount) ; + } +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentIdentifier = 0 ; +static uint32_t gReceiveCountFIFO0 = 0 ; +static uint32_t gReceiveCountFIFO1 = 0 ; +static bool gOk = true ; + +//----------------------------------------------------------------- + +void loop () { + if (gOk && (gSentIdentifier <= 0x7FF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- Receive frame + CANFDMessage frame ; + if (gOk && fdcan1.receiveFD0 (frame)) { + gReceiveCountFIFO0 += 1 ; + } + if (gOk && fdcan1.receiveFD1 (frame)) { + gReceiveCountFIFO1 += 1 ; + } +//--- Blink led and display + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent: ") ; + Serial.print (gSentIdentifier) ; + printCount (gReceiveCountFIFO0, 11) ; + printCount (gReceiveCountFIFO1, 36) ; + Serial.println () ; + } +} + +//----------------------------------------------------------------- diff --git a/lib/acanfd-stm32/examples/G431KB-LoopBackDemo-filters-and-dispatch/G431KB-LoopBackDemo-filters-and-dispatch.ino b/lib/acanfd-stm32/examples/G431KB-LoopBackDemo-filters-and-dispatch/G431KB-LoopBackDemo-filters-and-dispatch.ino new file mode 100644 index 0000000..2a7b9bc --- /dev/null +++ b/lib/acanfd-stm32/examples/G431KB-LoopBackDemo-filters-and-dispatch/G431KB-LoopBackDemo-filters-and-dispatch.ino @@ -0,0 +1,233 @@ +// This demo runs on NUCLEO_G431KB +// The FDCAN module is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pin. No external hardware is required. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_G431KB + #error This sketch runs on NUCLEO-G431KB Nucleo-32 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +static uint32_t gStandardSingleFilterMatchCount = 0 ; +static uint32_t gStandardDualFilterMatchCount = 0 ; +static uint32_t gStandardRangeFilterMatchCount = 0 ; +static uint32_t gStandardClassicFilterMatchCount = 0 ; +static uint32_t gExtendedSingleFilterMatchCount = 0 ; +static uint32_t gExtendedDualFilterMatchCount = 0 ; +static uint32_t gExtendedRangeFilterMatchCount = 0 ; +static uint32_t gExtendedClassicFilterMatchCount = 0 ; + +//----------------------------------------------------------------- + +static void callBackForStandardSingleFilter (const CANFDMessage & /* inMessage */) { + gStandardSingleFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForStandardDualFilter (const CANFDMessage & /* inMessage */) { + gStandardDualFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForStandardRangeFilter (const CANFDMessage & /* inMessage */) { + gStandardRangeFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForStandardClassicFilter (const CANFDMessage & /* inMessage */) { + gStandardClassicFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedSingleFilter (const CANFDMessage & /* inMessage */) { + gExtendedSingleFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedDualFilter (const CANFDMessage & /* inMessage */) { + gExtendedDualFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedRangeFilter (const CANFDMessage & /* inMessage */) { + gExtendedRangeFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedClassicFilter (const CANFDMessage & /* inMessage */) { + gExtendedClassicFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x8) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + + ACANFD_STM32_StandardFilters standardFilters ; +//--- Add classic filter: identifier and mask (8 matching identifiers) + standardFilters.addClassic (0x405, 0x7D5, ACANFD_STM32_FilterAction::FIFO0, callBackForStandardClassicFilter) ; +//--- Add range filter: low bound, high bound (36 matching identifiers) + standardFilters.addRange (0x100, 0x123, ACANFD_STM32_FilterAction::FIFO1, callBackForStandardRangeFilter) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + standardFilters.addDual (0x033, 0x44, ACANFD_STM32_FilterAction::FIFO0, callBackForStandardDualFilter) ; +//--- Add single filter: identifier (1 matching identifier) + standardFilters.addSingle (0x055, ACANFD_STM32_FilterAction::FIFO0, callBackForStandardSingleFilter) ; + + ACANFD_STM32_ExtendedFilters extendedFilters ; +//--- Add single filter: identifier (1 matching identifier) + extendedFilters.addSingle (0x5555, ACANFD_STM32_FilterAction::FIFO0, callBackForExtendedSingleFilter) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + extendedFilters.addDual (0x3333, 0x4444, ACANFD_STM32_FilterAction::FIFO0, callBackForExtendedDualFilter) ; +//--- Add range filter: low bound, high bound (565 matching identifiers) + extendedFilters.addRange (0x1000, 0x1234, ACANFD_STM32_FilterAction::FIFO1, callBackForExtendedRangeFilter) ; +//--- Add classic filter: identifier and mask (32 matching identifiers) + extendedFilters.addClassic (0x6789, 0x1FFF67BD, ACANFD_STM32_FilterAction::FIFO0, callBackForExtendedClassicFilter) ; + +//--- Reject extended frames that do not match any filter + settings.mNonMatchingExtendedFrameReception = ACANFD_STM32_FilterAction::REJECT ; + +// Therefore FIFO0 receives 1 + 2 + 32 = 35 frames, FIFO1 receives 565 frames. + + const uint32_t errorCode = fdcan1.beginFD (settings, standardFilters, extendedFilters) ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentIdentifier = 0 ; +static bool gOk = true ; +static bool gSendExtended = false ; + +//----------------------------------------------------------------- + +static void printCount (const uint32_t inActualCount, const uint32_t inExpectedCount) { + Serial.print (", ") ; + if (inActualCount == inExpectedCount) { + Serial.print ("ok") ; + }else{ + Serial.print (inActualCount) ; + Serial.print ("/") ; + Serial.print (inExpectedCount) ; + } +} + +//----------------------------------------------------------------- + +void loop () { +//--- Send standard frame ? + if (!gSendExtended && gOk && (gSentIdentifier <= 0x7FF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- All standard frame have been sent ? + if (!gSendExtended && gOk && (gSentIdentifier > 0x7FF)) { + gSendExtended = true ; + gSentIdentifier = 0 ; + } +//--- Send extended frame ? + if (gSendExtended && gOk && (gSentIdentifier <= 0x1FFFFFFF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + frame.ext = true ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- Receive frame + fdcan1.dispatchReceivedMessage () ; +//--- Blink led and display + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent: ") ; + Serial.print (gSentIdentifier) ; + printCount (gStandardSingleFilterMatchCount, 1) ; + printCount (gStandardDualFilterMatchCount, 2) ; + printCount (gStandardRangeFilterMatchCount, 36) ; + printCount (gStandardClassicFilterMatchCount, 8) ; + printCount (gExtendedSingleFilterMatchCount, 1) ; + printCount (gExtendedDualFilterMatchCount, 2) ; + printCount (gExtendedRangeFilterMatchCount, 565) ; + printCount (gExtendedClassicFilterMatchCount, 32) ; + Serial.println () ; + } +} + +//----------------------------------------------------------------- diff --git a/lib/acanfd-stm32/examples/G431KB-LoopBackDemo/G431KB-LoopBackDemo.ino b/lib/acanfd-stm32/examples/G431KB-LoopBackDemo/G431KB-LoopBackDemo.ino new file mode 100644 index 0000000..968e7dd --- /dev/null +++ b/lib/acanfd-stm32/examples/G431KB-LoopBackDemo/G431KB-LoopBackDemo.ino @@ -0,0 +1,134 @@ +// LoopBackDemo + +// This demo runs on NUCLEO_G431KB +// The FDCAN module is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pin (PA12). No external hardware is required. + +#ifndef ARDUINO_NUCLEO_G431KB + #error This sketch runs on NUCLEO-G431KB Nucleo-32 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x5) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + uint32_t errorCode = fdcan1.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan1 ok") ; + }else{ + Serial.print ("Error can: 0x") ; + Serial.println (errorCode, HEX) ; + while (1) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + } + Serial.print ("Systick ") ; + Serial.println (SysTick->LOAD + 1) ; +} + +//----------------------------------------------------------------- + +static uint32_t gSendDate = 0 ; +static uint32_t gSentCount = 0 ; +static uint32_t gReceivedCount = 0 ; + +//----------------------------------------------------------------- + +void loop () { + if (gSendDate < millis ()) { + // Serial.print ("CPU frequency: ") ; + // Serial.print (F_CPU) ; + // Serial.println (" Hz") ; + // Serial.print ("PCLK1 frequency: ") ; + // Serial.print (HAL_RCC_GetPCLK1Freq ()) ; + // Serial.println (" Hz") ; + // Serial.print ("PCLK2 frequency: ") ; + // Serial.print (HAL_RCC_GetPCLK2Freq ()) ; + // Serial.println (" Hz") ; + // Serial.print ("HCLK frequency: ") ; + // Serial.print (HAL_RCC_GetHCLKFreq ()) ; + // Serial.println (" Hz") ; + // Serial.print ("SysClock frequency: ") ; + // Serial.print (HAL_RCC_GetSysClockFreq ()) ; + // Serial.println (" Hz") ; + // Serial.print ("CAN Clock: ") ; + // Serial.print (fdcanClock ()) ; + // Serial.println (" Hz") ; + CANFDMessage message ; + message.id = 0x7FF ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + // digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + gSendDate += 1000 ; + gSentCount += 1 ; + Serial.print ("fdcan1 sent: ") ; + Serial.println (gSentCount) ; + } + } + CANFDMessage messageFD ; + if (fdcan1.receiveFD0 (messageFD)) { + gReceivedCount += 1 ; + Serial.print ("fdcan1 received: ") ; + Serial.println (gReceivedCount) ; + } +} diff --git a/lib/acanfd-stm32/examples/G431KB-LoopBackDemoIntensive/G431KB-LoopBackDemoIntensive.ino b/lib/acanfd-stm32/examples/G431KB-LoopBackDemoIntensive/G431KB-LoopBackDemoIntensive.ino new file mode 100644 index 0000000..5ab11cc --- /dev/null +++ b/lib/acanfd-stm32/examples/G431KB-LoopBackDemoIntensive/G431KB-LoopBackDemoIntensive.ino @@ -0,0 +1,229 @@ +// This demo runs on NUCLEO_G431KB +// The fdcan1 module is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pin. No external hardware is required. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_G431KB + #error This sketch runs on NUCLEO-G431KB Nucleo-32 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +static ACANFD_STM32_FIFO gBuffer ; + +//----------------------------------------------------------------- + +void setup () { + gBuffer.initWithSize (25) ; + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x5) ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + const uint32_t errorCode = fdcan1.beginFD (settings) ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t pseudoRandomValue (void) { + static uint32_t gSeed = 0 ; + gSeed = 8253729U * gSeed + 2396403U ; + return gSeed ; +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentCount = 0 ; +static uint32_t gReceiveCount = 0 ; +static bool gOk = true ; +static uint32_t gCANRemoteFrameCount = 0 ; +static uint32_t gCanDataFrameCount = 0 ; +static uint32_t gCanFDNoBRSDataFrameCount = 0 ; +static uint32_t gCanFDWithBRSDataFrameCount = 0 ; +static uint32_t gStandardFrameCount = 0 ; +static uint32_t gExtendedFrameCount = 0 ; + +//----------------------------------------------------------------- + +static const uint8_t CANFD_LENGTH_FROM_CODE [16] = { + 0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64 +} ; + +//----------------------------------------------------------------- + +void loop () { + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent ") ; + Serial.print (gSentCount) ; + Serial.print (", received ") ; + Serial.print (gReceiveCount) ; + Serial.print (" (") ; + Serial.print (gCANRemoteFrameCount) ; + Serial.print (", ") ; + Serial.print (gCanDataFrameCount) ; + Serial.print (", ") ; + Serial.print (gCanFDNoBRSDataFrameCount) ; + Serial.print (", ") ; + Serial.print (gCanFDWithBRSDataFrameCount) ; + Serial.print (", ") ; + Serial.print (gStandardFrameCount) ; + Serial.print (", ") ; + Serial.print (gExtendedFrameCount) ; + Serial.print (", ") ; + Serial.print (fdcan1.statusFlags (), HEX) ; + Serial.println (")") ; + } +//--- Send buffer index +// 0: fifo +// 1 ... settings.mHardwareDedicacedTxBufferCount: dedicaced buffer + const uint8_t sendBufferIndex = 0 ; +//--- Send frame + if (gOk && (!gBuffer.isFull ()) && fdcan1.sendBufferNotFullForIndex (sendBufferIndex)) { + CANFDMessage frame ; + frame.idx = sendBufferIndex ; + const uint32_t r = pseudoRandomValue () ; + frame.ext = (r & (1 << 29)) != 0 ; + frame.type = CANFDMessage::Type (r >> 30) ; + frame.id = r & 0x1FFFFFFFU ; + if (frame.ext) { + gExtendedFrameCount += 1 ; + }else{ + gStandardFrameCount += 1 ; + frame.id &= 0x7FFU ; + } + switch (frame.type) { + case CANFDMessage::CAN_REMOTE : + gCANRemoteFrameCount += 1 ; + frame.len = pseudoRandomValue () % 9 ; + break ; + case CANFDMessage::CAN_DATA : + gCanDataFrameCount += 1 ; + frame.len = pseudoRandomValue () % 9 ; + for (uint32_t i=0 ; i should be included only once in a sketch, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x1) ; + // settings.mModuleMode = ACANFD_STM32_Settings::NORMAL_FD ; // Useless, as it is default value + settings.mOpenCollectorOutput = true ; // Required, as all TxCAN pins are connected together + settings.mInputPullup = true ; // Required, otherwise the recessive level cannot be ensured + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + +//--- beginFD is called without any receive filter, all sent frames are received +// by receiveFD0 throught receiveFIFO0 + settings.mTxPin = PB_9 ; // DO NOT USE Pxy, Px_y + settings.mRxPin = PB_8 ; // DO NOT USE Pxy, Px_y + uint32_t errorCode = fdcan1.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan1 configuration ok") ; + }else{ + Serial.print ("Error fdcan1: 0x") ; + Serial.println (errorCode, HEX) ; + } + + + settings.mTxPin = PB_13 ; // DO NOT USE Pxy, Px_y + settings.mRxPin = PB_12 ; // DO NOT USE Pxy, Px_y + errorCode = fdcan2.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan2 configuration ok") ; + }else{ + Serial.print ("Error fdcan2: 0x") ; + Serial.println (errorCode, HEX) ; + } + + settings.mTxPin = PB_4 ; // DO NOT USE Pxy, Px_y + settings.mRxPin = PB_3 ; // DO NOT USE Pxy, Px_y + errorCode = fdcan3.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan3 configuration ok") ; + }else{ + Serial.print ("Error fdcan3: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t gSendDate = 0 ; +static uint32_t gSentCount1 = 0 ; +static uint32_t gReceivedCount1 = 0 ; +static uint32_t gSentCount2 = 0 ; +static uint32_t gReceivedCount2 = 0 ; +static uint32_t gSentCount3 = 0 ; +static uint32_t gReceivedCount3 = 0 ; + +static const uint32_t MESSAGE_COUNT = 100 * 1000 ; + +//----------------------------------------------------------------- + +void loop () { + if (gSendDate < millis ()) { + gSendDate += 2000 ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("CPU frequency: ") ; + Serial.print (F_CPU) ; + Serial.println (" Hz") ; + Serial.print ("PCLK1 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK1Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("PCLK2 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK2Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("HCLK frequency: ") ; + Serial.print (HAL_RCC_GetHCLKFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("SysClock frequency: ") ; + Serial.print (HAL_RCC_GetSysClockFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("CAN Clock: ") ; + Serial.print (fdcanClock ()) ; + Serial.println (" Hz") ; + Serial.print ("fdcan1 sent: ") ; + Serial.print (gSentCount1) ; + Serial.print (", received: ") ; + Serial.println (gReceivedCount1) ; + Serial.print ("fdcan2 sent: ") ; + Serial.print (gSentCount2) ; + Serial.print (", received: ") ; + Serial.println (gReceivedCount2) ; + Serial.print ("fdcan3 sent: ") ; + Serial.print (gSentCount3) ; + Serial.print (", received: ") ; + Serial.println (gReceivedCount3) ; + } + CANFDMessage message ; + message.ext = true ; // Extended identifier + message.type = CANFDMessage::CANFD_NO_BIT_RATE_SWITCH ; // DO NOT USE CANFD_WITH_BIT_RATE_SWITCH + message.len = 5 ; + for (uint8_t i=0 ; i should be included only once in a sketch, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x1) ; + // settings.mModuleMode = ACANFD_STM32_Settings::NORMAL_FD ; // Useless, as it is default value + settings.mOpenCollectorOutput = true ; // Required, as all TxCAN pins are connected together + settings.mInputPullup = true ; // Required, otherwise the recessive level cannot be ensured + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + +//--- beginFD is called without any receive filter, all sent frames are received +// by receiveFD0 throught receiveFIFO0 + uint32_t errorCode = fdcan1.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan1 configuration ok") ; + }else{ + Serial.print ("Error fdcan1: 0x") ; + Serial.println (errorCode, HEX) ; + } + + + errorCode = fdcan2.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan2 configuration ok") ; + }else{ + Serial.print ("Error fdcan2: 0x") ; + Serial.println (errorCode, HEX) ; + } + + + errorCode = fdcan3.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan3 configuration ok") ; + }else{ + Serial.print ("Error fdcan3: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t gSendDate = 0 ; +static uint32_t gSentCount1 = 0 ; +static uint32_t gReceivedCount1 = 0 ; +static uint32_t gSentCount2 = 0 ; +static uint32_t gReceivedCount2 = 0 ; +static uint32_t gSentCount3 = 0 ; +static uint32_t gReceivedCount3 = 0 ; + +static const uint32_t MESSAGE_COUNT = 100 * 1000 ; + +//----------------------------------------------------------------- + +void loop () { + if (gSendDate < millis ()) { + gSendDate += 2000 ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("CPU frequency: ") ; + Serial.print (F_CPU) ; + Serial.println (" Hz") ; + Serial.print ("PCLK1 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK1Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("PCLK2 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK2Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("HCLK frequency: ") ; + Serial.print (HAL_RCC_GetHCLKFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("SysClock frequency: ") ; + Serial.print (HAL_RCC_GetSysClockFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("CAN Clock: ") ; + Serial.print (fdcanClock ()) ; + Serial.println (" Hz") ; + Serial.print ("fdcan1 sent: ") ; + Serial.print (gSentCount1) ; + Serial.print (", received: ") ; + Serial.println (gReceivedCount1) ; + Serial.print ("fdcan2 sent: ") ; + Serial.print (gSentCount2) ; + Serial.print (", received: ") ; + Serial.println (gReceivedCount2) ; + Serial.print ("fdcan3 sent: ") ; + Serial.print (gSentCount3) ; + Serial.print (", received: ") ; + Serial.println (gReceivedCount3) ; + } + CANFDMessage message ; + message.ext = true ; // Extended identifier + message.type = CANFDMessage::CANFD_NO_BIT_RATE_SWITCH ; // DO NOT USE CANFD_WITH_BIT_RATE_SWITCH + message.len = 5 ; + for (uint8_t i=0 ; i should be included only once, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x8) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + ACANFD_STM32_ExtendedFilters extendedFilters ; +//--- Add single filter: identifier (1 matching identifier) + extendedFilters.addSingle (0x5555, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + extendedFilters.addDual (0x3333, 0x4444, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add range filter: low bound, high bound (565 matching identifiers) + extendedFilters.addRange (0x1000, 0x1234, ACANFD_STM32_FilterAction::FIFO1) ; +//--- Add classic filter: identifier and mask (32 matching identifiers) + extendedFilters.addClassic (0x6789, 0x1FFF67BD, ACANFD_STM32_FilterAction::FIFO0) ; + +//--- Reject extended frames that do not match any filter + settings.mNonMatchingExtendedFrameReception = ACANFD_STM32_FilterAction::REJECT ; + + const uint32_t errorCode = fdcan1.beginFD (settings, extendedFilters) ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = 0 ; +static uint32_t gSentIdentifier = 0 ; +static uint32_t gReceiveCountFIFO0 = 0 ; +static uint32_t gReceiveCountFIFO1 = 0 ; +static bool gOk = true ; + +//----------------------------------------------------------------- + +static void printCount (const uint32_t inActualCount, const uint32_t inExpectedCount) { + Serial.print (", ") ; + if (inActualCount == inExpectedCount) { + Serial.print ("ok") ; + }else{ + Serial.print (inActualCount) ; + Serial.print ("/") ; + Serial.print (inExpectedCount) ; + } +} + +//----------------------------------------------------------------- + +void loop () { + if (gOk && (gSentIdentifier <= 0x1FFFFFFF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + frame.ext = true ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- Receive frame + CANFDMessage frame ; + if (gOk && fdcan1.receiveFD0 (frame)) { + gReceiveCountFIFO0 += 1 ; + } + if (gOk && fdcan1.receiveFD1 (frame)) { + gReceiveCountFIFO1 += 1 ; + } +//--- Blink led and display + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent: ") ; + Serial.print (gSentIdentifier) ; + printCount (gReceiveCountFIFO0, 35) ; + printCount (gReceiveCountFIFO1, 565) ; + Serial.println () ; + } +} + +//----------------------------------------------------------------- diff --git a/lib/acanfd-stm32/examples/G474RE-LoopBackDemo-StandardFilters/G474RE-LoopBackDemo-StandardFilters.ino b/lib/acanfd-stm32/examples/G474RE-LoopBackDemo-StandardFilters/G474RE-LoopBackDemo-StandardFilters.ino new file mode 100644 index 0000000..ac437ca --- /dev/null +++ b/lib/acanfd-stm32/examples/G474RE-LoopBackDemo-StandardFilters/G474RE-LoopBackDemo-StandardFilters.ino @@ -0,0 +1,144 @@ +// This demo runs on NUCLEO_G474RE +// It shows how handle standard frame receive filters +// The FDCAN1 module is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pin. No external hardware is required. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_G474RE + #error This sketch runs on NUCLEO-G474RE Nucleo-64 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x2) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + ACANFD_STM32_StandardFilters standardFilters ; +//--- Add classic filter: identifier and mask (8 matching identifiers) + standardFilters.addClassic (0x405, 0x7D5, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add range filter: low bound, high bound (36 matching identifiers) + standardFilters.addRange (0x100, 0x123, ACANFD_STM32_FilterAction::FIFO1) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + standardFilters.addDual (0x033, 0x44, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add single filter: identifier (1 matching identifier) + standardFilters.addSingle (0x055, ACANFD_STM32_FilterAction::FIFO0) ; + +//--- Reject standard frames that do not match any filter + settings.mNonMatchingStandardFrameReception = ACANFD_STM32_FilterAction::REJECT ; + + const uint32_t errorCode = fdcan1.beginFD (settings, standardFilters) ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static void printCount (const uint32_t inActualCount, const uint32_t inExpectedCount) { + Serial.print (", ") ; + if (inActualCount == inExpectedCount) { + Serial.print ("ok") ; + }else{ + Serial.print (inActualCount) ; + Serial.print ("/") ; + Serial.print (inExpectedCount) ; + } +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentIdentifier = 0 ; +static uint32_t gReceiveCountFIFO0 = 0 ; +static uint32_t gReceiveCountFIFO1 = 0 ; +static bool gOk = true ; + +//----------------------------------------------------------------- + +void loop () { + if (gOk && (gSentIdentifier <= 0x7FF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- Receive frame + CANFDMessage frame ; + if (gOk && fdcan1.receiveFD0 (frame)) { + gReceiveCountFIFO0 += 1 ; + } + if (gOk && fdcan1.receiveFD1 (frame)) { + gReceiveCountFIFO1 += 1 ; + } +//--- Blink led and display + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent: ") ; + Serial.print (gSentIdentifier) ; + printCount (gReceiveCountFIFO0, 11) ; + printCount (gReceiveCountFIFO1, 36) ; + Serial.println () ; + } +} + +//----------------------------------------------------------------- diff --git a/lib/acanfd-stm32/examples/G474RE-LoopBackDemo-customSystemClock/G474RE-LoopBackDemo-customSystemClock.ino b/lib/acanfd-stm32/examples/G474RE-LoopBackDemo-customSystemClock/G474RE-LoopBackDemo-customSystemClock.ino new file mode 100644 index 0000000..7899d18 --- /dev/null +++ b/lib/acanfd-stm32/examples/G474RE-LoopBackDemo-customSystemClock/G474RE-LoopBackDemo-customSystemClock.ino @@ -0,0 +1,258 @@ +//----------------------------------------------------------------- +// This demo runs on NUCLEO_G474RE +// The system clock are defined for getting a 160 MHz can clock +// The 3 FDCAN modules is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pins. No external hardware is required. +// Default TxCAN pins are used: +// - fdcan1: PA_12 +// - fdcan2: PB_6 +// - fdcan3: PA_15 +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_G474RE + #error This sketch runs on NUCLEO-G474RE Nucleo-64 board +#endif + +//----------------------------------------------------------------- +// WHY DEFINE CUSTOM SYSTEM CLOCK CONFIGURATION? +//----------------------------------------------------------------- +// In short: because default fdcan clock (168 MHz) can make a given bit rate unavailable. +// For example, I want 5 Mbit/s a data bit rate (arbitration bit rate is not significant for this problem) +// The 5 Mbit/s data bit rate is not available because 5 is not a divisor of 168. +// The only solution is to change the fdcan clock. +// We need a fdcan clock frequency multiple of 5 MHz, minimum 10 times 5 MHz, for allowing correct bit timing. +// Note setting a custom fdcan clock frequency affect also CPU speed. Note max CPU frequency is 170 MHz. +// A valid frequency is 160 MHz, but it would be valid to choose 170 MHz or 165 MHz. +//----------------------------------------------------------------- +// DEFINE CUSTOM SYSTEM CLOCK CONFIGURATION +//----------------------------------------------------------------- +// For any board, System Clock can be overriden. I think it is valid, as it is described in: +// https://github.com/stm32duino/Arduino_Core_STM32/wiki/Custom-definitions#systemclock_config +// For the NUCLEO_G474RE the SystemClock_Config function file is defined in STM32duino package. On my Mac, it is located at: +// ~/Library/Arduino15/packages/STMicroelectronics/hardware/stm32/2.6.0/variants/STM32G4xx/G473R\(B-C-E\)T_G474R\(B-C-E\)T_G483RET_G484RET/variant_NUCLEO_G474RE.cpp +// The SystemClock_Config function is duplicated below and modified. +// For a full understanding of the clock tree, consider using STM32CubeMX +// The very very simplified explaination of the clock tree is: +// - fdcan clock is PCLK1 +// - PCLK1 = HSE_CLOCK * PLLN / PLLM / PLLP +// HSE_CLOCK = 24 MHz, we cannot change that, it is given by STLink +// In the original file, PLLN=28, PLLM=2, PLLP=2 --> PCLK1 = 24 MHz * 28 / 2 / 2 = 168 MHz +// Caution: all values are not valid, I strongly suggest using STM32CubeMX for checking a given setting. +// For a PCLK1=160 MHz, it is valid to select PLLM=3 and PLLN=40: PCLK1 = 24 MHz * 40 / 3 / 2 = 160 MHz +// Note: for PLLM=3, use the RCC_PLLM_DIV3 symbol. +// For a PCLK1=170 MHz, it is valid to select PLLM=6 and PLLN=85: PCLK1 = 24 MHz * 85 / 6 / 2 = 170 MHz +// Note: for PLLM=6, use the RCC_PLLM_DIV6 symbol. +// For a PCLK1=165 MHz, it is valid to select PLLM=8 and PLLN=110: PCLK1 = 24 MHz * 110 / 8 / 2 = 165 MHz +// Note: for PLLM=8, use the RCC_PLLM_DIV8 symbol. +// +// Always validate your setting by checking actual CAN clock (call fdcanClock function, see below), +// and examine actual Data Bit Rate (call settings.actualDataBitRate function, see below) +//----------------------------------------------------------------- + +extern "C" void SystemClock_Config (void) { // extern "C" IS REQUIRED! + RCC_OscInitTypeDef RCC_OscInitStruct = {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {}; +#ifdef USBCON + RCC_PeriphCLKInitTypeDef PeriphClkInit = {}; +#endif + + /* Configure the main internal regulator output voltage */ + HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1_BOOST); + /* Initializes the CPU, AHB and APB busses clocks */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.HSEState = RCC_HSE_ON; + RCC_OscInitStruct.HSI48State = RCC_HSI48_ON; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; + RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV8 ; // Original value: RCC_PLLM_DIV2 + RCC_OscInitStruct.PLL.PLLN = 110 ; // Original value: 28 + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; + RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2; + RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { + Error_Handler(); + } + /* Initializes the CPU, AHB and APB busses clocks */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK + | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_8) != HAL_OK) { + Error_Handler(); + } + +#ifdef USBCON + /* Initializes the peripherals clocks */ + PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB; + PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_HSI48; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK) { + Error_Handler(); + } +#endif +} + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x10) ; + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + + uint32_t errorCode = fdcan1.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan1 configuration ok") ; + }else{ + Serial.print ("Error fdcan1: 0x") ; + Serial.println (errorCode, HEX) ; + } + + + errorCode = fdcan2.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan2 configuration ok") ; + }else{ + Serial.print ("Error fdcan2: 0x") ; + Serial.println (errorCode, HEX) ; + } + + + errorCode = fdcan3.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan3 configuration ok") ; + }else{ + Serial.print ("Error fdcan3: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t gSendDate = 0 ; +static uint32_t gSentCount1 = 0 ; +static uint32_t gReceivedCount1 = 0 ; +static uint32_t gSentCount2 = 0 ; +static uint32_t gReceivedCount2 = 0 ; +static uint32_t gSentCount3 = 0 ; +static uint32_t gReceivedCount3 = 0 ; + +//----------------------------------------------------------------- + +void loop () { + if (gSendDate < millis ()) { + gSendDate += 1000 ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("CPU frequency: ") ; + Serial.print (F_CPU) ; + Serial.println (" Hz") ; + Serial.print ("PCLK1 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK1Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("PCLK2 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK2Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("HCLK frequency: ") ; + Serial.print (HAL_RCC_GetHCLKFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("SysClock frequency: ") ; + Serial.print (HAL_RCC_GetSysClockFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("CAN Clock: ") ; + Serial.print (fdcanClock ()) ; + Serial.println (" Hz") ; + CANFDMessage message ; + message.id = 0x7FF ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; + uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount1 += 1 ; + Serial.print ("fdcan1 sent: ") ; + Serial.println (gSentCount1) ; + } + sendStatus = fdcan2.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount2 += 1 ; + Serial.print ("fdcan2 sent: ") ; + Serial.println (gSentCount2) ; + } + sendStatus = fdcan3.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount3 += 1 ; + Serial.print ("fdcan3 sent: ") ; + Serial.println (gSentCount3) ; + } + } + CANFDMessage messageFD ; + if (fdcan1.receiveFD0 (messageFD)) { + gReceivedCount1 += 1 ; + Serial.print ("fdcan1 received: ") ; + Serial.println (gReceivedCount1) ; + } + if (fdcan2.receiveFD0 (messageFD)) { + gReceivedCount2 += 1 ; + Serial.print ("fdcan2 received: ") ; + Serial.println (gReceivedCount2) ; + } + if (fdcan3.receiveFD0 (messageFD)) { + gReceivedCount3 += 1 ; + Serial.print ("fdcan3 received: ") ; + Serial.println (gReceivedCount3) ; + } +} diff --git a/lib/acanfd-stm32/examples/G474RE-LoopBackDemo-filters-and-dispatch/G474RE-LoopBackDemo-filters-and-dispatch.ino b/lib/acanfd-stm32/examples/G474RE-LoopBackDemo-filters-and-dispatch/G474RE-LoopBackDemo-filters-and-dispatch.ino new file mode 100644 index 0000000..9a795cb --- /dev/null +++ b/lib/acanfd-stm32/examples/G474RE-LoopBackDemo-filters-and-dispatch/G474RE-LoopBackDemo-filters-and-dispatch.ino @@ -0,0 +1,234 @@ +// This demo runs on NUCLEO_G474RE +// It shows how handle receive filters and receive dispatch +// The FDCAN1 module is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pin. No external hardware is required. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_G474RE + #error This sketch runs on NUCLEO-G474RE Nucleo-64 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +static uint32_t gStandardSingleFilterMatchCount = 0 ; +static uint32_t gStandardDualFilterMatchCount = 0 ; +static uint32_t gStandardRangeFilterMatchCount = 0 ; +static uint32_t gStandardClassicFilterMatchCount = 0 ; +static uint32_t gExtendedSingleFilterMatchCount = 0 ; +static uint32_t gExtendedDualFilterMatchCount = 0 ; +static uint32_t gExtendedRangeFilterMatchCount = 0 ; +static uint32_t gExtendedClassicFilterMatchCount = 0 ; + +//----------------------------------------------------------------- + +static void callBackForStandardSingleFilter (const CANFDMessage & /* inMessage */) { + gStandardSingleFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForStandardDualFilter (const CANFDMessage & /* inMessage */) { + gStandardDualFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForStandardRangeFilter (const CANFDMessage & /* inMessage */) { + gStandardRangeFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForStandardClassicFilter (const CANFDMessage & /* inMessage */) { + gStandardClassicFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedSingleFilter (const CANFDMessage & /* inMessage */) { + gExtendedSingleFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedDualFilter (const CANFDMessage & /* inMessage */) { + gExtendedDualFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedRangeFilter (const CANFDMessage & /* inMessage */) { + gExtendedRangeFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedClassicFilter (const CANFDMessage & /* inMessage */) { + gExtendedClassicFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x4) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + + ACANFD_STM32_StandardFilters standardFilters ; +//--- Add classic filter: identifier and mask (8 matching identifiers) + standardFilters.addClassic (0x405, 0x7D5, ACANFD_STM32_FilterAction::FIFO0, callBackForStandardClassicFilter) ; +//--- Add range filter: low bound, high bound (36 matching identifiers) + standardFilters.addRange (0x100, 0x123, ACANFD_STM32_FilterAction::FIFO1, callBackForStandardRangeFilter) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + standardFilters.addDual (0x033, 0x44, ACANFD_STM32_FilterAction::FIFO0, callBackForStandardDualFilter) ; +//--- Add single filter: identifier (1 matching identifier) + standardFilters.addSingle (0x055, ACANFD_STM32_FilterAction::FIFO0, callBackForStandardSingleFilter) ; + + ACANFD_STM32_ExtendedFilters extendedFilters ; +//--- Add single filter: identifier (1 matching identifier) + extendedFilters.addSingle (0x5555, ACANFD_STM32_FilterAction::FIFO0, callBackForExtendedSingleFilter) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + extendedFilters.addDual (0x3333, 0x4444, ACANFD_STM32_FilterAction::FIFO0, callBackForExtendedDualFilter) ; +//--- Add range filter: low bound, high bound (565 matching identifiers) + extendedFilters.addRange (0x1000, 0x1234, ACANFD_STM32_FilterAction::FIFO1, callBackForExtendedRangeFilter) ; +//--- Add classic filter: identifier and mask (32 matching identifiers) + extendedFilters.addClassic (0x6789, 0x1FFF67BD, ACANFD_STM32_FilterAction::FIFO0, callBackForExtendedClassicFilter) ; + +//--- Reject extended frames that do not match any filter + settings.mNonMatchingExtendedFrameReception = ACANFD_STM32_FilterAction::REJECT ; + +// Therefore FIFO0 receives 1 + 2 + 32 = 35 frames, FIFO1 receives 565 frames. + + const uint32_t errorCode = fdcan1.beginFD (settings, standardFilters, extendedFilters) ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentIdentifier = 0 ; +static bool gOk = true ; +static bool gSendExtended = false ; + +//----------------------------------------------------------------- + +static void printCount (const uint32_t inActualCount, const uint32_t inExpectedCount) { + Serial.print (", ") ; + if (inActualCount == inExpectedCount) { + Serial.print ("ok") ; + }else{ + Serial.print (inActualCount) ; + Serial.print ("/") ; + Serial.print (inExpectedCount) ; + } +} + +//----------------------------------------------------------------- + +void loop () { +//--- Send standard frame ? + if (!gSendExtended && gOk && (gSentIdentifier <= 0x7FF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- All standard frame have been sent ? + if (!gSendExtended && gOk && (gSentIdentifier > 0x7FF)) { + gSendExtended = true ; + gSentIdentifier = 0 ; + } +//--- Send extended frame ? + if (gSendExtended && gOk && (gSentIdentifier <= 0x1FFFFFFF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + frame.ext = true ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- Receive frame + fdcan1.dispatchReceivedMessage () ; +//--- Blink led and display + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent: ") ; + Serial.print (gSentIdentifier) ; + printCount (gStandardSingleFilterMatchCount, 1) ; + printCount (gStandardDualFilterMatchCount, 2) ; + printCount (gStandardRangeFilterMatchCount, 36) ; + printCount (gStandardClassicFilterMatchCount, 8) ; + printCount (gExtendedSingleFilterMatchCount, 1) ; + printCount (gExtendedDualFilterMatchCount, 2) ; + printCount (gExtendedRangeFilterMatchCount, 565) ; + printCount (gExtendedClassicFilterMatchCount, 32) ; + Serial.println () ; + } +} + +//----------------------------------------------------------------- diff --git a/lib/acanfd-stm32/examples/G474RE-LoopBackDemo/G474RE-LoopBackDemo.ino b/lib/acanfd-stm32/examples/G474RE-LoopBackDemo/G474RE-LoopBackDemo.ino new file mode 100644 index 0000000..9ec49d7 --- /dev/null +++ b/lib/acanfd-stm32/examples/G474RE-LoopBackDemo/G474RE-LoopBackDemo.ino @@ -0,0 +1,182 @@ +//----------------------------------------------------------------- +// This demo runs on NUCLEO_G474RE +// The can clock frequency is 168 MHz. +// The 3 FDCAN modules are configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pins. No external hardware is required. +// Default TxCAN pins are used: +// - fdcan1: PA_12 +// - fdcan2: PB_6 +// - fdcan3: PA_15 +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_G474RE + #error This sketch runs on NUCLEO-G474RE Nucleo-64 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once in a sketch, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + + Serial.print ("CPU frequency: ") ; + Serial.print (F_CPU) ; + Serial.println (" Hz") ; + Serial.print ("PCLK1 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK1Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("PCLK2 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK2Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("HCLK frequency: ") ; + Serial.print (HAL_RCC_GetHCLKFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("SysClock frequency: ") ; + Serial.print (HAL_RCC_GetSysClockFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("FDCAN Clock: ") ; + Serial.print (fdcanClock ()) ; + Serial.println (" Hz") ; + + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x2) ; + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + +//--- beginFD is called without any receive filter, all sent frames are received +// by receiveFD0 throught receiveFIFO0 + uint32_t errorCode = fdcan1.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan1 configuration ok") ; + }else{ + Serial.print ("Error fdcan1: 0x") ; + Serial.println (errorCode, HEX) ; + } + + + errorCode = fdcan2.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan2 configuration ok") ; + }else{ + Serial.print ("Error fdcan2: 0x") ; + Serial.println (errorCode, HEX) ; + } + + + errorCode = fdcan3.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan3 configuration ok") ; + }else{ + Serial.print ("Error fdcan3: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t gSendDate = 0 ; +static uint32_t gSentCount1 = 0 ; +static uint32_t gReceivedCount1 = 0 ; +static uint32_t gSentCount2 = 0 ; +static uint32_t gReceivedCount2 = 0 ; +static uint32_t gSentCount3 = 0 ; +static uint32_t gReceivedCount3 = 0 ; + +//----------------------------------------------------------------- + +void loop () { + if (gSendDate < millis ()) { + gSendDate += 1000 ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + CANFDMessage message ; + message.id = 0x7FF ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; +//--- tryToSendReturnStatusFD returns 0 if message has been successfully +// appended in transmit buffer. +// A not zero returned value contains an error code (see doc) + uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount1 += 1 ; + Serial.print ("fdcan1 sent: ") ; + Serial.println (gSentCount1) ; + } + sendStatus = fdcan2.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount2 += 1 ; + Serial.print ("fdcan2 sent: ") ; + Serial.println (gSentCount2) ; + } + sendStatus = fdcan3.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount3 += 1 ; + Serial.print ("fdcan3 sent: ") ; + Serial.println (gSentCount3) ; + } + } + CANFDMessage messageFD ; + if (fdcan1.receiveFD0 (messageFD)) { + gReceivedCount1 += 1 ; + Serial.print ("fdcan1 received: ") ; + Serial.println (gReceivedCount1) ; + } + if (fdcan2.receiveFD0 (messageFD)) { + gReceivedCount2 += 1 ; + Serial.print ("fdcan2 received: ") ; + Serial.println (gReceivedCount2) ; + } + if (fdcan3.receiveFD0 (messageFD)) { + gReceivedCount3 += 1 ; + Serial.print ("fdcan3 received: ") ; + Serial.println (gReceivedCount3) ; + } +} diff --git a/lib/acanfd-stm32/examples/H723ZG-LoopBackDemo/H723ZG-LoopBackDemo.ino b/lib/acanfd-stm32/examples/H723ZG-LoopBackDemo/H723ZG-LoopBackDemo.ino new file mode 100644 index 0000000..0b13d08 --- /dev/null +++ b/lib/acanfd-stm32/examples/H723ZG-LoopBackDemo/H723ZG-LoopBackDemo.ino @@ -0,0 +1,232 @@ +// LoopBackDemo + +// This demo runs on NUCLEO_H723ZG +// The FDCAN1 module is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pin. No external hardware is required. + +#ifndef ARDUINO_NUCLEO_H723ZG + #error This sketch runs on NUCLEO-H723ZG Nucleo-144 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +// Before including , you should define +// Message RAM size for FDCAN1 and Message RAM size for FDCAN2. +// Maximum required size is 2,560 (2,560 32-bit words). +// A 0 size means the FDCAN module is not configured; its TxCAN and RxCAN pins +// can be freely used for an other function. +// The begin method checks if actual size is greater or equal to required size. +// Hint: if you do not want to compute required size, print +// fdcan1.messageRamRequiredMinimumSize () for getting it. +//----------------------------------------------------------------- + +static const uint32_t FDCAN1_MESSAGE_RAM_WORD_SIZE = 800 ; +static const uint32_t FDCAN2_MESSAGE_RAM_WORD_SIZE = 800 ; +static const uint32_t FDCAN3_MESSAGE_RAM_WORD_SIZE = 800 ; + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + digitalWrite (LED_BUILTIN, HIGH) ; + Serial.begin (9600) ; + while (!Serial) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x4) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + // settings.mTxPin = PB_9 ; + // settings.mRxPin = PB_8 ; + uint32_t errorCode = fdcan1.beginFD (settings) ; + Serial.print ("FDCAN1 Message RAM required minimum size: ") ; + Serial.print (fdcan1.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + if (0 == errorCode) { + Serial.println ("fdcan1 ok") ; + }else{ + Serial.print ("Error fdcan1: 0x") ; + Serial.println (errorCode, HEX) ; + } + + // settings.mTxPin = 255 ; + // settings.mRxPin = 255 ; + errorCode = fdcan2.beginFD (settings) ; + Serial.print ("FDCAN2 Message RAM required minimum size: ") ; + Serial.print (fdcan2.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + if (0 == errorCode) { + Serial.println ("fdcan2 ok") ; + }else{ + Serial.print ("Error fdcan2: 0x") ; + Serial.println (errorCode, HEX) ; + } + + // settings.mTxPin = 255 ; + // settings.mRxPin = 255 ; + errorCode = fdcan3.beginFD (settings) ; + Serial.print ("FDCAN3 Message RAM required minimum size: ") ; + Serial.print (fdcan3.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + if (0 == errorCode) { + Serial.println ("fdcan3 ok") ; + }else{ + Serial.print ("Error fdcan3: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t gSendDate1 = 0 ; +static uint32_t gSentCount1 = 0 ; +static uint32_t gReceivedCount1 = 0 ; + +static uint32_t gSendDate2 = 0 ; +static uint32_t gSentCount2 = 0 ; +static uint32_t gReceivedCount2 = 0 ; + +static uint32_t gSendDate3 = 0 ; +static uint32_t gSentCount3 = 0 ; +static uint32_t gReceivedCount3 = 0 ; + +//----------------------------------------------------------------- + +void loop () { + if (gSendDate1 < millis ()) { + // Serial.print ("CPU frequency: ") ; + // Serial.print (F_CPU) ; + // Serial.println (" Hz") ; + // Serial.print ("PCLK1 frequency: ") ; + // Serial.print (HAL_RCC_GetPCLK1Freq ()) ; + // Serial.println (" Hz") ; + // Serial.print ("PCLK2 frequency: ") ; + // Serial.print (HAL_RCC_GetPCLK2Freq ()) ; + // Serial.println (" Hz") ; + // Serial.print ("HCLK frequency: ") ; + // Serial.print (HAL_RCC_GetHCLKFreq ()) ; + // Serial.println (" Hz") ; + // Serial.print ("SysClock frequency: ") ; + // Serial.print (HAL_RCC_GetSysClockFreq ()) ; + // Serial.println (" Hz") ; + // Serial.print ("CAN Clock: ") ; + // Serial.print (fdcanClock ()) ; + // Serial.println (" Hz") ; + CANFDMessage message ; + message.id = 0x7FF ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + gSendDate1 += 1000 ; + gSentCount1 += 1 ; + Serial.print ("FDCAN1 sent: ") ; + Serial.println (gSentCount1) ; + } + } + if (gSendDate2 < millis ()) { + CANFDMessage message ; + message.id = 0x7FE ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; + const uint32_t sendStatus = fdcan2.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + gSendDate2 += 999 ; + gSentCount2 += 1 ; + Serial.print ("FDCAN2 sent: ") ; + Serial.println (gSentCount2) ; + } + } +if (gSendDate3 < millis ()) { + CANFDMessage message ; + message.id = 0x7FD ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; + const uint32_t sendStatus = fdcan3.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + gSendDate3 += 1001 ; + gSentCount3 += 1 ; + Serial.print ("FDCAN3 sent: ") ; + Serial.println (gSentCount3) ; + } + } + CANFDMessage messageFD ; + if (fdcan1.receiveFD0 (messageFD)) { + gReceivedCount1 += 1 ; + Serial.print ("FDCAN1 received: ") ; + Serial.println (gReceivedCount1) ; + } + if (fdcan2.receiveFD0 (messageFD)) { + gReceivedCount2 += 1 ; + Serial.print ("FDCAN2 received: ") ; + Serial.println (gReceivedCount2) ; + } + if (fdcan3.receiveFD0 (messageFD)) { + gReceivedCount3 += 1 ; + Serial.print ("FDCAN3 received: ") ; + Serial.println (gReceivedCount3) ; + } +} diff --git a/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo-CAN1-ExtendedFilters/H743ZI2-LoopBackDemo-CAN1-ExtendedFilters.ino b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo-CAN1-ExtendedFilters/H743ZI2-LoopBackDemo-CAN1-ExtendedFilters.ino new file mode 100644 index 0000000..9553607 --- /dev/null +++ b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo-CAN1-ExtendedFilters/H743ZI2-LoopBackDemo-CAN1-ExtendedFilters.ino @@ -0,0 +1,165 @@ +// CAN1 external LoopBackDemo for NUCLEO_H743ZI2 +// No external hardware required. +// You can observe emitted CANFD frames on CANH / CANL pins. +// This sketch is an example of extended filters. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_H743ZI2 + #error This sketch runs on NUCLEO-H743ZI2 Nucleo-144 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +// Before including , you should define +// Message RAM size for FDCAN1 and Message RAM size for FDCAN2. +// Maximum required size is 2,560 (2,560 32-bit words). +// A 0 size means the FDCAN module is not configured; its TxCAN and RxCAN pins +// can be freely used for an other function. +// The begin method checks if actual size is greater or equal to required size. +// Hint: if you do not want to compute required size, print +// fdcan1.messageRamRequiredMinimumSize () for getting it. +//----------------------------------------------------------------- + +static const uint32_t FDCAN1_MESSAGE_RAM_WORD_SIZE = 2560 ; +static const uint32_t FDCAN2_MESSAGE_RAM_WORD_SIZE = 0 ; // FDCAN2 not used + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x8) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + ACANFD_STM32_ExtendedFilters extendedFilters ; +//--- Add single filter: identifier (1 matching identifier) + extendedFilters.addSingle (0x5555, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + extendedFilters.addDual (0x3333, 0x4444, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add range filter: low bound, high bound (565 matching identifiers) + extendedFilters.addRange (0x1000, 0x1234, ACANFD_STM32_FilterAction::FIFO1) ; +//--- Add classic filter: identifier and mask (32 matching identifiers) + extendedFilters.addClassic (0x6789, 0x1FFF67BD, ACANFD_STM32_FilterAction::FIFO0) ; + +//--- Reject extended frames that do not match any filter + settings.mNonMatchingExtendedFrameReception = ACANFD_STM32_FilterAction::REJECT ; + +// Therefore FIFO0 receives 1 + 2 + 32 = 35 frames, FIFO1 receives 565 frames. + +//--- Allocate FIFO 1 + settings.mHardwareRxFIFO1Size = 10 ; // By default, 0 + settings.mDriverReceiveFIFO1Size = 10 ; // By default, 0 + + const uint32_t errorCode = fdcan1.beginFD (settings, extendedFilters) ; + + Serial.print ("Message RAM required minimum size: ") ; + Serial.print (fdcan1.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentIdentifier = 0 ; +static uint32_t gReceiveCountFIFO0 = 0 ; +static uint32_t gReceiveCountFIFO1 = 0 ; +static bool gOk = true ; + +//----------------------------------------------------------------- + +static void printCount (const uint32_t inActualCount, const uint32_t inExpectedCount) { + Serial.print (", ") ; + if (inActualCount == inExpectedCount) { + Serial.print ("ok") ; + }else{ + Serial.print (inActualCount) ; + Serial.print ("/") ; + Serial.print (inExpectedCount) ; + } +} + +//----------------------------------------------------------------- + +void loop () { + if (gOk && (gSentIdentifier <= 0x1FFFFFFF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + frame.ext = true ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- Receive frame + CANFDMessage frame ; + if (gOk && fdcan1.receiveFD0 (frame)) { + gReceiveCountFIFO0 += 1 ; + } + if (gOk && fdcan1.receiveFD1 (frame)) { + gReceiveCountFIFO1 += 1 ; + } +//--- Blink led and display + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent: ") ; + Serial.print (gSentIdentifier) ; + printCount (gReceiveCountFIFO0, 35) ; + printCount (gReceiveCountFIFO1, 565) ; + Serial.println () ; + } +} + +//----------------------------------------------------------------- diff --git a/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo-CAN1-StandardFilters/H743ZI2-LoopBackDemo-CAN1-StandardFilters.ino b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo-CAN1-StandardFilters/H743ZI2-LoopBackDemo-CAN1-StandardFilters.ino new file mode 100644 index 0000000..75d9255 --- /dev/null +++ b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo-CAN1-StandardFilters/H743ZI2-LoopBackDemo-CAN1-StandardFilters.ino @@ -0,0 +1,161 @@ +// CAN1 external LoopBackDemo for NUCLEO_H743ZI2 +// No external hardware required. +// This sketch is an example of standard filters. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_H743ZI2 + #error This sketch runs on NUCLEO-H743ZI2 Nucleo-144 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +// Before including , you should define +// Message RAM size for FDCAN1 and Message RAM size for FDCAN2. +// Maximum required size is 2,560 (2,560 32-bit words). +// A 0 size means the FDCAN module is not configured; its TxCAN and RxCAN pins +// can be freely used for an other function. +// The begin method checks if actual size is greater or equal to required size. +// Hint: if you do not want to compute required size, print +// fdcan1.messageRamRequiredMinimumSize () for getting it. +//----------------------------------------------------------------- + +static const uint32_t FDCAN1_MESSAGE_RAM_WORD_SIZE = 2560 ; +static const uint32_t FDCAN2_MESSAGE_RAM_WORD_SIZE = 0 ; // FDCAN2 not used + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x2) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + ACANFD_STM32_StandardFilters standardFilters ; +//--- Add classic filter: identifier and mask (8 matching identifiers) + standardFilters.addClassic (0x405, 0x7D5, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add range filter: low bound, high bound (36 matching identifiers) + standardFilters.addRange (0x100, 0x123, ACANFD_STM32_FilterAction::FIFO1) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + standardFilters.addDual (0x033, 0x44, ACANFD_STM32_FilterAction::FIFO0) ; +//--- Add single filter: identifier (1 matching identifier) + standardFilters.addSingle (0x055, ACANFD_STM32_FilterAction::FIFO0) ; + +//--- Reject standard frames that do not match any filter + settings.mNonMatchingStandardFrameReception = ACANFD_STM32_FilterAction::REJECT ; + +//--- Allocate FIFO 1 + settings.mHardwareRxFIFO1Size = 10 ; // By default, 0 + settings.mDriverReceiveFIFO1Size = 10 ; // By default, 0 + + const uint32_t errorCode = fdcan1.beginFD (settings, standardFilters) ; + + Serial.print ("Message RAM required minimum size: ") ; + Serial.print (fdcan1.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static void printCount (const uint32_t inActualCount, const uint32_t inExpectedCount) { + Serial.print (", ") ; + if (inActualCount == inExpectedCount) { + Serial.print ("ok") ; + }else{ + Serial.print (inActualCount) ; + Serial.print ("/") ; + Serial.print (inExpectedCount) ; + } +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentIdentifier = 0 ; +static uint32_t gReceiveCountFIFO0 = 0 ; +static uint32_t gReceiveCountFIFO1 = 0 ; +static bool gOk = true ; + +//----------------------------------------------------------------- + +void loop () { + if (gOk && (gSentIdentifier <= 0x7FF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- Receive frame + CANFDMessage frame ; + if (gOk && fdcan1.receiveFD0 (frame)) { + gReceiveCountFIFO0 += 1 ; + } + if (gOk && fdcan1.receiveFD1 (frame)) { + gReceiveCountFIFO1 += 1 ; + } +//--- Blink led and display + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent: ") ; + Serial.print (gSentIdentifier) ; + printCount (gReceiveCountFIFO0, 11) ; + printCount (gReceiveCountFIFO1, 36) ; + Serial.println () ; + } +} + +//----------------------------------------------------------------- diff --git a/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo-CAN1-dispatch/H743ZI2-LoopBackDemo-CAN1-dispatch.ino b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo-CAN1-dispatch/H743ZI2-LoopBackDemo-CAN1-dispatch.ino new file mode 100644 index 0000000..0044ff0 --- /dev/null +++ b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo-CAN1-dispatch/H743ZI2-LoopBackDemo-CAN1-dispatch.ino @@ -0,0 +1,252 @@ +// CAN1 external LoopBackDemo for NUCLEO_H743ZI2 +// No external hardware required. +// You can observe emitted CANFD frames on CANH / CANL pins. +// This sketch is an example of standard and extended filters, with dispatch function. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_H743ZI2 + #error This sketch runs on NUCLEO-H743ZI2 Nucleo-144 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +// Before including , you should define +// Message RAM size for FDCAN1 and Message RAM size for FDCAN2. +// Maximum required size is 2,560 (2,560 32-bit words). +// A 0 size means the FDCAN module is not configured; its TxCAN and RxCAN pins +// can be freely used for an other function. +// The begin method checks if actual size is greater or equal to required size. +// Hint: if you do not want to compute required size, print +// fdcan1.messageRamRequiredMinimumSize () for getting it. +//----------------------------------------------------------------- + +static const uint32_t FDCAN1_MESSAGE_RAM_WORD_SIZE = 2560 ; +static const uint32_t FDCAN2_MESSAGE_RAM_WORD_SIZE = 0 ; // FDCAN2 not used + +#include + +//----------------------------------------------------------------- + +static uint32_t gStandardSingleFilterMatchCount = 0 ; +static uint32_t gStandardDualFilterMatchCount = 0 ; +static uint32_t gStandardRangeFilterMatchCount = 0 ; +static uint32_t gStandardClassicFilterMatchCount = 0 ; +static uint32_t gExtendedSingleFilterMatchCount = 0 ; +static uint32_t gExtendedDualFilterMatchCount = 0 ; +static uint32_t gExtendedRangeFilterMatchCount = 0 ; +static uint32_t gExtendedClassicFilterMatchCount = 0 ; + +//----------------------------------------------------------------- + +static void callBackForStandardSingleFilter (const CANFDMessage & /* inMessage */) { + gStandardSingleFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForStandardDualFilter (const CANFDMessage & /* inMessage */) { + gStandardDualFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForStandardRangeFilter (const CANFDMessage & /* inMessage */) { + gStandardRangeFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForStandardClassicFilter (const CANFDMessage & /* inMessage */) { + gStandardClassicFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedSingleFilter (const CANFDMessage & /* inMessage */) { + gExtendedSingleFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedDualFilter (const CANFDMessage & /* inMessage */) { + gExtendedDualFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedRangeFilter (const CANFDMessage & /* inMessage */) { + gExtendedRangeFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +static void callBackForExtendedClassicFilter (const CANFDMessage & /* inMessage */) { + gExtendedClassicFilterMatchCount += 1 ; +} + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x8) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + + ACANFD_STM32_StandardFilters standardFilters ; +//--- Add classic filter: identifier and mask (8 matching identifiers) + standardFilters.addClassic (0x405, 0x7D5, ACANFD_STM32_FilterAction::FIFO0, callBackForStandardClassicFilter) ; +//--- Add range filter: low bound, high bound (36 matching identifiers) + standardFilters.addRange (0x100, 0x123, ACANFD_STM32_FilterAction::FIFO1, callBackForStandardRangeFilter) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + standardFilters.addDual (0x033, 0x44, ACANFD_STM32_FilterAction::FIFO0, callBackForStandardDualFilter) ; +//--- Add single filter: identifier (1 matching identifier) + standardFilters.addSingle (0x055, ACANFD_STM32_FilterAction::FIFO0, callBackForStandardSingleFilter) ; + + ACANFD_STM32_ExtendedFilters extendedFilters ; +//--- Add single filter: identifier (1 matching identifier) + extendedFilters.addSingle (0x5555, ACANFD_STM32_FilterAction::FIFO0, callBackForExtendedSingleFilter) ; +//--- Add dual filter: identifier1, identifier2 (2 matching identifiers) + extendedFilters.addDual (0x3333, 0x4444, ACANFD_STM32_FilterAction::FIFO0, callBackForExtendedDualFilter) ; +//--- Add range filter: low bound, high bound (565 matching identifiers) + extendedFilters.addRange (0x1000, 0x1234, ACANFD_STM32_FilterAction::FIFO1, callBackForExtendedRangeFilter) ; +//--- Add classic filter: identifier and mask (32 matching identifiers) + extendedFilters.addClassic (0x6789, 0x1FFF67BD, ACANFD_STM32_FilterAction::FIFO0, callBackForExtendedClassicFilter) ; + +//--- Reject extended frames that do not match any filter + settings.mNonMatchingExtendedFrameReception = ACANFD_STM32_FilterAction::REJECT ; + +// Therefore FIFO0 receives 1 + 2 + 32 = 35 frames, FIFO1 receives 565 frames. + +//--- Allocate FIFO 1 + settings.mHardwareRxFIFO1Size = 10 ; // By default, 0 + settings.mDriverReceiveFIFO1Size = 10 ; // By default, 0 + + const uint32_t errorCode = fdcan1.beginFD (settings, standardFilters, extendedFilters) ; + + Serial.print ("Message RAM required minimum size: ") ; + Serial.print (fdcan1.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentIdentifier = 0 ; +static bool gOk = true ; +static bool gSendExtended = false ; + +//----------------------------------------------------------------- + +static void printCount (const uint32_t inActualCount, const uint32_t inExpectedCount) { + Serial.print (", ") ; + if (inActualCount == inExpectedCount) { + Serial.print ("ok") ; + }else{ + Serial.print (inActualCount) ; + Serial.print ("/") ; + Serial.print (inExpectedCount) ; + } +} + +//----------------------------------------------------------------- + +void loop () { +//--- Send standard frame ? + if (!gSendExtended && gOk && (gSentIdentifier <= 0x7FF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- All standard frame have been sent ? + if (!gSendExtended && gOk && (gSentIdentifier > 0x7FF)) { + gSendExtended = true ; + gSentIdentifier = 0 ; + } +//--- Send extended frame ? + if (gSendExtended && gOk && (gSentIdentifier <= 0x1FFFFFFF) && fdcan1.sendBufferNotFullForIndex (0)) { + CANFDMessage frame ; + frame.id = gSentIdentifier ; + frame.ext = true ; + gSentIdentifier += 1 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (frame) ; + if (sendStatus != 0) { + gOk = false ; + Serial.print ("Sent error 0x") ; + Serial.println (sendStatus) ; + } + } +//--- Receive frame + fdcan1.dispatchReceivedMessage () ; +//--- Blink led and display + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent: ") ; + Serial.print (gSentIdentifier) ; + printCount (gStandardSingleFilterMatchCount, 1) ; + printCount (gStandardDualFilterMatchCount, 2) ; + printCount (gStandardRangeFilterMatchCount, 36) ; + printCount (gStandardClassicFilterMatchCount, 8) ; + printCount (gExtendedSingleFilterMatchCount, 1) ; + printCount (gExtendedDualFilterMatchCount, 2) ; + printCount (gExtendedRangeFilterMatchCount, 565) ; + printCount (gExtendedClassicFilterMatchCount, 32) ; + Serial.println () ; + } +} + +//----------------------------------------------------------------- diff --git a/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo/H743ZI2-LoopBackDemo.ino b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo/H743ZI2-LoopBackDemo.ino new file mode 100644 index 0000000..368da94 --- /dev/null +++ b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemo/H743ZI2-LoopBackDemo.ino @@ -0,0 +1,188 @@ +// LoopBackDemo + +// This demo runs on NUCLEO_H743ZI2 +// The FDCAN1 module is configured in external loop back mode: it +// internally receives every CAN frame it sends, and emitted frames +// can be observed on TxCAN pin. No external hardware is required. + +#ifndef ARDUINO_NUCLEO_H743ZI2 + #error This sketch runs on NUCLEO-H743ZI2 Nucleo-144 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +// Before including , you should define +// Message RAM size for FDCAN1 and Message RAM size for FDCAN2. +// Maximum required size is 2,560 (2,560 32-bit words). +// A 0 size means the FDCAN module is not configured; its TxCAN and RxCAN pins +// can be freely used for an other function. +// The begin method checks if actual size is greater or equal to required size. +// Hint: if you do not want to compute required size, print +// fdcan1.messageRamRequiredMinimumSize () for getting it. +//----------------------------------------------------------------- + +static const uint32_t FDCAN1_MESSAGE_RAM_WORD_SIZE = 1000 ; +static const uint32_t FDCAN2_MESSAGE_RAM_WORD_SIZE = 1000 ; + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + digitalWrite (LED_BUILTIN, HIGH) ; + Serial.begin (9600) ; + while (!Serial) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x4) ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + // settings.mTxPin = PB_9 ; + // settings.mRxPin = PB_8 ; + uint32_t errorCode = fdcan1.beginFD (settings) ; + Serial.print ("FDCAN1 Message RAM required minimum size: ") ; + Serial.print (fdcan1.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + if (0 == errorCode) { + Serial.println ("fdcan1 ok") ; + }else{ + Serial.print ("Error can: 0x") ; + Serial.println (errorCode, HEX) ; + } + + // settings.mTxPin = 255 ; + // settings.mRxPin = 255 ; + errorCode = fdcan2.beginFD (settings) ; + Serial.print ("FDCAN2 Message RAM required minimum size: ") ; + Serial.print (fdcan2.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + if (0 == errorCode) { + Serial.println ("fdcan2 ok") ; + }else{ + Serial.print ("Error can: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t gSendDate1 = 0 ; +static uint32_t gSentCount1 = 0 ; +static uint32_t gReceivedCount1 = 0 ; + +static uint32_t gSendDate2 = 0 ; +static uint32_t gSentCount2 = 0 ; +static uint32_t gReceivedCount2 = 0 ; + +//----------------------------------------------------------------- + +void loop () { + if (gSendDate1 < millis ()) { + Serial.print ("CPU frequency: ") ; + Serial.print (F_CPU) ; + Serial.println (" Hz") ; + Serial.print ("PCLK1 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK1Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("PCLK2 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK2Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("HCLK frequency: ") ; + Serial.print (HAL_RCC_GetHCLKFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("SysClock frequency: ") ; + Serial.print (HAL_RCC_GetSysClockFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("CAN Clock: ") ; + Serial.print (fdcanClock ()) ; + Serial.println (" Hz") ; + CANFDMessage message ; + message.id = 0x7FF ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; + const uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + gSendDate1 += 1000 ; + gSentCount1 += 1 ; + Serial.print ("FDCAN1 sent: ") ; + Serial.println (gSentCount1) ; + } + } + if (gSendDate2 < millis ()) { + CANFDMessage message ; + message.id = 0x7FF ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; + const uint32_t sendStatus = fdcan2.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + gSendDate2 += 999 ; + gSentCount2 += 1 ; + Serial.print ("FDCAN2 sent: ") ; + Serial.println (gSentCount2) ; + } + } + CANFDMessage messageFD ; + if (fdcan1.receiveFD0 (messageFD)) { + gReceivedCount1 += 1 ; + Serial.print ("FDCAN1 received: ") ; + Serial.println (gReceivedCount1) ; + } + if (fdcan2.receiveFD0 (messageFD)) { + gReceivedCount2 += 1 ; + Serial.print ("FDCAN2 received: ") ; + Serial.println (gReceivedCount2) ; + } +} diff --git a/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemoIntensive-CAN1/H743ZI2-LoopBackDemoIntensive-CAN1.ino b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemoIntensive-CAN1/H743ZI2-LoopBackDemoIntensive-CAN1.ino new file mode 100644 index 0000000..669a815 --- /dev/null +++ b/lib/acanfd-stm32/examples/H743ZI2-LoopBackDemoIntensive-CAN1/H743ZI2-LoopBackDemoIntensive-CAN1.ino @@ -0,0 +1,242 @@ +// FDCAN1 external LoopBackDemo for NUCLEO_H743ZI2 +// No external hardware required. +//----------------------------------------------------------------- + +#ifndef ARDUINO_NUCLEO_H743ZI2 + #error This sketch runs on NUCLEO-H743ZI2 Nucleo-144 board +#endif + +//----------------------------------------------------------------- +// IMPORTANT: +// should be included only once, generally from the .ino file +// From an other file, include +// Before including , you should define +// Message RAM size for FDCAN1 and Message RAM size for FDCAN2. +// Maximum required size is 2,560 (2,560 32-bit words). +// A 0 size means the FDCAN module is not configured; its TxCAN and RxCAN pins +// can be freely used for an other function. +// The begin method checks if actual size is greater or equal to required size. +// Hint: if you do not want to compute required size, print +// fdcan1.messageRamRequiredMinimumSize () for getting it. +//----------------------------------------------------------------- + +static const uint32_t FDCAN1_MESSAGE_RAM_WORD_SIZE = 2560 ; +static const uint32_t FDCAN2_MESSAGE_RAM_WORD_SIZE = 0 ; // FDCAN2 not used + +#include + +//----------------------------------------------------------------- + +static ACANFD_STM32_FIFO gBuffer ; + +//----------------------------------------------------------------- + +void setup () { + gBuffer.initWithSize (25) ; + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x4) ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + const uint32_t errorCode = fdcan1.beginFD (settings) ; + + Serial.print ("Message RAM required minimum size: ") ; + Serial.print (fdcan1.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t pseudoRandomValue (void) { + static uint32_t gSeed = 0 ; + gSeed = 8253729U * gSeed + 2396403U ; + return gSeed ; +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentCount = 0 ; +static uint32_t gReceiveCount = 0 ; +static bool gOk = true ; +static uint32_t gCANRemoteFrameCount = 0 ; +static uint32_t gCanDataFrameCount = 0 ; +static uint32_t gCanFDNoBRSDataFrameCount = 0 ; +static uint32_t gCanFDWithBRSDataFrameCount = 0 ; +static uint32_t gStandardFrameCount = 0 ; +static uint32_t gExtendedFrameCount = 0 ; + +//----------------------------------------------------------------- + +static const uint8_t CANFD_LENGTH_FROM_CODE [16] = { + 0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64 +} ; + +//----------------------------------------------------------------- + +void loop () { + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent ") ; + Serial.print (gSentCount) ; + Serial.print (", received ") ; + Serial.print (gReceiveCount) ; + Serial.print (" (") ; + Serial.print (gCANRemoteFrameCount) ; + Serial.print (", ") ; + Serial.print (gCanDataFrameCount) ; + Serial.print (", ") ; + Serial.print (gCanFDNoBRSDataFrameCount) ; + Serial.print (", ") ; + Serial.print (gCanFDWithBRSDataFrameCount) ; + Serial.print (", ") ; + Serial.print (gStandardFrameCount) ; + Serial.print (", ") ; + Serial.print (gExtendedFrameCount) ; + Serial.print (", ") ; + Serial.print (fdcan1.statusFlags (), HEX) ; + Serial.println (")") ; + } +//--- Send buffer index +// 0: fifo +// 1 ... settings.mHardwareDedicacedTxBufferCount: dedicaced buffer + const uint8_t sendBufferIndex = 0 ; +//--- Send frame + CANFDMessage frame ; + frame.idx = sendBufferIndex ; + const uint32_t r = pseudoRandomValue () ; + frame.ext = (r & (1 << 29)) != 0 ; + frame.type = CANFDMessage::Type (r >> 30) ; + frame.id = r & 0x1FFFFFFFU ; + if (gOk && (!gBuffer.isFull ()) && fdcan1.sendBufferNotFullForIndex (sendBufferIndex)) { + if (frame.ext) { + gExtendedFrameCount += 1 ; + }else{ + gStandardFrameCount += 1 ; + frame.id &= 0x7FFU ; + } + switch (frame.type) { + case CANFDMessage::CAN_REMOTE : + gCANRemoteFrameCount += 1 ; + frame.len = pseudoRandomValue () % 9 ; + break ; + case CANFDMessage::CAN_DATA : + gCanDataFrameCount += 1 ; + frame.len = pseudoRandomValue () % 9 ; + for (uint32_t i=0 ; i should be included only once, generally from the .ino file +// From an other file, include +// Before including , you should define +// Message RAM size for FDCAN1 and Message RAM size for FDCAN2. +// Maximum required size is 2,560 (2,560 32-bit words). +// A 0 size means the FDCAN module is not configured; its TxCAN and RxCAN pins +// can be freely used for an other function. +// The begin method checks if actual size is greater or equal to required size. +// Hint: if you do not want to compute required size, print +// fdcan2.messageRamRequiredMinimumSize () for getting it. +//----------------------------------------------------------------- + +static const uint32_t FDCAN1_MESSAGE_RAM_WORD_SIZE = 0 ; // FDCAN1 not used +static const uint32_t FDCAN2_MESSAGE_RAM_WORD_SIZE = 2560 ; + +#include + +//----------------------------------------------------------------- + +static ACANFD_STM32_FIFO gBuffer ; + +//----------------------------------------------------------------- + +void setup () { + gBuffer.initWithSize (50) ; + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + delay (50) ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + } + Serial.println ("CAN1 CANFD loopback test") ; + ACANFD_STM32_Settings settings (1000 * 1000, DataBitRateFactor::x4) ; + + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration Sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data Sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + + const uint32_t errorCode = fdcan2.beginFD (settings) ; + + Serial.print ("Message RAM required minimum size: ") ; + Serial.print (fdcan2.messageRamRequiredMinimumSize ()) ; + Serial.println (" words") ; + + if (0 == errorCode) { + Serial.println ("can configuration ok") ; + }else{ + Serial.print ("Error can configuration: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t pseudoRandomValue (void) { + static uint32_t gSeed = 0 ; + gSeed = 8253729U * gSeed + 2396403U ; + return gSeed ; +} + +//----------------------------------------------------------------- + +static const uint32_t PERIOD = 1000 ; +static uint32_t gBlinkDate = PERIOD ; +static uint32_t gSentCount = 0 ; +static uint32_t gReceiveCount = 0 ; +static bool gOk = true ; +static uint32_t gCANRemoteFrameCount = 0 ; +static uint32_t gCanDataFrameCount = 0 ; +static uint32_t gCanFDNoBRSDataFrameCount = 0 ; +static uint32_t gCanFDWithBRSDataFrameCount = 0 ; +static uint32_t gStandardFrameCount = 0 ; +static uint32_t gExtendedFrameCount = 0 ; + +//----------------------------------------------------------------- + +static const uint8_t CANFD_LENGTH_FROM_CODE [16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64} ; + +//----------------------------------------------------------------- + +void loop () { + if (gBlinkDate <= millis ()) { + gBlinkDate += PERIOD ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + Serial.print ("Sent ") ; + Serial.print (gSentCount) ; + Serial.print (", received ") ; + Serial.print (gReceiveCount) ; + Serial.print (" (") ; + Serial.print (gCANRemoteFrameCount) ; + Serial.print (", ") ; + Serial.print (gCanDataFrameCount) ; + Serial.print (", ") ; + Serial.print (gCanFDNoBRSDataFrameCount) ; + Serial.print (", ") ; + Serial.print (gCanFDWithBRSDataFrameCount) ; + Serial.print (", ") ; + Serial.print (gStandardFrameCount) ; + Serial.print (", ") ; + Serial.print (gExtendedFrameCount) ; + Serial.print (", ") ; + Serial.print (gBuffer.count ()) ; + Serial.println (")") ; + } +//--- Send buffer index +// 0: fifo +// 1 ... settings.mHardwareDedicacedTxBufferCount: dedicaced buffer + const uint8_t sendBufferIndex = 0 ; +//--- Send frame + if (gOk && !gBuffer.isFull () && fdcan2.sendBufferNotFullForIndex (sendBufferIndex)) { + CANFDMessage frame ; + frame.idx = sendBufferIndex ; + const uint32_t r = pseudoRandomValue () ; + frame.ext = (r & (1 << 29)) != 0 ; + frame.type = CANFDMessage::Type (r >> 30) ; + frame.id = r & 0x1FFFFFFF ; + if (frame.ext) { + gExtendedFrameCount += 1 ; + }else{ + gStandardFrameCount += 1 ; + frame.id &= 0x7FF ; + } + switch (frame.type) { + case CANFDMessage::CAN_REMOTE : + gCANRemoteFrameCount += 1 ; + frame.len = pseudoRandomValue () % 9 ; + break ; + case CANFDMessage::CAN_DATA : + gCanDataFrameCount += 1 ; + frame.len = pseudoRandomValue () % 9 ; + for (uint32_t i=0 ; i should be included only once in a sketch, generally from the .ino file +// From an other file, include +//----------------------------------------------------------------- + +#include + +//----------------------------------------------------------------- + +void setup () { + pinMode (LED_BUILTIN, OUTPUT) ; + Serial.begin (9600) ; + while (!Serial) { + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + delay (50) ; + } + + Serial.print ("CPU frequency: ") ; + Serial.print (F_CPU) ; + Serial.println (" Hz") ; + Serial.print ("PCLK1 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK1Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("PCLK2 frequency: ") ; + Serial.print (HAL_RCC_GetPCLK2Freq ()) ; + Serial.println (" Hz") ; + Serial.print ("HCLK frequency: ") ; + Serial.print (HAL_RCC_GetHCLKFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("SysClock frequency: ") ; + Serial.print (HAL_RCC_GetSysClockFreq ()) ; + Serial.println (" Hz") ; + Serial.print ("FDCAN Clock: ") ; + Serial.print (fdcanClock ()) ; + Serial.println (" Hz") ; + + ACANFD_STM32_Settings settings (500 * 1000, DataBitRateFactor::x2) ; + settings.mModuleMode = ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK ; + + Serial.print ("Bit Rate prescaler: ") ; + Serial.println (settings.mBitRatePrescaler) ; + Serial.print ("Arbitration Phase segment 1: ") ; + Serial.println (settings.mArbitrationPhaseSegment1) ; + Serial.print ("Arbitration Phase segment 2: ") ; + Serial.println (settings.mArbitrationPhaseSegment2) ; + Serial.print ("Arbitration SJW: ") ; + Serial.println (settings.mArbitrationSJW) ; + Serial.print ("Actual Arbitration Bit Rate: ") ; + Serial.print (settings.actualArbitrationBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Arbitration sample point: ") ; + Serial.print (settings.arbitrationSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Arbitration Bit Rate ? ") ; + Serial.println (settings.exactArbitrationBitRate () ? "yes" : "no") ; + Serial.print ("Data Phase segment 1: ") ; + Serial.println (settings.mDataPhaseSegment1) ; + Serial.print ("Data Phase segment 2: ") ; + Serial.println (settings.mDataPhaseSegment2) ; + Serial.print ("Data SJW: ") ; + Serial.println (settings.mDataSJW) ; + Serial.print ("Actual Data Bit Rate: ") ; + Serial.print (settings.actualDataBitRate ()) ; + Serial.println (" bit/s") ; + Serial.print ("Data sample point: ") ; + Serial.print (settings.dataSamplePointFromBitStart ()) ; + Serial.println ("%") ; + Serial.print ("Exact Data Bit Rate ? ") ; + Serial.println (settings.exactDataBitRate () ? "yes" : "no") ; + +//--- beginFD is called without any receive filter, all sent frames are received +// by receiveFD0 throught receiveFIFO0 + uint32_t errorCode = fdcan1.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan1 configuration ok") ; + }else{ + Serial.print ("Error fdcan1: 0x") ; + Serial.println (errorCode, HEX) ; + } + + + errorCode = fdcan2.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan2 configuration ok") ; + }else{ + Serial.print ("Error fdcan2: 0x") ; + Serial.println (errorCode, HEX) ; + } + + + errorCode = fdcan3.beginFD (settings) ; + if (0 == errorCode) { + Serial.println ("fdcan3 configuration ok") ; + }else{ + Serial.print ("Error fdcan3: 0x") ; + Serial.println (errorCode, HEX) ; + } +} + +//----------------------------------------------------------------- + +static uint32_t gSendDate = 0 ; +static uint32_t gSentCount1 = 0 ; +static uint32_t gReceivedCount1 = 0 ; +static uint32_t gSentCount2 = 0 ; +static uint32_t gReceivedCount2 = 0 ; +static uint32_t gSentCount3 = 0 ; +static uint32_t gReceivedCount3 = 0 ; + +//----------------------------------------------------------------- + +void loop () { + if (gSendDate < millis ()) { + gSendDate += 1000 ; + digitalWrite (LED_BUILTIN, !digitalRead (LED_BUILTIN)) ; + CANFDMessage message ; + message.id = 0x7FF ; + message.len = 8 ; + message.data [0] = 0 ; + message.data [1] = 1 ; + message.data [2] = 2 ; + message.data [3] = 3 ; + message.data [4] = 4 ; + message.data [5] = 5 ; + message.data [6] = 6 ; + message.data [7] = 7 ; +//--- tryToSendReturnStatusFD returns 0 if message has been successfully +// appended in transmit buffer. +// A not zero returned value contains an error code (see doc) + uint32_t sendStatus = fdcan1.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount1 += 1 ; + Serial.print ("fdcan1 sent: ") ; + Serial.println (gSentCount1) ; + } + sendStatus = fdcan2.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount2 += 1 ; + Serial.print ("fdcan2 sent: ") ; + Serial.println (gSentCount2) ; + } + sendStatus = fdcan3.tryToSendReturnStatusFD (message) ; + if (sendStatus == 0) { + gSentCount3 += 1 ; + Serial.print ("fdcan3 sent: ") ; + Serial.println (gSentCount3) ; + } + } + CANFDMessage messageFD ; + if (fdcan1.receiveFD0 (messageFD)) { + gReceivedCount1 += 1 ; + Serial.print ("fdcan1 received: ") ; + Serial.println (gReceivedCount1) ; + } + if (fdcan2.receiveFD0 (messageFD)) { + gReceivedCount2 += 1 ; + Serial.print ("fdcan2 received: ") ; + Serial.println (gReceivedCount2) ; + } + if (fdcan3.receiveFD0 (messageFD)) { + gReceivedCount3 += 1 ; + Serial.print ("fdcan3 received: ") ; + Serial.println (gReceivedCount3) ; + } +} diff --git a/lib/acanfd-stm32/extras/acanfd-stm32.pdf b/lib/acanfd-stm32/extras/acanfd-stm32.pdf new file mode 100644 index 0000000..c5900f4 Binary files /dev/null and b/lib/acanfd-stm32/extras/acanfd-stm32.pdf differ diff --git a/lib/acanfd-stm32/keywords.txt b/lib/acanfd-stm32/keywords.txt new file mode 100644 index 0000000..8cad563 --- /dev/null +++ b/lib/acanfd-stm32/keywords.txt @@ -0,0 +1,30 @@ +####################################### +# Datatypes (KEYWORD1) +####################################### + +ACANFD_STM32_Settings KEYWORD1 +ACANFD_STM32 KEYWORD1 +ACANFD_STM32_StandardFilters KEYWORD1 +ACANFD_STM32_ExtendedFilters KEYWORD1 +CANMessage KEYWORD1 +CANFDMessage KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +begin KEYWORD2 +end KEYWORD2 +tryToSendReturnStatus KEYWORD2 +availableFD0 KEYWORD2 +receiveFD0 KEYWORD2 +availableFD1 KEYWORD2 +receiveFD1 KEYWORD2 +dispatchReceivedMessage KEYWORD2 +dispatchReceivedMessage0 KEYWORD2 +dispatchReceivedMessage1 KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/lib/acanfd-stm32/library.properties b/lib/acanfd-stm32/library.properties new file mode 100644 index 0000000..b5e8bb8 --- /dev/null +++ b/lib/acanfd-stm32/library.properties @@ -0,0 +1,9 @@ +name=ACANFD_STM32 +version=1.1.2-rc1 +author=Pierre Molinaro +maintainer=Pierre Molinaro +sentence=A STM32 FDCAN driver. +paragraph=This library is a FDCAN network driver for NUCLEO-G431KC, NUCLEO-G474RE, WeActStudio G474 (experimental), NUCLEO-H723ZG and NUCLEO-H743ZI2 boards. Default configuration enables reception of all frames. Reception filters can be easily defined. SystemClock can be redefined in order to match a given data bit rate. Compatible with ACAN2517FD library. +category=Communication +url=https://github.com/pierremolinaro/acanfd-stm32 +architectures=* diff --git a/lib/acanfd-stm32/src/ACANFD-STM32-fixed-ram-sections.cpp b/lib/acanfd-stm32/src/ACANFD-STM32-fixed-ram-sections.cpp new file mode 100644 index 0000000..f7838e7 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD-STM32-fixed-ram-sections.cpp @@ -0,0 +1,657 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ +// THIS FILE IS SPECIFIC TO FDCAN MODULES WITH FIXED SIZE RAM SECTIONS +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +#ifndef HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS + #error "HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS is not defined" +#endif + +#if HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS == false + +//------------------------------------------------------------------------------ + +static const uint32_t WORD_COUNT_FOR_PAYLOAD_64_BYTES = 18 ; + +//------------------------------------------------------------------------------ +// Constructor +//------------------------------------------------------------------------------ + +ACANFD_STM32::ACANFD_STM32 (volatile FDCAN_GlobalTypeDef * inPeripheralModuleBasePointer, + const uint32_t inRamBaseAddress, + const std::optional inIRQs, + const std::initializer_list & inTxPinArray, + const std::initializer_list & inRxPinArray) : +mRamBaseAddress (inRamBaseAddress), +mTxPinArray (inTxPinArray), +mRxPinArray (inRxPinArray), +mIRQs (inIRQs), +mPeripheralPtr (inPeripheralModuleBasePointer) { +} + +//------------------------------------------------------------------------------ +// beginFD method +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32::beginFD (const ACANFD_STM32_Settings & inSettings, + const ACANFD_STM32_ExtendedFilters & inExtendedFilters) { + return beginFD (inSettings, ACANFD_STM32_StandardFilters (), inExtendedFilters) ; +} + +//------------------------------------------------------------------------------ +// beginFD method +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32::beginFD (const ACANFD_STM32_Settings & inSettings, + const ACANFD_STM32_StandardFilters & inStandardFilters, + const ACANFD_STM32_ExtendedFilters & inExtendedFilters) { + uint32_t errorFlags = inSettings.checkBitSettingConsistency () ; + + +//------------------------------------------------------ Check settings + if (inStandardFilters.count () > 28) { + errorFlags |= kTooManyStandardFilters ; + } + if (inExtendedFilters.count () > 8) { + errorFlags |= kTooManyExtendedFilters ; + } + +//---------------------------------------------- Configure TxPin + bool pinFound = inSettings.mTxPin == 255 ; // Use default TxPin ? + auto txPinIterator = mTxPinArray.begin () ; + while (!pinFound && (txPinIterator != mTxPinArray.end ())) { + pinFound = txPinIterator->mPinName == inSettings.mTxPin ; + if (!pinFound) { + txPinIterator ++ ; + } + } + if (pinFound) { + GPIO_TypeDef * gpio = set_GPIO_Port_Clock (txPinIterator->mPinName >> 4) ; + const uint32_t pinIndex = txPinIterator->mPinName & 0x0F ; + const uint32_t txPinMask = uint32_t (1) << pinIndex ; + LL_GPIO_SetPinMode (gpio, txPinMask, LL_GPIO_MODE_ALTERNATE) ; + const uint32_t output = inSettings.mOpenCollectorOutput + ? LL_GPIO_OUTPUT_OPENDRAIN + : LL_GPIO_OUTPUT_PUSHPULL + ; + LL_GPIO_SetPinOutputType (gpio, txPinMask, output) ; + LL_GPIO_SetPinSpeed (gpio, txPinMask, LL_GPIO_SPEED_HIGH) ; + if (pinIndex < 8) { + LL_GPIO_SetAFPin_0_7 (gpio, txPinMask, txPinIterator->mPinAlternateMode) ; + }else{ + LL_GPIO_SetAFPin_8_15 (gpio, txPinMask, txPinIterator->mPinAlternateMode) ; + } + }else{ // Tx Pin not found + errorFlags |= kInvalidTxPin ; + } + + +//---------------------------------------------- Configure RxPin + auto rxPinIterator = mRxPinArray.begin () ; + pinFound = inSettings.mRxPin == 255 ; // Use default RxPin ? + while (!pinFound && (rxPinIterator != mRxPinArray.end ())) { + pinFound = rxPinIterator->mPinName == inSettings.mRxPin ; + if (!pinFound) { + rxPinIterator ++ ; + } + } + if (pinFound) { + GPIO_TypeDef * gpio = set_GPIO_Port_Clock (rxPinIterator->mPinName >> 4) ; + const uint32_t pinIndex = rxPinIterator->mPinName & 0x0F ; + const uint32_t rxPinMask = uint32_t (1) << pinIndex ; + const uint32_t input = inSettings.mInputPullup + ? LL_GPIO_PULL_UP + : LL_GPIO_PULL_NO + ; + LL_GPIO_SetPinPull (gpio, rxPinMask, input) ; + LL_GPIO_SetPinMode (gpio, rxPinMask, LL_GPIO_MODE_ALTERNATE) ; + if (pinIndex < 8) { + LL_GPIO_SetAFPin_0_7 (gpio, rxPinMask, rxPinIterator->mPinAlternateMode) ; + }else{ + LL_GPIO_SetAFPin_8_15 (gpio, rxPinMask, rxPinIterator->mPinAlternateMode) ; + } + }else{ // Rx Pin not found + errorFlags |= kInvalidRxPin ; + } + + +//------------------------------------------------------ Start configuring CAN module + mPeripheralPtr->CCCR = FDCAN_CCCR_INIT ; + while ((mPeripheralPtr->CCCR & FDCAN_CCCR_INIT) == 0) { + } + + +//------------------------------------------------------ Enable configuration change + mPeripheralPtr->CCCR = FDCAN_CCCR_INIT | FDCAN_CCCR_CCE ; + mPeripheralPtr->CCCR = FDCAN_CCCR_INIT | FDCAN_CCCR_CCE | FDCAN_CCCR_TEST ; + uint32_t cccr = FDCAN_CCCR_BRSE | FDCAN_CCCR_FDOE | FDCAN_CCCR_PXHD ; + +//------------------------------------------------------ Select mode + mPeripheralPtr->TEST = 0 ; + switch (inSettings.mModuleMode) { + case ACANFD_STM32_Settings::NORMAL_FD : + break ; + case ACANFD_STM32_Settings::INTERNAL_LOOP_BACK : + mPeripheralPtr->TEST = FDCAN_TEST_LBCK ; + cccr |= FDCAN_CCCR_MON | FDCAN_CCCR_TEST ; + break ; + case ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK : + mPeripheralPtr->TEST = FDCAN_TEST_LBCK ; + cccr |= FDCAN_CCCR_TEST ; + break ; + case ACANFD_STM32_Settings::BUS_MONITORING : + cccr |= FDCAN_CCCR_MON ; + break ; + } + if (!inSettings.mEnableRetransmission) { + cccr |= FDCAN_CCCR_DAR ; + } + + +//------------------------------------------------------ Set nominal Bit Timing and Prescaler + mPeripheralPtr->NBTP = + ((inSettings.mArbitrationSJW - 1) << 25) + | + ((inSettings.mBitRatePrescaler - 1) << 16) + | + ((inSettings.mArbitrationPhaseSegment1 - 1) << 8) + | + ((inSettings.mArbitrationPhaseSegment2 - 1) << 0) + ; + + +//------------------------------------------------------ Set data Bit Timing and Prescaler + mPeripheralPtr->DBTP = + ((inSettings.mBitRatePrescaler - 1) << 16) + | + ((inSettings.mDataPhaseSegment1 - 1) << 8) + | + ((inSettings.mDataPhaseSegment2 - 1) << 4) + | + ((inSettings.mDataSJW - 1) << 0) + | + // Enable Transceiver Delay Compensation ? + ((inSettings.mTransceiverDelayCompensation > 0) ? FDCAN_DBTP_TDC : 0) + ; + + +//------------------------------------------------------ Transmitter Delay Compensation + mPeripheralPtr->TDCR = inSettings.mTransceiverDelayCompensation << 8 ; + + +//------------------------------------------------------ Global Filter Configuration + mPeripheralPtr->RXGFC = + (uint32_t (inSettings.mNonMatchingStandardFrameReception) << FDCAN_RXGFC_ANFS_Pos) + | + (uint32_t (inSettings.mNonMatchingExtendedFrameReception) << FDCAN_RXGFC_ANFE_Pos) + | + (uint32_t (inSettings.mDiscardReceivedStandardRemoteFrames) << FDCAN_RXGFC_RRFS_Pos) + | + (uint32_t (inSettings.mDiscardReceivedExtendedRemoteFrames) << FDCAN_RXGFC_RRFE_Pos) + | + (inStandardFilters.count () << FDCAN_RXGFC_LSS_Pos) // Standard filter count (up to 28) + | + (inExtendedFilters.count () << FDCAN_RXGFC_LSE_Pos) // Standard filter count (up to 8) + ; + + +//-------------------- Allocate Standard ID Filters (0 ... 28 elements -> 0 ... 28 words) + mStandardFilterCallBackArray.setCapacity (inStandardFilters.count ()) ; + for (uint32_t i=0 ; i 0 ... 16 words) + mExtendedFilterCallBackArray.setCapacity (inExtendedFilters.count ()) ; + for (uint32_t i=0 ; iIE = interruptRegister ; + mPeripheralPtr->TXBTIE = ~0U ; + mPeripheralPtr->ILS = FDCAN_ILS_RXFIFO1 | FDCAN_ILS_RXFIFO0 ; // Received message on IRQ1, others on IRQ0 + NVIC_EnableIRQ (mIRQs.value ().mIRQ0) ; + NVIC_EnableIRQ (mIRQs.value ().mIRQ1) ; + mPeripheralPtr->ILE = FDCAN_ILE_EINT1 | FDCAN_ILE_EINT0 ; + }else{ + mPeripheralPtr->IE = 0 ; // All interrupts disabled + } + //------------------------------------------------------ Activate CAN controller + mPeripheralPtr->CCCR = FDCAN_CCCR_INIT | FDCAN_CCCR_CCE | cccr ; + mPeripheralPtr->CCCR = cccr ; // Reset INIT bit + while ((mPeripheralPtr->CCCR & FDCAN_CCCR_INIT) != 0) { } + } +//--- Return error code (0 --> no error) + return errorFlags ; +} + +//------------------------------------------------------------------------------ +// end +//------------------------------------------------------------------------------ + +void ACANFD_STM32::end (void) { +//--- Disable interrupts + if (mIRQs) { + NVIC_DisableIRQ (mIRQs.value ().mIRQ0) ; + NVIC_DisableIRQ (mIRQs.value ().mIRQ1) ; + } +//--- Free receive FIFOs + mDriverReceiveFIFO0.free () ; + mDriverReceiveFIFO1.free () ; +//--- Free transmit FIFO + mDriverTransmitFIFO.free () ; +} + +//------------------------------------------------------------------------------ +// poll +//------------------------------------------------------------------------------ + +void ACANFD_STM32::poll (void) { + if (!mIRQs) { + noInterrupts () ; + isr0 () ; + isr1 () ; + interrupts () ; + } +} + +//------------------------------------------------------------------------------ +// RECEPTION +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::availableFD0 (void) { + noInterrupts () ; + const bool hasMessage = !mDriverReceiveFIFO0.isEmpty () ; + interrupts () ; + return hasMessage ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::receiveFD0 (CANFDMessage & outMessage) { + noInterrupts () ; + const bool hasMessage = mDriverReceiveFIFO0.remove (outMessage) ; + interrupts () ; + return hasMessage ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::availableFD1 (void) { + noInterrupts () ; + const bool hasMessage = !mDriverReceiveFIFO1.isEmpty () ; + interrupts () ; + return hasMessage ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::receiveFD1 (CANFDMessage & outMessage) { + noInterrupts () ; + const bool hasMessage = mDriverReceiveFIFO1.remove (outMessage) ; + interrupts () ; + return hasMessage ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::dispatchReceivedMessage (void) { + CANFDMessage message ; + bool result = false ; + if (receiveFD0 (message)) { + result = true ; + internalDispatchReceivedMessage (message) ; + } + if (receiveFD1 (message)) { + result = true ; + internalDispatchReceivedMessage (message) ; + } + return result ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::dispatchReceivedMessageFIFO0 (void) { + CANFDMessage message ; + const bool result = receiveFD0 (message) ; + if (result) { + internalDispatchReceivedMessage (message) ; + } + return result ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::dispatchReceivedMessageFIFO1 (void) { + CANFDMessage message ; + const bool result = receiveFD1 (message) ; + if (result) { + internalDispatchReceivedMessage (message) ; + } + return result ; +} + +//------------------------------------------------------------------------------ + +void ACANFD_STM32::internalDispatchReceivedMessage (const CANFDMessage & inMessage) { + const uint32_t filterIndex = inMessage.idx ; + ACANFDCallBackRoutine callBack = nullptr ; + if (inMessage.ext) { + if (filterIndex == 255) { + callBack = mNonMatchingStandardMessageCallBack ; + }else if (filterIndex < mExtendedFilterCallBackArray.count ()) { + callBack = mExtendedFilterCallBackArray [filterIndex] ; + } + }else{ // Standard message + if (filterIndex == 255) { + callBack = mNonMatchingExtendedMessageCallBack ; + }else if (filterIndex < mStandardFilterCallBackArray.count ()) { + callBack = mStandardFilterCallBackArray [filterIndex] ; + } + } + if (callBack != nullptr) { + callBack (inMessage) ; + } +} + +//------------------------------------------------------------------------------ +// EMISSION +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::sendBufferNotFullForIndex (const uint32_t inMessageIndex) { + bool canSend = false ; + noInterrupts () ; + if (inMessageIndex == 0) { // Send via Tx FIFO ? + canSend = !mDriverTransmitFIFO.isFull () ; + }else{ // Send via dedicaced Tx Buffer ? + const uint32_t numberOfDedicacedTxBuffers = (mPeripheralPtr->TXBC >> 16) & 0x3F ; + if (inMessageIndex <= numberOfDedicacedTxBuffers) { + const uint32_t txBufferIndex = inMessageIndex - 1 ; + canSend = (mPeripheralPtr->TXBRP & (1U << txBufferIndex)) == 0 ; + } + } + interrupts () ; + return canSend ; +} + +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32::tryToSendReturnStatusFD (const CANFDMessage & inMessage) { + noInterrupts () ; + uint32_t sendStatus = 0 ; // Ok + if (!inMessage.isValid ()) { + sendStatus = kInvalidMessage ; + }else if (inMessage.idx == 0) { // Send via Tx FIFO ? + const uint32_t txfqs = mPeripheralPtr->TXFQS ; + const uint32_t hardwareTransmitFifoFreeLevel = txfqs & 0x3F ; + if ((hardwareTransmitFifoFreeLevel > 0) && mDriverTransmitFIFO.isEmpty ()) { + const uint32_t putIndex = (txfqs >> 16) & 0x1F ; + writeTxBuffer (inMessage, putIndex) ; + }else if (!mDriverTransmitFIFO.isFull ()) { + mDriverTransmitFIFO.append (inMessage) ; + }else{ + sendStatus = kTransmitBufferOverflow ; + } + }else{ // Send via dedicaced Tx Buffer ? + const uint32_t numberOfDedicacedTxBuffers = (mPeripheralPtr->TXBC >> 16) & 0x3F ; + if (inMessage.idx <= numberOfDedicacedTxBuffers) { + const uint32_t txBufferIndex = inMessage.idx - 1 ; + const bool hardwareTxBufferIsEmpty = (mPeripheralPtr->TXBRP & (1U << txBufferIndex)) == 0 ; + if (hardwareTxBufferIsEmpty) { + writeTxBuffer (inMessage, txBufferIndex) ; + }else{ + sendStatus = kTransmitBufferOverflow ; + } + }else{ + sendStatus = kTransmitBufferIndexTooLarge ; + } + } + interrupts () ; + return sendStatus ; +} + +//------------------------------------------------------------------------------ + +void ACANFD_STM32::writeTxBuffer (const CANFDMessage & inMessage, const uint32_t inTxBufferIndex) { +//--- Compute Tx Buffer address + volatile uint32_t * txBufferPtr = (uint32_t *) (mRamBaseAddress + 0x278) ; + txBufferPtr += inTxBufferIndex * WORD_COUNT_FOR_PAYLOAD_64_BYTES ; +//--- Identifier and extended bit + uint32_t element0 ; + if (inMessage.ext) { + element0 = (inMessage.id & 0x1FFFFFFFU) | (1U << 30) ; + }else{ + element0 = (inMessage.id & 0x7FFU) << 18 ; + } +//--- Control + uint32_t lengthCode ; + if (inMessage.len > 48) { + lengthCode = 15 ; + }else if (inMessage.len > 32) { + lengthCode = 14 ; + }else if (inMessage.len > 24) { + lengthCode = 13 ; + }else if (inMessage.len > 20) { + lengthCode = 12 ; + }else if (inMessage.len > 16) { + lengthCode = 11 ; + }else if (inMessage.len > 12) { + lengthCode = 10 ; + }else if (inMessage.len > 8) { + lengthCode = 9 ; + }else{ + lengthCode = inMessage.len ; + } + uint32_t element1 = uint32_t (lengthCode) << 16 ; +//--- + switch (inMessage.type) { + case CANFDMessage::CAN_REMOTE : + element0 |= 1 << 29 ; // Set RTR bit + break ; + case CANFDMessage::CAN_DATA : + txBufferPtr [2] = inMessage.data32 [0] ; + txBufferPtr [3] = inMessage.data32 [1] ; + break ; + case CANFDMessage::CANFD_NO_BIT_RATE_SWITCH : + { element1 |= 1 << 21 ; // Set FDF bit + const uint32_t wc = (inMessage.len + 3) / 4 ; + for (uint32_t i=0 ; iTXBAR = 1U << inTxBufferIndex ; +} + +//------------------------------------------------------------------------------ +// INTERRUPT SERVICE ROUTINES +//------------------------------------------------------------------------------ + +static void getMessageFrom (const uint32_t * inMessageRamAddress, + CANFDMessage & outMessage) { + const uint32_t w0 = inMessageRamAddress [0] ; + outMessage.id = w0 & 0x1FFFFFFF ; + const bool remote = (w0 & (1 << 29)) != 0 ; + outMessage.ext = (w0 & (1 << 30)) != 0 ; +// const bool esi = (w0 & (1 << 31)) != 0 ; + if (!outMessage.ext) { + outMessage.id >>= 18 ; + } + const uint32_t w1 = inMessageRamAddress [1] ; + const uint32_t dlc = (w1 >> 16) & 0xF ; + static const uint8_t CANFD_LENGTH_FROM_CODE [16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64} ; + outMessage.len = CANFD_LENGTH_FROM_CODE [dlc] ; + const bool fdf = (w1 & (1 << 21)) != 0 ; + const bool brs = (w1 & (1 << 20)) != 0 ; + if (fdf) { // CANFD frame + outMessage.type = brs ? CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH : CANFDMessage::CANFD_NO_BIT_RATE_SWITCH ; + }else if (remote) { + outMessage.type = CANFDMessage::CAN_REMOTE ; + }else{ + outMessage.type = CANFDMessage::CAN_DATA ; + } +//--- Filter index + if ((w1 & (1U << 31)) != 0) { // Filter index available ? + outMessage.idx = 255 ; // Not available + }else{ + const uint32_t filterIndex = (w1 >> 24) & 0x7F ; + outMessage.idx = uint8_t (filterIndex) ; + } +//--- Get data + if (outMessage.type != CANFDMessage::CAN_REMOTE) { + const uint32_t wc = (outMessage.len + 3) / 4 ; + for (uint32_t i=0 ; iIR = FDCAN_IR_TC ; +//--- Write message into transmit fifo ? + bool writeMessage = true ; + CANFDMessage message ; + while (writeMessage) { + const uint32_t txfqs = mPeripheralPtr->TXFQS ; + const uint32_t txFifoFreeLevel = txfqs & 0x3F ; + if ((txFifoFreeLevel > 0) && mDriverTransmitFIFO.remove (message)) { + const uint32_t putIndex = (txfqs >> 16) & 0x1F ; + writeTxBuffer (message, putIndex) ; + }else{ + writeMessage = false ; + } + } +} + +//------------------------------------------------------------------------------ + +void ACANFD_STM32::isr1 (void) { +//--- Interrupt Acknowledge + const uint32_t it = mPeripheralPtr->IR ; + const uint32_t ack = it & (FDCAN_IR_RF0N | FDCAN_IR_RF1N) ; + mPeripheralPtr->IR = ack ; +//--- Get messages + CANFDMessage message ; + bool loop = true ; + while (loop) { + //--- Get from FIFO 0 + const uint32_t rxf0s = mPeripheralPtr->RXF0S ; + const uint32_t fifo0NotEmpty = (rxf0s & 0x7FU) > 0 ; + if (fifo0NotEmpty) { + //--- Get read index + const uint32_t readIndex = (rxf0s >> 8) & 0x3F ; + //--- Compute message RAM address + const uint32_t * address = (uint32_t *) (mRamBaseAddress + 0x00B0) ; + address += readIndex * WORD_COUNT_FOR_PAYLOAD_64_BYTES ; + //--- Get message + getMessageFrom (address, message) ; + //--- Clear receive flag + mPeripheralPtr->RXF0A = readIndex ; + //--- Enter message into driver receive buffer 0 + mDriverReceiveFIFO0.append (message) ; + } + //--- Get from FIFO 1 + const uint32_t rxf1s = mPeripheralPtr->RXF1S ; + const uint32_t fifo1NotEmpty = (rxf1s & 0x7FU) > 0 ; + if (fifo1NotEmpty) { + //--- Get read index + const uint32_t readIndex = (rxf1s >> 8) & 0x3F ; + //--- Compute message RAM address + const uint32_t * address = (uint32_t *) (mRamBaseAddress + 0x0188) ; + address += readIndex * WORD_COUNT_FOR_PAYLOAD_64_BYTES ; + //--- Get message + getMessageFrom (address, message) ; + //--- Clear receive flag + mPeripheralPtr->RXF1A = readIndex ; + //--- Enter message into driver receive buffer 1 + mDriverReceiveFIFO1.append (message) ; + } + //--- Loop ? + loop = fifo0NotEmpty || fifo1NotEmpty ; + } +} + +//------------------------------------------------------------------------------ +//--- Status Flags (returns 0 if no error) +// Bit 0 : hardware RxFIFO 0 overflow +// Bit 1 : driver RxFIFO 0 overflow +// Bit 2 : hardware RxFIFO 1 overflow +// Bit 3 : driver RxFIFO 2 overflow +// Bit 4 : bus off + +uint32_t ACANFD_STM32::statusFlags (void) const { + uint32_t result = 0 ; // Ok + const uint32_t ir = mPeripheralPtr->IR ; + const uint32_t psr = mPeripheralPtr->PSR ; +//--- Hardware RxFIFO 0 overflow ? + if ((ir & FDCAN_IR_RF0L) != 0) { + result |= 1U << 0 ; + } +//--- Driver RxFIFO 0 overflow ? + if (mDriverReceiveFIFO0.didOverflow ()) { + result |= 1U << 1 ; + } +//--- Hardware RxFIFO 1 overflow ? + if ((ir & FDCAN_IR_RF1L) != 0) { + result |= 1U << 2 ; + } +//--- Driver RxFIFO 1 overflow ? + if (mDriverReceiveFIFO1.didOverflow ()) { + result |= 1U << 3 ; + } +//--- Bus off ? + if ((psr & FDCAN_PSR_BO) != 0) { + result |= 1U << 4 ; + } +//--- + return result ; +} + +//------------------------------------------------------------------------------ + +ACANFD_STM32::BusStatus::BusStatus (volatile FDCAN_GlobalTypeDef * inModulePtr) : +mErrorCount (uint16_t (inModulePtr->ECR)), +mProtocolStatus (inModulePtr->PSR) { +} + +//------------------------------------------------------------------------------ + +#endif +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD-STM32-fixed-ram-sections.h b/lib/acanfd-stm32/src/ACANFD-STM32-fixed-ram-sections.h new file mode 100644 index 0000000..2418c80 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD-STM32-fixed-ram-sections.h @@ -0,0 +1,183 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ +// THIS FILE IS SPECIFIC TO FDCAN MODULES WITH FIXED SIZE RAM SECTIONS +//------------------------------------------------------------------------------ + +#pragma once + +//------------------------------------------------------------------------------ + +#include +#include +#include + +#include + +//------------------------------------------------------------------------------ + +class ACANFD_STM32 { + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // TX and Rx pins available port + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: class PinPort final { + public: const uint8_t mPinName ; + public: const uint8_t mPinAlternateMode ; + + public: PinPort (const uint8_t inPinName, + const uint8_t inPinAlternateMode) : + mPinName (inPinName), + mPinAlternateMode (inPinAlternateMode) { + } + } ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // IRQs + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: class IRQs final { + public: const IRQn_Type mIRQ0 ; + public: const IRQn_Type mIRQ1 ; + + public: IRQs (const IRQn_Type inIRQ0, + const IRQn_Type inIRQ1) : + mIRQ0 (inIRQ0), + mIRQ1 (inIRQ1) { + } + } ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Bus Status + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: class BusStatus { + public: BusStatus (volatile FDCAN_GlobalTypeDef * inModulePtr) ; + public: const uint16_t mErrorCount ; // Copy of ECR register + public: const uint32_t mProtocolStatus ; // Copy of PSR register + public: inline uint16_t txErrorCount (void) const { return isBusOff () ? 256 : uint8_t (mErrorCount) ; } + public: inline uint8_t rxErrorCount (void) const { return uint8_t (mErrorCount >> 8) ; } + public: inline bool isBusOff (void) const { return (mProtocolStatus & (1 << 7)) != 0 ; } + public: inline uint8_t transceiverDelayCompensationOffset (void) const { return uint8_t (mProtocolStatus >> 16) & 0x7F ; } + } ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // ACANFD_STM32 class + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + +//-------------------- Constructor + + public: ACANFD_STM32 (volatile FDCAN_GlobalTypeDef * inPeripheralModuleBasePointer, + const uint32_t inRamBaseAddress, + const std::optional inIRQs, + const std::initializer_list & inTxPinArray, + const std::initializer_list & inRxPinArray) ; + +//-------------------- begin; returns a result code : +// 0 : Ok +// other: every bit denotes an error + public: static const uint32_t kMessageRamAllocatedSizeTooSmall = 1 << 20 ; + public: static const uint32_t kMessageRamOverflow = 1 << 21 ; + public: static const uint32_t kHardwareRxFIFO0SizeGreaterThan64 = 1 << 22 ; + public: static const uint32_t kHardwareTransmitFIFOSizeGreaterThan32 = 1 << 23 ; + public: static const uint32_t kDedicacedTransmitTxBufferCountGreaterThan30 = 1 << 24 ; + public: static const uint32_t kTxBufferCountGreaterThan32 = 1 << 25 ; + public: static const uint32_t kHardwareTransmitFIFOSizeEqualToZero = 1 << 26 ; + public: static const uint32_t kHardwareRxFIFO1SizeGreaterThan64 = 1 << 27 ; + public: static const uint32_t kTooManyStandardFilters = 1 << 28 ; + public: static const uint32_t kTooManyExtendedFilters = 1 << 29 ; + public: static const uint32_t kInvalidTxPin = 1 << 30 ; + public: static const uint32_t kInvalidRxPin = 1 << 31 ; + + public: uint32_t beginFD (const ACANFD_STM32_Settings & inSettings, + const ACANFD_STM32_StandardFilters & inStandardFilters = ACANFD_STM32_StandardFilters (), + const ACANFD_STM32_ExtendedFilters & inExtendedFilters = ACANFD_STM32_ExtendedFilters ()) ; + + public: uint32_t beginFD (const ACANFD_STM32_Settings & inSettings, + const ACANFD_STM32_ExtendedFilters & inExtendedFilters) ; + +//-------------------- end + public: void end (void) ; + +//--- Testing send buffer + public: bool sendBufferNotFullForIndex (const uint32_t inTxBufferIndex) ; + +//--- Transmitting messages and return status (returns 0 if ok) + public: uint32_t tryToSendReturnStatusFD (const CANFDMessage & inMessage) ; + public: static const uint32_t kInvalidMessage = 1 ; + public: static const uint32_t kTransmitBufferIndexTooLarge = 2 ; + public: static const uint32_t kTransmitBufferOverflow = 3 ; + + public: inline uint32_t transmitFIFOSize (void) const { return mDriverTransmitFIFO.size () ; } + public: inline uint32_t transmitFIFOCount (void) const { return mDriverTransmitFIFO.count () ; } + public: inline uint32_t transmitFIFOPeakCount (void) const { return mDriverTransmitFIFO.peakCount () ; } + +//--- Receiving messages + public: bool availableFD0 (void) ; + public: bool receiveFD0 (CANFDMessage & outMessage) ; + public: bool availableFD1 (void) ; + public: bool receiveFD1 (CANFDMessage & outMessage) ; + public: bool dispatchReceivedMessage (void) ; + public: bool dispatchReceivedMessageFIFO0 (void) ; + public: bool dispatchReceivedMessageFIFO1 (void) ; + +//--- Driver Transmit buffer + protected: ACANFD_STM32_FIFO mDriverTransmitFIFO ; + +//--- Poll + public: void poll (void) ; + +//--- Driver receive FIFO 0 + protected: ACANFD_STM32_FIFO mDriverReceiveFIFO0 ; + public: uint32_t driverReceiveFIFO0Size (void) { return mDriverReceiveFIFO0.size () ; } + public: uint32_t driverReceiveFIFO0Count (void) { return mDriverReceiveFIFO0.count () ; } + public: uint32_t driverReceiveFIFO0PeakCount (void) { return mDriverReceiveFIFO0.peakCount () ; } + public: void resetDriverReceiveFIFO0PeakCount (void) { mDriverReceiveFIFO0.resetPeakCount () ; } + +//--- Driver receive FIFO 1 + protected: ACANFD_STM32_FIFO mDriverReceiveFIFO1 ; + public: uint32_t driverReceiveFIFO1Size (void) { return mDriverReceiveFIFO1.size () ; } + public: uint32_t driverReceiveFIFO1Count (void) { return mDriverReceiveFIFO1.count () ; } + public: uint32_t driverReceiveFIFO1PeakCount (void) { return mDriverReceiveFIFO1.peakCount () ; } + public: void resetDriverReceiveFIFO1PeakCount (void) { mDriverReceiveFIFO1.resetPeakCount () ; } + + +//--- Constant public properties + public: const uint32_t mRamBaseAddress ; + public: const std::initializer_list mTxPinArray ; + public: const std::initializer_list mRxPinArray ; + public: const std::optional mIRQs ; + +//--- Protected properties + protected: volatile FDCAN_GlobalTypeDef * mPeripheralPtr ; + protected: ACANFD_STM32_DynamicArray < ACANFDCallBackRoutine > mStandardFilterCallBackArray ; + protected: ACANFD_STM32_DynamicArray < ACANFDCallBackRoutine > mExtendedFilterCallBackArray ; + protected: ACANFDCallBackRoutine mNonMatchingStandardMessageCallBack = nullptr ; + protected: ACANFDCallBackRoutine mNonMatchingExtendedMessageCallBack = nullptr ; + +//--- Internal methods + public: void isr0 (void) ; + public: void isr1 (void) ; + private: void writeTxBuffer (const CANFDMessage & inMessage, + const uint32_t inTxBufferIndex) ; + private: void internalDispatchReceivedMessage (const CANFDMessage & inMessage) ; + +//--- Status Flags (returns 0 if no error) +// Bit 0 : hardware RxFIFO 0 overflow +// Bit 1 : driver RxFIFO 0 overflow +// Bit 2 : hardware RxFIFO 1 overflow +// Bit 3 : driver RxFIFO 2 overflow +// Bit 4 : bus off + public: uint32_t statusFlags (void) const ; + +//--- Bus Status + public: inline BusStatus getStatus (void) const { return BusStatus (mPeripheralPtr) ; } + +//--- No copy + private : ACANFD_STM32 (const ACANFD_STM32 &) = delete ; + private : ACANFD_STM32 & operator = (const ACANFD_STM32 &) = delete ; + +} ; + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD-STM32-programmable-ram-sections.cpp b/lib/acanfd-stm32/src/ACANFD-STM32-programmable-ram-sections.cpp new file mode 100644 index 0000000..00e0499 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD-STM32-programmable-ram-sections.cpp @@ -0,0 +1,751 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ +// THIS FILE IS SPECIFIC TO FDCAN MODULES WITH PROGRAMMABLE RAM SECTIONS +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +#ifndef HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS + #error "HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS is not defined" +#endif + +#if HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS == true + +//------------------------------------------------------------------------------ +// Constructor +//------------------------------------------------------------------------------ + +ACANFD_STM32::ACANFD_STM32 (volatile FDCAN_GlobalTypeDef * inPeripheralModuleBasePointer, + const uint32_t inMessageRAMWordStartOffset, + const uint32_t inMessageRamAllocatedWordSize, + const std::optional inIRQs, + const std::initializer_list & inTxPinArray, + const std::initializer_list & inRxPinArray) : +mMessageRAMStartWordOffset (inMessageRAMWordStartOffset), +mMessageRamAllocatedWordSize (inMessageRamAllocatedWordSize), +mTxPinArray (inTxPinArray), +mRxPinArray (inRxPinArray), +mIRQs (inIRQs), +mPeripheralPtr (inPeripheralModuleBasePointer) { +} + +//------------------------------------------------------------------------------ +// beginFD method +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32::beginFD (const ACANFD_STM32_Settings & inSettings, + const ACANFD_STM32_ExtendedFilters & inExtendedFilters) { + return beginFD (inSettings, ACANFD_STM32_StandardFilters (), inExtendedFilters) ; +} + +//------------------------------------------------------------------------------ +// beginFD method +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32::beginFD (const ACANFD_STM32_Settings & inSettings, + const ACANFD_STM32_StandardFilters & inStandardFilters, + const ACANFD_STM32_ExtendedFilters & inExtendedFilters) { + uint32_t errorFlags = inSettings.checkBitSettingConsistency () ; + + +//------------------------------------------------------ Check settings + if (inSettings.mHardwareRxFIFO0Size > 64) { + errorFlags |= kHardwareRxFIFO0SizeGreaterThan64 ; + } + if (inSettings.mHardwareRxFIFO1Size > 64) { + errorFlags |= kHardwareRxFIFO1SizeGreaterThan64 ; + } + if (inSettings.mHardwareTransmitTxFIFOSize > 32) { + errorFlags |= kHardwareTransmitFIFOSizeGreaterThan32 ; + } + if (inSettings.mHardwareTransmitTxFIFOSize == 0) { + errorFlags |= kHardwareTransmitFIFOSizeEqualToZero ; + } + if (inSettings.mHardwareDedicacedTxBufferCount > 30) { + errorFlags |= kDedicacedTransmitTxBufferCountGreaterThan30 ; + } + if ((inSettings.mHardwareTransmitTxFIFOSize + inSettings.mHardwareDedicacedTxBufferCount) > 32) { + errorFlags |= kTxBufferCountGreaterThan32 ; + } + if (inStandardFilters.count () > 128) { + errorFlags |= kTooManyStandardFilters ; + } + if (inExtendedFilters.count () > 128) { + errorFlags |= kTooManyExtendedFilters ; + } + + +//---------------------------------------------- Configure TxPin + bool pinFound = inSettings.mTxPin == 255 ; // Use default TxPin ? + auto txPinIterator = mTxPinArray.begin () ; + while (!pinFound && (txPinIterator != mTxPinArray.end ())) { + pinFound = txPinIterator->mPinName == inSettings.mTxPin ; + if (!pinFound) { + txPinIterator ++ ; + } + } + if (pinFound) { + GPIO_TypeDef * gpio = set_GPIO_Port_Clock (txPinIterator->mPinName >> 4) ; + const uint32_t pinIndex = txPinIterator->mPinName & 0x0F ; + const uint32_t txPinMask = uint32_t (1) << pinIndex ; + LL_GPIO_SetPinMode (gpio, txPinMask, LL_GPIO_MODE_ALTERNATE) ; + const uint32_t output = inSettings.mOpenCollectorOutput + ? LL_GPIO_OUTPUT_OPENDRAIN + : LL_GPIO_OUTPUT_PUSHPULL + ; + LL_GPIO_SetPinOutputType (gpio, txPinMask, output) ; + LL_GPIO_SetPinSpeed (gpio, txPinMask, LL_GPIO_SPEED_HIGH) ; + if (pinIndex < 8) { + LL_GPIO_SetAFPin_0_7 (gpio, txPinMask, txPinIterator->mPinAlternateMode) ; + }else{ + LL_GPIO_SetAFPin_8_15 (gpio, txPinMask, txPinIterator->mPinAlternateMode) ; + } + }else{ // Tx Pin not found + errorFlags |= kInvalidTxPin ; + } + + +//---------------------------------------------- Configure RxPin + auto rxPinIterator = mRxPinArray.begin () ; + pinFound = inSettings.mRxPin == 255 ; // Use default RxPin ? + while (!pinFound && (rxPinIterator != mRxPinArray.end ())) { + pinFound = rxPinIterator->mPinName == inSettings.mRxPin ; + if (!pinFound) { + rxPinIterator ++ ; + } + } + if (pinFound) { + GPIO_TypeDef * gpio = set_GPIO_Port_Clock (rxPinIterator->mPinName >> 4) ; + const uint32_t pinIndex = rxPinIterator->mPinName & 0x0F ; + const uint32_t rxPinMask = uint32_t (1) << pinIndex ; + const uint32_t input = inSettings.mInputPullup + ? LL_GPIO_PULL_UP + : LL_GPIO_PULL_NO + ; + LL_GPIO_SetPinPull (gpio, rxPinMask, input) ; + LL_GPIO_SetPinMode (gpio, rxPinMask, LL_GPIO_MODE_ALTERNATE) ; + if (pinIndex < 8) { + LL_GPIO_SetAFPin_0_7 (gpio, rxPinMask, rxPinIterator->mPinAlternateMode) ; + }else{ + LL_GPIO_SetAFPin_8_15 (gpio, rxPinMask, rxPinIterator->mPinAlternateMode) ; + } + }else{ // Rx Pin not found + errorFlags |= kInvalidRxPin ; + } + + +//------------------------------------------------------ Start configuring CAN module + mPeripheralPtr->CCCR = FDCAN_CCCR_INIT ; + while ((mPeripheralPtr->CCCR & FDCAN_CCCR_INIT) == 0) { + } + + +//------------------------------------------------------ Enable configuration change + mPeripheralPtr->CCCR = FDCAN_CCCR_INIT | FDCAN_CCCR_CCE ; + mPeripheralPtr->CCCR = FDCAN_CCCR_INIT | FDCAN_CCCR_CCE | FDCAN_CCCR_TEST ; + uint32_t cccr = FDCAN_CCCR_BRSE | FDCAN_CCCR_FDOE | FDCAN_CCCR_PXHD ; + +//------------------------------------------------------ Select fdcan_tq_clk CAN clock + FDCAN_CCU->CCFG = FDCANCCU_CCFG_BCC | (0 << FDCANCCU_CCFG_CDIV_Pos) ; + +//------------------------------------------------------ Select mode + mPeripheralPtr->TEST = 0 ; + switch (inSettings.mModuleMode) { + case ACANFD_STM32_Settings::NORMAL_FD : + break ; + case ACANFD_STM32_Settings::INTERNAL_LOOP_BACK : + mPeripheralPtr->TEST = FDCAN_TEST_LBCK ; + cccr |= FDCAN_CCCR_MON | FDCAN_CCCR_TEST ; + break ; + case ACANFD_STM32_Settings::EXTERNAL_LOOP_BACK : + mPeripheralPtr->TEST = FDCAN_TEST_LBCK ; + cccr |= FDCAN_CCCR_TEST ; + break ; + case ACANFD_STM32_Settings::BUS_MONITORING : + cccr |= FDCAN_CCCR_MON ; + break ; + } + if (!inSettings.mEnableRetransmission) { + cccr |= FDCAN_CCCR_DAR ; + } + + +//------------------------------------------------------ Set nominal Bit Timing and Prescaler + mPeripheralPtr->NBTP = + (uint32_t (inSettings.mArbitrationSJW - 1) << 25) + | + (uint32_t (inSettings.mBitRatePrescaler - 1) << 16) + | + (uint32_t (inSettings.mArbitrationPhaseSegment1 - 1) << 8) + | + (uint32_t (inSettings.mArbitrationPhaseSegment2 - 1) << 0) + ; + + +//------------------------------------------------------ Set data Bit Timing and Prescaler + mPeripheralPtr->DBTP = + (uint32_t (inSettings.mBitRatePrescaler - 1) << 16) + | + (uint32_t (inSettings.mDataPhaseSegment1 - 1) << 8) + | + (uint32_t (inSettings.mDataPhaseSegment2 - 1) << 4) + | + (uint32_t (inSettings.mDataSJW - 1) << 0) + | + // Enable Transceiver Delay Compensation ? + ((inSettings.mTransceiverDelayCompensation > 0) ? FDCAN_DBTP_TDC : 0) + ; + + +//------------------------------------------------------ Transmitter Delay Compensation + mPeripheralPtr->TDCR = uint32_t (inSettings.mTransceiverDelayCompensation) << 8 ; + + +//------------------------------------------------------ Global Filter Configuration + mPeripheralPtr->GFC = + (uint32_t (inSettings.mNonMatchingStandardFrameReception) << FDCAN_GFC_ANFS_Pos) + | + (uint32_t (inSettings.mNonMatchingExtendedFrameReception) << FDCAN_GFC_ANFE_Pos) + | + (uint32_t (inSettings.mDiscardReceivedStandardRemoteFrames) << FDCAN_GFC_RRFS_Pos) + | + (uint32_t (inSettings.mDiscardReceivedExtendedRemoteFrames) << FDCAN_GFC_RRFE_Pos) + ; + + +//------------------------------------------------------ Configure message RAM + uint32_t messageRAMOffset = mMessageRAMStartWordOffset ; + +//--- Allocate Standard ID Filters (0 ... 128 elements -> 0 ... 128 words) + mPeripheralPtr->SIDFC = + (messageRAMOffset << 2) // Standard ID Filter Configuration + | + (inStandardFilters.count () << 16) // Standard filter count + ; + mStandardFilterCallBackArray.setCapacity (inStandardFilters.count ()) ; + for (uint32_t i=0 ; i 0 ... 128 words) + mPeripheralPtr->XIDFC = + (messageRAMOffset << 2) // Standard ID Filter Configuration + | + (inExtendedFilters.count () << 16) // Standard filter count + ; + mExtendedFilterCallBackArray.setCapacity (inExtendedFilters.count ()) ; + for (uint32_t i=0 ; i 0 ... 1152 words) + mRxFIFO0Pointer = (uint32_t *) (SRAMCAN_BASE + (messageRAMOffset << 2)) ; + mHardwareRxFIFO0Payload = inSettings.mHardwareRxFIFO0Payload ; + mPeripheralPtr->RXF0C = + (messageRAMOffset << 2) // FOSA + | + (uint32_t (inSettings.mHardwareRxFIFO0Size) << 16) // F0S + ; + mPeripheralPtr->RXESC = uint32_t (inSettings.mHardwareRxFIFO0Payload) ; // Rx FIFO 0 element size + messageRAMOffset += inSettings.mHardwareRxFIFO0Size * ACANFD_STM32_Settings::wordCountForPayload (mHardwareRxFIFO0Payload) ; + +//--- Allocate Rx FIFO 1 (0 ... 64 elements -> 0 ... 1152 words) + mRxFIFO1Pointer = (uint32_t *) (SRAMCAN_BASE + (messageRAMOffset << 2)) ; + mHardwareRxFIFO1Payload = inSettings.mHardwareRxFIFO1Payload ; + mPeripheralPtr->RXF1C = + (messageRAMOffset << 2) // FOSA + | + (uint32_t (inSettings.mHardwareRxFIFO1Size) << 16) // F0S + ; + mPeripheralPtr->RXESC |= uint32_t (inSettings.mHardwareRxFIFO1Payload) << 4 ; // Rx FIFO 1 element size + messageRAMOffset += inSettings.mHardwareRxFIFO1Size * ACANFD_STM32_Settings::wordCountForPayload (mHardwareRxFIFO1Payload) ; + +//--- Allocate Rx Buffers (0 ... 64 elements -> 0 ... 1152 words) + mPeripheralPtr->RXBC = 0 ; // Empty +//--- Allocate Tx Event / FIFO (0 ... 32 elements -> 0 ... 64 words) + mPeripheralPtr->TXEFC = 0 ; // Empty +//--- Allocate Tx Buffers (0 ... 32 elements -> 0 ... 576 words) + mHardwareTxBufferPayload = inSettings.mHardwareTransmitBufferPayload ; + mPeripheralPtr->TXESC = uint32_t (mHardwareTxBufferPayload) ; + mTxBuffersPointer = (uint32_t *) (SRAMCAN_BASE + (messageRAMOffset << 2)) ; + mPeripheralPtr->TXBC = + (messageRAMOffset << 2) // Tx Buffer start address + | + (inSettings.mHardwareTransmitTxFIFOSize << 24) // Number of Transmit FIFO / Queue buffers + | + (inSettings.mHardwareDedicacedTxBufferCount << 16) // Number of Dedicaced Tx buffers + ; + const uint32_t txBufferCount = inSettings.mHardwareDedicacedTxBufferCount + inSettings.mHardwareTransmitTxFIFOSize ; + messageRAMOffset += txBufferCount * ACANFD_STM32_Settings::wordCountForPayload (mHardwareTxBufferPayload) ; + mMessageRamRequiredWordSize = messageRAMOffset - mMessageRAMStartWordOffset ; + +//------------------------------------------------------ Check Message RAM Allocation + if (mMessageRamRequiredWordSize > mMessageRamAllocatedWordSize) { + errorFlags |= kMessageRamAllocatedSizeTooSmall ; + } + if (messageRAMOffset > FDCAN_MESSAGE_RAM_WORD_SIZE) { + errorFlags |= kMessageRamOverflow ; + } + if (errorFlags == 0) { + //------------------------------------------------------ Configure Driver buffers + mDriverTransmitFIFO.initWithSize (inSettings.mDriverTransmitFIFOSize) ; + mDriverReceiveFIFO0.initWithSize (inSettings.mDriverReceiveFIFO0Size) ; + mDriverReceiveFIFO1.initWithSize (inSettings.mDriverReceiveFIFO1Size) ; + mNonMatchingStandardMessageCallBack = inSettings.mNonMatchingStandardMessageCallBack ; + mNonMatchingExtendedMessageCallBack = inSettings.mNonMatchingExtendedMessageCallBack ; + //------------------------------------------------------ Interrupts + if (mIRQs) { + uint32_t interruptRegister = FDCAN_IE_TCE ; // Enable Transmission Completed Interrupt + interruptRegister |= FDCAN_IE_RF0NE ; // Receive FIFO 0 Non Empty + interruptRegister |= FDCAN_IE_RF1NE ; // Receive FIFO 1 Non Empty + mPeripheralPtr->IE = interruptRegister ; + mPeripheralPtr->TXBTIE = ((1U << inSettings.mHardwareTransmitTxFIFOSize) - 1U) << inSettings.mHardwareDedicacedTxBufferCount ; + mPeripheralPtr->ILS = FDCAN_ILS_RF1NL | FDCAN_ILS_RF0NL ; // Received message on IRQ1, others on IRQ0 + NVIC_EnableIRQ (mIRQs.value ().mIRQ0) ; + NVIC_EnableIRQ (mIRQs.value ().mIRQ1) ; + mPeripheralPtr->ILE = FDCAN_ILE_EINT1 | FDCAN_ILE_EINT0 ; + }else{ + mPeripheralPtr->IE = 0 ; + } + + //------------------------------------------------------ Activate CAN controller + mPeripheralPtr->CCCR = FDCAN_CCCR_INIT | FDCAN_CCCR_CCE | cccr ; + mPeripheralPtr->CCCR = cccr ; // Reset INIT bit + while ((mPeripheralPtr->CCCR & FDCAN_CCCR_INIT) != 0) { } + } +//--- Return error flags (0 --> no error) + return errorFlags ; +} + +//------------------------------------------------------------------------------ +// end +//------------------------------------------------------------------------------ + +void ACANFD_STM32::end (void) { +//--- Disable interrupts + if (mIRQs) { + NVIC_DisableIRQ (mIRQs.value ().mIRQ0) ; + NVIC_DisableIRQ (mIRQs.value ().mIRQ1) ; + } +//--- Free receive FIFOs + mDriverReceiveFIFO0.free () ; + mDriverReceiveFIFO1.free () ; +//--- Free transmit FIFO + mDriverTransmitFIFO.free () ; +} + +//------------------------------------------------------------------------------ +// poll +//------------------------------------------------------------------------------ + +void ACANFD_STM32::poll (void) { + if (!mIRQs) { + noInterrupts () ; + isr0 () ; + isr1 () ; + interrupts () ; + } +} + +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32::messageRamRequiredMinimumSize (void) { + return mMessageRamRequiredWordSize ; +} + +//------------------------------------------------------------------------------ +// RECEPTION +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::availableFD0 (void) { + noInterrupts () ; + const bool hasMessage = !mDriverReceiveFIFO0.isEmpty () ; + interrupts () ; + return hasMessage ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::receiveFD0 (CANFDMessage & outMessage) { + noInterrupts () ; + const bool hasMessage = mDriverReceiveFIFO0.remove (outMessage) ; + interrupts () ; + return hasMessage ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::availableFD1 (void) { + noInterrupts () ; + const bool hasMessage = !mDriverReceiveFIFO1.isEmpty () ; + interrupts () ; + return hasMessage ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::receiveFD1 (CANFDMessage & outMessage) { + noInterrupts () ; + const bool hasMessage = mDriverReceiveFIFO1.remove (outMessage) ; + interrupts () ; + return hasMessage ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::dispatchReceivedMessage (void) { + CANFDMessage message ; + bool result = false ; + if (receiveFD0 (message)) { + result = true ; + internalDispatchReceivedMessage (message) ; + } + if (receiveFD1 (message)) { + result = true ; + internalDispatchReceivedMessage (message) ; + } + return result ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::dispatchReceivedMessageFIFO0 (void) { + CANFDMessage message ; + const bool result = receiveFD0 (message) ; + if (result) { + internalDispatchReceivedMessage (message) ; + } + return result ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::dispatchReceivedMessageFIFO1 (void) { + CANFDMessage message ; + const bool result = receiveFD1 (message) ; + if (result) { + internalDispatchReceivedMessage (message) ; + } + return result ; +} + +//------------------------------------------------------------------------------ + +void ACANFD_STM32::internalDispatchReceivedMessage (const CANFDMessage & inMessage) { + const uint32_t filterIndex = inMessage.idx ; + ACANFDCallBackRoutine callBack = nullptr ; + if (inMessage.ext) { + if (filterIndex == 255) { + callBack = mNonMatchingStandardMessageCallBack ; + }else if (filterIndex < mExtendedFilterCallBackArray.count ()) { + callBack = mExtendedFilterCallBackArray [filterIndex] ; + } + }else{ // Standard message + if (filterIndex == 255) { + callBack = mNonMatchingExtendedMessageCallBack ; + }else if (filterIndex < mStandardFilterCallBackArray.count ()) { + callBack = mStandardFilterCallBackArray [filterIndex] ; + } + } + if (callBack != nullptr) { + callBack (inMessage) ; + } +} + +//------------------------------------------------------------------------------ +// EMISSION +//------------------------------------------------------------------------------ + +bool ACANFD_STM32::sendBufferNotFullForIndex (const uint32_t inMessageIndex) { + bool canSend = false ; + noInterrupts () ; + if (inMessageIndex == 0) { // Send via Tx FIFO ? + canSend = !mDriverTransmitFIFO.isFull () ; + }else{ // Send via dedicaced Tx Buffer ? + const uint32_t numberOfDedicacedTxBuffers = (mPeripheralPtr->TXBC >> 16) & 0x3F ; + if (inMessageIndex <= numberOfDedicacedTxBuffers) { + const uint32_t txBufferIndex = inMessageIndex - 1 ; + canSend = (mPeripheralPtr->TXBRP & (1U << txBufferIndex)) == 0 ; + } + } + interrupts () ; + return canSend ; +} + +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32::tryToSendReturnStatusFD (const CANFDMessage & inMessage) { + noInterrupts () ; + uint32_t sendStatus = 0 ; // Ok + if (!inMessage.isValid ()) { + sendStatus = kInvalidMessage ; + }else if (inMessage.idx == 0) { // Send via Tx FIFO ? + const uint32_t txfqs = mPeripheralPtr->TXFQS ; + const uint32_t hardwareTransmitFifoFreeLevel = txfqs & 0x3F ; + if ((hardwareTransmitFifoFreeLevel > 0) && mDriverTransmitFIFO.isEmpty ()) { + const uint32_t putIndex = (txfqs >> 16) & 0x1F ; + writeTxBuffer (inMessage, putIndex) ; + }else if (!mDriverTransmitFIFO.isFull ()) { + mDriverTransmitFIFO.append (inMessage) ; + }else{ + sendStatus = kTransmitBufferOverflow ; + } + }else{ // Send via dedicaced Tx Buffer ? + const uint32_t numberOfDedicacedTxBuffers = (mPeripheralPtr->TXBC >> 16) & 0x3F ; + if (inMessage.idx <= numberOfDedicacedTxBuffers) { + const uint32_t txBufferIndex = inMessage.idx - 1 ; + const bool hardwareTxBufferIsEmpty = (mPeripheralPtr->TXBRP & (1U << txBufferIndex)) == 0 ; + if (hardwareTxBufferIsEmpty) { + writeTxBuffer (inMessage, txBufferIndex) ; + }else{ + sendStatus = kTransmitBufferOverflow ; + } + }else{ + sendStatus = kTransmitBufferIndexTooLarge ; + } + } + interrupts () ; + return sendStatus ; +} + +//------------------------------------------------------------------------------ + +void ACANFD_STM32::writeTxBuffer (const CANFDMessage & inMessage, const uint32_t inTxBufferIndex) { +//--- Compute Tx Buffer address + volatile uint32_t * txBufferPtr = mTxBuffersPointer ; + txBufferPtr += inTxBufferIndex * ACANFD_STM32_Settings::wordCountForPayload (mHardwareTxBufferPayload) ; +//--- Identifier and extended bit + uint32_t element0 ; + if (inMessage.ext) { + element0 = (inMessage.id & 0x1FFFFFFFU) | (1U << 30) ; + }else{ + element0 = (inMessage.id & 0x7FFU) << 18 ; + } +//--- Control + uint32_t lengthCode ; + if (inMessage.len > 48) { + lengthCode = 15 ; + }else if (inMessage.len > 32) { + lengthCode = 14 ; + }else if (inMessage.len > 24) { + lengthCode = 13 ; + }else if (inMessage.len > 20) { + lengthCode = 12 ; + }else if (inMessage.len > 16) { + lengthCode = 11 ; + }else if (inMessage.len > 12) { + lengthCode = 10 ; + }else if (inMessage.len > 8) { + lengthCode = 9 ; + }else{ + lengthCode = inMessage.len ; + } + uint32_t element1 = lengthCode << 16 ; +//--- + const uint32_t lg = ACANFD_STM32_Settings::frameDataByteCountForPayload (mHardwareTxBufferPayload) ; + const uint32_t sentCount = (lg < inMessage.len) ? lg : inMessage.len ; +//--- + switch (inMessage.type) { + case CANFDMessage::CAN_REMOTE : + element0 |= 1 << 29 ; // Set RTR bit + break ; + case CANFDMessage::CAN_DATA : + txBufferPtr [2] = inMessage.data32 [0] ; + txBufferPtr [3] = inMessage.data32 [1] ; + break ; + case CANFDMessage::CANFD_NO_BIT_RATE_SWITCH : + { element1 |= 1 << 21 ; // Set FDF bit + const uint32_t wc = (sentCount + 3) / 4 ; + for (uint32_t i=0 ; iTXBAR = 1U << inTxBufferIndex ; +} + +//------------------------------------------------------------------------------ +// INTERRUPT SERVICE ROUTINES +//------------------------------------------------------------------------------ + +static void getMessageFrom (const uint32_t * inMessageRamAddress, + const ACANFD_STM32_Settings::Payload inPayLoad, + CANFDMessage & outMessage) { + const uint32_t lg = ACANFD_STM32_Settings::frameDataByteCountForPayload (inPayLoad) ; + const uint32_t w0 = inMessageRamAddress [0] ; + outMessage.id = w0 & 0x1FFFFFFF ; + const bool remote = (w0 & (1 << 29)) != 0 ; + outMessage.ext = (w0 & (1 << 30)) != 0 ; +// const bool esi = (w0 & (1 << 31)) != 0 ; + if (!outMessage.ext) { + outMessage.id >>= 18 ; + } + const uint32_t w1 = inMessageRamAddress [1] ; + const uint32_t dlc = (w1 >> 16) & 0xF ; + static const uint8_t CANFD_LENGTH_FROM_CODE [16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64} ; + outMessage.len = CANFD_LENGTH_FROM_CODE [dlc] ; + const bool fdf = (w1 & (1 << 21)) != 0 ; + const bool brs = (w1 & (1 << 20)) != 0 ; + if (fdf) { // CANFD frame + outMessage.type = brs ? CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH : CANFDMessage::CANFD_NO_BIT_RATE_SWITCH ; + }else if (remote) { + outMessage.type = CANFDMessage::CAN_REMOTE ; + }else{ + outMessage.type = CANFDMessage::CAN_DATA ; + } +//--- Filter index + if ((w1 & (1U << 31)) != 0) { // Filter index available ? + outMessage.idx = 255 ; // Not available + }else{ + const uint32_t filterIndex = (w1 >> 24) & 0x7F ; + outMessage.idx = uint8_t (filterIndex) ; + } +//--- Get data + if (outMessage.type != CANFDMessage::CAN_REMOTE) { + const uint32_t receiveByteCount = (lg < outMessage.len) ? lg : outMessage.len ; + const uint32_t wc = (receiveByteCount + 3) / 4 ; + for (uint32_t i=0 ; iIR = FDCAN_IR_TC ; +//--- Write message into transmit fifo ? + bool writeMessage = true ; + CANFDMessage message ; + while (writeMessage) { + const uint32_t txfqs = mPeripheralPtr->TXFQS ; + const uint32_t txFifoFreeLevel = txfqs & 0x3F ; + if ((txFifoFreeLevel > 0) && mDriverTransmitFIFO.remove (message)) { + const uint32_t putIndex = (txfqs >> 16) & 0x1F ; + writeTxBuffer (message, putIndex) ; + }else{ + writeMessage = false ; + } + } +} + +//------------------------------------------------------------------------------ + +void ACANFD_STM32::isr1 (void) { +//--- Interrupt Acknowledge + const uint32_t it = mPeripheralPtr->IR ; + const uint32_t ack = it & (FDCAN_IR_RF0N | FDCAN_IR_RF1N) ; + mPeripheralPtr->IR = ack ; +//--- Get messages + CANFDMessage message ; + bool loop = true ; + while (loop) { + //--- Get from FIFO 0 + const uint32_t rxf0s = mPeripheralPtr->RXF0S ; + const uint32_t fifo0NotEmpty = (rxf0s & 0x7FU) > 0 ; + if (fifo0NotEmpty) { + //--- Get read index + const uint32_t readIndex = (rxf0s >> 8) & 0x3F ; + //--- Compute message RAM address + const uint32_t * address = mRxFIFO0Pointer ; + address += readIndex * ACANFD_STM32_Settings::wordCountForPayload (mHardwareRxFIFO0Payload) ; + //--- Get message + getMessageFrom (address, mHardwareRxFIFO0Payload, message) ; + //--- Clear receive flag + mPeripheralPtr->RXF0A = readIndex ; + //--- Enter message into driver receive buffer 0 + mDriverReceiveFIFO0.append (message) ; + } + //--- Get from FIFO 1 + const uint32_t rxf1s = mPeripheralPtr->RXF1S ; + const uint32_t fifo1NotEmpty = (rxf1s & 0x7FU) > 0 ; + if (fifo1NotEmpty) { + //--- Get read index + const uint32_t readIndex = (rxf1s >> 8) & 0x3F ; + //--- Compute message RAM address + const uint32_t * address = mRxFIFO1Pointer ; + address += readIndex * ACANFD_STM32_Settings::wordCountForPayload (mHardwareRxFIFO1Payload) ; + //--- Get message + getMessageFrom (address, mHardwareRxFIFO1Payload, message) ; + //--- Clear receive flag + mPeripheralPtr->RXF1A = readIndex ; + //--- Enter message into driver receive buffer 1 + mDriverReceiveFIFO1.append (message) ; + } + //--- Loop ? + loop = fifo0NotEmpty || fifo1NotEmpty ; + } +} + +//------------------------------------------------------------------------------ +//--- Status Flags (returns 0 if no error) +// Bit 0 : hardware RxFIFO 0 overflow +// Bit 1 : driver RxFIFO 0 overflow +// Bit 2 : hardware RxFIFO 1 overflow +// Bit 3 : driver RxFIFO 2 overflow +// Bit 4 : bus off + +uint32_t ACANFD_STM32::statusFlags (void) const { + uint32_t result = 0 ; // Ok + const uint32_t ir = mPeripheralPtr->IR ; + const uint32_t psr = mPeripheralPtr->PSR ; +//--- Hardware RxFIFO 0 overflow ? + if ((ir & FDCAN_IR_RF0L) != 0) { + result |= 1U << 0 ; + } +//--- Driver RxFIFO 0 overflow ? + if (mDriverReceiveFIFO0.didOverflow ()) { + result |= 1U << 1 ; + } +//--- Hardware RxFIFO 1 overflow ? + if ((ir & FDCAN_IR_RF1L) != 0) { + result |= 1U << 2 ; + } +//--- Driver RxFIFO 1 overflow ? + if (mDriverReceiveFIFO1.didOverflow ()) { + result |= 1U << 3 ; + } +//--- Bus off ? + if ((psr & FDCAN_PSR_BO) != 0) { + result |= 1U << 4 ; + } +//--- + return result ; +} + +//------------------------------------------------------------------------------ + +ACANFD_STM32::BusStatus::BusStatus (volatile FDCAN_GlobalTypeDef * inModulePtr) : +mErrorCount (uint16_t (inModulePtr->ECR)), +mProtocolStatus (inModulePtr->PSR) { +} + +//------------------------------------------------------------------------------ + +#endif +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD-STM32-programmable-ram-sections.h b/lib/acanfd-stm32/src/ACANFD-STM32-programmable-ram-sections.h new file mode 100644 index 0000000..f50c5b8 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD-STM32-programmable-ram-sections.h @@ -0,0 +1,206 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ +// THIS FILE IS SPECIFIC TO FDCAN MODULES WITH PROGRAMMABLE RAM SECTIONS +//------------------------------------------------------------------------------ + +#pragma once + +//------------------------------------------------------------------------------ + +#include +#include +#include + +#include + +//------------------------------------------------------------------------------ + +class ACANFD_STM32 { + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // TX and Rx pins available port + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: class PinPort final { + public: const uint8_t mPinName ; + public: const uint8_t mPinAlternateMode ; + + public: PinPort (const uint8_t inPinName, + const uint8_t inPinAlternateMode) : + mPinName (inPinName), + mPinAlternateMode (inPinAlternateMode) { + } + } ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // IRQs + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: class IRQs final { + public: const IRQn_Type mIRQ0 ; + public: const IRQn_Type mIRQ1 ; + + public: IRQs (const IRQn_Type inIRQ0, + const IRQn_Type inIRQ1) : + mIRQ0 (inIRQ0), + mIRQ1 (inIRQ1) { + } + } ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Bus Status + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: class BusStatus { + public: BusStatus (volatile FDCAN_GlobalTypeDef * inModulePtr) ; + public: const uint16_t mErrorCount ; // Copy of ECR register + public: const uint32_t mProtocolStatus ; // Copy of PSR register + public: inline uint16_t txErrorCount (void) const { return isBusOff () ? 256 : uint8_t (mErrorCount) ; } + public: inline uint8_t rxErrorCount (void) const { return uint8_t (mErrorCount >> 8) ; } + public: inline bool isBusOff (void) const { return (mProtocolStatus & (1 << 7)) != 0 ; } + public: inline uint8_t transceiverDelayCompensationOffset (void) const { return uint8_t (mProtocolStatus >> 16) & 0x7F ; } + } ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // ACANFD_STM32 class + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + +//-------------------- Constructor + + public: ACANFD_STM32 (volatile FDCAN_GlobalTypeDef * inPeripheralModuleBasePointer, + const uint32_t inMessageRAMWordStartOffset, + const uint32_t inMessageRamAllocatedWordSize, + const std::optional inIRQs, + const std::initializer_list & inTxPinArray, + const std::initializer_list & inRxPinArray) ; + +//-------------------- begin; returns a result code : +// 0 : Ok +// other: every bit denotes an error + public: static const uint32_t kMessageRamAllocatedSizeTooSmall = 1 << 20 ; + public: static const uint32_t kMessageRamOverflow = 1 << 21 ; + public: static const uint32_t kHardwareRxFIFO0SizeGreaterThan64 = 1 << 22 ; + public: static const uint32_t kHardwareTransmitFIFOSizeGreaterThan32 = 1 << 23 ; + public: static const uint32_t kDedicacedTransmitTxBufferCountGreaterThan30 = 1 << 24 ; + public: static const uint32_t kTxBufferCountGreaterThan32 = 1 << 25 ; + public: static const uint32_t kHardwareTransmitFIFOSizeEqualToZero = 1 << 26 ; + public: static const uint32_t kHardwareRxFIFO1SizeGreaterThan64 = 1 << 27 ; + public: static const uint32_t kTooManyStandardFilters = 1 << 28 ; + public: static const uint32_t kTooManyExtendedFilters = 1 << 29 ; + public: static const uint32_t kInvalidTxPin = 1 << 30 ; + public: static const uint32_t kInvalidRxPin = 1 << 31 ; + + public: uint32_t beginFD (const ACANFD_STM32_Settings & inSettings, + const ACANFD_STM32_StandardFilters & inStandardFilters = ACANFD_STM32_StandardFilters (), + const ACANFD_STM32_ExtendedFilters & inExtendedFilters = ACANFD_STM32_ExtendedFilters ()) ; + + public: uint32_t beginFD (const ACANFD_STM32_Settings & inSettings, + const ACANFD_STM32_ExtendedFilters & inExtendedFilters) ; + + +//-------------------- end + public: void end (void) ; + +//--- Getting Message RAM required minimum size + public: uint32_t messageRamRequiredMinimumSize (void) ; + +//--- Testing send buffer + public: bool sendBufferNotFullForIndex (const uint32_t inTxBufferIndex) ; + +//--- Transmitting messages and return status (returns 0 if ok) + public: uint32_t tryToSendReturnStatusFD (const CANFDMessage & inMessage) ; + public: static const uint32_t kInvalidMessage = 1 ; + public: static const uint32_t kTransmitBufferIndexTooLarge = 2 ; + public: static const uint32_t kTransmitBufferOverflow = 3 ; + + public: inline uint32_t transmitFIFOSize (void) const { return mDriverTransmitFIFO.size () ; } + public: inline uint32_t transmitFIFOCount (void) const { return mDriverTransmitFIFO.count () ; } + public: inline uint32_t transmitFIFOPeakCount (void) const { return mDriverTransmitFIFO.peakCount () ; } + public: inline ACANFD_STM32_Settings::Payload hardwareTxBufferPayload (void) const { + return mHardwareTxBufferPayload ; + } + +//--- Receiving messages + public: bool availableFD0 (void) ; + public: bool receiveFD0 (CANFDMessage & outMessage) ; + public: bool availableFD1 (void) ; + public: bool receiveFD1 (CANFDMessage & outMessage) ; + public: bool dispatchReceivedMessage (void) ; + public: bool dispatchReceivedMessageFIFO0 (void) ; + public: bool dispatchReceivedMessageFIFO1 (void) ; + +//--- poll + public: void poll (void) ; + +//--- Driver Transmit buffer + protected: ACANFD_STM32_FIFO mDriverTransmitFIFO ; + +//--- Driver receive FIFO 0 + protected: ACANFD_STM32_FIFO mDriverReceiveFIFO0 ; + public: uint32_t driverReceiveFIFO0Size (void) { return mDriverReceiveFIFO0.size () ; } + public: uint32_t driverReceiveFIFO0Count (void) { return mDriverReceiveFIFO0.count () ; } + public: uint32_t driverReceiveFIFO0PeakCount (void) { return mDriverReceiveFIFO0.peakCount () ; } + public: void resetDriverReceiveFIFO0PeakCount (void) { mDriverReceiveFIFO0.resetPeakCount () ; } + public: inline ACANFD_STM32_Settings::Payload hardwareRxFIFO0Payload (void) const { + return mHardwareRxFIFO0Payload ; + } + +//--- Driver receive FIFO 1 + protected: ACANFD_STM32_FIFO mDriverReceiveFIFO1 ; + public: uint32_t driverReceiveFIFO1Size (void) { return mDriverReceiveFIFO1.size () ; } + public: uint32_t driverReceiveFIFO1Count (void) { return mDriverReceiveFIFO1.count () ; } + public: uint32_t driverReceiveFIFO1PeakCount (void) { return mDriverReceiveFIFO1.peakCount () ; } + public: void resetDriverReceiveFIFO1PeakCount (void) { mDriverReceiveFIFO1.resetPeakCount () ; } + public: inline ACANFD_STM32_Settings::Payload hardwareRxFIFO1Payload (void) const { + return mHardwareRxFIFO1Payload ; + } + + +//--- Constant public properties + public: const uint32_t mMessageRAMStartWordOffset ; + public: const uint32_t mMessageRamAllocatedWordSize ; + public: const std::initializer_list mTxPinArray ; + public: const std::initializer_list mRxPinArray ; + public: const std::optional mIRQs ; + +//--- Protected properties + protected: volatile FDCAN_GlobalTypeDef * mPeripheralPtr ; + protected: uint32_t mMessageRamRequiredWordSize = 0 ; + protected: const uint32_t * mRxFIFO0Pointer = nullptr ; + protected: const uint32_t * mRxFIFO1Pointer = nullptr ; + protected: volatile uint32_t * mTxBuffersPointer = nullptr ; + protected: ACANFD_STM32_DynamicArray < ACANFDCallBackRoutine > mStandardFilterCallBackArray ; + protected: ACANFD_STM32_DynamicArray < ACANFDCallBackRoutine > mExtendedFilterCallBackArray ; + protected: ACANFDCallBackRoutine mNonMatchingStandardMessageCallBack = nullptr ; + protected: ACANFDCallBackRoutine mNonMatchingExtendedMessageCallBack = nullptr ; + protected: ACANFD_STM32_Settings::Payload mHardwareRxFIFO0Payload = ACANFD_STM32_Settings::PAYLOAD_64_BYTES ; + protected: ACANFD_STM32_Settings::Payload mHardwareRxFIFO1Payload = ACANFD_STM32_Settings::PAYLOAD_64_BYTES ; + protected: ACANFD_STM32_Settings::Payload mHardwareTxBufferPayload = ACANFD_STM32_Settings::PAYLOAD_64_BYTES ; + +//--- Internal methods + public: void isr0 (void) ; + public: void isr1 (void) ; + private: void writeTxBuffer (const CANFDMessage & inMessage, const uint32_t inTxBufferIndex) ; + private: void internalDispatchReceivedMessage (const CANFDMessage & inMessage) ; + +//--- Status Flags (returns 0 if no error) +// Bit 0 : hardware RxFIFO 0 overflow +// Bit 1 : driver RxFIFO 0 overflow +// Bit 2 : hardware RxFIFO 1 overflow +// Bit 3 : driver RxFIFO 2 overflow +// Bit 4 : bus off + public: uint32_t statusFlags (void) const ; + +//--- Bus Status + public: inline BusStatus getStatus (void) const { return BusStatus (mPeripheralPtr) ; } + +//--- No copy + private : ACANFD_STM32 (const ACANFD_STM32 &) = delete ; + private : ACANFD_STM32 & operator = (const ACANFD_STM32 &) = delete ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + +} ; + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32.h b/lib/acanfd-stm32/src/ACANFD_STM32.h new file mode 100644 index 0000000..612acf9 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32.h @@ -0,0 +1,25 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ + +#pragma once + +//------------------------------------------------------------------------------ + +#ifdef ARDUINO_NUCLEO_H743ZI2 + #include "ACANFD_STM32_NUCLEO_H743ZI2-objects.h" +#elif defined (ARDUINO_NUCLEO_G431KB) + #include "ACANFD_STM32_NUCLEO_G431KB-objects.h" +#elif defined (ARDUINO_NUCLEO_G474RE) + #include "ACANFD_STM32_NUCLEO_G474RE-objects.h" +#elif defined (ARDUINO_WEACT_G474CE) + #include "ACANFD_STM32_NUCLEO_G474RE-objects.h" +#elif defined (ARDUINO_NUCLEO_G0B1RE) + #include "ACANFD_STM32_NUCLEO_G0B1RE-objects.h" +#elif defined (ARDUINO_NUCLEO_H723ZG) + #include "ACANFD_STM32_NUCLEO_H723ZG-objects.h" +#else + #error "Unhandled Nucleo Board" +#endif + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_CANFDMessage.h b/lib/acanfd-stm32/src/ACANFD_STM32_CANFDMessage.h new file mode 100644 index 0000000..10e2a9e --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_CANFDMessage.h @@ -0,0 +1,134 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ +// Generic CANFD Message +// by Pierre Molinaro +// +//------------------------------------------------------------------------------ + +#ifndef GENERIC_CANFD_MESSAGE_DEFINED +#define GENERIC_CANFD_MESSAGE_DEFINED + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ +// CANFDMessage class +//------------------------------------------------------------------------------ +// Note that "len" field contains the actual length, not its encoding in CANFD frames +// Valid values are: 0, 1, ..., 8, 12, 16, 20, 24, 32, 48, 64. +// Having other values is an error that prevents frame to be sent by tryToSend +// You can use the "pad" method for padding with 0 bytes to the next valid length + +class CANFDMessage { + +//············································································· +// Constructors +//············································································· + + public : CANFDMessage (void) : + id (0), // Frame identifier + ext (false), // false -> base frame, true -> extended frame + type (CANFD_WITH_BIT_RATE_SWITCH), + idx (0), // This field is used by the driver + len (0), // Length of data (0 ... 64) + data () { + } + +//············································································· + + public : CANFDMessage (const CANMessage & inMessage) : + id (inMessage.id), // Frame identifier + ext (inMessage.ext), // false -> base frame, true -> extended frame + type (inMessage.rtr ? CAN_REMOTE : CAN_DATA), + idx (inMessage.idx), // This field is used by the driver + len (inMessage.len), // Length of data (0 ... 8) + data () { + data64 [0] = inMessage.data64 ; + } + +//············································································· +// Enumerated Type +//············································································· + + public: typedef enum : uint8_t { + CAN_REMOTE, + CAN_DATA, + CANFD_NO_BIT_RATE_SWITCH, + CANFD_WITH_BIT_RATE_SWITCH + } Type ; + +//············································································· +// Properties +//············································································· + + public : uint32_t id ; // Frame identifier + public : bool ext ; // false -> base frame, true -> extended frame + public : Type type ; + public : uint8_t idx ; // This field is used by the driver + public : uint8_t len ; // Length of data (0 ... 64) + public : union { + uint64_t data64 [ 8] ; // Caution: subject to endianness + int64_t data_s64 [ 8] ; // Caution: subject to endianness + uint32_t data32 [16] ; // Caution: subject to endianness + int32_t data_s32 [16] ; // Caution: subject to endianness + float dataFloat [16] ; // Caution: subject to endianness + uint16_t data16 [32] ; // Caution: subject to endianness + int16_t data_s16 [32] ; // Caution: subject to endianness + int8_t data_s8 [64] ; + uint8_t data [64] ; + } ; + +//············································································· +// Methods +//············································································· + + public: void pad (void) { + uint8_t paddedLength = len ; + if ((len > 8) && (len < 12)) { + paddedLength = 12 ; + }else if ((len > 12) && (len < 16)) { + paddedLength = 16 ; + }else if ((len > 16) && (len < 20)) { + paddedLength = 20 ; + }else if ((len > 20) && (len < 24)) { + paddedLength = 24 ; + }else if ((len > 24) && (len < 32)) { + paddedLength = 32 ; + }else if ((len > 32) && (len < 48)) { + paddedLength = 48 ; + }else if ((len > 48) && (len < 64)) { + paddedLength = 64 ; + } + while (len < paddedLength) { + data [len] = 0 ; + len += 1 ; + } + } + +//············································································· + + public: bool isValid (void) const { + if ((type == CAN_REMOTE) || (type == CAN_DATA)) { // Remote frame + return len <= 8 ; + }else{ // Data frame + return + (len <= 8) || (len == 12) || (len == 16) || (len == 20) + || + (len == 24) || (len == 32) || (len == 48) || (len == 64) + ; + } + } + +//············································································· + +} ; + +//------------------------------------------------------------------------------ + +typedef void (*ACANFDCallBackRoutine) (const CANFDMessage & inMessage) ; + +//------------------------------------------------------------------------------ + +#endif +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_CANMessage.h b/lib/acanfd-stm32/src/ACANFD_STM32_CANMessage.h new file mode 100644 index 0000000..4dc320e --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_CANMessage.h @@ -0,0 +1,51 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ +// Generic CAN Message +// by Pierre Molinaro +// +// This file is common to the following libraries +// https://github.com/pierremolinaro/acan +// https://github.com/pierremolinaro/acan2515 +// https://github.com/pierremolinaro/acan2517 +// https://github.com/pierremolinaro/acan2517FD +// +//------------------------------------------------------------------------------ + +#ifndef GENERIC_CAN_MESSAGE_DEFINED +#define GENERIC_CAN_MESSAGE_DEFINED + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +class CANMessage { + public : uint32_t id = 0 ; // Frame identifier + public : bool ext = false ; // false -> standard frame, true -> extended frame + public : bool rtr = false ; // false -> data frame, true -> remote frame + public : uint8_t idx = 0 ; // This field is used by the driver + public : uint8_t len = 0 ; // Length of data (0 ... 8) + public : union { + uint64_t data64 ; // Caution: subject to endianness + int64_t data_s64 ; // Caution: subject to endianness + uint32_t data32 [2] ; // Caution: subject to endianness + int32_t data_s32 [2] ; // Caution: subject to endianness + uint16_t data16 [4] ; // Caution: subject to endianness + int16_t data_s16 [4] ; // Caution: subject to endianness + float dataFloat [2] ; // Caution: subject to endianness + int8_t data_s8 [8] ; + uint8_t data [8] = {0, 0, 0, 0, 0, 0, 0, 0} ; + } ; +} ; + +//------------------------------------------------------------------------------ + +typedef enum {kStandard, kExtended} tFrameFormat ; +typedef enum {kData, kRemote} tFrameKind ; +typedef void (*ACANCallBackRoutine) (const CANMessage & inMessage) ; + +//------------------------------------------------------------------------------ + +#endif +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_DataBitRateFactor.h b/lib/acanfd-stm32/src/ACANFD_STM32_DataBitRateFactor.h new file mode 100644 index 0000000..dac67b5 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_DataBitRateFactor.h @@ -0,0 +1,29 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ + +#ifndef ACANFD_DATA_BIT_RATE_FACTOR_DEFINED +#define ACANFD_DATA_BIT_RATE_FACTOR_DEFINED + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +enum class DataBitRateFactor : uint8_t { + x1 = 1, + x2 = 2, + x3 = 3, + x4 = 4, + x5 = 5, + x6 = 6, + x7 = 7, + x8 = 8, + x9 = 9, + x10 = 10 +} ; + +//------------------------------------------------------------------------------ + +#endif +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_DynamicArray.h b/lib/acanfd-stm32/src/ACANFD_STM32_DynamicArray.h new file mode 100644 index 0000000..d2f4bbe --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_DynamicArray.h @@ -0,0 +1,71 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ + +#pragma once + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ +// Dynamic Array +//------------------------------------------------------------------------------ + +template class ACANFD_STM32_DynamicArray { +//--- Default constructor + public: ACANFD_STM32_DynamicArray (void) { } + +//--- Destructor + public: ~ ACANFD_STM32_DynamicArray (void) { delete [] mArray ; } + +//--- Append + public: void append (const T & inObject) { + if (mCapacity == mCount) { + mCapacity += 64 ; + T * newArray = new T [mCapacity] ; + for (uint32_t i=0 ; i + +//------------------------------------------------------------------------------ +// Default constructor +//------------------------------------------------------------------------------ + +ACANFD_STM32_FIFO::ACANFD_STM32_FIFO (void) : +mBuffer (nullptr), +mSize (0), +mReadIndex (0), +mCount (0), +mPeakCount (0) { +} + +//------------------------------------------------------------------------------ +// Destructor +//------------------------------------------------------------------------------ + +ACANFD_STM32_FIFO:: ~ ACANFD_STM32_FIFO (void) { + delete [] mBuffer ; +} + +//------------------------------------------------------------------------------ +// initWithSize +//------------------------------------------------------------------------------ + +void ACANFD_STM32_FIFO::initWithSize (const uint16_t inSize) { + delete [] mBuffer ; + mBuffer = new CANFDMessage [inSize] ; + mSize = inSize ; + mReadIndex = 0 ; + mCount = 0 ; + mPeakCount = 0 ; +} + +//------------------------------------------------------------------------------ +// append +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_FIFO::append (const CANFDMessage & inMessage) { + const bool ok = mCount < mSize ; + if (ok) { + uint16_t writeIndex = mReadIndex + mCount ; + if (writeIndex >= mSize) { + writeIndex -= mSize ; + } + mBuffer [writeIndex] = inMessage ; + mCount += 1 ; + if (mPeakCount < mCount) { + mPeakCount = mCount ; + } + }else{ + mPeakCount = mSize + 1 ; + } + return ok ; +} + +//------------------------------------------------------------------------------ +// Remove +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_FIFO::remove (CANFDMessage & outMessage) { + const bool ok = mCount > 0 ; + if (ok) { + outMessage = mBuffer [mReadIndex] ; + mCount -= 1 ; + mReadIndex += 1 ; + if (mReadIndex == mSize) { + mReadIndex = 0 ; + } + } + return ok ; +} + +//------------------------------------------------------------------------------ +// Free +//------------------------------------------------------------------------------ + +void ACANFD_STM32_FIFO::free (void) { + delete [] mBuffer ; mBuffer = nullptr ; + mSize = 0 ; + mReadIndex = 0 ; + mCount = 0 ; + mPeakCount = 0 ; +} + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_FIFO.h b/lib/acanfd-stm32/src/ACANFD_STM32_FIFO.h new file mode 100644 index 0000000..9144b14 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_FIFO.h @@ -0,0 +1,86 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ + +#pragma once + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +class ACANFD_STM32_FIFO { + + //············································································ + // Default constructor + //············································································ + + public: ACANFD_STM32_FIFO (void) ; + + //············································································ + // Destructor + //············································································ + + public: ~ ACANFD_STM32_FIFO (void) ; + + //············································································ + // Private properties + //············································································ + + private: CANFDMessage * mBuffer ; + private: uint16_t mSize ; + private: uint16_t mReadIndex ; + private: uint16_t mCount ; + private: uint16_t mPeakCount ; // > mSize if overflow did occur + + //············································································ + // Accessors + //············································································ + + public: inline uint16_t size (void) const { return mSize ; } + public: inline uint16_t count (void) const { return mCount ; } + public: inline bool isEmpty (void) const { return (mCount == 0) && (mSize > 0) ; } + public: inline bool isFull (void) const { return mCount == mSize ; } + public: inline bool didOverflow (void) const { return mPeakCount > mSize ; } + public: inline uint16_t peakCount (void) const { return mPeakCount ; } + + //············································································ + // initWithSize + //············································································ + + public: void initWithSize (const uint16_t inSize) ; + + //············································································ + // append + //············································································ + + public: bool append (const CANFDMessage & inMessage) ; + + //············································································ + // Remove + //············································································ + + public: bool remove (CANFDMessage & outMessage) ; + + //············································································ + // Free + //············································································ + + public: void free (void) ; + + //············································································ + // Reset Peak Count + //············································································ + + public: inline void resetPeakCount (void) { mPeakCount = mCount ; } + + //············································································ + // No copy + //············································································ + + private: ACANFD_STM32_FIFO (const ACANFD_STM32_FIFO &) = delete ; + private: ACANFD_STM32_FIFO & operator = (const ACANFD_STM32_FIFO &) = delete ; +} ; + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_Filters.cpp b/lib/acanfd-stm32/src/ACANFD_STM32_Filters.cpp new file mode 100644 index 0000000..b621c06 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_Filters.cpp @@ -0,0 +1,146 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ +// Standard filters +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_StandardFilters::addSingle (const uint16_t inIdentifier, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack) { + return addDual (inIdentifier, inIdentifier, inAction, inCallBack) ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_StandardFilters::addDual (const uint16_t inIdentifier1, + const uint16_t inIdentifier2, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack) { + const bool ok = (inIdentifier1 <= 0x7FF) && (inIdentifier2 <= 0x7FF) ; + if (ok) { + uint32_t filter = inIdentifier2 ; + filter |= uint32_t (inIdentifier1) << 16 ; + filter |= (1U << 30) ; // Dual filter + filter |= ((uint32_t (inAction) + 1) << 27) ; // Filter action + mFilterArray.append (filter) ; + mCallBackArray.append (inCallBack) ; + } + return ok ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_StandardFilters::addRange (const uint16_t inIdentifier1, + const uint16_t inIdentifier2, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack) { + const bool ok = (inIdentifier1 <= inIdentifier2) && (inIdentifier2 <= 0x7FF) ; + if (ok) { + uint32_t filter = inIdentifier2 ; + filter |= uint32_t (inIdentifier1) << 16 ; + filter |= ((uint32_t (inAction) + 1) << 27) ; // Filter action + mFilterArray.append (filter) ; // Filter type is 0 (RANGE) + mCallBackArray.append (inCallBack) ; + } + return ok ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_StandardFilters::addClassic (const uint16_t inIdentifier, + const uint16_t inMask, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack) { + const bool ok = (inIdentifier <= 0x7FF) + && (inMask <= 0x7FF) + && ((inIdentifier & inMask) == inIdentifier) ; + if (ok) { + uint32_t filter = inMask ; + filter |= uint32_t (inIdentifier) << 16 ; + filter |= (2U << 30) ; // Classic filter + filter |= ((uint32_t (inAction) + 1) << 27) ; // Filter action + mFilterArray.append (filter) ; + mCallBackArray.append (inCallBack) ; + } + return ok ; +} + +//------------------------------------------------------------------------------ +// Extended filters +//------------------------------------------------------------------------------ + +static const uint32_t MAX_EXTENDED_IDENTIFIER = 0x1FFFFFFF ; + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_ExtendedFilters::addSingle (const uint32_t inIdentifier, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack) { + return addDual (inIdentifier, inIdentifier, inAction, inCallBack) ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_ExtendedFilters::addDual (const uint32_t inIdentifier1, + const uint32_t inIdentifier2, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack) { + const bool ok = (inIdentifier1 <= MAX_EXTENDED_IDENTIFIER) + && (inIdentifier2 <= MAX_EXTENDED_IDENTIFIER) ; + if (ok) { + uint32_t filter = inIdentifier1 ; + filter |= ((uint32_t (inAction) + 1) << 29) ; // Filter action + mFilterArray.append (filter) ; + filter = inIdentifier2 ; + filter |= (1U << 30) ; // Dual filter + mFilterArray.append (filter) ; + mCallBackArray.append (inCallBack) ; + } + return ok ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_ExtendedFilters::addRange (const uint32_t inIdentifier1, + const uint32_t inIdentifier2, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack) { + const bool ok = (inIdentifier1 <= inIdentifier2) + && (inIdentifier2 <= MAX_EXTENDED_IDENTIFIER) ; + if (ok) { + uint32_t filter = inIdentifier1 ; // Filter type is 0 (RANGE) + filter |= ((uint32_t (inAction) + 1) << 29) ; // Filter action + mFilterArray.append (filter) ; + filter = inIdentifier2 ; + mFilterArray.append (filter) ; // Filter type is 0 (RANGE) + mCallBackArray.append (inCallBack) ; + } + return ok ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_ExtendedFilters::addClassic (const uint32_t inIdentifier, + const uint32_t inMask, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack) { + const bool ok = (inIdentifier <= MAX_EXTENDED_IDENTIFIER) + && (inMask <= MAX_EXTENDED_IDENTIFIER) + && ((inIdentifier & inMask) == inIdentifier) ; + if (ok) { + uint32_t filter = inIdentifier ; + filter |= ((uint32_t (inAction) + 1) << 29) ; // Filter action + mFilterArray.append (filter) ; + filter = inMask ; + filter |= (2U << 30) ; // Classic filter + mFilterArray.append (filter) ; + mCallBackArray.append (inCallBack) ; + } + return ok ; +} + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_Filters.h b/lib/acanfd-stm32/src/ACANFD_STM32_Filters.h new file mode 100644 index 0000000..b76f71f --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_Filters.h @@ -0,0 +1,116 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ + +#pragma once + +//------------------------------------------------------------------------------ + +#include +#include + +//------------------------------------------------------------------------------ + +enum class ACANFD_STM32_FilterAction { + FIFO0 = 0, + FIFO1 = 1, + REJECT = 2 +} ; + +//------------------------------------------------------------------------------ +// Standard filters +//------------------------------------------------------------------------------ + +class ACANFD_STM32_StandardFilters { +//--- Default constructor + public: ACANFD_STM32_StandardFilters (void) { } + +//--- Append filter + public: bool addSingle (const uint16_t inIdentifier, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack = nullptr) ; + + public: bool addDual (const uint16_t inIdentifier1, + const uint16_t inIdentifier2, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack = nullptr) ; + + public: bool addRange (const uint16_t inIdentifier1, + const uint16_t inIdentifier2, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack = nullptr) ; + + public: bool addClassic (const uint16_t inIdentifier, + const uint16_t inMask, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack = nullptr) ; + +//--- Access + public: uint32_t count () const { + return mFilterArray.count () ; + } + + public: uint32_t filterAtIndex (const uint32_t inIndex) const { + return mFilterArray [inIndex] ; + } + + public: ACANFDCallBackRoutine callBackAtIndex (const uint32_t inIndex) const { + return mCallBackArray [inIndex] ; + } + +//--- Private properties + private: ACANFD_STM32_DynamicArray mFilterArray ; + private: ACANFD_STM32_DynamicArray < ACANFDCallBackRoutine > mCallBackArray ; + +//--- No copy + private : ACANFD_STM32_StandardFilters (const ACANFD_STM32_StandardFilters &) = delete ; + private : ACANFD_STM32_StandardFilters & operator = (const ACANFD_STM32_StandardFilters &) = delete ; +} ; + +//------------------------------------------------------------------------------ +// Extended filters +//------------------------------------------------------------------------------ + +class ACANFD_STM32_ExtendedFilters { +//--- Default constructor + public: ACANFD_STM32_ExtendedFilters (void) { } + +//--- Append filter + public: bool addSingle (const uint32_t inExtendedIdentifier, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack = nullptr) ; + + public: bool addDual (const uint32_t inExtendedIdentifier1, + const uint32_t inExtendedIdentifier2, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack = nullptr) ; + + public: bool addRange (const uint32_t inExtendedIdentifier1, + const uint32_t inExtendedIdentifier2, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack = nullptr) ; + + public: bool addClassic (const uint32_t inExtendedIdentifier, + const uint32_t inExtendedMask, + const ACANFD_STM32_FilterAction inAction, + const ACANFDCallBackRoutine inCallBack = nullptr) ; + +//--- Access + public: uint32_t count () const { return mCallBackArray.count () ; } + public: uint32_t firstWordAtIndex (const uint32_t inIndex) const { return mFilterArray [inIndex * 2] ; } + public: uint32_t secondWordAtIndex (const uint32_t inIndex) const { return mFilterArray [inIndex * 2 + 1] ; } + public: ACANFDCallBackRoutine callBackAtIndex (const uint32_t inIndex) const { + return mCallBackArray [inIndex] ; + } + +//--- Private properties + private: ACANFD_STM32_DynamicArray mFilterArray ; + private: ACANFD_STM32_DynamicArray < void (*) (const CANFDMessage & inMessage) > mCallBackArray ; + +//--- No copy + private : ACANFD_STM32_ExtendedFilters (const ACANFD_STM32_ExtendedFilters &) = delete ; + private : ACANFD_STM32_ExtendedFilters & operator = (const ACANFD_STM32_ExtendedFilters &) = delete ; +} ; + + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G0B1RE-objects.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G0B1RE-objects.h new file mode 100644 index 0000000..84dc2c3 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G0B1RE-objects.h @@ -0,0 +1,73 @@ +#pragma once + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ +// NUCLEO-G0B1RE: STM32G0B1RE (DS13560 Rev 4, december 2022) +// FDCAN1_RX : PA11, PB8, PC4, PD0 +// FDCAN1_TX : PA12, PB9, PC5, PD1 +// FDCAN2_RX : PB0, PB5, PB12, PC2; not available on Nucleo-64 board: PD14 +// FDCAN2_TX : PB1, PB6, PB13, PC3; not available on Nucleo-64 board: PD15 +//------------------------------------------------------------------------------ +// FDCAN1 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_tx_pin_array { + ACANFD_STM32::PinPort (PA_12, 3), + ACANFD_STM32::PinPort (PB_9, 3), + ACANFD_STM32::PinPort (PC_5, 3), + ACANFD_STM32::PinPort (PD_1, 3) +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_rx_pin_array { + ACANFD_STM32::PinPort (PA_11, 3), + ACANFD_STM32::PinPort (PB_8, 3), + ACANFD_STM32::PinPort (PC_4, 3), + ACANFD_STM32::PinPort (PD_0, 3) +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan1 ( + FDCAN1, // CAN Peripheral base address + SRAMCAN_BASE, + std::nullopt, // No interrupt + fdcan1_tx_pin_array, + fdcan1_rx_pin_array +) ; + +//------------------------------------------------------------------------------ +// FDCAN2 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan2_tx_pin_array { + ACANFD_STM32::PinPort (PB_1, 3), + ACANFD_STM32::PinPort (PB_6, 3), + ACANFD_STM32::PinPort (PB_13, 3), + ACANFD_STM32::PinPort (PC_3, 3) +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan2_rx_pin_array { + ACANFD_STM32::PinPort (PB_0, 3), + ACANFD_STM32::PinPort (PB_5, 3), + ACANFD_STM32::PinPort (PB_12, 3), + ACANFD_STM32::PinPort (PC_2, 3) +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan2 ( + FDCAN2, // CAN Peripheral base address + SRAMCAN_BASE + 212 * 4, + std::nullopt, // No interrupt + fdcan2_tx_pin_array, + fdcan2_rx_pin_array +) ; + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G0B1RE-settings.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G0B1RE-settings.h new file mode 100644 index 0000000..4613f67 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G0B1RE-settings.h @@ -0,0 +1,28 @@ +#pragma once + +//------------------------------------------------------------------------------ +// Settings for NUCLEO-G0B1RE +//------------------------------------------------------------------------------ + +#define HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS (false) + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +inline uint32_t fdcanClock (void) { + if (!__HAL_RCC_FDCAN_IS_CLK_ENABLED ()) { // Is not enabled ? + //--- Enable CAN clock + __HAL_RCC_FDCAN_CLK_ENABLE () ; + //--- Reset CAN peripherals + __HAL_RCC_FDCAN_FORCE_RESET () ; + __HAL_RCC_FDCAN_RELEASE_RESET () ; + //--- Select CAN clock + LL_RCC_SetFDCANClockSource (RCC_FDCANCLKSOURCE_PCLK1) ; + } + return HAL_RCC_GetPCLK1Freq () ; +} + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G431KB-objects.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G431KB-objects.h new file mode 100644 index 0000000..f14a54f --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G431KB-objects.h @@ -0,0 +1,54 @@ +#pragma once + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ +// NUCLEO-G431KB: STM32G431KB (DS12589 Rev 6, October 2021) +// FDCAN_RX : PA11, PB8 +// FDCAN_TX : PA12, PB9 +//------------------------------------------------------------------------------ +// FDCAN1 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_tx_pin_array { + ACANFD_STM32::PinPort (PA_12, 9), // Tx Pin: PA_12, AF9 + ACANFD_STM32::PinPort (PB_9, 9), // Tx Pin: PB_9, AF9 +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_rx_pin_array { + ACANFD_STM32::PinPort (PA_11, 9), // Rx Pin: PA_11, AF9 + ACANFD_STM32::PinPort (PB_8, 9), // Rx Pin: PB_9, AF9 +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan1 ( + FDCAN1, // CAN Peripheral base address + SRAMCAN_BASE, // CAN RAM Base Address + ACANFD_STM32::IRQs (FDCAN1_IT0_IRQn, FDCAN1_IT1_IRQn), // interrupts + fdcan1_tx_pin_array, + fdcan1_rx_pin_array +) ; + +//------------------------------------------------------------------------------ + +extern "C" void FDCAN1_IT0_IRQHandler (void) ; +extern "C" void FDCAN1_IT1_IRQHandler (void) ; + +//------------------------------------------------------------------------------ + +void FDCAN1_IT0_IRQHandler (void) { + fdcan1.isr0 () ; +} + +//------------------------------------------------------------------------------ + +void FDCAN1_IT1_IRQHandler (void) { + fdcan1.isr1 () ; +} + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G431KB-settings.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G431KB-settings.h new file mode 100644 index 0000000..bb51987 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G431KB-settings.h @@ -0,0 +1,28 @@ +#pragma once + +//------------------------------------------------------------------------------ +// Settings for NUCLEO-G431KB +//------------------------------------------------------------------------------ + +#define HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS (false) + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +inline uint32_t fdcanClock (void) { + if (!__HAL_RCC_FDCAN_IS_CLK_ENABLED ()) { // Is not enabled ? + //--- Enable CAN clock + __HAL_RCC_FDCAN_CLK_ENABLE () ; + //--- Reset CAN peripherals + __HAL_RCC_FDCAN_FORCE_RESET () ; + __HAL_RCC_FDCAN_RELEASE_RESET () ; + //--- Select CAN clock + LL_RCC_SetFDCANClockSource (RCC_FDCANCLKSOURCE_PCLK1) ; + } + return HAL_RCC_GetPCLK1Freq () ; +} + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G474RE-objects.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G474RE-objects.h new file mode 100644 index 0000000..640d250 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G474RE-objects.h @@ -0,0 +1,144 @@ +#pragma once + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ +// NUCLEO-G474RE: STM32G474RE (DS12288 Rev 5, November 2021) +// FDCAN1_RX : PA_11, PB_8 +// FDCAN1_TX : PA_12, PB_9 +// FDCAN2_RX : PB_5, PB_12 +// FDCAN2_TX : PB_6, PB_13 +// FDCAN3_RX : PA_8, PB_3 +// FDCAN3_TX : PA_15, PB_4 +//------------------------------------------------------------------------------ +// FDCAN1 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_tx_pin_array { + ACANFD_STM32::PinPort (PA_12, 9), // Tx Pin: PA_12, AF9 + ACANFD_STM32::PinPort (PB_9, 9) // Tx Pin: PB_9, AF9 +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_rx_pin_array { + ACANFD_STM32::PinPort (PA_11, 9), // Rx Pin: PA_11, AF9 + ACANFD_STM32::PinPort (PB_8, 9) // Rx Pin: PB_9, AF9 +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan1 ( + FDCAN1, // CAN Peripheral base address + SRAMCAN_BASE, + ACANFD_STM32::IRQs (FDCAN1_IT0_IRQn, FDCAN1_IT1_IRQn), // Interrupts + fdcan1_tx_pin_array, + fdcan1_rx_pin_array +) ; + +//------------------------------------------------------------------------------ + +extern "C" void FDCAN1_IT0_IRQHandler (void) ; +extern "C" void FDCAN1_IT1_IRQHandler (void) ; + +//------------------------------------------------------------------------------ + +void FDCAN1_IT0_IRQHandler (void) { + fdcan1.isr0 () ; +} + +//------------------------------------------------------------------------------ + +void FDCAN1_IT1_IRQHandler (void) { + fdcan1.isr1 () ; +} + +//------------------------------------------------------------------------------ +// FDCAN2 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan2_tx_pin_array { + ACANFD_STM32::PinPort (PB_6, 9), // Tx Pin: PB_6, AF9 + ACANFD_STM32::PinPort (PB_13, 9) // Tx Pin: PB_13, AF9 +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan2_rx_pin_array { + ACANFD_STM32::PinPort (PB_5, 9), // Rx Pin: PB_5, AF9 + ACANFD_STM32::PinPort (PB_12, 9) // Rx Pin: PB_12, AF9 +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan2 ( + FDCAN2, // CAN Peripheral base address + SRAMCAN_BASE + 212 * 4, + ACANFD_STM32::IRQs (FDCAN2_IT0_IRQn, FDCAN2_IT1_IRQn), // Interrupts + fdcan2_tx_pin_array, + fdcan2_rx_pin_array +) ; + +//------------------------------------------------------------------------------ + +extern "C" void FDCAN2_IT0_IRQHandler (void) ; +extern "C" void FDCAN2_IT1_IRQHandler (void) ; + +//------------------------------------------------------------------------------ + +void FDCAN2_IT0_IRQHandler (void) { + fdcan2.isr0 () ; +} + +//------------------------------------------------------------------------------ + +void FDCAN2_IT1_IRQHandler (void) { + fdcan2.isr1 () ; +} + +//------------------------------------------------------------------------------ +// FDCAN3 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan3_tx_pin_array { + ACANFD_STM32::PinPort (PA_15, 11), // Tx Pin: PA_15, AF11 + ACANFD_STM32::PinPort (PB_4, 11) // Tx Pin: PB_4, AF11 +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan3_rx_pin_array { + ACANFD_STM32::PinPort (PA_8, 11), // Rx Pin: PA_8, AF11 + ACANFD_STM32::PinPort (PB_3, 11) // Rx Pin: PB_3, AF11 +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan3 ( + FDCAN3, // CAN Peripheral base address + SRAMCAN_BASE + 212 * 4 * 2, + ACANFD_STM32::IRQs (FDCAN3_IT0_IRQn, FDCAN3_IT1_IRQn), // Interrupts + fdcan3_tx_pin_array, + fdcan3_rx_pin_array +) ; + +//------------------------------------------------------------------------------ + +extern "C" void FDCAN3_IT0_IRQHandler (void) ; +extern "C" void FDCAN3_IT1_IRQHandler (void) ; + +//------------------------------------------------------------------------------ + +void FDCAN3_IT0_IRQHandler (void) { + fdcan3.isr0 () ; +} + +//------------------------------------------------------------------------------ + +void FDCAN3_IT1_IRQHandler (void) { + fdcan3.isr1 () ; +} + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G474RE-settings.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G474RE-settings.h new file mode 100644 index 0000000..725b64d --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_G474RE-settings.h @@ -0,0 +1,28 @@ +#pragma once + +//------------------------------------------------------------------------------ +// Settings for NUCLEO-G474RE +//------------------------------------------------------------------------------ + +#define HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS (false) + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +inline uint32_t fdcanClock (void) { + if (!__HAL_RCC_FDCAN_IS_CLK_ENABLED ()) { // Is not enabled ? + //--- Enable CAN clock + __HAL_RCC_FDCAN_CLK_ENABLE () ; + //--- Reset CAN peripherals + __HAL_RCC_FDCAN_FORCE_RESET () ; + __HAL_RCC_FDCAN_RELEASE_RESET () ; + //--- Select CAN clock + LL_RCC_SetFDCANClockSource (RCC_FDCANCLKSOURCE_PCLK1) ; + } + return HAL_RCC_GetPCLK1Freq () ; +} + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H723ZG-objects.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H723ZG-objects.h new file mode 100644 index 0000000..b3e7cc9 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H723ZG-objects.h @@ -0,0 +1,151 @@ +#pragma once + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ +// NUCLEO-H743ZI2: STM32H743 (DS13313 Rev 3, Dec 2021) +// FDCAN1_RX : PA11, PD0 (ZIO D67), PB8 +// FDCAN1_TX : PA12, PD1 (ZIO D66), PB9 +// FDCAN2_RX : PB12 (ZIO D19), PB5 (ZIO D11 or D22 ?) +// FDCAN2_TX : PB13 (ZIO D18), PB6 (ZIO D1) +// FDCAN3_RX : PF6, PD12, PG10 +// FDCAN3_TX : PF7, PD13, PG9 +//------------------------------------------------------------------------------ +// FDCAN1 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_tx_pin_array { + ACANFD_STM32::PinPort (PD_1, 9), // Tx Pin: PD_1, AF9 + ACANFD_STM32::PinPort (PB_9, 9), // Tx Pin: PB_9, AF9 + ACANFD_STM32::PinPort (PA_12, 9) // Tx Pin: PA_12, AF9 +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_rx_pin_array { + ACANFD_STM32::PinPort (PD_0, 9), // Rx Pin: PD_0, AF9 + ACANFD_STM32::PinPort (PB_8, 9), // Rx Pin: PB_9, AF9 + ACANFD_STM32::PinPort (PA_11, 9) // Rx Pin: PA_11, AF9 +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan1 ( + FDCAN1, // CAN Peripheral base address + 0, // Message RAM start word offset + FDCAN1_MESSAGE_RAM_WORD_SIZE, // Message RAM word size + ACANFD_STM32::IRQs (FDCAN1_IT0_IRQn, FDCAN1_IT1_IRQn), // Interrupts + fdcan1_tx_pin_array, + fdcan1_rx_pin_array +) ; + +//------------------------------------------------------------------------------ + +extern "C" void FDCAN1_IT0_IRQHandler (void) ; +extern "C" void FDCAN1_IT1_IRQHandler (void) ; + +//------------------------------------------------------------------------------ + +void FDCAN1_IT0_IRQHandler (void) { + fdcan1.isr0 () ; +} + +//------------------------------------------------------------------------------ + +void FDCAN1_IT1_IRQHandler (void){ + fdcan1.isr1 () ; +} + +//------------------------------------------------------------------------------ +// FDCAN2 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan2_tx_pin_array { + ACANFD_STM32::PinPort (PB_6, 9), // Tx Pin: PB_6, AF9 + ACANFD_STM32::PinPort (PB_13, 9) // Tx Pin: PB_13, AF9 +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan2_rx_pin_array { + ACANFD_STM32::PinPort (PB_5, 9), // Rx Pin: PB_5, AF9 + ACANFD_STM32::PinPort (PB_12, 9) // Rx Pin: PB_12, AF9 +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan2 ( + FDCAN2, // CAN Peripheral base address + FDCAN1_MESSAGE_RAM_WORD_SIZE, // Message RAM start word offset + FDCAN2_MESSAGE_RAM_WORD_SIZE, // Message RAM word size + ACANFD_STM32::IRQs (FDCAN2_IT0_IRQn, FDCAN2_IT1_IRQn), // Interrupts + fdcan2_tx_pin_array, + fdcan2_rx_pin_array +) ; + +//------------------------------------------------------------------------------ + +extern "C" void FDCAN2_IT0_IRQHandler (void) ; +extern "C" void FDCAN2_IT1_IRQHandler (void) ; + +//------------------------------------------------------------------------------ + +void FDCAN2_IT0_IRQHandler (void) { + fdcan2.isr0 () ; +} + +//------------------------------------------------------------------------------ + +void FDCAN2_IT1_IRQHandler (void){ + fdcan2.isr1 () ; +} + +//------------------------------------------------------------------------------ +// FDCAN3 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan3_tx_pin_array { + ACANFD_STM32::PinPort (PF_7, 3), // Tx Pin: PF_7, AF3 + ACANFD_STM32::PinPort (PD_13, 3), // Tx Pin: PD_13, AF3 + ACANFD_STM32::PinPort (PG_9, 2) // Tx Pin: PG_9, AF2 +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan3_rx_pin_array { + ACANFD_STM32::PinPort (PF_6, 2), // Rx Pin: PF_6, AF2 + ACANFD_STM32::PinPort (PD_12, 3), // Rx Pin: PD_12, AF3 + ACANFD_STM32::PinPort (PG_10, 2) // Tx Pin: PG_10, AF2 +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan3 ( + FDCAN3, // CAN Peripheral base address + FDCAN1_MESSAGE_RAM_WORD_SIZE + FDCAN2_MESSAGE_RAM_WORD_SIZE, // Message RAM start word offset + FDCAN3_MESSAGE_RAM_WORD_SIZE, // Message RAM word size + ACANFD_STM32::IRQs (FDCAN3_IT0_IRQn, FDCAN3_IT1_IRQn), // Interrupts + fdcan3_tx_pin_array, + fdcan3_rx_pin_array +) ; + +//------------------------------------------------------------------------------ + +extern "C" void FDCAN3_IT0_IRQHandler (void) ; +extern "C" void FDCAN3_IT1_IRQHandler (void) ; + +//------------------------------------------------------------------------------ + +void FDCAN3_IT0_IRQHandler (void) { + fdcan3.isr0 () ; +} + +//------------------------------------------------------------------------------ + +void FDCAN3_IT1_IRQHandler (void){ + fdcan3.isr1 () ; +} + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H723ZG-settings.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H723ZG-settings.h new file mode 100644 index 0000000..55e70c7 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H723ZG-settings.h @@ -0,0 +1,29 @@ +#pragma once + +//------------------------------------------------------------------------------ +// Settings for NUCLEO-H743ZI2 +//------------------------------------------------------------------------------ + +#define HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS (true) +static const uint32_t FDCAN_MESSAGE_RAM_WORD_SIZE = 2560 ; + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +inline uint32_t fdcanClock (void) { + if (!__HAL_RCC_FDCAN_IS_CLK_ENABLED ()) { + //--- Enable CAN clock + __HAL_RCC_FDCAN_CLK_ENABLE () ; + //--- Reset CAN peripherals + __HAL_RCC_FDCAN_FORCE_RESET () ; + __HAL_RCC_FDCAN_RELEASE_RESET () ; + //--- Select CAN clock + LL_RCC_SetFDCANClockSource (LL_RCC_FDCAN_CLKSOURCE_PLL1Q) ; + } + return HAL_RCC_GetPCLK1Freq () ; +} + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H743ZI2-objects.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H743ZI2-objects.h new file mode 100644 index 0000000..d6b9c6c --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H743ZI2-objects.h @@ -0,0 +1,103 @@ +#pragma once + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ +// NUCLEO-H743ZI2: STM32H743 (DS12110 Rev 9, March 2022) +// FDCAN1_RX : PA11, PB8, PD0 (ZIO D67), PH14 +// FDCAN1_TX : PA12, PB9, PD1 (ZIO D66), PH13 +// FDCAN2_RX : PB12 (ZIO D19), PB5 (ZIO D11 or D22 ?) +// FDCAN2_TX : PB13 (ZIO D18), PB6 (ZIO D1) +//------------------------------------------------------------------------------ +// FDCAN1 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_tx_pin_array{ + ACANFD_STM32::PinPort (PD_1, 9), // Tx Pin: PD_1, AF9 + ACANFD_STM32::PinPort (PB_9, 9), // Tx Pin: PB_9, AF9 + ACANFD_STM32::PinPort (PA_12, 9) // Tx Pin: PA_12, AF9 +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan1_rx_pin_array { + ACANFD_STM32::PinPort (PD_0, 9), // Rx Pin: PD_0, AF9 + ACANFD_STM32::PinPort (PB_8, 9), // Rx Pin: PB_9, AF9 + ACANFD_STM32::PinPort (PA_11, 9) // Rx Pin: PA_11, AF9 +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan1 ( + FDCAN1, // CAN Peripheral base address + 0, // Message RAM start word offset + FDCAN1_MESSAGE_RAM_WORD_SIZE, // Message RAM word size + ACANFD_STM32::IRQs (FDCAN1_IT0_IRQn, FDCAN1_IT1_IRQn), // Interrupts + fdcan1_tx_pin_array, + fdcan1_rx_pin_array +) ; + +//------------------------------------------------------------------------------ + +extern "C" void FDCAN1_IT0_IRQHandler (void) ; +extern "C" void FDCAN1_IT1_IRQHandler (void) ; + +//------------------------------------------------------------------------------ + +void FDCAN1_IT0_IRQHandler (void) { + fdcan1.isr0 () ; +} + +//------------------------------------------------------------------------------ + +void FDCAN1_IT1_IRQHandler (void){ + fdcan1.isr1 () ; +} + +//------------------------------------------------------------------------------ +// FDCAN2 +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan2_tx_pin_array { + ACANFD_STM32::PinPort (PB_6, 9), // Tx Pin: PB_6, AF9 + ACANFD_STM32::PinPort (PB_13, 9) // Tx Pin: PB_13, AF9 +} ; + +//------------------------------------------------------------------------------ + +const std::initializer_list fdcan2_rx_pin_array { + ACANFD_STM32::PinPort (PB_5, 9), // Rx Pin: PB_5, AF9 + ACANFD_STM32::PinPort (PB_12, 9) // Rx Pin: PB_12, AF9 +} ; + +//------------------------------------------------------------------------------ + +ACANFD_STM32 fdcan2 ( + FDCAN2, // CAN Peripheral base address + FDCAN1_MESSAGE_RAM_WORD_SIZE, // Message RAM start word offset + FDCAN2_MESSAGE_RAM_WORD_SIZE, // Message RAM word size + ACANFD_STM32::IRQs (FDCAN2_IT0_IRQn, FDCAN2_IT1_IRQn), // Interrupts + fdcan2_tx_pin_array, + fdcan2_rx_pin_array +) ; + +//------------------------------------------------------------------------------ + +extern "C" void FDCAN2_IT0_IRQHandler (void) ; +extern "C" void FDCAN2_IT1_IRQHandler (void) ; + +//------------------------------------------------------------------------------ + +void FDCAN2_IT0_IRQHandler (void) { + fdcan2.isr0 () ; +} + +//------------------------------------------------------------------------------ + +void FDCAN2_IT1_IRQHandler (void){ + fdcan2.isr1 () ; +} + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H743ZI2-settings.h b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H743ZI2-settings.h new file mode 100644 index 0000000..55e70c7 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_NUCLEO_H743ZI2-settings.h @@ -0,0 +1,29 @@ +#pragma once + +//------------------------------------------------------------------------------ +// Settings for NUCLEO-H743ZI2 +//------------------------------------------------------------------------------ + +#define HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS (true) +static const uint32_t FDCAN_MESSAGE_RAM_WORD_SIZE = 2560 ; + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +inline uint32_t fdcanClock (void) { + if (!__HAL_RCC_FDCAN_IS_CLK_ENABLED ()) { + //--- Enable CAN clock + __HAL_RCC_FDCAN_CLK_ENABLE () ; + //--- Reset CAN peripherals + __HAL_RCC_FDCAN_FORCE_RESET () ; + __HAL_RCC_FDCAN_RELEASE_RESET () ; + //--- Select CAN clock + LL_RCC_SetFDCANClockSource (LL_RCC_FDCAN_CLKSOURCE_PLL1Q) ; + } + return HAL_RCC_GetPCLK1Freq () ; +} + +//------------------------------------------------------------------------------ diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_Settings.cpp b/lib/acanfd-stm32/src/ACANFD_STM32_Settings.cpp new file mode 100644 index 0000000..524d974 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_Settings.cpp @@ -0,0 +1,276 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ + +#include +#include + +//------------------------------------------------------------------------------ +// BIT DECOMPOSITION CONSTRAINTS +// Data bit Rate: +// - The CAN bit time may be programmed in the range of 4 to 49 time quanta. +// - The CAN time quantum may be programmed in the range of 1 to 32 GCLK_CAN periods. +// - The bit rate configured for the CAN FD data phase via DBTP must be higher or +// equal to the bit rate configured for the arbitration phase via NBTP. +// Data bit Rate: +// - The CAN bit time may be programmed in the range of 4 to 385 time quanta. +// - The CAN time quantum may be programmed in the range of 1 to 512 GCLK_CAN periods. +//------------------------------------------------------------------------------ + +static const uint32_t MIN_DATA_PS1 = 2 ; +static const uint32_t MAX_DATA_PS1 = 32 ; +static const uint32_t MIN_DATA_PS2 = 1 ; +static const uint32_t MAX_DATA_PS2 = 16 ; + +static const uint32_t MAX_DATA_SJW = MAX_DATA_PS2 ; + +static const uint32_t MIN_DATA_TQ_COUNT = 1 + MIN_DATA_PS1 + MIN_DATA_PS2 ; +static const uint32_t MAX_DATA_TQ_COUNT = 1 + MAX_DATA_PS1 + MAX_DATA_PS2 ; + +static const uint32_t MAX_TRANSCEIVER_DELAY_COMPENSATION = 127 ; + +//------------------------------------------------------------------------------ + +static const uint32_t MIN_ARBITRATION_PS1 = 2 ; +static const uint32_t MAX_ARBITRATION_PS1 = 256 ; +static const uint32_t MIN_ARBITRATION_PS2 = 1 ; +static const uint32_t MAX_ARBITRATION_PS2 = 128 ; + +static const uint32_t MAX_ARBITRATION_SJW = MAX_ARBITRATION_PS2 ; + +static const uint32_t MAX_ARBITRATION_TQ_COUNT = 1 + MAX_ARBITRATION_PS1 + MAX_ARBITRATION_PS2 ; + + +static const uint32_t MAX_BRP = 32 ; + +//------------------------------------------------------------------------------ +// CONSTRUCTORS +//------------------------------------------------------------------------------ + +ACANFD_STM32_Settings::ACANFD_STM32_Settings (const uint32_t inDesiredArbitrationBitRate, + const DataBitRateFactor inDataBitRateFactor, + const uint32_t inTolerancePPM) : +ACANFD_STM32_Settings (inDesiredArbitrationBitRate, 75, inDataBitRateFactor, 75, inTolerancePPM) { +} + +//------------------------------------------------------------------------------ + +ACANFD_STM32_Settings::ACANFD_STM32_Settings (const uint32_t inDesiredArbitrationBitRate, + const uint32_t inDesiredArbitrationSamplePoint, + const DataBitRateFactor inDataBitRateFactor, + const uint32_t inDesiredDataSamplePoint, + const uint32_t inTolerancePPM) : +mDesiredArbitrationBitRate (inDesiredArbitrationBitRate), +mDataBitRateFactor (inDataBitRateFactor) { +//---------------------------------------------- Configure CANFD bit decomposition + const uint32_t dataBitRate = inDesiredArbitrationBitRate * uint32_t (inDataBitRateFactor) ; + uint32_t dataTQCount = std::min ( + MAX_DATA_TQ_COUNT, + MAX_ARBITRATION_TQ_COUNT / uint32_t (inDataBitRateFactor) + ) ; + uint32_t smallestDifference = UINT32_MAX ; + mBitRatePrescaler = MAX_BRP ; // Setting for slowest bitrate + uint32_t bestDataTQCount = dataTQCount ; // Setting for slowest bitrate + const uint32_t FDCAN_ROOT_CLOCK_FREQUENCY = fdcanClock () ; +// Serial.print ("FDCAN_ROOT_CLOCK_FREQUENCY = ") ; +// Serial.println (FDCAN_ROOT_CLOCK_FREQUENCY) ; + uint32_t BRP = FDCAN_ROOT_CLOCK_FREQUENCY / (dataBitRate * dataTQCount) ; +//--- Loop for finding best BRP and best TQCount + while ((smallestDifference > 0) && (dataTQCount >= MIN_DATA_TQ_COUNT) && (BRP <= MAX_BRP)) { + //--- Compute error using BRP (caution: BRP should be > 0) + if (BRP > 0) { + const uint32_t difference = FDCAN_ROOT_CLOCK_FREQUENCY - dataBitRate * dataTQCount * BRP ; // difference is always >= 0 + if (difference < smallestDifference) { + smallestDifference = difference ; + mBitRatePrescaler = BRP ; + bestDataTQCount = dataTQCount ; + } + } + //--- Compute difference using BRP+1 (caution: BRP+1 should be <= MAX_BRP) + if (BRP < MAX_BRP) { + const uint32_t difference = dataBitRate * dataTQCount * (BRP + 1) - FDCAN_ROOT_CLOCK_FREQUENCY ; // difference is always >= 0 + if (difference < smallestDifference) { + smallestDifference = difference ; + mBitRatePrescaler = BRP + 1 ; + bestDataTQCount = dataTQCount ; + } + } + //--- Continue with next value of TQCount + dataTQCount -= 1 ; + BRP = FDCAN_ROOT_CLOCK_FREQUENCY / (dataBitRate * dataTQCount) ; + } +//-------------------------- Set Data segment lengthes +//--- Compute PS1 + const uint32_t dataSP = inDesiredDataSamplePoint * bestDataTQCount ; + mDataPhaseSegment1 = dataSP / 100 - 1 ; + { const uint32_t diff1 = dataSP - (mDataPhaseSegment1 + 1) * 100 ; + if (diff1 > 0) { + const uint32_t diff2 = (mDataPhaseSegment1 + 2) * 100 - dataSP ; + if (diff2 < diff1) { + mDataPhaseSegment1 += 1 ; + } + } + } + if (mDataPhaseSegment1 > MAX_DATA_PS1) { + mDataPhaseSegment1 = MAX_DATA_PS1 ; // Always 1 <= PS1 <= 32 + } +//--- Set PS2 to remaining TQCount + mDataPhaseSegment2 = bestDataTQCount - mDataPhaseSegment1 - 1 ; +//--- Adjust PS1 and PS2 if PS2 is too large + if (mDataPhaseSegment2 > MAX_DATA_PS2) { + mDataPhaseSegment1 -= mDataPhaseSegment2 - MAX_DATA_PS2 ; + mDataPhaseSegment2 = MAX_DATA_PS2 ; + } +//--- Set RJW to PS2 + mDataSJW = mDataPhaseSegment2 ; +//-------------------------- Set TDCO + mTransceiverDelayCompensation = (dataBitRate <= 1'000'000) + ? 0 + : ((mBitRatePrescaler * bestDataTQCount) / 2) + ; +//-------------------------- Set Arbitration segment lengthes + const uint32_t bestArbitrationTQCount = bestDataTQCount * uint32_t (inDataBitRateFactor) ; +//--- Compute PS1 + const uint32_t arbitrationSP = inDesiredArbitrationSamplePoint * bestArbitrationTQCount ; + mArbitrationPhaseSegment1 = arbitrationSP / 100 - 1 ; + { const uint32_t diff1 = arbitrationSP - (mArbitrationPhaseSegment1 + 1) * 100 ; + if (diff1 > 0) { + const uint32_t diff2 = (mArbitrationPhaseSegment1 + 2) * 100 - arbitrationSP ; + if (diff2 < diff1) { + mArbitrationPhaseSegment1 += 1 ; + } + } + } + if (mArbitrationPhaseSegment1 > MAX_ARBITRATION_PS1) { + mArbitrationPhaseSegment1 = MAX_ARBITRATION_PS1 ; // Always 1 <= PS1 <= 256 + } +//--- Set PS2 to remaining TQCount + mArbitrationPhaseSegment2 = uint8_t (bestArbitrationTQCount - mArbitrationPhaseSegment1 - 1) ; +//--- Adjust PS1 and PS2 if PS2 is too large + if (mArbitrationPhaseSegment2 > MAX_ARBITRATION_PS2) { + mArbitrationPhaseSegment1 -= mArbitrationPhaseSegment2 - MAX_ARBITRATION_PS2 ; + mArbitrationPhaseSegment2 = MAX_DATA_PS2 ; + } +//--- Set RJW to PS2 + mArbitrationSJW = mArbitrationPhaseSegment2 ; +//-------------------------- Final check of the configuration + const uint32_t W = bestArbitrationTQCount * mDesiredArbitrationBitRate * mBitRatePrescaler ; + const uint64_t diff = (FDCAN_ROOT_CLOCK_FREQUENCY > W) ? (FDCAN_ROOT_CLOCK_FREQUENCY - W) : (W - FDCAN_ROOT_CLOCK_FREQUENCY) ; + const uint64_t ppm = uint64_t (1000 * 1000) ; + mBitSettingOk = (diff * ppm) <= (uint64_t (W) * inTolerancePPM) ; +} ; + +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32_Settings::actualArbitrationBitRate (void) const { + const uint32_t FDCAN_ROOT_CLOCK_FREQUENCY = fdcanClock () ; + const uint32_t TQCount = 1 /* Sync Seg */ + mArbitrationPhaseSegment1 + mArbitrationPhaseSegment2 ; + return FDCAN_ROOT_CLOCK_FREQUENCY / (mBitRatePrescaler * TQCount) ; +} + +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32_Settings::actualDataBitRate (void) const { + const uint32_t FDCAN_ROOT_CLOCK_FREQUENCY = fdcanClock () ; + const uint32_t TQCount = 1 /* Sync Seg */ + mDataPhaseSegment1 + mDataPhaseSegment2 ; + return FDCAN_ROOT_CLOCK_FREQUENCY / (mBitRatePrescaler * TQCount) ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_Settings::exactArbitrationBitRate (void) const { + const uint32_t FDCAN_ROOT_CLOCK_FREQUENCY = fdcanClock () ; + const uint32_t TQCount = 1 /* Sync Seg */ + mArbitrationPhaseSegment1 + mArbitrationPhaseSegment2 ; + return FDCAN_ROOT_CLOCK_FREQUENCY == (mBitRatePrescaler * mDesiredArbitrationBitRate * TQCount) ; +} + +//------------------------------------------------------------------------------ + +bool ACANFD_STM32_Settings::exactDataBitRate (void) const { + const uint32_t FDCAN_ROOT_CLOCK_FREQUENCY = fdcanClock () ; + const uint32_t TQCount = 1 /* Sync Seg */ + mDataPhaseSegment1 + mDataPhaseSegment2 ; + return FDCAN_ROOT_CLOCK_FREQUENCY == (mBitRatePrescaler * mDesiredArbitrationBitRate * TQCount * uint32_t (mDataBitRateFactor)) ; +} + +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32_Settings::ppmFromWishedBitRate (void) const { + const uint32_t FDCAN_ROOT_CLOCK_FREQUENCY = fdcanClock () ; + const uint32_t TQCount = 1 /* Sync Seg */ + mArbitrationPhaseSegment1 + mArbitrationPhaseSegment2 ; + const uint32_t W = TQCount * mDesiredArbitrationBitRate * mBitRatePrescaler ; + const uint64_t diff = (FDCAN_ROOT_CLOCK_FREQUENCY > W) ? (FDCAN_ROOT_CLOCK_FREQUENCY - W) : (W - FDCAN_ROOT_CLOCK_FREQUENCY) ; + const uint64_t ppm = uint64_t (1000 * 1000) ; + return uint32_t ((diff * ppm) / W) ; +} + +//------------------------------------------------------------------------------ + +float ACANFD_STM32_Settings::arbitrationSamplePointFromBitStart (void) const { + const uint32_t TQCount = 1 /* Sync Seg */ + mArbitrationPhaseSegment1 + mArbitrationPhaseSegment2 ; + const uint32_t samplePoint = 1 /* Sync Seg */ + mArbitrationPhaseSegment1 ; + const float partPerCent = 100.0 ; + return (float (samplePoint) * partPerCent) / float (TQCount) ; +} + +//------------------------------------------------------------------------------ + +float ACANFD_STM32_Settings::dataSamplePointFromBitStart (void) const { + const uint32_t TQCount = 1 /* Sync Seg */ + mDataPhaseSegment1 + mDataPhaseSegment2 ; + const uint32_t samplePoint = 1 /* Sync Seg */ + mDataPhaseSegment1 ; + const float partPerCent = 100.0 ; + return (float (samplePoint) * partPerCent) / float (TQCount) ; +} + +//------------------------------------------------------------------------------ + +uint32_t ACANFD_STM32_Settings::checkBitSettingConsistency (void) const { + uint32_t errorCode = 0 ; // Means no error + if (mBitRatePrescaler == 0) { + errorCode |= kBitRatePrescalerIsZero ; + }else if (mBitRatePrescaler > MAX_BRP) { + errorCode |= kBitRatePrescalerIsGreaterThan32 ; + } + if (mArbitrationPhaseSegment1 < MIN_ARBITRATION_PS1) { + errorCode |= kArbitrationPhaseSegment1IsZero ; + }else if (mArbitrationPhaseSegment1 > MAX_ARBITRATION_PS1) { + errorCode |= kArbitrationPhaseSegment1IsGreaterThan256 ; + } + if (mArbitrationPhaseSegment2 < MIN_ARBITRATION_PS2) { + errorCode |= kArbitrationPhaseSegment2IsLowerThan2 ; + }else if (mArbitrationPhaseSegment2 > MAX_ARBITRATION_PS2) { + errorCode |= kArbitrationPhaseSegment2IsGreaterThan128 ; + } + if (mArbitrationSJW == 0) { + errorCode |= kArbitrationSJWIsZero ; + }else if (mArbitrationSJW > MAX_ARBITRATION_SJW) { + errorCode |= kArbitrationSJWIsGreaterThan128 ; + } + if (mArbitrationSJW > mArbitrationPhaseSegment2) { + errorCode |= kArbitrationSJWIsGreaterThanPhaseSegment2 ; + } + if (mDataPhaseSegment1 < MIN_DATA_PS1) { + errorCode |= kDataPhaseSegment1IsZero ; + }else if (mDataPhaseSegment1 > MAX_DATA_PS1) { + errorCode |= kDataPhaseSegment1IsGreaterThan32 ; + } + if (mDataPhaseSegment2 < MIN_DATA_PS2) { + errorCode |= kDataPhaseSegment2IsLowerThan2 ; + }else if (mDataPhaseSegment2 > MAX_DATA_PS2) { + errorCode |= kDataPhaseSegment2IsGreaterThan16 ; + } + if (mDataSJW == 0) { + errorCode |= kDataSJWIsZero ; + }else if (mDataSJW > MAX_DATA_SJW) { + errorCode |= kDataSJWIsGreaterThan16 ; + } + if (mDataSJW > mDataPhaseSegment2) { + errorCode |= kDataSJWIsGreaterThanPhaseSegment2 ; + } + if (mTransceiverDelayCompensation > MAX_TRANSCEIVER_DELAY_COMPENSATION) { + errorCode |= kTransceiverDelayCompensationIsGreaterThan127 ; + } + return errorCode ; +} + +//------------------------------------------------------------------------------ + +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_Settings.h b/lib/acanfd-stm32/src/ACANFD_STM32_Settings.h new file mode 100644 index 0000000..20c07a5 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_Settings.h @@ -0,0 +1,232 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ + +#pragma once + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +#ifdef ARDUINO_NUCLEO_H743ZI2 + #include "ACANFD_STM32_NUCLEO_H743ZI2-settings.h" +#elif defined (ARDUINO_NUCLEO_G431KB) + #include "ACANFD_STM32_NUCLEO_G431KB-settings.h" +#elif defined (ARDUINO_NUCLEO_G474RE) + #include "ACANFD_STM32_NUCLEO_G474RE-settings.h" +#elif defined (ARDUINO_WEACT_G474CE) + #include "ACANFD_STM32_WEACT_G474CE-settings.h" +#elif defined (ARDUINO_NUCLEO_G0B1RE) + #include "ACANFD_STM32_NUCLEO_G0B1RE-settings.h" +#elif defined (ARDUINO_NUCLEO_H723ZG) + #include "ACANFD_STM32_NUCLEO_H723ZG-settings.h" +#else + #error "Unhandled Nucleo Board" +#endif + +//------------------------------------------------------------------------------ + +#include +#include + +//------------------------------------------------------------------------------ + +uint32_t fdcanClock (void) ; + +//------------------------------------------------------------------------------ +// ACANFD_STM32_Settings class +//------------------------------------------------------------------------------ + +class ACANFD_STM32_Settings { + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Enumerations + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: typedef enum : uint8_t { + NORMAL_FD, + INTERNAL_LOOP_BACK, + EXTERNAL_LOOP_BACK, + BUS_MONITORING + } ModuleMode ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Constructors for a given bit rate + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: ACANFD_STM32_Settings (const uint32_t inDesiredArbitrationBitRate, + const DataBitRateFactor inDataBitRateFactor, + const uint32_t inTolerancePPM = 1000) ; + + public: ACANFD_STM32_Settings (const uint32_t inDesiredArbitrationBitRate, + const uint32_t inDesiredArbitrationSamplePoint, + const DataBitRateFactor inDataBitRateFactor, + const uint32_t inDesiredDataSamplePoint, + const uint32_t inTolerancePPM = 1000) ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Properties + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + +//--- CAN FD bit timing + public: const uint32_t mDesiredArbitrationBitRate ; // In bit/s + public: const DataBitRateFactor mDataBitRateFactor ; +//--- bitrate prescaler is common to arbitration bitrate and data bitrate + public: uint32_t mBitRatePrescaler = 32 ; // 1...32 +//--- Arbitration segments + public: uint32_t mArbitrationPhaseSegment1 = 256 ; // 1...256 + public: uint32_t mArbitrationPhaseSegment2 = 128 ; // 2...128 + public: uint32_t mArbitrationSJW = 128 ; // 1...128 +//--- Data segments + public: uint32_t mDataPhaseSegment1 = 32 ; // 1...32 + public: uint32_t mDataPhaseSegment2 = 16 ; // 2...16 + public: uint32_t mDataSJW = 16 ; // 1...16 + +//--- Transceiver Delay Compensation + public: uint32_t mTransceiverDelayCompensation = 0 ; // 0 ... 127 + + public: bool mBitSettingOk = true ; // The above configuration is correct + +//--- Module Mode + public : ModuleMode mModuleMode = NORMAL_FD ; + +//--- Driver receive FIFO Sizes + public: uint16_t mDriverReceiveFIFO0Size = 60 ; + public: uint16_t mDriverReceiveFIFO1Size = 60 ; + +//--- Remote frame reception + public: bool mDiscardReceivedStandardRemoteFrames = false ; + public: bool mDiscardReceivedExtendedRemoteFrames = false ; + +//-- Accept non matching frames ? + public: ACANFD_STM32_FilterAction mNonMatchingStandardFrameReception = ACANFD_STM32_FilterAction::FIFO0 ; + public: ACANFD_STM32_FilterAction mNonMatchingExtendedFrameReception = ACANFD_STM32_FilterAction::FIFO0 ; + public: void (*mNonMatchingStandardMessageCallBack) (const CANFDMessage & inMessage) = nullptr ; + public: void (*mNonMatchingExtendedMessageCallBack) (const CANFDMessage & inMessage) = nullptr ; + +//--- Driver transmit buffer Size + public: uint16_t mDriverTransmitFIFOSize = 10 ; + +//--- Automatic retransmission + public: bool mEnableRetransmission = true ; + +//--- TxPin is push/pull, or open collector output + public: bool mOpenCollectorOutput = false ; // false --> push / pull, true --> open collector + +//--- RxPin has internal pullup ? + public: bool mInputPullup = false ; // false --> no pull up, no pull down, true --> pullup, no pull down + +//--- Tx Pin + public: uint8_t mTxPin = 255 ; // By default, uses the first entry of Tx pin array + +//--- Rx Pin + public: uint8_t mRxPin = 255 ; // By default, uses the first entry of Rx pin array + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Accessors + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + +//--- Compute actual bitrate + public: uint32_t actualArbitrationBitRate (void) const ; + public: uint32_t actualDataBitRate (void) const ; + +//--- Exact bitrate ? + public: bool exactArbitrationBitRate (void) const ; + public: bool exactDataBitRate (void) const ; + +//--- Distance between actual bitrate and requested bitrate (in ppm, part-per-million) + public: uint32_t ppmFromWishedBitRate (void) const ; + +//--- Distance of sample point from bit start (in ppc, part-per-cent, denoted by %) + public: float arbitrationSamplePointFromBitStart (void) const ; + public: float dataSamplePointFromBitStart (void) const ; + +//--- Bit settings are consistent ? (returns 0 if ok) + public: uint32_t checkBitSettingConsistency (void) const ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Constants returned by CANBitSettingConsistency + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: static const uint32_t kBitRatePrescalerIsZero = 1U << 0 ; + public: static const uint32_t kBitRatePrescalerIsGreaterThan32 = 1U << 1 ; + public: static const uint32_t kArbitrationPhaseSegment1IsZero = 1U << 2 ; + public: static const uint32_t kArbitrationPhaseSegment1IsGreaterThan256 = 1U << 3 ; + public: static const uint32_t kArbitrationPhaseSegment2IsLowerThan2 = 1U << 4 ; + public: static const uint32_t kArbitrationPhaseSegment2IsGreaterThan128 = 1U << 5 ; + public: static const uint32_t kArbitrationSJWIsZero = 1U << 6 ; + public: static const uint32_t kArbitrationSJWIsGreaterThan128 = 1U << 7 ; + public: static const uint32_t kArbitrationSJWIsGreaterThanPhaseSegment2 = 1U << 8 ; + + public: static const uint32_t kDataPhaseSegment1IsZero = 1U << 10 ; + public: static const uint32_t kDataPhaseSegment1IsGreaterThan32 = 1U << 11 ; + public: static const uint32_t kDataPhaseSegment2IsLowerThan2 = 1U << 12 ; + public: static const uint32_t kDataPhaseSegment2IsGreaterThan16 = 1U << 13 ; + public: static const uint32_t kDataSJWIsZero = 1U << 14 ; + public: static const uint32_t kDataSJWIsGreaterThan16 = 1U << 15 ; + public: static const uint32_t kDataSJWIsGreaterThanPhaseSegment2 = 1U << 16 ; + public: static const uint32_t kTransceiverDelayCompensationIsGreaterThan127 = 1U << 17 ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + // Extension for programmable RAM section CANFD modules + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + #ifndef HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS + #error "HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS is not defined" + #endif + + #if HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS == true + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: typedef enum : uint8_t { + PAYLOAD_8_BYTES = 0, + PAYLOAD_12_BYTES = 1, + PAYLOAD_16_BYTES = 2, + PAYLOAD_20_BYTES = 3, + PAYLOAD_24_BYTES = 4, + PAYLOAD_32_BYTES = 5, + PAYLOAD_48_BYTES = 6, + PAYLOAD_64_BYTES = 7 + } Payload ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: static uint32_t wordCountForPayload (const Payload inPayload) { + const uint32_t WORD_COUNT [8] = {4, 5, 6, 7, 8, 10, 14, 18} ; + return WORD_COUNT [uint32_t (inPayload)] ; + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + public: static uint32_t frameDataByteCountForPayload (const Payload inPayload) { + return (wordCountForPayload (inPayload) - 2) * 4 ; + } + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + //--- Hardware Rx FIFO 0 + public: uint8_t mHardwareRxFIFO0Size = 20 ; // 0 ... 64 + public: Payload mHardwareRxFIFO0Payload = PAYLOAD_64_BYTES ; + + //--- Hardware Rx FIFO 1 + public: uint8_t mHardwareRxFIFO1Size = 2 ; // 0 ... 64 + public: Payload mHardwareRxFIFO1Payload = PAYLOAD_64_BYTES ; + + //--- Hardware Transmit Buffers + // Required: mHardwareTransmitTxFIFOSize + mHardwareDedicacedTxBufferCount <= 32 + public: uint8_t mHardwareTransmitTxFIFOSize = 10 ; // 1 ... 32 + public: uint8_t mHardwareDedicacedTxBufferCount = 1 ; // 0 ... 30 + public: Payload mHardwareTransmitBufferPayload = PAYLOAD_64_BYTES ; + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + #endif + + // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + +} ; + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_WEACT_G474CE-settings.h b/lib/acanfd-stm32/src/ACANFD_STM32_WEACT_G474CE-settings.h new file mode 100644 index 0000000..ac5118b --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_WEACT_G474CE-settings.h @@ -0,0 +1,30 @@ +#ifdef ACANFD +#pragma once + +//------------------------------------------------------------------------------ +// Settings for NUCLEO-G474RE +//------------------------------------------------------------------------------ + +#define HAS_PROGRAMMABLE_FDCAN_RAM_SECTIONS (false) + +//------------------------------------------------------------------------------ + +#include + +//------------------------------------------------------------------------------ + +inline uint32_t fdcanClock (void) { + if (!__HAL_RCC_FDCAN_IS_CLK_ENABLED ()) { // Is not enabled ? + //--- Enable CAN clock + __HAL_RCC_FDCAN_CLK_ENABLE () ; + //--- Reset CAN peripherals + __HAL_RCC_FDCAN_FORCE_RESET () ; + __HAL_RCC_FDCAN_RELEASE_RESET () ; + //--- Select CAN clock + LL_RCC_SetFDCANClockSource (RCC_FDCANCLKSOURCE_PCLK1) ; + } + return HAL_RCC_GetPCLK1Freq () ; +} + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/acanfd-stm32/src/ACANFD_STM32_from_cpp.h b/lib/acanfd-stm32/src/ACANFD_STM32_from_cpp.h new file mode 100644 index 0000000..a0b8808 --- /dev/null +++ b/lib/acanfd-stm32/src/ACANFD_STM32_from_cpp.h @@ -0,0 +1,25 @@ +#ifdef ACANFD +//------------------------------------------------------------------------------ + +#pragma once + +//------------------------------------------------------------------------------ + +#ifdef ARDUINO_NUCLEO_H743ZI2 + #include "ACANFD-STM32-programmable-ram-sections.h" +#elif defined (ARDUINO_NUCLEO_G431KB) + #include "ACANFD-STM32-fixed-ram-sections.h" +#elif defined (ARDUINO_NUCLEO_G474RE) + #include "ACANFD-STM32-fixed-ram-sections.h" +#elif defined (ARDUINO_WEACT_G474CE) + #include "ACANFD-STM32-fixed-ram-sections.h" +#elif defined (ARDUINO_NUCLEO_G0B1RE) + #include "ACANFD-STM32-fixed-ram-sections.h" +#elif defined (ARDUINO_NUCLEO_H723ZG) + #include "ACANFD-STM32-programmable-ram-sections.h" +#else + #error "Unhandled Nucleo Board" +#endif + +//------------------------------------------------------------------------------ +#endif \ No newline at end of file diff --git a/lib/libArduinoDroneCAN/LICENSE b/lib/libArduinoDroneCAN/LICENSE new file mode 100644 index 0000000..99bc771 --- /dev/null +++ b/lib/libArduinoDroneCAN/LICENSE @@ -0,0 +1,28 @@ +BSD 3-Clause License + +Copyright (c) 2025, Beyond Robotix + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/lib/libArduinoDroneCAN/README.md b/lib/libArduinoDroneCAN/README.md new file mode 100644 index 0000000..7e8128d --- /dev/null +++ b/lib/libArduinoDroneCAN/README.md @@ -0,0 +1,9 @@ +# ArduinoDroneCANlib +Arduino library to allow easy DroneCAN integration + +Although this repo contains the actual library code, we use https://github.com/BeyondRobotix/Arduino-DroneCAN primarily! + +See that repo for +- examples +- folder structure +- support diff --git a/lib/libArduinoDroneCAN/keywords.txt b/lib/libArduinoDroneCAN/keywords.txt new file mode 100644 index 0000000..e69de29 diff --git a/lib/libArduinoDroneCAN/library.properties b/lib/libArduinoDroneCAN/library.properties new file mode 100644 index 0000000..443fc1d --- /dev/null +++ b/lib/libArduinoDroneCAN/library.properties @@ -0,0 +1,9 @@ +name=ArduinoDroneCAN +version=1.0.0 +author=Beyond Robotix +maintainer=Beyond Robotix +sentence=Allows easy integration of DroneCAN into your Arduino projects +paragraph= +category= +url=https://github.com/BeyondRobotix/ArduinoDroneCANlib +architectures=stm32 \ No newline at end of file diff --git a/lib/libArduinoDroneCAN/src/app.cpp b/lib/libArduinoDroneCAN/src/app.cpp new file mode 100644 index 0000000..7dde42e --- /dev/null +++ b/lib/libArduinoDroneCAN/src/app.cpp @@ -0,0 +1,175 @@ + +#include "app.h" + +/* + * This does everything we need to allow the app to start from the bootloader + * If your app is bootloaderless, you don't need this. You'll need to set your program start to 0x08000000 in the linker in ldscript.ld though + */ +void app_setup() +{ +#ifndef DISABLE_APP_SETUP +#ifdef CANL431 + SCB->VTOR = 0x0800A000; + RCC_OscInitTypeDef RCC_OscInitStruct = {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {}; + + /** Configure the main internal regulator output voltage + */ + if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; + RCC_OscInitStruct.PLL.PLLM = 1; + RCC_OscInitStruct.PLL.PLLN = 10; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV7; + RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2; + RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) + { + Error_Handler(); + } + + // Reinitialize SysTick to enable the delay() function + // Configure SysTick to generate an interrupt every 1ms (SystemCoreClock / 1000) + if (SysTick_Config(SystemCoreClock / 1000)) + { + // If SysTick configuration fails, handle the error + Error_Handler(); + } + NVIC_EnableIRQ(SysTick_IRQn); +#endif // CANL431 + +#ifdef CANH7 + + SCB->VTOR = 0x0800A000; + RCC_OscInitTypeDef RCC_OscInitStruct = {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {}; + + /** Supply configuration update enable + */ +#if defined(SMPS) + /** If SMPS is available on this MCU, assume that the MCU's board is + * built to power the MCU using the SMPS since it's more efficient. + * In this case, we must configure the MCU to use DIRECT_SMPS_SUPPLY. + * + * N.B.: if the hardware configuration does not match the argument to + * HAL_PWREx_ConfigSupply(), the board will deadlock at this function call. + * This can manifest immediately or after a RESET/power cycle. + * + * Trying to flash the board at this point will result in errors such as + * "No STM32 target found". To overcome this problem, erase the MCU's flash. + * + * The following settings in STM32CubeProgrammer appear to work for this purpose: + * - Mode: Power down + * - Reset mode: Hardware reset + */ + HAL_PWREx_ConfigSupply(PWR_DIRECT_SMPS_SUPPLY); +#else + /** No SMPS available: use the internal voltage regulator (LDO). + */ + HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY); +#endif + + /** Configure the main internal regulator output voltage + */ + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0); + + while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) + { + } + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_CSI | RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_LSI; + RCC_OscInitStruct.HSIState = RCC_HSI_DIV1; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.LSIState = RCC_LSI_ON; + RCC_OscInitStruct.HSI48State = RCC_HSI48_ON; + RCC_OscInitStruct.CSIState = RCC_CSI_ON; + RCC_OscInitStruct.CSICalibrationValue = RCC_CSICALIBRATION_DEFAULT; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; + RCC_OscInitStruct.PLL.PLLM = 4; + RCC_OscInitStruct.PLL.PLLN = 60; + RCC_OscInitStruct.PLL.PLLP = 2; + RCC_OscInitStruct.PLL.PLLQ = 5; + RCC_OscInitStruct.PLL.PLLR = 2; + RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3; + RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; + RCC_OscInitStruct.PLL.PLLFRACN = 0; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2 | RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; + RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; + RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; + RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) + { + Error_Handler(); + } + + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; + + /** Initializes the peripherals clock + */ + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC | RCC_PERIPHCLK_CEC | RCC_PERIPHCLK_CKPER | RCC_PERIPHCLK_HRTIM1 | RCC_PERIPHCLK_I2C123 | RCC_PERIPHCLK_I2C4 | RCC_PERIPHCLK_LPTIM1 | RCC_PERIPHCLK_LPTIM2 | RCC_PERIPHCLK_LPTIM3 | RCC_PERIPHCLK_QSPI | RCC_PERIPHCLK_SDMMC | RCC_PERIPHCLK_USB | RCC_PERIPHCLK_SPI123 | RCC_PERIPHCLK_SPI45 | RCC_PERIPHCLK_SPI6 | RCC_PERIPHCLK_LPUART1 | RCC_PERIPHCLK_USART16 | RCC_PERIPHCLK_USART234578 | RCC_PERIPHCLK_RTC; + PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_CLKP; + PeriphClkInitStruct.CecClockSelection = RCC_CECCLKSOURCE_CSI; + PeriphClkInitStruct.CkperClockSelection = RCC_CLKPSOURCE_HSI; + PeriphClkInitStruct.Hrtim1ClockSelection = RCC_HRTIM1CLK_CPUCLK; + PeriphClkInitStruct.I2c123ClockSelection = RCC_I2C123CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.I2c4ClockSelection = RCC_I2C4CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Lptim1ClockSelection = RCC_LPTIM1CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.Lptim2ClockSelection = RCC_LPTIM2CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Lptim345ClockSelection = RCC_LPTIM345CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Lpuart1ClockSelection = RCC_LPUART1CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.QspiClockSelection = RCC_QSPICLKSOURCE_PLL; + PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSI; + PeriphClkInitStruct.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL; + PeriphClkInitStruct.Spi123ClockSelection = RCC_SPI123CLKSOURCE_PLL; + PeriphClkInitStruct.Spi45ClockSelection = RCC_SPI45CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.Spi6ClockSelection = RCC_SPI6CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Usart16ClockSelection = RCC_USART16CLKSOURCE_D2PCLK2; + PeriphClkInitStruct.Usart234578ClockSelection = RCC_USART234578CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_HSI48; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) + { + Error_Handler(); + } + +#endif // CANH7 +#endif // DISABLE_APP_SETUP +} diff --git a/lib/libArduinoDroneCAN/src/app.h b/lib/libArduinoDroneCAN/src/app.h new file mode 100644 index 0000000..8adee6f --- /dev/null +++ b/lib/libArduinoDroneCAN/src/app.h @@ -0,0 +1,18 @@ +#ifndef APP +#define APP + +extern "C" +{ + #ifdef CANL431 + #include "stm32l4xx.h" // or stm32l431xx.h if needed + #endif + #ifdef CANH7 + #include "stm32h743xx.h" + #endif +} +#include "Arduino.h" + + +void app_setup(); + +#endif // APP \ No newline at end of file diff --git a/lib/libArduinoDroneCAN/src/canH723.cpp b/lib/libArduinoDroneCAN/src/canH723.cpp new file mode 100644 index 0000000..52787cd --- /dev/null +++ b/lib/libArduinoDroneCAN/src/canH723.cpp @@ -0,0 +1,151 @@ +#ifdef ARDUINO_NUCLEO_H723ZG +#include "Arduino.h" +#include "canH723.h" + +// --- Message RAM Configuration --- +// These values MUST be defined as macros before including the ACANFD_STM32.h header. +// The library's internal headers use these macros to configure the FDCAN peripherals. +#define FDCAN1_MESSAGE_RAM_WORD_SIZE 800 +#define FDCAN2_MESSAGE_RAM_WORD_SIZE 800 +#define FDCAN3_MESSAGE_RAM_WORD_SIZE 800 + +// The ACANFD_STM32 library requires this main header to be included in one .cpp file +// to instantiate the CAN objects (fdcan1, fdcan2, fdcan3). +#include + +// --- FDCAN peripheral handles --- +// An array to easily select the CAN interface at runtime. +static ACANFD_STM32* can_ifaces[] = {&fdcan1, &fdcan2, &fdcan3}; +// Pointer to the currently active CAN interface. +static ACANFD_STM32* active_can_iface = nullptr; +// Maximum number of supported CAN interfaces on the H723ZG. +const int MAX_CAN_IFACES = sizeof(can_ifaces) / sizeof(can_ifaces[0]); + + +/** + * @brief Initializes the FDCAN controller. + * + * @param bitrate The desired bitrate from the BITRATE enum. + * @param can_iface_index Selects the CAN interface. 0 for FDCAN1, 1 for FDCAN2, 2 for FDCAN3. + * @return true on success, false on failure. + */ +bool CANInit(BITRATE bitrate, int can_iface_index) { + can_iface_index = 0; + if (can_iface_index < 0 || can_iface_index >= MAX_CAN_IFACES) { + return false; + } + + active_can_iface = can_ifaces[can_iface_index]; + + uint32_t nominal_bitrate = 0; + switch (bitrate) { + case CAN_50KBPS: + nominal_bitrate = 50 * 1000; + break; + case CAN_100KBPS: + nominal_bitrate = 100 * 1000; + break; + case CAN_125KBPS: + nominal_bitrate = 125 * 1000; + break; + case CAN_250KBPS: + nominal_bitrate = 250 * 1000; + break; + case CAN_500KBPS: + nominal_bitrate = 500 * 1000; + break; + case CAN_1000KBPS: + default: + nominal_bitrate = 1000 * 1000; + break; + } + + // Configure settings for CAN FD with a 4x data rate multiplier. + // This is a common configuration for ArduPilot UAVCAN nodes. + ACANFD_STM32_Settings settings(nominal_bitrate, DataBitRateFactor::x4); + settings.mModuleMode = ACANFD_STM32_Settings::NORMAL_FD; + + // Start the FDCAN peripheral. The return code is 0 on success. + settings.mTxPin = PD_1; + settings.mRxPin = PD_0; + uint32_t error_code = active_can_iface->beginFD(settings); + + return error_code == 0; +} + +/** + * @brief Sends a CAN message using the initialized FDCAN peripheral. + * + * This function converts a CanardCANFrame to the library's CANFDMessage format. + * It forces all outgoing messages to use the Extended ID format for Ardupilot compatibility. + * + * @param tx_msg A pointer to the CanardCANFrame to be sent. + */ +void CANSend(const CanardCANFrame *tx_msg) { + if (!active_can_iface || !tx_msg) { + return; + } + + CANFDMessage message; + // ArduPilot's CAN drivers exclusively use extended frames. + message.ext = true; + message.id = tx_msg->id & CANARD_CAN_EXT_ID_MASK; + message.len = tx_msg->data_len; + message.type = CANFDMessage::CAN_DATA; + + memcpy(message.data, tx_msg->data, tx_msg->data_len); + + // Attempt to send the message. This is non-blocking. + uint32_t send_status = active_can_iface->tryToSendReturnStatusFD(message); + if (send_status != 0) { + Serial.println("Failed to send CAN message"); + } +} + + +/** + * @brief Receives a CAN message if one is available. + * + * This function checks the RX FIFO 0, and if a message is present, it populates + * the provided CanardCANFrame struct. + * + * @param rx_msg A pointer to a CanardCANFrame that will be filled with the received data. + */ +void CANReceive(CanardCANFrame *rx_msg) { + if (!active_can_iface || !rx_msg) { + return; + } + + CANFDMessage message; + // Check RX FIFO 0 for a new message. + if (active_can_iface->receiveFD0(message)) { + // Populate the Canard frame from the received library message. + + rx_msg->id = (0x1FFFFFFFU & message.id) & 0x1FFFFFFFU; + rx_msg->id |= 1U << 31; // https://github.com/ArduPilot/ardupilot/blob/4d31a7320a1d2c38e2d742ae63c34f914febaa8f/libraries/AP_HAL_ChibiOS/CanIface.cpp#L570 + + rx_msg->data_len = message.len; + + memcpy(rx_msg->data, message.data, message.len); + } else { + // No message received, set ID to 0 to indicate an invalid/empty frame. + rx_msg->id = 0; + rx_msg->data_len = 0; + } +} + +/** + * @brief Checks for available CAN messages. + * + * @return The number of messages pending in the driver's software receive FIFO 0. + */ +uint8_t CANMsgAvail(void) { + if (!active_can_iface) { + return 0; + } + // Return the number of messages available in RX FIFO 0. + return active_can_iface->availableFD0(); +} + +#endif // ARDUINO_NUCLEO_H723ZG + diff --git a/lib/libArduinoDroneCAN/src/canH723.h b/lib/libArduinoDroneCAN/src/canH723.h new file mode 100644 index 0000000..06031b5 --- /dev/null +++ b/lib/libArduinoDroneCAN/src/canH723.h @@ -0,0 +1,51 @@ +#ifdef ARDUINO_NUCLEO_H723ZG +#ifndef CAN_DRIVER_H7_ +#define CAN_DRIVER_H7_ + +#include +#include + +/** + * @brief Defines standard CAN bitrates. + * This enum provides a list of common bitrates for CAN communication. + */ +enum BITRATE +{ + CAN_50KBPS, + CAN_100KBPS, + CAN_125KBPS, + CAN_250KBPS, + CAN_500KBPS, + CAN_1000KBPS +}; + +/** + * @brief Initializes the FDCAN controller with a specified bitrate and interface. + * @param bitrate The desired communication speed from the BITRATE enum. + * @param can_iface_index Selects the hardware CAN interface (0 for FDCAN1, 1 for FDCAN2, etc.). + * @return Returns true if initialization is successful, false otherwise. + */ +bool CANInit(BITRATE bitrate, int can_iface_index); + +/** + * @brief Sends a CAN frame. + * This function queues a CAN frame for transmission. It's non-blocking. + * @param tx_msg A pointer to the CanardCANFrame structure containing the message to be sent. + */ +void CANSend(const CanardCANFrame *tx_msg); + +/** + * @brief Receives a CAN frame. + * If a message is available in the FIFO, this function populates the provided struct with its data. + * @param rx_msg A pointer to a CanardCANFrame structure to be filled with the received message data. + */ +void CANReceive(CanardCANFrame *rx_msg); + +/** + * @brief Checks for available CAN messages in the receive buffer. + * @return The number of messages currently pending in the RX FIFO. + */ +uint8_t CANMsgAvail(void); + +#endif // CAN_DRIVER_H7_ +#endif // DARDUINO_NUCLEO_H723ZG diff --git a/lib/libArduinoDroneCAN/src/canH743.cpp b/lib/libArduinoDroneCAN/src/canH743.cpp new file mode 100644 index 0000000..eed7ade --- /dev/null +++ b/lib/libArduinoDroneCAN/src/canH743.cpp @@ -0,0 +1,155 @@ +#ifdef CANH743 +#include "Arduino.h" +#include "canH743.h" + +// This implementation relies on the ACANFD_STM32 library. +// It includes the board-specific object and settings files you provided. +// Please ensure the ACANFD_STM32 library is available in your project's include path. +static const uint32_t FDCAN1_MESSAGE_RAM_WORD_SIZE = 2560; +static const uint32_t FDCAN2_MESSAGE_RAM_WORD_SIZE = 2560; // FDCAN2 not used + +// A static pointer to the active CAN driver instance (FDCAN1 or FDCAN2). +// This allows the C-style API functions to interact with the C++ CAN object. +static ACANFD_STM32 *gCANDriver = nullptr; + +// This mask is used to extract the 29-bit extended ID from a canard frame ID. +#define CAN_EXT_ID_MASK 0x1FFFFFFFU + + +/** + * @brief Initializes the FDCAN controller. + * + * @param bitrate The desired bitrate from the BITRATE enum. + * @param can_iface_index Selects the CAN interface. 0 for FDCAN1, 1 for FDCAN2 on the NUCLEO-H743ZI2. + * @return true on success, false on failure. + */ +bool CANInit(BITRATE bitrate, int can_iface_index) { + + // Select the FDCAN peripheral instance based on the provided index. + if (can_iface_index == 2) { + gCANDriver = &fdcan1; + } else if (can_iface_index == 1) { + gCANDriver = &fdcan2; + } else { + // If an invalid index is provided, fail initialization. + return false; + } + + // Determine the nominal bitrate from the enum. + uint32_t desiredBitrate = 1000 * 1000; // Default to 1Mbit/s + switch (bitrate) { + case CAN_50KBPS: desiredBitrate = 50 * 1000; break; + case CAN_100KBPS: desiredBitrate = 100 * 1000; break; + case CAN_125KBPS: desiredBitrate = 125 * 1000; break; + case CAN_250KBPS: desiredBitrate = 250 * 1000; break; + case CAN_500KBPS: desiredBitrate = 500 * 1000; break; + case CAN_1000KBPS: desiredBitrate = 1000 * 1000; break; + } + + // Configure FDCAN settings. We use the constructor that takes the arbitration bitrate + // and a data bitrate factor. Setting the factor to x1 disables CAN-FD's bitrate switching, + // ensuring we operate in classic CAN mode, matching the L431 driver. + ACANFD_STM32_Settings settings(desiredBitrate, DataBitRateFactor::x1); + + settings.mTxPin = PD_1; + settings.mRxPin = PH_14; + + // The ACANFD library's beginFD() method configures and starts the peripheral. + // It returns 0 on success. + const uint32_t status = gCANDriver->beginFD(settings); + + if (status == 0) { + return true; // Initialization successful + } else { + gCANDriver = nullptr; // Nullify pointer on failure + return false; // Initialization failed + } +} + +/** + * @brief Sends a CAN message using the initialized FDCAN peripheral. + * + * This function mimics the behavior of the canL431 driver, forcing all + * outgoing messages to use the Extended ID format for Ardupilot compatibility. + * + * @param tx_msg A pointer to the CanardCANFrame to be sent. + */ +void CANSend(const CanardCANFrame *tx_msg) { + if (!gCANDriver) { + return; // Do nothing if the driver is not initialized. + } + + CANFDMessage message; + + // Force Extended ID format, matching the canL431 driver's behavior. + message.ext = true; + message.id = tx_msg->id & CAN_EXT_ID_MASK; // Mask to get the 29-bit ID. + + // Copy the data payload and length. + message.len = tx_msg->data_len; + for (uint8_t i = 0; i < message.len; i++) { + message.data[i] = tx_msg->data[i]; + } + + // Specify a classic CAN data frame. + message.type = CANFDMessage::CAN_DATA; + + // Use the non-blocking send method from the library. + uint32_t ret = gCANDriver->tryToSendReturnStatusFD(message); + if (ret != 0) + { + Serial.println(ret); + } +} + +/** + * @brief Receives a CAN message if one is available. + * + * This function checks the RX FIFO, and if a message is present, it populates + * the provided CanardCANFrame struct. It only processes extended frames to maintain + * compatibility with the supplied L431 driver's logic. + * + * @param rx_msg A pointer to a CanardCANFrame that will be filled with the received data. + */ +void CANReceive(CanardCANFrame *rx_msg) { + if (!gCANDriver || !gCANDriver->availableFD0()) { + return; // Do nothing if driver is not initialized or FIFO is empty. + } + + CANFDMessage message; + if (gCANDriver->receiveFD0(message)) { + // Only process extended frames, as implied by the L431 driver logic. + if (message.ext) { + // Populate the CanardCANFrame for the application. + // Set the EFF flag (bit 31) for canard/Ardupilot compatibility. + rx_msg->id = message.id | CANARD_CAN_FRAME_EFF; + rx_msg->data_len = message.len; + + for (int i = 0; i < rx_msg->data_len; i++) { + rx_msg->data[i] = message.data[i]; + } + + // Set the canfd flag if CAN-FD is enabled in canard build configuration. + #if CANARD_ENABLE_CANFD + rx_msg->canfd = (message.type == CANFDMessage::CANFD_WITH_BIT_RATE_SWITCH || message.type == CANFDMessage::CANFD_NO_BIT_RATE_SWITCH); + #endif + } + // Standard frames are ignored. + } +} + +/** + * @brief Checks for available CAN messages. + * + * @return The number of messages pending in the driver's software receive FIFO 0. + */ +uint8_t CANMsgAvail(void) { + if (!gCANDriver) { + return 0; + } + // Return the fill level of the driver's receive FIFO. + return gCANDriver->driverReceiveFIFO0Count(); +} + +#endif // CANH7 + diff --git a/lib/libArduinoDroneCAN/src/canH743.h b/lib/libArduinoDroneCAN/src/canH743.h new file mode 100644 index 0000000..4c4d8f2 --- /dev/null +++ b/lib/libArduinoDroneCAN/src/canH743.h @@ -0,0 +1,51 @@ +#ifdef CANH743 +#ifndef CAN_DRIVER_H7_ +#define CAN_DRIVER_H7_ + +#include +#include + +/** + * @brief Defines standard CAN bitrates. + * This enum provides a list of common bitrates for CAN communication. + */ +enum BITRATE +{ + CAN_50KBPS, + CAN_100KBPS, + CAN_125KBPS, + CAN_250KBPS, + CAN_500KBPS, + CAN_1000KBPS +}; + +/** + * @brief Initializes the FDCAN controller with a specified bitrate and interface. + * @param bitrate The desired communication speed from the BITRATE enum. + * @param can_iface_index Selects the hardware CAN interface (0 for FDCAN1, 1 for FDCAN2, etc.). + * @return Returns true if initialization is successful, false otherwise. + */ +bool CANInit(BITRATE bitrate, int can_iface_index); + +/** + * @brief Sends a CAN frame. + * This function queues a CAN frame for transmission. It's non-blocking. + * @param tx_msg A pointer to the CanardCANFrame structure containing the message to be sent. + */ +void CANSend(const CanardCANFrame *tx_msg); + +/** + * @brief Receives a CAN frame. + * If a message is available in the FIFO, this function populates the provided struct with its data. + * @param rx_msg A pointer to a CanardCANFrame structure to be filled with the received message data. + */ +void CANReceive(CanardCANFrame *rx_msg); + +/** + * @brief Checks for available CAN messages in the receive buffer. + * @return The number of messages currently pending in the RX FIFO. + */ +uint8_t CANMsgAvail(void); + +#endif // CAN_DRIVER_H7_ +#endif // CANH7 diff --git a/lib/libArduinoDroneCAN/src/canL431.cpp b/lib/libArduinoDroneCAN/src/canL431.cpp new file mode 100644 index 0000000..e49d3b1 --- /dev/null +++ b/lib/libArduinoDroneCAN/src/canL431.cpp @@ -0,0 +1,397 @@ +#ifdef CANL431 +#include "Arduino.h" +#include "canL431.h" + +#define DEBUG 0 +#define AF3 0x03 +#define AF8 0x08 +#define AF9 0x09 +#define AF10 0x0A + +CAN_bit_timing_config_t can_configs[6] = {{2, 13, 100}, {2, 13, 50}, {2, 13, 40}, {2, 13, 20}, {2, 13, 10}, {1, 8, 8}}; + +/** + * Print registers. + */ +void printRegister(char *buf, uint32_t reg) +{ + if (DEBUG == 0) + return; + Serial.print(buf); + Serial.print("0x"); + Serial.print(reg, HEX); + Serial.println(); +} + +/** + * Initializes the CAN GPIO registers. + * + * @params: addr - Specified GPIO register address. + * @params: index - Specified GPIO index. + * @params: afry - Specified Alternative function selection AF0-AF15. + * @params: speed - Specified OSPEEDR register value.(Optional) + * + */ +void CANSetGpio(GPIO_TypeDef *addr, uint8_t index, uint8_t afry, uint8_t speed = 3) +{ + uint8_t _index2 = index * 2; + uint8_t _index4 = index * 4; + uint8_t ofs = 0; + uint8_t setting; + + if (index > 7) + { + _index4 = (index - 8) * 4; + ofs = 1; + } + + uint32_t mask; + // printRegister("GPIO_AFR(b)=", addr->AFR[1]); + mask = 0xF << _index4; + addr->AFR[ofs] &= ~mask; // Reset alternate function + // setting = 0x9; // AF9 + setting = afry; // Alternative function selection + mask = setting << _index4; + addr->AFR[ofs] |= mask; // Set alternate function + // printRegister("GPIO_AFR(a)=", addr->AFR[1]); + + // printRegister("GPIO_MODER(b)=", addr->MODER); + mask = 0x3 << _index2; + addr->MODER &= ~mask; // Reset mode + setting = 0x2; // Alternate function mode + mask = setting << _index2; + addr->MODER |= mask; // Set mode + // printRegister("GPIO_MODER(a)=", addr->MODER); + + // printRegister("GPIO_OSPEEDR(b)=", addr->OSPEEDR); + mask = 0x3 << _index2; + addr->OSPEEDR &= ~mask; // Reset speed + setting = speed; + mask = setting << _index2; + addr->OSPEEDR |= mask; // Set speed + // printRegister("GPIO_OSPEEDR(a)=", addr->OSPEEDR); + + // printRegister("GPIO_OTYPER(b)=", addr->OTYPER); + mask = 0x1 << index; + addr->OTYPER &= ~mask; // Reset Output push-pull + // printRegister("GPIO_OTYPER(a)=", addr->OTYPER); + + // printRegister("GPIO_PUPDR(b)=", addr->PUPDR); + mask = 0x3 << _index2; + addr->PUPDR &= ~mask; // Reset port pull-up/pull-down + // printRegister("GPIO_PUPDR(a)=", addr->PUPDR); +} + +/** + * Initializes the CAN filter registers. + * + * The bxCAN provides up to 14 scalable/configurable identifier filter banks, for selecting the incoming messages, that the software needs and discarding the others. + * + * @preconditions - This register can be written only when the filter initialization mode is set (FINIT=1) in the CAN_FMR register. + * @params: index - Specified filter index. index 27:14 are available in connectivity line devices only. + * @params: scale - Select filter scale. + * 0: Dual 16-bit scale configuration + * 1: Single 32-bit scale configuration + * @params: mode - Select filter mode. + * 0: Two 32-bit registers of filter bank x are in Identifier Mask mode + * 1: Two 32-bit registers of filter bank x are in Identifier List mode + * @params: fifo - Select filter assigned. + * 0: Filter assigned to FIFO 0 + * 1: Filter assigned to FIFO 1 + * @params: bank1 - Filter bank register 1 + * @params: bank2 - Filter bank register 2 + * + */ +void CANSetFilter(uint8_t index, uint8_t scale, uint8_t mode, uint8_t fifo, uint32_t bank1, uint32_t bank2) +{ + if (index > 13) + return; + + CAN1->FA1R &= ~(0x1UL << index); // Deactivate filter + + if (scale == 0) + { + CAN1->FS1R &= ~(0x1UL << index); // Set filter to Dual 16-bit scale configuration + } + else + { + CAN1->FS1R |= (0x1UL << index); // Set filter to single 32 bit configuration + } + if (mode == 0) + { + CAN1->FM1R &= ~(0x1UL << index); // Set filter to Mask mode + } + else + { + CAN1->FM1R |= (0x1UL << index); // Set filter to List mode + } + + if (fifo == 0) + { + CAN1->FFA1R &= ~(0x1UL << index); // Set filter assigned to FIFO 0 + } + else + { + CAN1->FFA1R |= (0x1UL << index); // Set filter assigned to FIFO 1 + } + + CAN1->sFilterRegister[index].FR1 = bank1; // Set filter bank registers1 + CAN1->sFilterRegister[index].FR2 = bank2; // Set filter bank registers2 + + CAN1->FA1R |= (0x1UL << index); // Activate filter +} + +/** + * Initializes the CAN controller with specified bit rate. + * + * @params: bitrate - Specified bitrate. If this value is not one of the defined constants, bit rate will be defaulted to 125KBS + * @params: remap - Select CAN port. + * =0:CAN_RX mapped to PA11, CAN_TX mapped to PA12 + * =1:CAN_RX mapped to PB5 , CAN_TX mapped to PB6 + * =2:CAN_RX mapped to PB8 , CAN_TX mapped to PB9 + * =3:CAN_RX mapped to PB12, CAN_TX mapped to PB13 + * + */ +bool CANInit(BITRATE bitrate, int remap) +{ + // Reference manual + // https://www.st.com/content/ccc/resource/technical/document/reference_manual/4a/19/6e/18/9d/92/43/32/DM00043574.pdf/files/DM00043574.pdf/jcr:content/translations/en.DM00043574.pdf + + CAN1->MCR = CAN_MCR_RESET; + + RCC->APB1ENR1 |= 0x2000000UL; // Enable CAN clock + + if (remap == 0) + { + RCC->AHB2ENR |= 0x1UL; // Enable GPIOA clock + CANSetGpio(GPIOA, 11, AF9); // Set PA11 to AF9 + CANSetGpio(GPIOA, 12, AF9); // Set PA12 to AF9 + } + + if (remap == 1) + { + RCC->AHB2ENR |= 0x2UL; // Enable GPIOB clock + CANSetGpio(GPIOB, 5, AF3); // Set PB5 to AF3 + CANSetGpio(GPIOB, 6, AF8); // Set PB6 to AF8 + } + + if (remap == 2) + { + RCC->AHB2ENR |= 0x2UL; // Enable GPIOB clock + CANSetGpio(GPIOB, 8, AF9); // Set PB8 to AF9 + CANSetGpio(GPIOB, 9, AF9); // Set PB9 to AF9 + } + + if (remap == 3) + { + RCC->AHB2ENR |= 0x2UL; // Enable GPIOB clock + CANSetGpio(GPIOB, 12, AF10); // Set PB12 to AF10 + CANSetGpio(GPIOB, 13, AF10); // Set PB13 to AF10 + } + + CAN1->MCR |= 0x1UL; // Set CAN to Initialization mode + while (!(CAN1->MSR & 0x1UL)) + ; // Wait for Initialization mode + + // CAN1->MCR = 0x51UL; // Hardware initialization(No automatic retransmission) + CAN1->MCR = 0x41UL; // Hardware initialization(With automatic retransmission) + + // Set bit rates + // CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x1FF)); + // CAN1->BTR |= (((can_configs[bitrate].TS2-1) & 0x07) << 20) | (((can_configs[bitrate].TS1-1) & 0x0F) << 16) | ((can_configs[bitrate].BRP-1) & 0x1FF); + CAN1->BTR &= ~(((0x03) << 24) | ((0x07) << 20) | ((0x0F) << 16) | (0x3FF)); + CAN1->BTR |= (((can_configs[bitrate].TS2 - 1) & 0x07) << 20) | (((can_configs[bitrate].TS1 - 1) & 0x0F) << 16) | ((can_configs[bitrate].BRP - 1) & 0x3FF); + // printRegister("CAN1->BTR=", CAN1->BTR); + + // Configure Filters to default values + CAN1->FMR |= 0x1UL; // Set to filter initialization mode + + // Set fileter 0 + // Single 32-bit scale configuration + // Two 32-bit registers of filter bank x are in Identifier Mask mode + // Filter assigned to FIFO 0 + // Filter bank register to all 0 + CANSetFilter(0, 1, 0, 0, 0x0UL, 0x0UL); + + CAN1->FMR &= ~(0x1UL); // Deactivate initialization mode + + uint16_t TimeoutMilliseconds = 1000; + bool can1 = false; + CAN1->MCR &= ~(0x1UL); // Require CAN1 to normal mode + + // Wait for normal mode + // If the connection is not correct, it will not return to normal mode. + for (uint16_t wait_ack = 0; wait_ack < TimeoutMilliseconds; wait_ack++) + { + if ((CAN1->MSR & 0x1UL) == 0) + { + can1 = true; + break; + } + // delay(1); + CAN1->MCR &= ~(0x1UL); // Require CAN1 to normal mode + Serial.println("CAN normal mode attempt again"); + } + if (can1) + { + Serial.println("CAN1 initialize ok"); + } + else + { + Serial.println("CAN1 initialize fail!!"); + return false; + } + return true; +} + +#define STM32_CAN_TIR_TXRQ (1U << 0U) // Bit 0: Transmit Mailbox Request +#define STM32_CAN_RIR_RTR (1U << 1) // Bit 1: Remote Transmission Request +#define STM32_CAN_RIR_IDE (1U << 2) // Bit 2: Identifier Extension +#define STM32_CAN_TIR_RTR (1U << 1U) // Bit 1: Remote Transmission Request +#define STM32_CAN_TIR_IDE (1U << 2U) // Bit 2: Identifier Extension + +#define CAN_EXT_ID_MASK 0x1FFFFFFFU +#define CAN_STD_ID_MASK 0x000007FFU + +uint8_t dlcToDataLength(uint8_t dlc) +{ + /* + Data Length Code 9 10 11 12 13 14 15 + Number of data bytes 12 16 20 24 32 48 64 + */ + if (dlc <= 8) + { + return dlc; + } + else if (dlc == 9) + { + return 12; + } + else if (dlc == 10) + { + return 16; + } + else if (dlc == 11) + { + return 20; + } + else if (dlc == 12) + { + return 24; + } + else if (dlc == 13) + { + return 32; + } + else if (dlc == 14) + { + return 48; + } + return 64; +} + +/** + * Decodes CAN messages from the data registers and populates a + * CAN message struct with the data fields. + * + * @preconditions - A valid CAN message is received + * @params CAN_rx_msg - CAN message structure for reception + * + */ +void CANReceive(CanardCANFrame *CAN_rx_msg) +{ + uint32_t id = CAN1->sFIFOMailBox[0].RIR; + + // if ((id & STM32_CAN_RIR_IDE) == 0) + // { // Standard frame format + // CAN_rx_msg->id = (CAN_STD_ID_MASK & (id >> 21)); + // } + // else + // { // Extended frame format + CAN_rx_msg->id = (CAN_EXT_ID_MASK & (id >> 3)); + CAN_rx_msg->id |= 1U << 31; // https://github.com/ArduPilot/ardupilot/blob/4d31a7320a1d2c38e2d742ae63c34f914febaa8f/libraries/AP_HAL_ChibiOS/CanIface.cpp#L570 + // } + + CAN_rx_msg->data_len = dlcToDataLength((CAN1->sFIFOMailBox[0].RDTR) & 0xFUL); + + CAN_rx_msg->data[0] = uint8_t(0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 0)); + CAN_rx_msg->data[1] = uint8_t(0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 8)); + CAN_rx_msg->data[2] = uint8_t(0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 16)); + CAN_rx_msg->data[3] = uint8_t(0xFFUL & (CAN1->sFIFOMailBox[0].RDLR >> 24)); + CAN_rx_msg->data[4] = uint8_t(0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 0)); + CAN_rx_msg->data[5] = uint8_t(0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 8)); + CAN_rx_msg->data[6] = uint8_t(0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 16)); + CAN_rx_msg->data[7] = uint8_t(0xFFUL & (CAN1->sFIFOMailBox[0].RDHR >> 24)); + + // Release FIFO 0 output mailbox. + // Make the next incoming message available. + CAN1->RF0R |= 0x20UL; +} + +/** + * Encodes CAN messages using the CAN message struct and populates the + * data registers with the sent. + * + * @params CAN_tx_msg - CAN message structure for transmission + * + */ +void CANSend(const CanardCANFrame *CAN_tx_msg) +{ + volatile int count = 0; + + uint32_t out = 0; + // if ((CAN_tx_msg->id & STM32_CAN_RIR_IDE) == 0) + // { + // // standard frame format + // out = ((CAN_tx_msg->id & CAN_STD_ID_MASK) << 21U); + // } + // else + // { + // extended frame format + // force extended frame format + out = ((CAN_tx_msg->id & CAN_EXT_ID_MASK) << 3U) | STM32_CAN_TIR_IDE; + // } + + CAN1->sTxMailBox[0].TDTR &= ~(0xF); + CAN1->sTxMailBox[0].TDTR |= CAN_tx_msg->data_len & 0xFUL; + + CAN1->sTxMailBox[0].TDLR = (((uint32_t)CAN_tx_msg->data[3] << 24) | + ((uint32_t)CAN_tx_msg->data[2] << 16) | + ((uint32_t)CAN_tx_msg->data[1] << 8) | + ((uint32_t)CAN_tx_msg->data[0])); + CAN1->sTxMailBox[0].TDHR = (((uint32_t)CAN_tx_msg->data[7] << 24) | + ((uint32_t)CAN_tx_msg->data[6] << 16) | + ((uint32_t)CAN_tx_msg->data[5] << 8) | + ((uint32_t)CAN_tx_msg->data[4])); + + // Send Go + CAN1->sTxMailBox[0].TIR = out | STM32_CAN_TIR_TXRQ; + + // Wait until the mailbox is empty + while (CAN1->sTxMailBox[0].TIR & 0x1UL && count++ < 1000000) + ; + + // The mailbox don't becomes empty while loop + if (CAN1->sTxMailBox[0].TIR & 0x1UL) + { + Serial.print("Send Fail "); + Serial.print(CAN1->ESR); + Serial.print(" "); + Serial.print(CAN1->MSR); + Serial.print(" "); + Serial.println(CAN1->TSR); + } +} + +/** + * Returns whether there are CAN messages available. + * + * @returns If pending CAN messages are in the CAN controller + * + */ +uint8_t CANMsgAvail(void) +{ + // Check for pending FIFO 0 messages + return CAN1->RF0R & 0x3UL; +} +#endif \ No newline at end of file diff --git a/lib/libArduinoDroneCAN/src/canL431.h b/lib/libArduinoDroneCAN/src/canL431.h new file mode 100644 index 0000000..7e4fc27 --- /dev/null +++ b/lib/libArduinoDroneCAN/src/canL431.h @@ -0,0 +1,57 @@ +#ifdef CANL431 +#ifndef CAN_DRIVER +#define CAN_DRIVER +#include + +enum BITRATE +{ + CAN_50KBPS, + CAN_100KBPS, + CAN_125KBPS, + CAN_250KBPS, + CAN_500KBPS, + CAN_1000KBPS +}; + +// struct CAN_msg_t +// { +// uint32_t id; /* 29 bit identifier */ +// uint8_t data[8]; /* Data field */ +// uint8_t len; /* Length of data field in bytes */ +// uint8_t ch; /* Object channel(Not use) */ +// uint8_t format; /* 0 - STANDARD, 1- EXTENDED IDENTIFIER */ +// uint8_t type; /* 0 - DATA FRAME, 1 - REMOTE FRAME */ +// }; + +struct CAN_bit_timing_config_t +{ + uint8_t TS2; + uint8_t TS1; + uint8_t BRP; +}; + +/* Symbolic names for formats of CAN message */ +enum +{ + STANDARD_FORMAT = 0, + EXTENDED_FORMAT +} CAN_FORMAT; + +/* Symbolic names for type of CAN message */ +enum +{ + DATA_FRAME = 0, + REMOTE_FRAME +} CAN_FRAME; + +extern CAN_bit_timing_config_t can_configs[6]; + +uint8_t CANMsgAvail(void); +void CANSend(const CanardCANFrame *CAN_tx_msg); +void CANReceive(CanardCANFrame *CAN_rx_msg); +void CANSetFilters(uint16_t *ids, uint8_t num); +void CANSetFilter(uint16_t id); +bool CANInit(BITRATE bitrate, int remap); + +#endif // CAN_DRIVER +#endif // CANL431 \ No newline at end of file diff --git a/lib/libArduinoDroneCAN/src/dronecan.cpp b/lib/libArduinoDroneCAN/src/dronecan.cpp new file mode 100644 index 0000000..758277e --- /dev/null +++ b/lib/libArduinoDroneCAN/src/dronecan.cpp @@ -0,0 +1,935 @@ +#include + +/* + Kick off the CAN driver, Canard, do some parameter management +*/ +void DroneCAN::init(CanardOnTransferReception onTransferReceived, + CanardShouldAcceptTransfer shouldAcceptTransfer, + const std::vector ¶m_list, + const char *name) +{ + // start our CAN driver + CANInit(CAN_1000KBPS, 2); + + strncpy(this->node_name, name, sizeof(this->node_name)); + + IWatchdog.reload(); + + canardInit(&canard, + memory_pool, + sizeof(memory_pool), + onTransferReceived, + shouldAcceptTransfer, + NULL); + + if (this->node_id > 0) + { + canardSetLocalNodeID(&this->canard, node_id); + } + else + { + Serial.println("Waiting for DNA node allocation"); + } + + // initialise the internal LED + // pinMode(19, OUTPUT); + + IWatchdog.reload(); + + // put our user params into memory + this->set_parameters(param_list); + + // get the parameters in the EEPROM + this->read_parameter_memory(); + + while(canardGetLocalNodeID(&this->canard) == CANARD_BROADCAST_NODE_ID) + { + this->cycle(); + IWatchdog.reload(); + // digitalWrite(19, this->led_state); + this->led_state = !this->led_state; + } +} + +/* + Gets the node id our DNA requests on init +*/ +uint8_t DroneCAN::get_preferred_node_id() +{ + float ret = this->getParameter("NODEID"); + if (ret > 0 || ret < 127) + { + return (uint8_t)ret; + } + else + { + Serial.println("No NODEID in storage, setting.."); + this->setParameter("NODEID", PREFERRED_NODE_ID); + return PREFERRED_NODE_ID; + } +} + +/* + Processes any DroneCAN actions required. Call as quickly as practical ! +*/ +void DroneCAN::cycle() +{ + const uint32_t now = millis(); + + if (now - this->looptime > 1000) + { + this->looptime = millis(); + this->process1HzTasks(this->micros64()); + // digitalWrite(19, this->led_state); + this->led_state = !this->led_state; + } + + this->processRx(); + this->processTx(); + this->request_DNA(); +} + +/* + For compatibility +*/ +uint64_t DroneCAN::micros64() +{ + return (uint64_t)micros(); +} + +/* + Returns the unique STM CPU ID required for DNA +*/ +void DroneCAN::getUniqueID(uint8_t uniqueId[16]) +{ + memset(uniqueId, 0, 16); + + uint32_t cpuid0 = HAL_GetUIDw0(); + uint32_t cpuid1 = HAL_GetUIDw1(); + uint32_t cpuid2 = HAL_GetUIDw2(); + + uniqueId[0] = (uint8_t)(cpuid0 >> 24); + uniqueId[1] = (uint8_t)(cpuid0 >> 16); + uniqueId[2] = (uint8_t)(cpuid0 >> 8); + uniqueId[3] = (uint8_t)(cpuid0); + uniqueId[4] = (uint8_t)(cpuid1 >> 24); + uniqueId[5] = (uint8_t)(cpuid1 >> 16); + uniqueId[6] = (uint8_t)(cpuid1 >> 8); + uniqueId[7] = (uint8_t)(cpuid1); + uniqueId[8] = (uint8_t)(cpuid2 >> 24); + uniqueId[9] = (uint8_t)(cpuid2 >> 16); + uniqueId[10] = (uint8_t)(cpuid2 >> 8); + uniqueId[11] = (uint8_t)(cpuid2); + uniqueId[12] = 0; + uniqueId[13] = 0; + uniqueId[14] = 0; + uniqueId[15] = 0; +} + +/* + Responds to a request for node info from CAN devices +*/ +void DroneCAN::handle_GetNodeInfo(CanardRxTransfer *transfer) +{ + Serial.print("GetNodeInfo request from"); + Serial.println(transfer->source_node_id); + + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_GetNodeInfoResponse pkt; + + memset(&pkt, 0, sizeof(pkt)); + + node_status.uptime_sec = uptime; + pkt.status = node_status; + + // fill in your major and minor firmware version + pkt.software_version.major = this->version_major; + pkt.software_version.minor = this->version_minor; + pkt.software_version.optional_field_flags = 0; + pkt.software_version.vcs_commit = 0; // should put git hash in here + + // should fill in hardware version + pkt.hardware_version.major = this->hardware_version_major; + pkt.hardware_version.minor = this->hardware_version_minor; + + getUniqueID(pkt.hardware_version.unique_id); + + strncpy((char *)pkt.name.data, this->node_name, sizeof(pkt.name.data)); + pkt.name.len = strnlen((char *)pkt.name.data, sizeof(pkt.name.data)); + + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + + canardRequestOrRespond(&canard, + transfer->source_node_id, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); +} + +/* + handle parameter GetSet request +*/ +void DroneCAN::handle_param_GetSet(CanardRxTransfer *transfer) +{ + // Decode the incoming request + struct uavcan_protocol_param_GetSetRequest req; + if (uavcan_protocol_param_GetSetRequest_decode(transfer, &req)) + { + return; // malformed + } + + IWatchdog.reload(); + + // Figure out which parameter they meant + size_t idx = SIZE_MAX; + + if ((int)req.name.len > 0) + { + // Name‐based lookup + Serial.print("Name based lookup"); + for (size_t i = 0; i < parameters.size(); i++) + { + auto &p = parameters[i]; + if (req.name.len == strlen(p.name) && + memcmp(req.name.data, p.name, req.name.len) == 0) + { + idx = i; + Serial.println(idx); + break; + } + } + } + // If that failed, try index‐based lookup + if (idx == SIZE_MAX && req.index < parameters.size()) + { + idx = req.index; + Serial.print("Parameter index lookup"); + Serial.println(idx); + } + + IWatchdog.reload(); + + // If it’s a _set_ request, apply the new value + if (idx != SIZE_MAX && req.value.union_tag != UAVCAN_PROTOCOL_PARAM_VALUE_EMPTY) + { + auto &p = parameters[idx]; + switch (p.type) + { + case UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE: + p.value = req.value.integer_value; + EEPROM.put(idx * sizeof(float), p.value); + break; + case UAVCAN_PROTOCOL_PARAM_VALUE_REAL_VALUE: + p.value = req.value.real_value; + EEPROM.put(idx * sizeof(float), p.value); + break; + default: + // unsupported type + break; + } + } + + IWatchdog.reload(); + + // Now build the GetSet _response_, always sending one back + struct uavcan_protocol_param_GetSetResponse rsp; + memset(&rsp, 0, sizeof(rsp)); + + if (idx != SIZE_MAX) + { + auto &p = parameters[idx]; + // tag + value + rsp.value.union_tag = p.type; + if (p.type == UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE) + { + rsp.value.integer_value = p.value; + } + else + { + rsp.value.real_value = p.value; + } + + // copy name (must pad/zero any unused bytes) + size_t namelen = strlen(p.name); + rsp.name.len = namelen; + memset(rsp.name.data, 0, sizeof(rsp.name.data)); + memcpy(rsp.name.data, p.name, namelen); + } + // else idx==SIZE_MAX: leave rsp.name.len=0 / value empty + + // Encode & send + uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE]; + uint16_t len = uavcan_protocol_param_GetSetResponse_encode(&rsp, buffer); + canardRequestOrRespond(&canard, + transfer->source_node_id, + UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE, + UAVCAN_PROTOCOL_PARAM_GETSET_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + len); +} + +/* + handle parameter executeopcode request +*/ +void DroneCAN::handle_param_ExecuteOpcode(CanardRxTransfer *transfer) +{ + struct uavcan_protocol_param_ExecuteOpcodeRequest req; + if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) + { + return; + } + if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_ERASE) + { + // Reset all parameters to defaults + for (size_t i = 0; i < parameters.size(); i++) + { + parameters[i].value = parameters[i].min_value; // Or some default value + } + } + if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_SAVE) + { + // Save all the changed parameters to permanent storage + for (size_t i = 0; i < parameters.size(); i++) + { + EEPROM.put(i * 4, parameters[i].value); + } + } + + struct uavcan_protocol_param_ExecuteOpcodeResponse pkt; + memset(&pkt, 0, sizeof(pkt)); + + pkt.ok = true; + + uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE]; + uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer); + + canardRequestOrRespond(&canard, + transfer->source_node_id, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); +} + +/* + Read the EEPROM parameter storage and set the current parameter list to the read values +*/ +void DroneCAN::read_parameter_memory() +{ + float p_val = 0.0; + + for (size_t i = 0; i < parameters.size(); i++) + { + EEPROM.get(i * 4, p_val); + parameters[i].value = p_val; + } +} + +/* + Get a parameter from storage by name + Only handles float return values + returns __FLT_MIN__ if no parameter found with the provided name +*/ +float DroneCAN::getParameter(const char *name) +{ + for (size_t i = 0; i < parameters.size(); i++) + { + auto &p = parameters[i]; + if (strlen(name) == strlen(p.name) && + memcmp(name, p.name, strlen(name)) == 0) + { + return parameters[i].value; + } + } + return __FLT_MIN__; +} + +/* + Set a parameter from storage by name + Values get stored as floats + returns -1 if storage failed, 0 if good +*/ +int DroneCAN::setParameter(const char *name, float value) +{ + for (size_t i = 0; i < parameters.size(); i++) + { + auto &p = parameters[i]; + if (strlen(name) == strlen(p.name) && + memcmp(name, p.name, strlen(name)) == 0) + { + parameters[i].value = value; + return 0; + } + } + return -1; +} + +/* + handle a DNA allocation packet +*/ +int DroneCAN::handle_DNA_Allocation(CanardRxTransfer *transfer) +{ + if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) + { + // already allocated + return 0; + } + + Serial.println("We got a node ID message back"); + + // Rule C - updating the randomized time interval + DNA.send_next_node_id_allocation_request_at_ms = + millis() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + (random() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); + + if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) + { + Serial.println("Allocation request from another allocatee\n"); + DNA.node_id_allocation_unique_id_offset = 0; + return 0; + } + + // Copying the unique ID from the message + struct uavcan_protocol_dynamic_node_id_Allocation msg; + + uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg); + + // Obtaining the local unique ID + uint8_t my_unique_id[sizeof(msg.unique_id.data)]; + getUniqueID(my_unique_id); + + // Matching the received UID against the local one + if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) + { + Serial.println("DNA failed this time"); + DNA.node_id_allocation_unique_id_offset = 0; + // No match, return + return 0; + } + + if (msg.unique_id.len < sizeof(msg.unique_id.data)) + + { + // The allocator has confirmed part of unique ID, switching to + // the next stage and updating the timeout. + DNA.node_id_allocation_unique_id_offset = msg.unique_id.len; + DNA.send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; + Serial.println("second stage Node ID allocation"); + } + else + { + // Allocation complete - copying the allocated node ID from the message + canardSetLocalNodeID(&canard, msg.node_id); + Serial.print("Node ID allocated: "); + Serial.println(msg.node_id); + } + return 0; +} + +/* + ask for a dynamic node allocation +*/ +void DroneCAN::request_DNA() +{ + + // see if we are still doing DNA + if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) + { + return; + } + + // we're still waiting for a DNA allocation of our node ID + if (millis() < DNA.send_next_node_id_allocation_request_at_ms) + { + return; + } + + const uint32_t now = millis(); + static uint8_t node_id_allocation_transfer_id = 0; + + DNA.send_next_node_id_allocation_request_at_ms = + now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + (random() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); + + // Structure of the request is documented in the DSDL definition + // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation + uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; + uint8_t pref_node_id = (uint8_t)(this->get_preferred_node_id() << 1U); + + Serial.print("Requesting ID "); + Serial.println(pref_node_id / 2); // not sure why this is over 2 .. something to do with the bit shifting but this is what it actually sets + + allocation_request[0] = pref_node_id; + + if (DNA.node_id_allocation_unique_id_offset == 0) + { + allocation_request[0] |= 1; // First part of unique ID + } + + uint8_t my_unique_id[16]; + getUniqueID(my_unique_id); + + static const uint8_t MaxLenOfUniqueIDInRequest = 6; + uint8_t uid_size = (uint8_t)(16 - DNA.node_id_allocation_unique_id_offset); + + if (uid_size > MaxLenOfUniqueIDInRequest) + { + uid_size = MaxLenOfUniqueIDInRequest; + } + if (uid_size + DNA.node_id_allocation_unique_id_offset > 16) + { + uid_size = 16 - DNA.node_id_allocation_unique_id_offset; + } + + memmove(&allocation_request[1], &my_unique_id[DNA.node_id_allocation_unique_id_offset], uid_size); + + // Broadcasting the request + const int16_t bcast_res = canardBroadcast(&canard, + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, + &node_id_allocation_transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + &allocation_request[0], + (uint16_t)(uid_size + 1)); + if (bcast_res < 0) + { + Serial.print("Could not broadcast ID allocation req; error"); + Serial.println(bcast_res); + } + + // Preparing for timeout; if response is received, this value will be updated from the callback. + DNA.node_id_allocation_unique_id_offset = 0; +} + +/* + handle a BeginFirmwareUpdate request from a management tool like DroneCAN GUI tool or MissionPlanner + + There are multiple ways to handle firmware update over DroneCAN: + + 1) on BeginFirmwareUpdate reboot to the bootloader, and implement + the firmware upudate process in the bootloader. This is good on + boards with smaller amounts of flash + + 2) if you have enough flash for 2 copies of your firmware then you + can use an A/B scheme, where the new firmware is saved to the + inactive flash region and a tag is used to indicate which + firmware to boot next time + + 3) you could write the firmware to secondary storage (such as a + microSD) and the bootloader would flash it on next boot + + In this example firmware we will write it to a file + newfirmware.bin, which is option 3 + + Note that you cannot rely on the form of the filename. The client + may hash the filename before sending + */ +void DroneCAN::handle_begin_firmware_update(CanardRxTransfer *transfer) +{ + Serial.println("Update request received"); + + auto *comms = (struct app_bootloader_comms *)0x20000000; + + if (comms->magic != APP_BOOTLOADER_COMMS_MAGIC) + { + memset(comms, 0, sizeof(*comms)); + } + comms->magic = APP_BOOTLOADER_COMMS_MAGIC; + + uavcan_protocol_file_BeginFirmwareUpdateRequest req; + if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) + { + return; + } + + comms->server_node_id = req.source_node_id; + if (comms->server_node_id == 0) + { + comms->server_node_id = transfer->source_node_id; + } + memcpy(comms->path, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len); + comms->my_node_id = canardGetLocalNodeID(&canard); + + uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE]; + uavcan_protocol_file_BeginFirmwareUpdateResponse reply{}; + reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; + + uint16_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); + static uint8_t transfer_id; + CanardTxTransfer transfer_object = { + .transfer_type = CanardTransferTypeResponse, + .data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, + .data_type_id = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, + .inout_transfer_id = &transfer->transfer_id, + .priority = transfer->priority, + .payload = &buffer[0], + .payload_len = total_size, + }; + const auto res = canardRequestOrRespondObj(&canard, + transfer->source_node_id, + &transfer_object); + + uint8_t count = 50; + while (count--) + { + processTx(); + delay(1); + } + + NVIC_SystemReset(); +} + +/* + send a read for a firmware update. This asks the client (firmware + server) for a piece of the new firmware +*/ +void DroneCAN::send_firmware_read(void) +{ + uint32_t now = millis(); + if (now - fwupdate.last_read_ms < 750) + { + // the server may still be responding + return; + } + fwupdate.last_read_ms = now; + + uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE]; + + struct uavcan_protocol_file_ReadRequest pkt; + memset(&pkt, 0, sizeof(pkt)); + + pkt.path.path.len = strlen((const char *)fwupdate.path); + pkt.offset = fwupdate.offset; + memcpy(pkt.path.path.data, fwupdate.path, pkt.path.path.len); + + uint16_t total_size = uavcan_protocol_file_ReadRequest_encode(&pkt, buffer); + + canardRequestOrRespond(&canard, + fwupdate.node_id, + UAVCAN_PROTOCOL_FILE_READ_SIGNATURE, + UAVCAN_PROTOCOL_FILE_READ_ID, + &fwupdate.transfer_id, + CANARD_TRANSFER_PRIORITY_HIGH, + CanardRequest, + &buffer[0], + total_size); +} + +/* + handle response to send_firmware_read() +*/ +void DroneCAN::handle_file_read_response(CanardRxTransfer *transfer) +{ + if ((transfer->transfer_id + 1) % 32 != fwupdate.transfer_id || + transfer->source_node_id != fwupdate.node_id) + { + /* not for us */ + Serial.println("Firmware update: not for us"); + return; + } + struct uavcan_protocol_file_ReadResponse pkt; + if (uavcan_protocol_file_ReadResponse_decode(transfer, &pkt)) + { + /* bad packet */ + Serial.println("Firmware update: bad packet\n"); + return; + } + if (pkt.error.value != UAVCAN_PROTOCOL_FILE_ERROR_OK) + { + /* read failed */ + fwupdate.node_id = 0; + Serial.println("Firmware update read failure\n"); + return; + } + + // write(fwupdate.fd, pkt.data.data, pkt.data.len); + // if (pkt.data.len < 256) + // { + // /* firmware updare done */ + // close(fwupdate.fd); + // Serial.println("Firmwate update complete\n"); + // fwupdate.node_id = 0; + // return; + // } + fwupdate.offset += pkt.data.len; + + /* trigger a new read */ + fwupdate.last_read_ms = 0; +} + +/* + send the 1Hz NodeStatus message. This is what allows a node to show + up in the DroneCAN GUI tool and in the flight controller logs +*/ +void DroneCAN::send_NodeStatus(void) +{ + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + + node_status.uptime_sec = uptime++; + node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_OPERATIONAL; + node_status.sub_mode = 0; + // put whatever you like in here for display in GUI + node_status.vendor_specific_status_code = 0; + + /* + when doing a firmware update put the size in kbytes in VSSC so + the user can see how far it has reached + */ + if (fwupdate.node_id != 0) + { + node_status.vendor_specific_status_code = fwupdate.offset / 1024; + node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE; + } + + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + + // we need a static variable for the transfer ID. This is + // incremeneted on each transfer, allowing for detection of packet + // loss + static uint8_t transfer_id; + + canardBroadcast(&canard, + UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); +} + +/* + Handle re-occurring slow pace tasks +*/ +void DroneCAN::process1HzTasks(uint64_t timestamp_usec) +{ + /* + Purge transfers that are no longer transmitted. This can free up some memory + */ + canardCleanupStaleTransfers(&canard, timestamp_usec); + + /* + Transmit the node status message + */ + send_NodeStatus(); +} + +/* + Send any packets currently waiting with Canard +*/ +void DroneCAN::processTx() +{ + for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) + { + CANSend(txf); + canardPopTxQueue(&canard); // fuck it we ball + } +} + +/* + Look at our mailbox +*/ +void DroneCAN::processRx() +{ + const uint64_t timestamp = micros(); + if (CANMsgAvail()) + { + CANReceive(&CAN_rx_msg); + int ret = canardHandleRxFrame(&canard, &CAN_rx_msg, timestamp); + if (ret < 0) + { + // Serial.print("Canard RX fail"); + // Serial.println(ret); + } + } +} + +/* + Send a debug message over CAN +*/ +void DroneCAN::debug(const char *msg, uint8_t level) +{ + uavcan_protocol_debug_LogMessage pkt{}; + pkt.level.value = level; + pkt.text.len = strlen(msg); + strncpy((char *)pkt.text.data, msg, pkt.text.len); + + uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE]; + uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); + static uint8_t transfer_id; + canardBroadcast(&canard, + UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, + UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); +} + +/* + Bare minimum callback function for DroneCAN library requirements +*/ +void DroneCANonTransferReceived(DroneCAN &dronecan, CanardInstance *ins, CanardRxTransfer *transfer) +{ + if (transfer->transfer_type == CanardTransferTypeBroadcast) + { + // check if we want to handle a specific broadcast message + switch (transfer->data_type_id) + { + case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: + { + dronecan.handle_DNA_Allocation(transfer); + break; + } + } + } + // switch on data type ID to pass to the right handler function + else if (transfer->transfer_type == CanardTransferTypeRequest) + { + // check if we want to handle a specific service request + switch (transfer->data_type_id) + { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + { + dronecan.handle_GetNodeInfo(transfer); + break; + } + case UAVCAN_PROTOCOL_RESTARTNODE_ID: + { + + uavcan_protocol_RestartNodeResponse pkt{}; + pkt.ok = true; + uint8_t buffer[UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_MAX_SIZE]; + uint32_t len = uavcan_protocol_RestartNodeResponse_encode(&pkt, buffer); + static uint8_t transfer_id; + canardBroadcast(ins, + UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_SIGNATURE, + UAVCAN_PROTOCOL_RESTARTNODE_RESPONSE_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); + + Serial.println("Reset.."); + delay(200); + // yeeeeeet + NVIC_SystemReset(); + } + case UAVCAN_PROTOCOL_PARAM_GETSET_ID: + { + dronecan.handle_param_GetSet(transfer); + break; + } + case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: + { + dronecan.handle_param_ExecuteOpcode(transfer); + break; + } + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: + { + dronecan.handle_begin_firmware_update(transfer); + break; + } + } + } +} + +/* + Bare minimum message signing required for DroneCAN library +*/ +bool DroneCANshoudlAcceptTransfer(const CanardInstance *ins, + uint64_t *out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id) +{ + if (transfer_type == CanardTransferTypeRequest) + { + // Check if we want to handle a specific service request + switch (data_type_id) + { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_PARAM_GETSET_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_FILE_READ_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_RESTARTNODE_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_ID; + return true; + } + } + } + + if (transfer_type == CanardTransferTypeResponse) + { + // Check if we want to handle a specific service response + switch (data_type_id) + { + case UAVCAN_PROTOCOL_FILE_READ_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_PARAM_GETSET_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_GETSET_SIGNATURE; + return true; + } + } + } + + if (transfer_type == CanardTransferTypeBroadcast) + { + // Check if we want to handle a specific broadcast packet + switch (data_type_id) + { + case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_DEBUG_KEYVALUE_ID: + { + *out_data_type_signature = UAVCAN_PROTOCOL_DEBUG_KEYVALUE_SIGNATURE; + return true; + } + } + } + return false; +} diff --git a/lib/libArduinoDroneCAN/src/dronecan.h b/lib/libArduinoDroneCAN/src/dronecan.h new file mode 100644 index 0000000..26366b3 --- /dev/null +++ b/lib/libArduinoDroneCAN/src/dronecan.h @@ -0,0 +1,129 @@ +#ifndef ARDU_DRONECAN +#define ARDU_DRONECAN + +#include +#include +#ifdef CANL431 + #include +#endif +#ifdef CANH7 + #include +#endif +#ifdef ARDUINO_NUCLEO_H723ZG + #include +#endif +#include +#include +#include + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define C_TO_KELVIN(temp) (temp + 273.15f) +#define ARRAY_SIZE(x) (sizeof(x) / sizeof(x[0])) +#define APP_BOOTLOADER_COMMS_MAGIC 0xc544ad9a +#define PREFERRED_NODE_ID 69 + +class DroneCAN +{ +private: + uint8_t memory_pool[1024]; + struct uavcan_protocol_NodeStatus node_status; + CanardCANFrame CAN_TX_msg; + CanardCANFrame CAN_rx_msg; + CanardCANFrame rx_frame; + uint32_t looptime; + bool led_state = false; + uint64_t uptime = 0; + static constexpr uint16_t PARAM_EEPROM_BASE = 0x0000; // EEPROM base address + std::vector sorted_indices; // built on first use + int node_id = 0; + char node_name[80]; + + struct firmware_update + { + char path[256]; + uint8_t node_id; + uint8_t transfer_id; + uint32_t last_read_ms; + int fd; + uint32_t offset; + } fwupdate; + + struct dynamic_node_allocation + { + uint32_t send_next_node_id_allocation_request_at_ms; + uint32_t node_id_allocation_unique_id_offset; + } DNA; + + struct app_bootloader_comms + { + uint32_t magic; + uint32_t ip; + uint32_t netmask; + uint32_t gateway; + uint32_t reserved; + uint8_t server_node_id; + uint8_t my_node_id; + uint8_t path[201]; + }; + + static void getUniqueID(uint8_t id[16]); + uint8_t get_preferred_node_id(); + + void read_parameter_memory(); + void request_DNA(); + void send_firmware_read(); + void handle_file_read_response(CanardRxTransfer *transfer); + void send_NodeStatus(void); + void process1HzTasks(uint64_t timestamp_usec); + void processTx(); + void processRx(); + static uint64_t micros64(); + +public: + struct parameter + { + const char *name; + enum uavcan_protocol_param_Value_type_t type; + float value; + float min_value; + float max_value; + }; + + std::vector parameters; + + // copy a parameter list into the object + void set_parameters(const std::vector ¶m_list) + { + parameters = param_list; + } + + void init(CanardOnTransferReception onTransferReceived, CanardShouldAcceptTransfer shouldAcceptTransfer, const std::vector ¶m_list, const char *name); + + CanardInstance canard; + int version_major = 0; + int version_minor = 0; + int hardware_version_major = 0; + int hardware_version_minor = 0; + + // used in callbacks + void handle_GetNodeInfo(CanardRxTransfer *transfer); + void handle_param_GetSet(CanardRxTransfer *transfer); + void handle_param_ExecuteOpcode(CanardRxTransfer *transfer); + void handle_begin_firmware_update(CanardRxTransfer *transfer); + int handle_DNA_Allocation(CanardRxTransfer *transfer); + + // user methods + void cycle(); + void debug(const char *msg, uint8_t level); + float getParameter(const char *name); + int setParameter(const char *name, float value); +}; + +void DroneCANonTransferReceived(DroneCAN &dronecan, CanardInstance *ins, CanardRxTransfer *transfer); +bool DroneCANshoudlAcceptTransfer(const CanardInstance *ins, + uint64_t *out_data_type_signature, + uint16_t data_type_id, + CanardTransferType transfer_type, + uint8_t source_node_id); + +#endif // ARDU_DRONECAN \ No newline at end of file diff --git a/lib/libArduinoDroneCAN/src/simple_dronecanmessages.h b/lib/libArduinoDroneCAN/src/simple_dronecanmessages.h new file mode 100644 index 0000000..ad9d218 --- /dev/null +++ b/lib/libArduinoDroneCAN/src/simple_dronecanmessages.h @@ -0,0 +1,94 @@ +#include + +template +struct UavcanTraits; // no definition + +#define REGISTER_UAVCAN_TYPE(MSGTYPE, ENCFUNC, ID_MACRO, SIG_MACRO, MAXSZ_MACRO) \ + template <> \ + struct UavcanTraits \ + { \ + static constexpr uint32_t message_id = ID_MACRO; \ + static constexpr uint64_t signature = SIG_MACRO; \ + static constexpr size_t max_size = MAXSZ_MACRO; \ + \ + static uint32_t encode(MSGTYPE &m, uint8_t *buf) \ + { \ + return ENCFUNC(&m, buf); \ + } \ + }; + +/* +This only works for *some* dronecan messages. Most of the sensors/equipment type messages. +*/ +template +void sendUavcanMsg(CanardInstance &ins, + Msg &pkt, + uint8_t priority = CANARD_TRANSFER_PRIORITY_LOW) +{ + // per‐message‐type transfer counter + static uint8_t transfer_id = 0; + + uint8_t buffer[ UavcanTraits::max_size ]; + uint32_t len = UavcanTraits::encode(pkt, buffer); + canardBroadcast(&ins, + UavcanTraits::signature, + UavcanTraits::message_id, + &transfer_id, + priority, + buffer, + len +#if CANARD_ENABLE_CANFD + ,true ///< Is the frame canfd +#endif + ); + + // advance for next time (wraps 0→255→0 automatically) + ++transfer_id; +} + +REGISTER_UAVCAN_TYPE(dronecan_sensors_hygrometer_Hygrometer, dronecan_sensors_hygrometer_Hygrometer_encode, DRONECAN_SENSORS_HYGROMETER_HYGROMETER_ID, DRONECAN_SENSORS_HYGROMETER_HYGROMETER_SIGNATURE, DRONECAN_SENSORS_HYGROMETER_HYGROMETER_MAX_SIZE); +REGISTER_UAVCAN_TYPE(dronecan_sensors_magnetometer_MagneticFieldStrengthHiRes, dronecan_sensors_magnetometer_MagneticFieldStrengthHiRes_encode, DRONECAN_SENSORS_MAGNETOMETER_MAGNETICFIELDSTRENGTHHIRES_ID, DRONECAN_SENSORS_MAGNETOMETER_MAGNETICFIELDSTRENGTHHIRES_SIGNATURE, DRONECAN_SENSORS_MAGNETOMETER_MAGNETICFIELDSTRENGTHHIRES_MAX_SIZE); +REGISTER_UAVCAN_TYPE(dronecan_sensors_rc_RCInput, dronecan_sensors_rc_RCInput_encode, DRONECAN_SENSORS_RC_RCINPUT_ID, DRONECAN_SENSORS_RC_RCINPUT_SIGNATURE, DRONECAN_SENSORS_RC_RCINPUT_MAX_SIZE); +REGISTER_UAVCAN_TYPE(dronecan_sensors_rpm_RPM, dronecan_sensors_rpm_RPM_encode, DRONECAN_SENSORS_RPM_RPM_ID, DRONECAN_SENSORS_RPM_RPM_SIGNATURE, DRONECAN_SENSORS_RPM_RPM_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_actuator_ArrayCommand, uavcan_equipment_actuator_ArrayCommand_encode, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE); +//REGISTER_UAVCAN_TYPE(uavcan_equipment_actuator_Command, uavcan_equipment_actuator_Command_encode, UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_ID, UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ACTUATOR_COMMAND_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_actuator_Status, uavcan_equipment_actuator_Status_encode, UAVCAN_EQUIPMENT_ACTUATOR_STATUS_ID, UAVCAN_EQUIPMENT_ACTUATOR_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ACTUATOR_STATUS_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_ahrs_MagneticFieldStrength, uavcan_equipment_ahrs_MagneticFieldStrength_encode, UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_ID, UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_SIGNATURE, UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_ahrs_MagneticFieldStrength2, uavcan_equipment_ahrs_MagneticFieldStrength2_encode, UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH2_ID, UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH2_SIGNATURE, UAVCAN_EQUIPMENT_AHRS_MAGNETICFIELDSTRENGTH2_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_ahrs_RawIMU, uavcan_equipment_ahrs_RawIMU_encode, UAVCAN_EQUIPMENT_AHRS_RAWIMU_ID, UAVCAN_EQUIPMENT_AHRS_RAWIMU_SIGNATURE, UAVCAN_EQUIPMENT_AHRS_RAWIMU_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_ahrs_Solution, uavcan_equipment_ahrs_Solution_encode, UAVCAN_EQUIPMENT_AHRS_SOLUTION_ID, UAVCAN_EQUIPMENT_AHRS_SOLUTION_SIGNATURE, UAVCAN_EQUIPMENT_AHRS_SOLUTION_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_air_data_AngleOfAttack, uavcan_equipment_air_data_AngleOfAttack_encode, UAVCAN_EQUIPMENT_AIR_DATA_ANGLEOFATTACK_ID, UAVCAN_EQUIPMENT_AIR_DATA_ANGLEOFATTACK_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_ANGLEOFATTACK_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_air_data_IndicatedAirspeed, uavcan_equipment_air_data_IndicatedAirspeed_encode, UAVCAN_EQUIPMENT_AIR_DATA_INDICATEDAIRSPEED_ID, UAVCAN_EQUIPMENT_AIR_DATA_INDICATEDAIRSPEED_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_INDICATEDAIRSPEED_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_air_data_RawAirData, uavcan_equipment_air_data_RawAirData_encode, UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID, UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_air_data_Sideslip, uavcan_equipment_air_data_Sideslip_encode, UAVCAN_EQUIPMENT_AIR_DATA_SIDESLIP_ID, UAVCAN_EQUIPMENT_AIR_DATA_SIDESLIP_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_SIDESLIP_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_air_data_StaticPressure, uavcan_equipment_air_data_StaticPressure_encode, UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_ID, UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_STATICPRESSURE_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_air_data_StaticTemperature, uavcan_equipment_air_data_StaticTemperature_encode, UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_ID, UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_STATICTEMPERATURE_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_air_data_TrueAirspeed, uavcan_equipment_air_data_TrueAirspeed_encode, UAVCAN_EQUIPMENT_AIR_DATA_TRUEAIRSPEED_ID, UAVCAN_EQUIPMENT_AIR_DATA_TRUEAIRSPEED_SIGNATURE, UAVCAN_EQUIPMENT_AIR_DATA_TRUEAIRSPEED_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_camera_gimbal_AngularCommand, uavcan_equipment_camera_gimbal_AngularCommand_encode, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_ANGULARCOMMAND_ID, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_ANGULARCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_ANGULARCOMMAND_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_camera_gimbal_GEOPOICommand, uavcan_equipment_camera_gimbal_GEOPOICommand_encode, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_GEOPOICOMMAND_ID, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_GEOPOICOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_GEOPOICOMMAND_MAX_SIZE); +//REGISTER_UAVCAN_TYPE(uavcan_equipment_camera_gimbal_Mode, uavcan_equipment_camera_gimbal_Mode_encode, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_MODE_ID, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_MODE_SIGNATURE, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_MODE_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_camera_gimbal_Status, uavcan_equipment_camera_gimbal_Status_encode, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_STATUS_ID, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_CAMERA_GIMBAL_STATUS_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_device_Temperature, uavcan_equipment_device_Temperature_encode, UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID, UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE, UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_esc_RawCommand, uavcan_equipment_esc_RawCommand_encode, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_esc_RPMCommand, uavcan_equipment_esc_RPMCommand_encode, UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID, UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_esc_Status, uavcan_equipment_esc_Status_encode, UAVCAN_EQUIPMENT_ESC_STATUS_ID, UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_esc_StatusExtended, uavcan_equipment_esc_StatusExtended_encode, UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_ID, UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_SIGNATURE, UAVCAN_EQUIPMENT_ESC_STATUSEXTENDED_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_gnss_Auxiliary, uavcan_equipment_gnss_Auxiliary_encode, UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID, UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE); +//REGISTER_UAVCAN_TYPE(uavcan_equipment_gnss_ECEFPositionVelocity, uavcan_equipment_gnss_ECEFPositionVelocity_encode, UAVCAN_EQUIPMENT_GNSS_ECEFPOSITIONVELOCITY_ID, UAVCAN_EQUIPMENT_GNSS_ECEFPOSITIONVELOCITY_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_ECEFPOSITIONVELOCITY_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_gnss_Fix, uavcan_equipment_gnss_Fix_encode, UAVCAN_EQUIPMENT_GNSS_FIX_ID, UAVCAN_EQUIPMENT_GNSS_FIX_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_FIX_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_gnss_Fix2, uavcan_equipment_gnss_Fix2_encode, UAVCAN_EQUIPMENT_GNSS_FIX2_ID, UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_gnss_RTCMStream, uavcan_equipment_gnss_RTCMStream_encode, UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_ID, UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_SIGNATURE, UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_hardpoint_Command, uavcan_equipment_hardpoint_Command_encode, UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID, UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE, UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_hardpoint_Status, uavcan_equipment_hardpoint_Status_encode, UAVCAN_EQUIPMENT_HARDPOINT_STATUS_ID, UAVCAN_EQUIPMENT_HARDPOINT_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_HARDPOINT_STATUS_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_ice_FuelTankStatus, uavcan_equipment_ice_FuelTankStatus_encode, UAVCAN_EQUIPMENT_ICE_FUELTANKSTATUS_ID, UAVCAN_EQUIPMENT_ICE_FUELTANKSTATUS_SIGNATURE, UAVCAN_EQUIPMENT_ICE_FUELTANKSTATUS_MAX_SIZE); +//REGISTER_UAVCAN_TYPE(uavcan_equipment_ice_reciprocating_CylinderStatus, uavcan_equipment_ice_reciprocating_CylinderStatus_encode, UAVCAN_EQUIPMENT_ICE_RECIPROCATING_CYLINDERSTATUS_ID, UAVCAN_EQUIPMENT_ICE_RECIPROCATING_CYLINDERSTATUS_SIGNATURE, UAVCAN_EQUIPMENT_ICE_RECIPROCATING_CYLINDERSTATUS_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_ice_reciprocating_Status, uavcan_equipment_ice_reciprocating_Status_encode, UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_ID, UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_SIGNATURE, UAVCAN_EQUIPMENT_ICE_RECIPROCATING_STATUS_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_indication_BeepCommand, uavcan_equipment_indication_BeepCommand_encode, UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_ID, UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_INDICATION_BEEPCOMMAND_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_indication_LightsCommand, uavcan_equipment_indication_LightsCommand_encode, UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_ID, UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_INDICATION_LIGHTSCOMMAND_MAX_SIZE); +//REGISTER_UAVCAN_TYPE(uavcan_equipment_indication_RGB565, uavcan_equipment_indication_RGB565_encode, UAVCAN_EQUIPMENT_INDICATION_RGB565_ID, UAVCAN_EQUIPMENT_INDICATION_RGB565_SIGNATURE, UAVCAN_EQUIPMENT_INDICATION_RGB565_MAX_SIZE); +//REGISTER_UAVCAN_TYPE(uavcan_equipment_indication_SingleLightCommand, uavcan_equipment_indication_SingleLightCommand_encode, UAVCAN_EQUIPMENT_INDICATION_SINGLELIGHTCOMMAND_ID, UAVCAN_EQUIPMENT_INDICATION_SINGLELIGHTCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_INDICATION_SINGLELIGHTCOMMAND_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_power_BatteryInfo, uavcan_equipment_power_BatteryInfo_encode, UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID, UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE, UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_power_CircuitStatus, uavcan_equipment_power_CircuitStatus_encode, UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID, UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_SIGNATURE, UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_power_PrimaryPowerSupplyStatus, uavcan_equipment_power_PrimaryPowerSupplyStatus_encode, UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS_ID, UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS_SIGNATURE, UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_range_sensor_Measurement, uavcan_equipment_range_sensor_Measurement_encode, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_ID, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_SIGNATURE, UAVCAN_EQUIPMENT_RANGE_SENSOR_MEASUREMENT_MAX_SIZE); +REGISTER_UAVCAN_TYPE(uavcan_equipment_safety_ArmingStatus, uavcan_equipment_safety_ArmingStatus_encode, UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_ID, UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_SIGNATURE, UAVCAN_EQUIPMENT_SAFETY_ARMINGSTATUS_MAX_SIZE); \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 4445ea3..dc10ce4 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,7 +14,7 @@ default_envs = Micro-Node-Bootloader [env:Micro-Node-Bootloader] platform = ststm32 -board = BRMicroNode +board = MicroNode framework = arduino upload_protocol = stlink debug_tool = stlink @@ -22,14 +22,52 @@ monitor_speed = 115200 board_build.variants_dir = variants debug_build_flags = -O0 -g debug_init_break = tbreak none -board_build.ldscript = variants/BRMicroNode/ldscript.ld -build_flags = -Wno-error=deprecated-declarations +board_build.ldscript = variants/MicroNode/ldscript.ld +build_flags = + -Wno-error=deprecated-declarations + -DCANL431 extra_scripts = upload_bootloader_app.py [env:Micro-Node-No-Bootloader] platform = ststm32 -board = BRMicroNode +board = MicroNode +framework = arduino +upload_protocol = stlink +debug_tool = stlink +monitor_speed = 115200 +board_build.variants_dir = variants +debug_build_flags = -O0 -g +debug_init_break = tbreak none +board_build.ldscript = variants/MicroNode/ldscript-no-bootloader.ld +build_flags = + -Wno-error=deprecated-declarations + -DDISABLE_APP_SETUP + -DCANL431 + + +[env:Core-Node-No-Bootloader] +platform = ststm32 +board = CoreNode +framework = arduino +upload_protocol = stlink +debug_tool = stlink +monitor_speed = 115200 +board_build.variants_dir = variants +debug_build_flags = -O0 -g +debug_init_break = tbreak none +board_build.ldscript = variants/CoreNode/ldscript-no-bootloader.ld +build_flags = + -Wno-error=deprecated-declarations + -DDISABLE_APP_SETUP + -DCANH743 + -DARDUINO_NUCLEO_H743ZI2 + -ACANFD + + +[env:Micro-Node-Plus-No-Bootloader] +platform = ststm32 +board = MicroNodePlus framework = arduino upload_protocol = stlink debug_tool = stlink @@ -37,7 +75,9 @@ monitor_speed = 115200 board_build.variants_dir = variants debug_build_flags = -O0 -g debug_init_break = tbreak none -board_build.ldscript = variants/BRMicroNode/ldscript-no-bootloader.ld +board_build.ldscript = variants/MicroNodePlus/ldscript.ld build_flags = -Wno-error=deprecated-declarations - -DDISABLE_APP_SETUP \ No newline at end of file + -DARDUINO_GENERIC_H723ZGTX + -DARDUINO_NUCLEO_H723ZG + -DACANFD \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 71791be..21d685b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,6 +5,7 @@ #include #include + // set up your parameters here with default values. NODEID should be kept std::vector custom_parameters = { { "NODEID", UAVCAN_PROTOCOL_PARAM_VALUE_INTEGER_VALUE, 100, 0, 127 }, @@ -76,8 +77,9 @@ void setup() { // the following block of code should always run first. Adjust it at your own peril! app_setup(); - IWatchdog.begin(2000000); + IWatchdog.begin(2000000); Serial.begin(115200); + Serial.println("Starting!"); dronecan.version_major = 1; dronecan.version_minor = 0; dronecan.init( diff --git a/variants/BRMicroNode b/variants/BRMicroNode deleted file mode 160000 index 0a96ca1..0000000 --- a/variants/BRMicroNode +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0a96ca1796d2552e9a088d7483d9fbad24ae5f90 diff --git a/variants/CoreNode/CMakeLists.txt b/variants/CoreNode/CMakeLists.txt new file mode 100644 index 0000000..4dbf924 --- /dev/null +++ b/variants/CoreNode/CMakeLists.txt @@ -0,0 +1,33 @@ +# v3.21 implemented semantic changes regarding $ +# See https://cmake.org/cmake/help/v3.21/command/target_link_libraries.html#linking-object-libraries-via-target-objects +cmake_minimum_required(VERSION 3.21) + +add_library(variant INTERFACE) +add_library(variant_usage INTERFACE) + +target_include_directories(variant_usage INTERFACE + . +) + + +target_link_libraries(variant_usage INTERFACE + base_config +) + +target_link_libraries(variant INTERFACE variant_usage) + + + +add_library(variant_bin STATIC EXCLUDE_FROM_ALL + generic_clock.c + PeripheralPins.c + PeripheralPins_DISCO_H747I.c + variant_generic.cpp + variant_STM32H747I_DISCO.cpp +) +target_link_libraries(variant_bin PUBLIC variant_usage) + +target_link_libraries(variant INTERFACE + variant_bin +) + diff --git a/variants/CoreNode/PeripheralPins.c b/variants/CoreNode/PeripheralPins.c new file mode 100644 index 0000000..f5d0031 --- /dev/null +++ b/variants/CoreNode/PeripheralPins.c @@ -0,0 +1,802 @@ +/* + ******************************************************************************* + * Copyright (c) 2020, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +/* + * Automatically generated from STM32H742X(G-I)Hx.xml, STM32H743XGHx.xml + * STM32H743XIHx.xml, STM32H745XGHx.xml + * STM32H745XIHx.xml, STM32H747XGHx.xml + * STM32H747XIHx.xml, STM32H750XBHx.xml + * STM32H753XIHx.xml, STM32H755XIHx.xml + * STM32H757XIHx.xml + * CubeMX DB release 6.0.140 + */ +#if !defined(CUSTOM_PERIPHERAL_PINS) +#include "Arduino.h" +#include "PeripheralPins.h" + +/* ===== + * Notes: + * - The pins mentioned Px_y_ALTz are alternative possibilities which use other + * HW peripheral instances. You can use them the same way as any other "normal" + * pin (i.e. analogWrite(PA7_ALT1, 128);). + * + * - Commented lines are alternative possibilities which are not used per default. + * If you change them, you will have to know what you do + * ===== + */ + +//*** ADC *** + +#ifdef HAL_ADC_MODULE_ENABLED +WEAK const PinMap PinMap_ADC[] = { + {PA_0, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 16, 0)}, // ADC1_INP16 + {PA_0_C, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 0, 0)}, // ADC1_INP0 + {PA_0_C_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 0, 0)}, // ADC2_INP0 + {PA_1, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 17, 0)}, // ADC1_INP17 + {PA_1_C, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // ADC1_INP1 + {PA_1_C_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // ADC2_INP1 + {PA_2, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 14, 0)}, // ADC1_INP14 + {PA_2_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 14, 0)}, // ADC2_INP14 + {PA_3, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC1_INP15 + {PA_3_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC2_INP15 + {PA_4, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 18, 0)}, // ADC1_INP18 + {PA_4_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 18, 0)}, // ADC2_INP18 + {PA_5, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 19, 0)}, // ADC1_INP19 + {PA_5_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 19, 0)}, // ADC2_INP19 + {PA_6, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC1_INP3 + {PA_6_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC2_INP3 + {PA_7, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC1_INP7 + {PA_7_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC2_INP7 + {PB_0, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC1_INP9 + {PB_0_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC2_INP9 + {PB_1, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC1_INP5 + {PB_1_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC2_INP5 + {PC_0, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC1_INP10 + {PC_0_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC2_INP10 + {PC_0_ALT2, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC3_INP10 + {PC_1, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC1_INP11 + {PC_1_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC2_INP11 + {PC_1_ALT2, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC3_INP11 + {PC_2, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 12, 0)}, // ADC1_INP12 + {PC_2_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 12, 0)}, // ADC2_INP12 + {PC_2_ALT2, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 12, 0)}, // ADC3_INP12 + {PC_2_C, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 0, 0)}, // ADC3_INP0 + {PC_3, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 13, 0)}, // ADC1_INP13 + {PC_3_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 13, 0)}, // ADC2_INP13 + {PC_3_C, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // ADC3_INP1 + {PC_4, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC1_INP4 + {PC_4_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC2_INP4 + {PC_5, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC1_INP8 + {PC_5_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC2_INP8 + {PF_3, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC3_INP5 + {PF_4, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC3_INP9 + {PF_5, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC3_INP4 + {PF_6, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC3_INP8 + {PF_7, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC3_INP3 + {PF_8, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC3_INP7 + {PF_9, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC3_INP2 + {PF_10, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC3_INP6 + {PF_11, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC1_INP2 + {PF_12, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC1_INP6 + {PF_13, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC2_INP2 + {PF_14, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC2_INP6 + {PH_2, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 13, 0)}, // ADC3_INP13 + {PH_3, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 14, 0)}, // ADC3_INP14 + {PH_4, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC3_INP15 + {PH_5, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 16, 0)}, // ADC3_INP16 + {NC, NP, 0} +}; +#endif + +//*** DAC *** + +#ifdef HAL_DAC_MODULE_ENABLED +WEAK const PinMap PinMap_DAC[] = { + {PA_4, DAC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // DAC1_OUT1 + {PA_5, DAC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // DAC1_OUT2 + {NC, NP, 0} +}; +#endif + +//*** I2C *** + +#ifdef HAL_I2C_MODULE_ENABLED +WEAK const PinMap PinMap_I2C_SDA[] = { + {PB_7, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_7_ALT1, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C4)}, + {PB_9, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_9_ALT1, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C4)}, + {PB_11, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PC_9, I2C3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, + {PD_13, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {PF_0, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PF_15, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {PH_5, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PH_8, I2C3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, + {PH_12, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_I2C_MODULE_ENABLED +WEAK const PinMap PinMap_I2C_SCL[] = { + {PA_8, I2C3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, + {PB_6, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_6_ALT1, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C4)}, + {PB_8, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_8_ALT1, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C4)}, + {PB_10, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PD_12, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {PF_1, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PF_14, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {PH_4, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PH_7, I2C3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, + {PH_11, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {NC, NP, 0} +}; +#endif + +//*** No I3C *** + +//*** TIM *** + +#ifdef HAL_TIM_MODULE_ENABLED +WEAK const PinMap PinMap_TIM[] = { + {PA_0, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PA_0_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 1, 0)}, // TIM5_CH1 + {PA_0_C, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PA_0_C_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 1, 0)}, // TIM5_CH1 + {PA_1, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 + {PA_1_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 2, 0)}, // TIM5_CH2 + {PA_1_ALT2, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 1, 1)}, // TIM15_CH1N + {PA_1_C, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 + {PA_1_C_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 2, 0)}, // TIM5_CH2 + {PA_1_C_ALT2, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 1, 1)}, // TIM15_CH1N + {PA_2, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 + {PA_2_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 3, 0)}, // TIM5_CH3 + {PA_2_ALT2, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 1, 0)}, // TIM15_CH1 + {PA_3, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 4, 0)}, // TIM2_CH4 + {PA_3_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 4, 0)}, // TIM5_CH4 + {PA_3_ALT2, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 2, 0)}, // TIM15_CH2 + {PA_5, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PA_5_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 1)}, // TIM8_CH1N + {PA_6, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 + {PA_6_ALT1, TIM13, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM13, 1, 0)}, // TIM13_CH1 + {PA_7, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N + {PA_7_ALT1, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 + {PA_7_ALT2, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 1)}, // TIM8_CH1N + {PA_7_ALT3, TIM14, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM14, 1, 0)}, // TIM14_CH1 + {PA_8, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 0)}, // TIM1_CH1 + {PA_9, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 0)}, // TIM1_CH2 + {PA_10, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 0)}, // TIM1_CH3 + {PA_11, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 4, 0)}, // TIM1_CH4 + {PA_15, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PB_0, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N + {PB_0_ALT1, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 3, 0)}, // TIM3_CH3 + {PB_0_ALT2, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 1)}, // TIM8_CH2N + {PB_1, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N + {PB_1_ALT1, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 4, 0)}, // TIM3_CH4 + {PB_1_ALT2, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 1)}, // TIM8_CH3N + {PB_3, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 + {PB_4, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 + {PB_5, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 + {PB_6, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 1, 0)}, // TIM4_CH1 + {PB_6_ALT1, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM16, 1, 1)}, // TIM16_CH1N + {PB_7, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 2, 0)}, // TIM4_CH2 + {PB_7_ALT1, TIM17, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM17, 1, 1)}, // TIM17_CH1N + {PB_8, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 3, 0)}, // TIM4_CH3 + {PB_8_ALT1, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM16, 1, 0)}, // TIM16_CH1 + {PB_9, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 4, 0)}, // TIM4_CH4 + {PB_9_ALT1, TIM17, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM17, 1, 0)}, // TIM17_CH1 + {PB_10, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 + {PB_11, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 4, 0)}, // TIM2_CH4 + {PB_13, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N + {PB_14, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N + {PB_14_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 1)}, // TIM8_CH2N + {PB_14_ALT2, TIM12, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM12, 1, 0)}, // TIM12_CH1 + {PB_15, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N + {PB_15_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 1)}, // TIM8_CH3N + {PB_15_ALT2, TIM12, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM12, 2, 0)}, // TIM12_CH2 + {PC_6, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 + {PC_6_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 0)}, // TIM8_CH1 + {PC_7, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 + {PC_7_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 0)}, // TIM8_CH2 + {PC_8, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 3, 0)}, // TIM3_CH3 + {PC_8_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 0)}, // TIM8_CH3 + {PC_9, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 4, 0)}, // TIM3_CH4 + {PC_9_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 4, 0)}, // TIM8_CH4 + {PD_12, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 1, 0)}, // TIM4_CH1 + {PD_13, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 2, 0)}, // TIM4_CH2 + {PD_14, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 3, 0)}, // TIM4_CH3 + {PD_15, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 4, 0)}, // TIM4_CH4 + {PE_4, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 1, 1)}, // TIM15_CH1N + {PE_5, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 1, 0)}, // TIM15_CH1 + {PE_6, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 2, 0)}, // TIM15_CH2 + {PE_8, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N + {PE_9, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 0)}, // TIM1_CH1 + {PE_10, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N + {PE_11, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 0)}, // TIM1_CH2 + {PE_12, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N + {PE_13, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 0)}, // TIM1_CH3 + {PE_14, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 4, 0)}, // TIM1_CH4 + {PF_6, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM16, 1, 0)}, // TIM16_CH1 + {PF_7, TIM17, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM17, 1, 0)}, // TIM17_CH1 + {PF_8, TIM13, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM13, 1, 0)}, // TIM13_CH1 + {PF_8_ALT1, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM16, 1, 1)}, // TIM16_CH1N + {PF_9, TIM14, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM14, 1, 0)}, // TIM14_CH1 + {PF_9_ALT1, TIM17, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM17, 1, 1)}, // TIM17_CH1N + {PH_6, TIM12, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM12, 1, 0)}, // TIM12_CH1 + {PH_9, TIM12, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM12, 2, 0)}, // TIM12_CH2 + {PH_10, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 1, 0)}, // TIM5_CH1 + {PH_11, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 2, 0)}, // TIM5_CH2 + {PH_12, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 3, 0)}, // TIM5_CH3 + {PH_13, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 1)}, // TIM8_CH1N + {PH_14, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 1)}, // TIM8_CH2N + {PH_15, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 1)}, // TIM8_CH3N + {PI_0, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 4, 0)}, // TIM5_CH4 + {PI_2, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 4, 0)}, // TIM8_CH4 + {PI_5, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 0)}, // TIM8_CH1 + {PI_6, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 0)}, // TIM8_CH2 + {PI_7, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 0)}, // TIM8_CH3 + {PJ_6, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 0)}, // TIM8_CH2 + {PJ_7, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 1)}, // TIM8_CH2N + {PJ_8, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N + {PJ_8_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 0)}, // TIM8_CH1 + {PJ_9, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 0)}, // TIM1_CH3 + {PJ_9_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 1)}, // TIM8_CH1N + {PJ_10, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N + {PJ_10_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 0)}, // TIM8_CH2 + {PJ_11, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 0)}, // TIM1_CH2 + {PJ_11_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 1)}, // TIM8_CH2N + {PK_0, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N + {PK_0_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 0)}, // TIM8_CH3 + {PK_1, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 0)}, // TIM1_CH1 + {PK_1_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 1)}, // TIM8_CH3N + {NC, NP, 0} +}; +#endif + +//*** UART *** + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_TX[] = { + {PA_0, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PA_0_C, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PA_2, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_9, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_LPUART)}, + {PA_9_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PA_12, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_UART4)}, + {PA_15, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART7)}, + {PB_4, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART7)}, + {PB_6, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART)}, + {PB_6_ALT1, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_UART5)}, + {PB_6_ALT2, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_9, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_10, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PB_13, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_UART5)}, + {PB_14, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_USART1)}, + {PC_6, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PC_10, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PC_10_ALT1, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PC_12, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, + {PD_1, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PD_5, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PD_8, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PE_1, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {PE_8, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PF_7, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PG_14, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PH_13, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PJ_8, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_RX[] = { + {PA_1, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PA_1_C, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PA_3, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_8, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART7)}, + {PA_10, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_LPUART)}, + {PA_10_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PA_11, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_UART4)}, + {PB_3, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART7)}, + {PB_5, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_UART5)}, + {PB_7, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART)}, + {PB_7_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_8, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_11, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PB_12, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_UART5)}, + {PB_15, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_USART1)}, + {PC_7, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PC_11, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PC_11_ALT1, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PD_0, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PD_2, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, + {PD_6, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PD_9, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PE_0, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {PE_7, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PF_6, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PG_9, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PH_14, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PI_9, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PJ_9, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_RTS[] = { + {PA_1, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_1_C, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_12, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_LPUART)}, + {PA_12_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PA_15, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_14, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_14_ALT1, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PC_8, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, + {PD_4, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PD_12, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PD_15, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {PE_9, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PF_8, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PG_8, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PG_12, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_CTS[] = { + {PA_0, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_0_C, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_11, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_LPUART)}, + {PA_11_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_0, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_13, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PB_15, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PC_9, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, + {PD_3, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PD_11, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PD_14, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {PE_10, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PF_9, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PG_13, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PG_15, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {NC, NP, 0} +}; +#endif + +//*** SPI *** + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_MOSI[] = { + {PA_7, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_7_ALT1, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_2, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_SPI3)}, + {PB_5, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_5_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_SPI3)}, + {PB_5_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_15, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_1, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_3, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_3_C, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_12, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PD_6, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI3)}, + {PD_7, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PE_6, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PE_14, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PF_9, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PF_11, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PG_14, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {PI_3, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PJ_10, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_MISO[] = { + {PA_6, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_6_ALT1, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_4, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_4_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PB_4_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_14, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_2, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_2_C, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_11, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PE_5, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PE_13, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PF_8, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PG_9, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PG_12, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {PH_7, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PI_2, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PJ_11, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_SCLK[] = { + {PA_5, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_5_ALT1, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PA_9, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PA_12, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PB_3, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_3_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PB_3_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_10, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PB_13, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_10, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PD_3, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PE_2, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PE_12, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PF_7, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PG_11, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PG_13, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {PH_6, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PI_1, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PK_0, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_SSEL[] = { + {PA_4, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_4_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PA_4_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PA_11, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PA_15, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_15_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PA_15_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_SPI6)}, + {PB_4, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_SPI2)}, + {PB_9, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PB_12, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PE_4, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PE_11, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PF_6, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PG_8, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {PG_10, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PH_5, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PI_0, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PK_1, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {NC, NP, 0} +}; +#endif + +//*** FDCAN *** + +#ifdef HAL_FDCAN_MODULE_ENABLED +WEAK const PinMap PinMap_CAN_RD[] = { + {PH_14, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PA_11, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PB_5, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN2)}, + {PB_8, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PB_12, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN2)}, + {PD_0, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PI_9, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_FDCAN_MODULE_ENABLED +WEAK const PinMap PinMap_CAN_TD[] = { + {PD_1, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PA_12, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PB_6, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN2)}, + {PB_9, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PB_13, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN2)}, + {PH_13, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {NC, NP, 0} +}; +#endif + +//*** FDCAN *** + +#ifdef HAL_FDCAN_MODULE_ENABLED +WEAK const PinMap PinMap_FDCAN_TX[] = { + // Add the pin(s) you are using for FDCAN TX here + // Example for FDCAN1 on PA12 and PB9 + {PA_12, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_FDCAN1)}, + {PB_9, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_FDCAN1)}, + // Example for FDCAN2 on PB6 + {PB_6, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_FDCAN2)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_FDCAN_MODULE_ENABLED +WEAK const PinMap PinMap_FDCAN_RX[] = { + // Add the pin(s) you are using for FDCAN RX here + // Example for FDCAN1 on PA11 and PB8 + {PA_11, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_FDCAN1)}, + {PB_8, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_FDCAN1)}, + // Example for FDCAN2 on PB5 + {PB_5, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_FDCAN2)}, + {NC, NP, 0} +}; +#endif + +//*** ETHERNET *** + +#if defined(HAL_ETH_MODULE_ENABLED) || defined(HAL_ETH_LEGACY_MODULE_ENABLED) +WEAK const PinMap PinMap_Ethernet[] = { + {PA_0, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_CRS + {PA_0_C, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_CRS + {PA_1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_REF_CLK + {PA_1_ALT1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RX_CLK + {PA_1_C, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_REF_CLK + {PA_1_C_ALT1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RX_CLK + {PA_2, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_MDIO + {PA_3, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_COL + {PA_7, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_CRS_DV + {PA_7_ALT1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RX_DV + {PA_9, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_ER + {PB_0, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD2 + {PB_1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD3 + {PB_2, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_ER + {PB_5, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_PPS_OUT + {PB_8, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD3 + {PB_10, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RX_ER + {PB_11, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_EN + {PB_12, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD0 + {PB_13, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD1 + {PC_1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_MDC + {PC_2, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD2 + {PC_2_C, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD2 + {PC_3, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_CLK + {PC_3_C, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_CLK + {PC_4, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD0 + {PC_5, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD1 + {PE_2, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD3 + {PG_8, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_PPS_OUT + {PG_11, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_EN + {PG_12, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD1 + {PG_13, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD0 + {PG_14, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD1 + {PH_2, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_CRS + {PH_3, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_COL + {PH_6, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD2 + {PH_7, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD3 + {PI_10, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RX_ER + {PI_12, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_ER + {NC, NP, 0} +}; +#endif + +//*** QUADSPI *** + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_DATA0[] = { + {PC_9, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO0 + {PD_11, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO0 + {PE_7, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK2_IO0 + {PF_8, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO0 + {PH_2, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK2_IO0 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_DATA1[] = { + {PC_10, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO1 + {PD_12, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO1 + {PE_8, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK2_IO1 + {PF_9, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO1 + {PH_3, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK2_IO1 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_DATA2[] = { + {PE_2, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO2 + {PE_9, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK2_IO2 + {PF_7, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO2 + {PG_9, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK2_IO2 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_DATA3[] = { + {PA_1, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO3 + {PA_1_C, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO3 + {PD_13, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO3 + {PE_10, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK2_IO3 + {PF_6, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO3 + {PG_14, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK2_IO3 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_SCLK[] = { + {PB_2, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_CLK + {PF_10, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_CLK + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_SSEL[] = { + {PB_6, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_NCS + {PB_10, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_NCS + {PC_11, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK2_NCS + {PG_6, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_NCS + {NC, NP, 0} +}; +#endif + +//*** USB *** + +#if defined(HAL_PCD_MODULE_ENABLED) || defined(HAL_HCD_MODULE_ENABLED) +WEAK const PinMap PinMap_USB_OTG_FS[] = { + {PA_8, USB_OTG_FS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_FS)}, // USB_OTG_FS_SOF + {PA_9, USB_OTG_FS, STM_PIN_DATA(STM_MODE_INPUT, GPIO_NOPULL, GPIO_AF_NONE)}, // USB_OTG_FS_VBUS + {PA_10, USB_OTG_FS, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_PULLUP, GPIO_AF10_OTG1_FS)}, // USB_OTG_FS_ID + {PA_11, USB_OTG_FS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_FS)}, // USB_OTG_FS_DM + {PA_12, USB_OTG_FS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_FS)}, // USB_OTG_FS_DP + {NC, NP, 0} +}; +#endif + +#if defined(HAL_PCD_MODULE_ENABLED) || defined(HAL_HCD_MODULE_ENABLED) +WEAK const PinMap PinMap_USB_OTG_HS[] = { +#ifdef USE_USB_HS_IN_FS + {PA_4, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_OTG2_FS)}, // USB_OTG_HS_SOF + {PB_12, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_PULLUP, GPIO_AF12_OTG2_FS)}, // USB_OTG_HS_ID + {PB_13, USB_OTG_HS, STM_PIN_DATA(STM_MODE_INPUT, GPIO_NOPULL, GPIO_AF_NONE)}, // USB_OTG_HS_VBUS + {PB_14, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_OTG2_FS)}, // USB_OTG_HS_DM + {PB_15, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_OTG2_FS)}, // USB_OTG_HS_DP +#else + {PA_3, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_D0 + {PA_5, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_CK + {PB_0, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_D1 + {PB_1, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_D2 + {PB_5, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_D7 + {PB_10, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_D3 + {PB_11, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_D4 + {PB_12, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_D5 + {PB_13, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_D6 + {PC_0, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_STP + {PC_2, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_DIR + {PC_2_C, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_DIR + {PC_3, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_NXT + {PC_3_C, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_NXT + {PH_4, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_NXT + {PI_11, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG2_HS)}, // USB_OTG_HS_ULPI_DIR +#endif /* USE_USB_HS_IN_FS */ + {NC, NP, 0} +}; +#endif + +//*** SD *** + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_CMD[] = { + {PA_0, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_SDIO2)}, // SDMMC2_CMD + {PA_0_C, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_SDIO2)}, // SDMMC2_CMD + {PD_2, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF12_SDIO1)}, // SDMMC1_CMD + {PD_7, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_SDIO2)}, // SDMMC2_CMD + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_CK[] = { + {PC_1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_SDIO2)}, // SDMMC2_CK + {PC_12, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF12_SDIO1)}, // SDMMC1_CK + {PD_6, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_SDIO2)}, // SDMMC2_CK + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA0[] = { + {PB_14, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_SDIO2)}, // SDMMC2_D0 + {PC_8, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDIO1)}, // SDMMC1_D0 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA1[] = { + {PB_15, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_SDIO2)}, // SDMMC2_D1 + {PC_9, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDIO1)}, // SDMMC1_D1 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA2[] = { + {PB_3, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_SDIO2)}, // SDMMC2_D2 + {PC_10, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDIO1)}, // SDMMC1_D2 + {PG_11, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDIO2)}, // SDMMC2_D2 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA3[] = { + {PB_4, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_SDIO2)}, // SDMMC2_D3 + {PC_11, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDIO1)}, // SDMMC1_D3 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA4[] = { + {PB_8, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDIO1)}, // SDMMC1_D4 + {PB_8_ALT1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDIO2)}, // SDMMC2_D4 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA5[] = { + {PB_9, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDIO1)}, // SDMMC1_D5 + {PB_9_ALT1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDIO2)}, // SDMMC2_D5 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA6[] = { + {PC_6, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDIO1)}, // SDMMC1_D6 + {PC_6_ALT1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDIO2)}, // SDMMC2_D6 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA7[] = { + {PC_7, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDIO1)}, // SDMMC1_D7 + {PC_7_ALT1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDIO2)}, // SDMMC2_D7 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_CKIN[] = { + {PB_8, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF7_SDIO1)}, // SDMMC1_CKIN + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_CDIR[] = { + {PB_9, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF7_SDIO1)}, // SDMMC1_CDIR + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_D0DIR[] = { + {PC_6, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF8_SDIO1)}, // SDMMC1_D0DIR + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_D123DIR[] = { + {PC_7, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF8_SDIO1)}, // SDMMC1_D123DIR + {NC, NP, 0} +}; +#endif + +#endif /* !CUSTOM_PERIPHERAL_PINS */ diff --git a/variants/CoreNode/PinNamesVar.h b/variants/CoreNode/PinNamesVar.h new file mode 100644 index 0000000..3a37757 --- /dev/null +++ b/variants/CoreNode/PinNamesVar.h @@ -0,0 +1,140 @@ +/* Dual pad pin name */ +PA_0_C = PA_0 | PDUAL, +PA_1_C = PA_1 | PDUAL, +PC_2_C = PC_2 | PDUAL, +PC_3_C = PC_3 | PDUAL, + +/* Alternate pin name */ +PA_0_ALT1 = PA_0 | ALT1, +PA_0_C_ALT1 = PA_0_C | ALT1, +PA_1_ALT1 = PA_1 | ALT1, +PA_1_ALT2 = PA_1 | ALT2, +PA_1_C_ALT1 = PA_1_C | ALT1, +PA_1_C_ALT2 = PA_1_C | ALT2, +PA_2_ALT1 = PA_2 | ALT1, +PA_2_ALT2 = PA_2 | ALT2, +PA_3_ALT1 = PA_3 | ALT1, +PA_3_ALT2 = PA_3 | ALT2, +PA_4_ALT1 = PA_4 | ALT1, +PA_4_ALT2 = PA_4 | ALT2, +PA_5_ALT1 = PA_5 | ALT1, +PA_6_ALT1 = PA_6 | ALT1, +PA_7_ALT1 = PA_7 | ALT1, +PA_7_ALT2 = PA_7 | ALT2, +PA_7_ALT3 = PA_7 | ALT3, +PA_9_ALT1 = PA_9 | ALT1, +PA_10_ALT1 = PA_10 | ALT1, +PA_11_ALT1 = PA_11 | ALT1, +PA_12_ALT1 = PA_12 | ALT1, +PA_15_ALT1 = PA_15 | ALT1, +PA_15_ALT2 = PA_15 | ALT2, +PB_0_ALT1 = PB_0 | ALT1, +PB_0_ALT2 = PB_0 | ALT2, +PB_1_ALT1 = PB_1 | ALT1, +PB_1_ALT2 = PB_1 | ALT2, +PB_3_ALT1 = PB_3 | ALT1, +PB_3_ALT2 = PB_3 | ALT2, +PB_4_ALT1 = PB_4 | ALT1, +PB_4_ALT2 = PB_4 | ALT2, +PB_5_ALT1 = PB_5 | ALT1, +PB_5_ALT2 = PB_5 | ALT2, +PB_6_ALT1 = PB_6 | ALT1, +PB_6_ALT2 = PB_6 | ALT2, +PB_7_ALT1 = PB_7 | ALT1, +PB_8_ALT1 = PB_8 | ALT1, +PB_9_ALT1 = PB_9 | ALT1, +PB_14_ALT1 = PB_14 | ALT1, +PB_14_ALT2 = PB_14 | ALT2, +PB_15_ALT1 = PB_15 | ALT1, +PB_15_ALT2 = PB_15 | ALT2, +PC_0_ALT1 = PC_0 | ALT1, +PC_0_ALT2 = PC_0 | ALT2, +PC_1_ALT1 = PC_1 | ALT1, +PC_1_ALT2 = PC_1 | ALT2, +PC_2_ALT1 = PC_2 | ALT1, +PC_2_ALT2 = PC_2 | ALT2, +PC_3_ALT1 = PC_3 | ALT1, +PC_4_ALT1 = PC_4 | ALT1, +PC_5_ALT1 = PC_5 | ALT1, +PC_6_ALT1 = PC_6 | ALT1, +PC_7_ALT1 = PC_7 | ALT1, +PC_8_ALT1 = PC_8 | ALT1, +PC_9_ALT1 = PC_9 | ALT1, +PC_10_ALT1 = PC_10 | ALT1, +PC_11_ALT1 = PC_11 | ALT1, +PF_8_ALT1 = PF_8 | ALT1, +PF_9_ALT1 = PF_9 | ALT1, +PJ_8_ALT1 = PJ_8 | ALT1, +PJ_9_ALT1 = PJ_9 | ALT1, +PJ_10_ALT1 = PJ_10 | ALT1, +PJ_11_ALT1 = PJ_11 | ALT1, +PK_0_ALT1 = PK_0 | ALT1, +PK_1_ALT1 = PK_1 | ALT1, + +/* SYS_WKUP */ +#ifdef PWR_WAKEUP_PIN1 + SYS_WKUP1 = PA_0, +#endif +#ifdef PWR_WAKEUP_PIN2 + SYS_WKUP2 = PA_2, +#endif +#ifdef PWR_WAKEUP_PIN3 + SYS_WKUP3 = PI_8, +#endif +#ifdef PWR_WAKEUP_PIN4 + SYS_WKUP4 = PC_13, +#endif +#ifdef PWR_WAKEUP_PIN5 + SYS_WKUP5 = PI_11, +#endif +#ifdef PWR_WAKEUP_PIN6 + SYS_WKUP6 = PC_1, +#endif +#ifdef PWR_WAKEUP_PIN7 + SYS_WKUP7 = NC, +#endif +#ifdef PWR_WAKEUP_PIN8 + SYS_WKUP8 = NC, +#endif + +/* USB */ +#ifdef USBCON + USB_OTG_FS_DM = PA_11, + USB_OTG_FS_DP = PA_12, + USB_OTG_FS_ID = PA_10, + USB_OTG_FS_SOF = PA_8, + USB_OTG_FS_VBUS = PA_9, + USB_OTG_HS_DM = PB_14, + USB_OTG_HS_DP = PB_15, + USB_OTG_HS_ID = PB_12, + USB_OTG_HS_SOF = PA_4, + USB_OTG_HS_ULPI_CK = PA_5, + USB_OTG_HS_ULPI_D0 = PA_3, + USB_OTG_HS_ULPI_D1 = PB_0, + USB_OTG_HS_ULPI_D2 = PB_1, + USB_OTG_HS_ULPI_D3 = PB_10, + USB_OTG_HS_ULPI_D4 = PB_11, + USB_OTG_HS_ULPI_D5 = PB_12, + USB_OTG_HS_ULPI_D6 = PB_13, + USB_OTG_HS_ULPI_D7 = PB_5, + #ifdef USB_OTG_HS_ULPI_DIR_PC_2 + USB_OTG_HS_ULPI_DIR = PC_2, + #endif + #ifdef USB_OTG_HS_ULPI_DIR_PC_2_C + USB_OTG_HS_ULPI_DIR = PC_2_C, + #endif + #ifdef USB_OTG_HS_ULPI_DIR_PI_11 + USB_OTG_HS_ULPI_DIR = PI_11, + #endif + #ifdef USB_OTG_HS_ULPI_NXT_PC_3 + USB_OTG_HS_ULPI_NXT = PC_3, + #endif + #ifdef USB_OTG_HS_ULPI_NXT_PC_3_C + USB_OTG_HS_ULPI_NXT = PC_3_C, + #endif + #ifdef USB_OTG_HS_ULPI_NXT_PH_4 + USB_OTG_HS_ULPI_NXT = PH_4, + #endif + USB_OTG_HS_ULPI_STP = PC_0, + USB_OTG_HS_VBUS = PB_13, +#endif diff --git a/variants/CoreNode/boards_entry.txt b/variants/CoreNode/boards_entry.txt new file mode 100644 index 0000000..1ff3420 --- /dev/null +++ b/variants/CoreNode/boards_entry.txt @@ -0,0 +1,13 @@ +# This file help to add generic board entry. +# upload.maximum_size and product_line have to be verified +# and changed if needed. +# See: https://github.com/stm32duino/Arduino_Core_STM32/wiki/Add-a-new-variant-%28board%29 + +# Generic H743XIHx +GenH7.menu.pnum.GENERIC_H743XIHX=Generic H743XIHx +GenH7.menu.pnum.GENERIC_H743XIHX.upload.maximum_size=2097152 +GenH7.menu.pnum.GENERIC_H743XIHX.upload.maximum_data_size=884736 +GenH7.menu.pnum.GENERIC_H743XIHX.build.board=GENERIC_H743XIHX +GenH7.menu.pnum.GENERIC_H743XIHX.build.product_line=STM32H743xx +GenH7.menu.pnum.GENERIC_H743XIHX.build.variant=STM32H7xx/H742X(G-I)H_H743X(G-I)H_H745X(G-I)H_H747X(G-I)H_H750XBH_H753XIH_H755XIH_H757XIH +GenH7.menu.pnum.GENERIC_H743XIHX.debug.svd_file={runtime.tools.STM32_SVD.path}/svd/STM32H7xx/STM32H743.svd \ No newline at end of file diff --git a/variants/CoreNode/generic_clock.c b/variants/CoreNode/generic_clock.c new file mode 100644 index 0000000..4f9b4b9 --- /dev/null +++ b/variants/CoreNode/generic_clock.c @@ -0,0 +1,150 @@ +/* + ******************************************************************************* + * Copyright (c) 2020-2021, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +// #if defined(ARDUINO_GENERIC_H742XGHX) || defined(ARDUINO_GENERIC_H742XIHX) ||\ +// defined(ARDUINO_GENERIC_H743XGHX) || defined(ARDUINO_GENERIC_H743XIHX) ||\ +// defined(ARDUINO_GENERIC_H745XGHX) || defined(ARDUINO_GENERIC_H745XIHX) ||\ +// defined(ARDUINO_GENERIC_H747XGHX) || defined(ARDUINO_GENERIC_H747XIHX) ||\ +// defined(ARDUINO_GENERIC_H750XBHX) || defined(ARDUINO_GENERIC_H753XIHX) ||\ +// defined(ARDUINO_GENERIC_H755XIHX) || defined(ARDUINO_GENERIC_H757XIHX) +#include "pins_arduino.h" + +/** + * @brief System Clock Configuration + * @param None + * @retval None + */ +WEAK void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {}; + + /** Supply configuration update enable + */ +#if defined(SMPS) + /** If SMPS is available on this MCU, assume that the MCU's board is + * built to power the MCU using the SMPS since it's more efficient. + * In this case, we must configure the MCU to use DIRECT_SMPS_SUPPLY. + * + * N.B.: if the hardware configuration does not match the argument to + * HAL_PWREx_ConfigSupply(), the board will deadlock at this function call. + * This can manifest immediately or after a RESET/power cycle. + * + * Trying to flash the board at this point will result in errors such as + * "No STM32 target found". To overcome this problem, erase the MCU's flash. + * + * The following settings in STM32CubeProgrammer appear to work for this purpose: + * - Mode: Power down + * - Reset mode: Hardware reset + */ + HAL_PWREx_ConfigSupply(PWR_DIRECT_SMPS_SUPPLY); +#else + /** No SMPS available: use the internal voltage regulator (LDO). + */ + HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY); +#endif + + /** Configure the main internal regulator output voltage + */ + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0); + + while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) + { + } + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_CSI | RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_LSI; + RCC_OscInitStruct.HSIState = RCC_HSI_DIV1; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.LSIState = RCC_LSI_ON; + RCC_OscInitStruct.HSI48State = RCC_HSI48_ON; + RCC_OscInitStruct.CSIState = RCC_CSI_ON; + RCC_OscInitStruct.CSICalibrationValue = RCC_CSICALIBRATION_DEFAULT; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; + RCC_OscInitStruct.PLL.PLLM = 4; + RCC_OscInitStruct.PLL.PLLN = 60; + RCC_OscInitStruct.PLL.PLLP = 2; + RCC_OscInitStruct.PLL.PLLQ = 6; + RCC_OscInitStruct.PLL.PLLR = 2; + RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3; + RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; + RCC_OscInitStruct.PLL.PLLFRACN = 0; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2 | RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; + RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; + RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; + RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) + { + Error_Handler(); + } + + // ... (code before clock configuration is fine) + + // 1. Initialize PeriphClkInitStruct + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; + + /** Initializes the peripherals clock + */ + // 2. Select ALL peripherals, including FDCAN + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC | RCC_PERIPHCLK_CEC | RCC_PERIPHCLK_CKPER | RCC_PERIPHCLK_HRTIM1 | RCC_PERIPHCLK_I2C123 | RCC_PERIPHCLK_I2C4 | RCC_PERIPHCLK_LPTIM1 | RCC_PERIPHCLK_LPTIM2 | RCC_PERIPHCLK_LPTIM3 | RCC_PERIPHCLK_QSPI | RCC_PERIPHCLK_SDMMC | RCC_PERIPHCLK_USB | RCC_PERIPHCLK_SPI123 | RCC_PERIPHCLK_SPI45 | RCC_PERIPHCLK_SPI6 | RCC_PERIPHCLK_LPUART1 | RCC_PERIPHCLK_USART16 | RCC_PERIPHCLK_USART234578 | RCC_PERIPHCLK_RTC | RCC_PERIPHCLK_FDCAN ; + + // 3. Set ALL peripheral clock sources (INCLUDING FDCAN) + + /** Initializes the CANBUS peripherals clock + */ + PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; // This is correct! + + // NOTE: You must also set the clock sources for all other peripherals + // listed in PeriphClockSelection before calling HAL_RCCEx_PeriphCLKConfig. + PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_CLKP; + PeriphClkInitStruct.CecClockSelection = RCC_CECCLKSOURCE_CSI; + PeriphClkInitStruct.CkperClockSelection = RCC_CLKPSOURCE_HSI; + PeriphClkInitStruct.Hrtim1ClockSelection = RCC_HRTIM1CLK_CPUCLK; + PeriphClkInitStruct.I2c123ClockSelection = RCC_I2C123CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.I2c4ClockSelection = RCC_I2C4CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Lptim1ClockSelection = RCC_LPTIM1CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.Lptim2ClockSelection = RCC_LPTIM2CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Lptim345ClockSelection = RCC_LPTIM345CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Lpuart1ClockSelection = RCC_LPUART1CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.QspiClockSelection = RCC_QSPICLKSOURCE_PLL; + PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSI; + PeriphClkInitStruct.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL; + PeriphClkInitStruct.Spi123ClockSelection = RCC_SPI123CLKSOURCE_PLL; + PeriphClkInitStruct.Spi45ClockSelection = RCC_SPI45CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.Spi6ClockSelection = RCC_SPI6CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Usart16ClockSelection = RCC_USART16CLKSOURCE_D2PCLK2; + PeriphClkInitStruct.Usart234578ClockSelection = RCC_USART234578CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_HSI48; + + // 4. CALL the function ONCE at the end + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) + { + Error_Handler(); // Should now pass! + } +} // End of SystemClock_Config + +// #endif /* ARDUINO_GENERIC_* */ diff --git a/variants/CoreNode/ldscript-no-bootloader.ld b/variants/CoreNode/ldscript-no-bootloader.ld new file mode 100644 index 0000000..151cc64 --- /dev/null +++ b/variants/CoreNode/ldscript-no-bootloader.ld @@ -0,0 +1,195 @@ +/* +****************************************************************************** +** +** File : LinkerScript.ld +** +** Author : STM32CubeIDE +** +** Abstract : Linker script for STM32H7XIHx series +** 2048Kbytes FLASH +** 512Kbytes RAM_D1 +** 288Kbytes RAM_D2 (unused) +** 64Kbytes RAM_D3 (unused) +** 128Kbytes ITCMRAM (unused) +** 64Kbytes DTCMRAM (unused) +** +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Distribution: The file is distributed as is, without any warranty +** of any kind. +** +***************************************************************************** +** @attention +** +** Copyright (c) 2023 STMicroelectronics. +** All rights reserved. +** +** This software component is licensed by ST under BSD 3-Clause license, +** the "License"; You may not use this file except in compliance with the +** License. You may obtain a copy of the License at: +** opensource.org/licenses/BSD-3-Clause +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(RAM_D1) + LENGTH(RAM_D1); /* end of RAM_D1 */ +/* Generate a link error if heap and stack don't fit into RAM_D1 */ +_Min_Heap_Size = 0x200; /* required amount of heap */ +_Min_Stack_Size = 0x400; /* required amount of stack */ + +/* Specify the memory areas */ +MEMORY +{ + FLASH (rx) : ORIGIN = 0x08000000 + LD_FLASH_OFFSET, LENGTH = LD_MAX_SIZE - LD_FLASH_OFFSET + DTCMRAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K + RAM_D1 (xrw) : ORIGIN = 0x24000000, LENGTH = LD_MAX_DATA_SIZE + RAM_D2 (xrw) : ORIGIN = 0x30000000, LENGTH = 288K + RAM_D3 (xrw) : ORIGIN = 0x38000000, LENGTH = 64K + ITCMRAM (xrw) : ORIGIN = 0x00000000, LENGTH = 64K +} + +/* Sections */ +SECTIONS +{ + /* The startup code into "FLASH" Rom type memory */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data into "FLASH" Rom type memory */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data into "FLASH" Rom type memory */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab (READONLY) : { + . = ALIGN(4); + *(.ARM.extab* .gnu.linkonce.armextab.*) + . = ALIGN(4); + } >FLASH + + .ARM (READONLY) : { + . = ALIGN(4); + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + . = ALIGN(4); + } >FLASH + + .preinit_array (READONLY) : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + . = ALIGN(4); + } >FLASH + + .init_array (READONLY) : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + . = ALIGN(4); + } >FLASH + + .fini_array (READONLY) : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + . = ALIGN(4); + } >FLASH + + /* Used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections into "RAM" Ram type memory */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + *(.RamFunc) /* .RamFunc sections */ + *(.RamFunc*) /* .RamFunc* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM_D1 AT> FLASH + + /* Uninitialized data section into "RAM" Ram type memory */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss section */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM_D1 + + /* User_heap_stack section, used to check that there is enough "RAM" Ram type memory left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM_D1 + + /* Remove information from the compiler libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} + diff --git a/variants/CoreNode/ldscript.ld b/variants/CoreNode/ldscript.ld new file mode 100644 index 0000000..ce975f1 --- /dev/null +++ b/variants/CoreNode/ldscript.ld @@ -0,0 +1,195 @@ +/* +****************************************************************************** +** +** File : LinkerScript.ld +** +** Author : STM32CubeIDE +** +** Abstract : Linker script for STM32H7XIHx series +** 2048Kbytes FLASH +** 512Kbytes RAM_D1 +** 288Kbytes RAM_D2 (unused) +** 64Kbytes RAM_D3 (unused) +** 128Kbytes ITCMRAM (unused) +** 64Kbytes DTCMRAM (unused) +** +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Distribution: The file is distributed as is, without any warranty +** of any kind. +** +***************************************************************************** +** @attention +** +** Copyright (c) 2023 STMicroelectronics. +** All rights reserved. +** +** This software component is licensed by ST under BSD 3-Clause license, +** the "License"; You may not use this file except in compliance with the +** License. You may obtain a copy of the License at: +** opensource.org/licenses/BSD-3-Clause +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(RAM_D1) + LENGTH(RAM_D1); /* end of RAM_D1 */ +/* Generate a link error if heap and stack don't fit into RAM_D1 */ +_Min_Heap_Size = 0x200; /* required amount of heap */ +_Min_Stack_Size = 0x400; /* required amount of stack */ + +/* Specify the memory areas */ +MEMORY +{ + FLASH (rx) : ORIGIN = 0x0800A000 + LD_FLASH_OFFSET, LENGTH = LD_MAX_SIZE - LD_FLASH_OFFSET + DTCMRAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K + RAM_D1 (xrw) : ORIGIN = 0x24000000, LENGTH = LD_MAX_DATA_SIZE + RAM_D2 (xrw) : ORIGIN = 0x30000000, LENGTH = 288K + RAM_D3 (xrw) : ORIGIN = 0x38000000, LENGTH = 64K + ITCMRAM (xrw) : ORIGIN = 0x00000000, LENGTH = 64K +} + +/* Sections */ +SECTIONS +{ + /* The startup code into "FLASH" Rom type memory */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data into "FLASH" Rom type memory */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data into "FLASH" Rom type memory */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab (READONLY) : { + . = ALIGN(4); + *(.ARM.extab* .gnu.linkonce.armextab.*) + . = ALIGN(4); + } >FLASH + + .ARM (READONLY) : { + . = ALIGN(4); + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + . = ALIGN(4); + } >FLASH + + .preinit_array (READONLY) : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + . = ALIGN(4); + } >FLASH + + .init_array (READONLY) : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + . = ALIGN(4); + } >FLASH + + .fini_array (READONLY) : + { + . = ALIGN(4); + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + . = ALIGN(4); + } >FLASH + + /* Used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections into "RAM" Ram type memory */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + *(.RamFunc) /* .RamFunc sections */ + *(.RamFunc*) /* .RamFunc* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM_D1 AT> FLASH + + /* Uninitialized data section into "RAM" Ram type memory */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss section */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM_D1 + + /* User_heap_stack section, used to check that there is enough "RAM" Ram type memory left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM_D1 + + /* Remove information from the compiler libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} + diff --git a/variants/CoreNode/st_nucleo_h743zi.cfg b/variants/CoreNode/st_nucleo_h743zi.cfg new file mode 100644 index 0000000..ff01819 --- /dev/null +++ b/variants/CoreNode/st_nucleo_h743zi.cfg @@ -0,0 +1,12 @@ +# SPDX-License-Identifier: GPL-2.0-or-later + +# This is an ST NUCLEO-H743ZI board with single STM32H743ZI chip. +# http://www.st.com/en/evaluation-tools/nucleo-h743zi.html + +source [find interface/stlink.cfg] + +transport select hla_swd + +source [find target/stm32h7x_dual_bank.cfg] + +reset_config none diff --git a/variants/CoreNode/variant_CORENODE.cpp b/variants/CoreNode/variant_CORENODE.cpp new file mode 100644 index 0000000..42d727c --- /dev/null +++ b/variants/CoreNode/variant_CORENODE.cpp @@ -0,0 +1,358 @@ +/* + ******************************************************************************* + * Copyright (c) 2020, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +// #if defined(ARDUINO_GENERIC_H742XGHX) || defined(ARDUINO_GENERIC_H742XIHX) ||\ +// defined(ARDUINO_GENERIC_H743XGHX) || defined(ARDUINO_GENERIC_H743XIHX) ||\ +// defined(ARDUINO_GENERIC_H745XGHX) || defined(ARDUINO_GENERIC_H745XIHX) ||\ +// defined(ARDUINO_GENERIC_H747XGHX) || defined(ARDUINO_GENERIC_H747XIHX) ||\ +// defined(ARDUINO_GENERIC_H750XBHX) || defined(ARDUINO_GENERIC_H753XIHX) ||\ +// defined(ARDUINO_GENERIC_H755XIHX) || defined(ARDUINO_GENERIC_H757XIHX) +#include "pins_arduino.h" + +// Digital PinName array +const PinName digitalPin[] = { + PA_0, // D0/A0 + PA_1, // D1/A1 + PA_2, // D2/A2 + PA_3, // D3/A3 + PA_4, // D4/A4 + PA_5, // D5/A5 + PA_6, // D6/A6 + PA_7, // D7/A7 + PA_8, // D8 + PA_9, // D9 + PA_10, // D10 + PA_11, // D11 + PA_12, // D12 + PA_13, // D13 + PA_14, // D14 + PA_15, // D15 + PB_0, // D16/A8 + PB_1, // D17/A9 + PB_2, // D18 + PB_3, // D19 + PB_4, // D20 + PB_5, // D21 + PB_6, // D22 + PB_7, // D23 + PB_8, // D24 + PB_9, // D25 + PB_10, // D26 + PB_11, // D27 + PB_12, // D28 + PB_13, // D29 + PB_14, // D30 + PB_15, // D31 + PC_0, // D32/A10 + PC_1, // D33/A11 + PC_2, // D34/A12 + PC_3, // D35/A13 + PC_4, // D36/A14 + PC_5, // D37/A15 + PC_6, // D38 + PC_7, // D39 + PC_8, // D40 + PC_9, // D41 + PC_10, // D42 + PC_11, // D43 + PC_12, // D44 + PC_13, // D45 + PC_14, // D46 + PC_15, // D47 + PD_0, // D48 + PD_1, // D49 + PD_2, // D50 + PD_3, // D51 + PD_4, // D52 + PD_5, // D53 + PD_6, // D54 + PD_7, // D55 + PD_8, // D56 + PD_9, // D57 + PD_10, // D58 + PD_11, // D59 + PD_12, // D60 + PD_13, // D61 + PD_14, // D62 + PD_15, // D63 + PE_0, // D64 + PE_1, // D65 + PE_2, // D66 + PE_3, // D67 + PE_4, // D68 + PE_5, // D69 + PE_6, // D70 + PE_7, // D71 + PE_8, // D72 + PE_9, // D73 + PE_10, // D74 + PE_11, // D75 + PE_12, // D76 + PE_13, // D77 + PE_14, // D78 + PE_15, // D79 + PF_0, // D80 + PF_1, // D81 + PF_2, // D82 + PF_3, // D83/A16 + PF_4, // D84/A17 + PF_5, // D85/A18 + PF_6, // D86/A19 + PF_7, // D87/A20 + PF_8, // D88/A21 + PF_9, // D89/A22 + PF_10, // D90/A23 + PF_11, // D91/A24 + PF_12, // D92/A25 + PF_13, // D93/A26 + PF_14, // D94/A27 + PF_15, // D95 + PG_0, // D96 + PG_1, // D97 + PG_2, // D98 + PG_3, // D99 + PG_4, // D100 + PG_5, // D101 + PG_6, // D102 + PG_7, // D103 + PG_8, // D104 + PG_9, // D105 + PG_10, // D106 + PG_11, // D107 + PG_12, // D108 + PG_13, // D109 + PG_14, // D110 + PG_15, // D111 + PH_0, // D112 + PH_1, // D113 + PH_2, // D114/A28 + PH_3, // D115/A29 + PH_4, // D116/A30 + PH_5, // D117/A31 + PH_6, // D118 + PH_7, // D119 + PH_8, // D120 + PH_9, // D121 + PH_10, // D122 + PH_11, // D123 + PH_12, // D124 + PH_13, // D125 + PH_14, // D126 + PH_15, // D127 + PI_0, // D128 + PI_1, // D129 + PI_2, // D130 + PI_3, // D131 + PI_4, // D132 + PI_5, // D133 + PI_6, // D134 + PI_7, // D135 + PI_8, // D136 + PI_9, // D137 + PI_10, // D138 + PI_11, // D139 + PI_12, // D140 + PI_13, // D141 + PI_14, // D142 + PI_15, // D143 + PJ_0, // D144 + PJ_1, // D145 + PJ_2, // D146 + PJ_3, // D147 + PJ_4, // D148 + PJ_5, // D149 + PJ_6, // D150 + PJ_7, // D151 + PJ_8, // D152 + PJ_9, // D153 + PJ_10, // D154 + PJ_11, // D155 + PJ_12, // D156 + PJ_13, // D157 + PJ_14, // D158 + PJ_15, // D159 + PK_0, // D160 + PK_1, // D161 + PK_2, // D162 + PK_3, // D163 + PK_4, // D164 + PK_5, // D165 + PK_6, // D166 + PK_7, // D167 + PA_0_C, // D168/A32 + PA_1_C, // D169/A33 + PC_2_C, // D170/A34 + PC_3_C // D171/A35 +}; + +// Analog (Ax) pin number array +const uint32_t analogInputPin[] = { + 0, // A0, PA0 + 1, // A1, PA1 + 2, // A2, PA2 + 3, // A3, PA3 + 4, // A4, PA4 + 5, // A5, PA5 + 6, // A6, PA6 + 7, // A7, PA7 + 16, // A8, PB0 + 17, // A9, PB1 + 32, // A10, PC0 + 33, // A11, PC1 + 34, // A12, PC2 + 35, // A13, PC3 + 36, // A14, PC4 + 37, // A15, PC5 + 83, // A16, PF3 + 84, // A17, PF4 + 85, // A18, PF5 + 86, // A19, PF6 + 87, // A20, PF7 + 88, // A21, PF8 + 89, // A22, PF9 + 90, // A23, PF10 + 91, // A24, PF11 + 92, // A25, PF12 + 93, // A26, PF13 + 94, // A27, PF14 + 114, // A28, PH2 + 115, // A29, PH3 + 116, // A30, PH4 + 117, // A31, PH5 + 168, // A32, PA0_C + 169, // A33, PA1_C + 170, // A34, PC2_C + 171 // A35, PC3_C +}; + +// #endif /* ARDUINO_GENERIC_* */ +/** + * @brief System Clock Configuration + * @param None + * @retval None + */ +WEAK void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {}; + + /** Supply configuration update enable + */ +#if defined(SMPS) + /** If SMPS is available on this MCU, assume that the MCU's board is + * built to power the MCU using the SMPS since it's more efficient. + * In this case, we must configure the MCU to use DIRECT_SMPS_SUPPLY. + * + * N.B.: if the hardware configuration does not match the argument to + * HAL_PWREx_ConfigSupply(), the board will deadlock at this function call. + * This can manifest immediately or after a RESET/power cycle. + * + * Trying to flash the board at this point will result in errors such as + * "No STM32 target found". To overcome this problem, erase the MCU's flash. + * + * The following settings in STM32CubeProgrammer appear to work for this purpose: + * - Mode: Power down + * - Reset mode: Hardware reset + */ + HAL_PWREx_ConfigSupply(PWR_DIRECT_SMPS_SUPPLY); +#else + /** No SMPS available: use the internal voltage regulator (LDO). + */ + HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY); +#endif + + /** Configure the main internal regulator output voltage + */ + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0); + + while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) + { + } + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_CSI | RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_LSI; + RCC_OscInitStruct.HSIState = RCC_HSI_DIV1; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.LSIState = RCC_LSI_ON; + RCC_OscInitStruct.HSI48State = RCC_HSI48_ON; + RCC_OscInitStruct.CSIState = RCC_CSI_ON; + RCC_OscInitStruct.CSICalibrationValue = RCC_CSICALIBRATION_DEFAULT; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; + RCC_OscInitStruct.PLL.PLLM = 4; + RCC_OscInitStruct.PLL.PLLN = 60; + RCC_OscInitStruct.PLL.PLLP = 2; + RCC_OscInitStruct.PLL.PLLQ = 5; + RCC_OscInitStruct.PLL.PLLR = 2; + RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3; + RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; + RCC_OscInitStruct.PLL.PLLFRACN = 0; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2 | RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; + RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; + RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; + RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) + { + Error_Handler(); + } + + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; + + /** Initializes the peripherals clock + */ + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_ADC | RCC_PERIPHCLK_CEC | RCC_PERIPHCLK_CKPER | RCC_PERIPHCLK_HRTIM1 | RCC_PERIPHCLK_I2C123 | RCC_PERIPHCLK_I2C4 | RCC_PERIPHCLK_LPTIM1 | RCC_PERIPHCLK_LPTIM2 | RCC_PERIPHCLK_LPTIM3 | RCC_PERIPHCLK_QSPI | RCC_PERIPHCLK_SDMMC | RCC_PERIPHCLK_USB | RCC_PERIPHCLK_SPI123 | RCC_PERIPHCLK_SPI45 | RCC_PERIPHCLK_SPI6 | RCC_PERIPHCLK_LPUART1 | RCC_PERIPHCLK_USART16 | RCC_PERIPHCLK_USART234578 | RCC_PERIPHCLK_RTC | RCC_PERIPHCLK_FDCAN; + + /** Initializes the CANBUS peripherals clock + */ + + PeriphClkInitStruct.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) + { + Error_Handler(); + } + PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_CLKP; + PeriphClkInitStruct.CecClockSelection = RCC_CECCLKSOURCE_CSI; + PeriphClkInitStruct.CkperClockSelection = RCC_CLKPSOURCE_HSI; + PeriphClkInitStruct.Hrtim1ClockSelection = RCC_HRTIM1CLK_CPUCLK; + PeriphClkInitStruct.I2c123ClockSelection = RCC_I2C123CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.I2c4ClockSelection = RCC_I2C4CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Lptim1ClockSelection = RCC_LPTIM1CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.Lptim2ClockSelection = RCC_LPTIM2CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Lptim345ClockSelection = RCC_LPTIM345CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Lpuart1ClockSelection = RCC_LPUART1CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.QspiClockSelection = RCC_QSPICLKSOURCE_PLL; + PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSI; + PeriphClkInitStruct.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL; + PeriphClkInitStruct.Spi123ClockSelection = RCC_SPI123CLKSOURCE_PLL; + PeriphClkInitStruct.Spi45ClockSelection = RCC_SPI45CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.Spi6ClockSelection = RCC_SPI6CLKSOURCE_D3PCLK1; + PeriphClkInitStruct.Usart16ClockSelection = RCC_USART16CLKSOURCE_D2PCLK2; + PeriphClkInitStruct.Usart234578ClockSelection = RCC_USART234578CLKSOURCE_D2PCLK1; + PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_HSI48; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) + { + Error_Handler(); + } +} diff --git a/variants/CoreNode/variant_CORENODE.h b/variants/CoreNode/variant_CORENODE.h new file mode 100644 index 0000000..692a0e2 --- /dev/null +++ b/variants/CoreNode/variant_CORENODE.h @@ -0,0 +1,370 @@ +/* + ******************************************************************************* + * Copyright (c) 2020, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +#pragma once + +/*---------------------------------------------------------------------------- + * STM32 pins number + *----------------------------------------------------------------------------*/ +#define PA0 PIN_A0 +#define PA1 PIN_A1 +#define PA2 PIN_A2 +#define PA3 PIN_A3 +#define PA4 PIN_A4 +#define PA5 PIN_A5 +#define PA6 PIN_A6 +#define PA7 PIN_A7 +#define PA8 8 +#define PA9 9 +#define PA10 10 +#define PA11 11 +#define PA12 12 +#define PA13 13 +#define PA14 14 +#define PA15 15 +#define PB0 PIN_A8 +#define PB1 PIN_A9 +#define PB2 18 +#define PB3 19 +#define PB4 20 +#define PB5 21 +#define PB6 22 +#define PB7 23 +#define PB8 24 +#define PB9 25 +#define PB10 26 +#define PB11 27 +#define PB12 28 +#define PB13 29 +#define PB14 30 +#define PB15 31 +#define PC0 PIN_A10 +#define PC1 PIN_A11 +#define PC2 PIN_A12 +#define PC3 PIN_A13 +#define PC4 PIN_A14 +#define PC5 PIN_A15 +#define PC6 38 +#define PC7 39 +#define PC8 40 +#define PC9 41 +#define PC10 42 +#define PC11 43 +#define PC12 44 +#define PC13 45 +#define PC14 46 +#define PC15 47 +#define PD0 48 +#define PD1 49 +#define PD2 50 +#define PD3 51 +#define PD4 52 +#define PD5 53 +#define PD6 54 +#define PD7 55 +#define PD8 56 +#define PD9 57 +#define PD10 58 +#define PD11 59 +#define PD12 60 +#define PD13 61 +#define PD14 62 +#define PD15 63 +#define PE0 64 +#define PE1 65 +#define PE2 66 +#define PE3 67 +#define PE4 68 +#define PE5 69 +#define PE6 70 +#define PE7 71 +#define PE8 72 +#define PE9 73 +#define PE10 74 +#define PE11 75 +#define PE12 76 +#define PE13 77 +#define PE14 78 +#define PE15 79 +#define PF0 80 +#define PF1 81 +#define PF2 82 +#define PF3 PIN_A16 +#define PF4 PIN_A17 +#define PF5 PIN_A18 +#define PF6 PIN_A19 +#define PF7 PIN_A20 +#define PF8 PIN_A21 +#define PF9 PIN_A22 +#define PF10 PIN_A23 +#define PF11 PIN_A24 +#define PF12 PIN_A25 +#define PF13 PIN_A26 +#define PF14 PIN_A27 +#define PF15 95 +#define PG0 96 +#define PG1 97 +#define PG2 98 +#define PG3 99 +#define PG4 100 +#define PG5 101 +#define PG6 102 +#define PG7 103 +#define PG8 104 +#define PG9 105 +#define PG10 106 +#define PG11 107 +#define PG12 108 +#define PG13 109 +#define PG14 110 +#define PG15 111 +#define PH0 112 +#define PH1 113 +#define PH2 PIN_A28 +#define PH3 PIN_A29 +#define PH4 PIN_A30 +#define PH5 PIN_A31 +#define PH6 118 +#define PH7 119 +#define PH8 120 +#define PH9 121 +#define PH10 122 +#define PH11 123 +#define PH12 124 +#define PH13 125 +#define PH14 126 +#define PH15 127 +#define PI0 128 +#define PI1 129 +#define PI2 130 +#define PI3 131 +#define PI4 132 +#define PI5 133 +#define PI6 134 +#define PI7 135 +#define PI8 136 +#define PI9 137 +#define PI10 138 +#define PI11 139 +#define PI12 140 +#define PI13 141 +#define PI14 142 +#define PI15 143 +#define PJ0 144 +#define PJ1 145 +#define PJ2 146 +#define PJ3 147 +#define PJ4 148 +#define PJ5 149 +#define PJ6 150 +#define PJ7 151 +#define PJ8 152 +#define PJ9 153 +#define PJ10 154 +#define PJ11 155 +#define PJ12 156 +#define PJ13 157 +#define PJ14 158 +#define PJ15 159 +#define PK0 160 +#define PK1 161 +#define PK2 162 +#define PK3 163 +#define PK4 164 +#define PK5 165 +#define PK6 166 +#define PK7 167 +#define PA0_C PIN_A32 +#define PA1_C PIN_A33 +#define PC2_C PIN_A34 +#define PC3_C PIN_A35 + +// Alternate pins number +#define PA0_ALT1 (PA0 | ALT1) +#define PA0_C_ALT1 (PA0_C | ALT1) +#define PA1_ALT1 (PA1 | ALT1) +#define PA1_ALT2 (PA1 | ALT2) +#define PA1_C_ALT1 (PA1_C | ALT1) +#define PA1_C_ALT2 (PA1_C | ALT2) +#define PA2_ALT1 (PA2 | ALT1) +#define PA2_ALT2 (PA2 | ALT2) +#define PA3_ALT1 (PA3 | ALT1) +#define PA3_ALT2 (PA3 | ALT2) +#define PA4_ALT1 (PA4 | ALT1) +#define PA4_ALT2 (PA4 | ALT2) +#define PA5_ALT1 (PA5 | ALT1) +#define PA6_ALT1 (PA6 | ALT1) +#define PA7_ALT1 (PA7 | ALT1) +#define PA7_ALT2 (PA7 | ALT2) +#define PA7_ALT3 (PA7 | ALT3) +#define PA9_ALT1 (PA9 | ALT1) +#define PA10_ALT1 (PA10 | ALT1) +#define PA11_ALT1 (PA11 | ALT1) +#define PA12_ALT1 (PA12 | ALT1) +#define PA15_ALT1 (PA15 | ALT1) +#define PA15_ALT2 (PA15 | ALT2) +#define PB0_ALT1 (PB0 | ALT1) +#define PB0_ALT2 (PB0 | ALT2) +#define PB1_ALT1 (PB1 | ALT1) +#define PB1_ALT2 (PB1 | ALT2) +#define PB3_ALT1 (PB3 | ALT1) +#define PB3_ALT2 (PB3 | ALT2) +#define PB4_ALT1 (PB4 | ALT1) +#define PB4_ALT2 (PB4 | ALT2) +#define PB5_ALT1 (PB5 | ALT1) +#define PB5_ALT2 (PB5 | ALT2) +#define PB6_ALT1 (PB6 | ALT1) +#define PB6_ALT2 (PB6 | ALT2) +#define PB7_ALT1 (PB7 | ALT1) +#define PB8_ALT1 (PB8 | ALT1) +#define PB9_ALT1 (PB9 | ALT1) +#define PB14_ALT1 (PB14 | ALT1) +#define PB14_ALT2 (PB14 | ALT2) +#define PB15_ALT1 (PB15 | ALT1) +#define PB15_ALT2 (PB15 | ALT2) +#define PC0_ALT1 (PC0 | ALT1) +#define PC0_ALT2 (PC0 | ALT2) +#define PC1_ALT1 (PC1 | ALT1) +#define PC1_ALT2 (PC1 | ALT2) +#define PC2_ALT1 (PC2 | ALT1) +#define PC2_ALT2 (PC2 | ALT2) +#define PC3_ALT1 (PC3 | ALT1) +#define PC4_ALT1 (PC4 | ALT1) +#define PC5_ALT1 (PC5 | ALT1) +#define PC6_ALT1 (PC6 | ALT1) +#define PC7_ALT1 (PC7 | ALT1) +#define PC8_ALT1 (PC8 | ALT1) +#define PC9_ALT1 (PC9 | ALT1) +#define PC10_ALT1 (PC10 | ALT1) +#define PC11_ALT1 (PC11 | ALT1) +#define PF8_ALT1 (PF8 | ALT1) +#define PF9_ALT1 (PF9 | ALT1) +#define PJ8_ALT1 (PJ8 | ALT1) +#define PJ9_ALT1 (PJ9 | ALT1) +#define PJ10_ALT1 (PJ10 | ALT1) +#define PJ11_ALT1 (PJ11 | ALT1) +#define PK0_ALT1 (PK0 | ALT1) +#define PK1_ALT1 (PK1 | ALT1) + +#define NUM_DIGITAL_PINS 172 +#define NUM_DUALPAD_PINS 4 +#define NUM_ANALOG_INPUTS 36 + +// On-board LED pin number +#ifndef LED_BUILTIN + #define LED_BUILTIN PNUM_NOT_DEFINED +#endif + +// On-board user button +#ifndef USER_BTN + #define USER_BTN PNUM_NOT_DEFINED +#endif + +// SPI definitions +#ifndef PIN_SPI_SS + #define PIN_SPI_SS PA4 +#endif +#ifndef PIN_SPI_SS1 + #define PIN_SPI_SS1 PA15 +#endif +#ifndef PIN_SPI_SS2 + #define PIN_SPI_SS2 PG10 +#endif +#ifndef PIN_SPI_SS3 + #define PIN_SPI_SS3 PNUM_NOT_DEFINED +#endif +#ifndef PIN_SPI_MOSI + #define PIN_SPI_MOSI PA7 +#endif +#ifndef PIN_SPI_MISO + #define PIN_SPI_MISO PA6 +#endif +#ifndef PIN_SPI_SCK + #define PIN_SPI_SCK PA5 +#endif + +// I2C definitions +#ifndef PIN_WIRE_SDA + #define PIN_WIRE_SDA PB7 +#endif +#ifndef PIN_WIRE_SCL + #define PIN_WIRE_SCL PB6 +#endif + +// Timer Definitions +// Use TIM6/TIM7 when possible as servo and tone don't need GPIO output pin +#ifndef TIMER_TONE + #define TIMER_TONE TIM6 +#endif +#ifndef TIMER_SERVO + #define TIMER_SERVO TIM7 +#endif + +// UART Definitions +#ifndef SERIAL_UART_INSTANCE + #define SERIAL_UART_INSTANCE 4 +#endif + +// Default pin used for generic 'Serial' instance +// Mandatory for Firmata +#ifndef PIN_SERIAL_RX + #define PIN_SERIAL_RX PE0 +#endif +#ifndef PIN_SERIAL_TX + #define PIN_SERIAL_TX PE1 +#endif + +// Extra HAL modules +#if !defined(HAL_DAC_MODULE_DISABLED) + #define HAL_DAC_MODULE_ENABLED +#endif +#if !defined(HAL_ETH_MODULE_DISABLED) + #define HAL_ETH_MODULE_ENABLED +#endif +#if !defined(HAL_QSPI_MODULE_DISABLED) + #define HAL_QSPI_MODULE_ENABLED +#endif +#if !defined(HAL_SD_MODULE_DISABLED) + #define HAL_SD_MODULE_ENABLED +#endif + +#if !defined(HAL_FDCAN_MODULE_DISABLED) + #define HAL_FDCAN_MODULE_ENABLED +#endif + +/*---------------------------------------------------------------------------- + * Arduino objects - C++ only + *----------------------------------------------------------------------------*/ + +#ifdef __cplusplus + // These serial port names are intended to allow libraries and architecture-neutral + // sketches to automatically default to the correct port name for a particular type + // of use. For example, a GPS module would normally connect to SERIAL_PORT_HARDWARE_OPEN, + // the first hardware serial port whose RX/TX pins are not dedicated to another use. + // + // SERIAL_PORT_MONITOR Port which normally prints to the Arduino Serial Monitor + // + // SERIAL_PORT_USBVIRTUAL Port which is USB virtual serial + // + // SERIAL_PORT_LINUXBRIDGE Port which connects to a Linux system via Bridge library + // + // SERIAL_PORT_HARDWARE Hardware serial port, physical RX & TX pins. + // + // SERIAL_PORT_HARDWARE_OPEN Hardware serial ports which are open for use. Their RX & TX + // pins are NOT connected to anything by default. + #ifndef SERIAL_PORT_MONITOR + #define SERIAL_PORT_MONITOR Serial + #endif + #ifndef SERIAL_PORT_HARDWARE + #define SERIAL_PORT_HARDWARE Serial + #endif +#endif diff --git a/variants/MicroNode/CMakeLists.txt b/variants/MicroNode/CMakeLists.txt new file mode 100644 index 0000000..2a4d55b --- /dev/null +++ b/variants/MicroNode/CMakeLists.txt @@ -0,0 +1,31 @@ +# v3.21 implemented semantic changes regarding $ +# See https://cmake.org/cmake/help/v3.21/command/target_link_libraries.html#linking-object-libraries-via-target-objects +cmake_minimum_required(VERSION 3.21) + +add_library(variant INTERFACE) +add_library(variant_usage INTERFACE) + +target_include_directories(variant_usage INTERFACE + . +) + + +target_link_libraries(variant_usage INTERFACE + base_config +) + +target_link_libraries(variant INTERFACE variant_usage) + + + +add_library(variant_bin STATIC EXCLUDE_FROM_ALL + generic_clock.c + PeripheralPins.c + variant_generic.cpp +) +target_link_libraries(variant_bin PUBLIC variant_usage) + +target_link_libraries(variant INTERFACE + variant_bin +) + diff --git a/variants/MicroNode/PeripheralPins.c b/variants/MicroNode/PeripheralPins.c new file mode 100644 index 0000000..35a1475 --- /dev/null +++ b/variants/MicroNode/PeripheralPins.c @@ -0,0 +1,297 @@ +/* + ******************************************************************************* + * Copyright (c) 2020, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +/* + * Automatically generated from STM32L431C(B-C)Tx.xml, STM32L431C(B-C)Ux.xml + * CubeMX DB release 6.0.120 + */ +#if !defined(CUSTOM_PERIPHERAL_PINS) +#include "Arduino.h" +#include "PeripheralPins.h" + +/* ===== + * Notes: + * - The pins mentioned Px_y_ALTz are alternative possibilities which use other + * HW peripheral instances. You can use them the same way as any other "normal" + * pin (i.e. analogWrite(PA7_ALT1, 128);). + * + * - Commented lines are alternative possibilities which are not used per default. + * If you change them, you will have to know what you do + * ===== + */ + +//*** ADC *** + +#ifdef HAL_ADC_MODULE_ENABLED +WEAK const PinMap PinMap_ADC[] = { + {PA_0, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC1_IN5 + {PA_1, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC1_IN6 + {PA_2, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC1_IN7 + {PA_3, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC1_IN8 + {PA_4, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC1_IN9 + {PA_5, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC1_IN10 + {PA_6, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC1_IN11 + {PA_7, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 12, 0)}, // ADC1_IN12 + {PB_0, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC1_IN15 + {PB_1, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 16, 0)}, // ADC1_IN16 + {NC, NP, 0} +}; +#endif + +//*** DAC *** + +#ifdef HAL_DAC_MODULE_ENABLED +WEAK const PinMap PinMap_DAC[] = { + {PA_4, DAC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // DAC1_OUT1 + {PA_5, DAC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // DAC1_OUT2 + {NC, NP, 0} +}; +#endif + +//*** I2C *** + +#ifdef HAL_I2C_MODULE_ENABLED +WEAK const PinMap PinMap_I2C_SDA[] = { + {PA_10, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_4, I2C3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, + {PB_7, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_9, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_11, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PB_14, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_I2C_MODULE_ENABLED +WEAK const PinMap PinMap_I2C_SCL[] = { + {PA_7, I2C3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, + {PA_9, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_6, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_8, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_10, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PB_13, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {NC, NP, 0} +}; +#endif + +//*** No I3C *** + +//*** TIM *** + +#ifdef HAL_TIM_MODULE_ENABLED +WEAK const PinMap PinMap_TIM[] = { + {PA_0, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PA_1, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 + {PA_1_ALT1, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM15, 1, 1)}, // TIM15_CH1N + {PA_2, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 + {PA_2_ALT1, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM15, 1, 0)}, // TIM15_CH1 + {PA_3, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 4, 0)}, // TIM2_CH4 + {PA_3_ALT1, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM15, 2, 0)}, // TIM15_CH2 + {PA_5, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PA_6, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM16, 1, 0)}, // TIM16_CH1 + {PA_7, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N + {PA_8, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 0)}, // TIM1_CH1 + {PA_9, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 0)}, // TIM1_CH2 + {PA_10, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 0)}, // TIM1_CH3 + {PA_11, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 4, 0)}, // TIM1_CH4 + {PA_15, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PB_0, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N + {PB_1, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N + {PB_3, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 + {PB_6, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM16, 1, 1)}, // TIM16_CH1N + {PB_8, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM16, 1, 0)}, // TIM16_CH1 + {PB_10, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 + {PB_11, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 4, 0)}, // TIM2_CH4 + {PB_13, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N + {PB_13_ALT1, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM15, 1, 1)}, // TIM15_CH1N + {PB_14, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N + {PB_14_ALT1, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM15, 1, 0)}, // TIM15_CH1 + {PB_15, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N + {PB_15_ALT1, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM15, 2, 0)}, // TIM15_CH2 + {NC, NP, 0} +}; +#endif + +//*** UART *** + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_TX[] = { + {PA_2, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART1)}, + {PA_2_ALT1, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_9, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_6, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_10, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PB_11, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART1)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_RX[] = { + {PA_3, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART1)}, + {PA_3_ALT1, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_10, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PA_15, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_USART2)}, + {PB_7, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_10, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART1)}, + {PB_11, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_RTS[] = { + {PA_1, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_12, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PA_15, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PB_1, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART1)}, + {PB_1_ALT1, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PB_3, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_12, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART1)}, + {PB_14, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_CTS[] = { + {PA_0, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_6, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART1)}, + {PA_6_ALT1, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PA_11, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_4, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_13, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART1)}, + {PB_13_ALT1, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {NC, NP, 0} +}; +#endif + +//*** SPI *** + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_MOSI[] = { + {PA_7, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_12, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_5, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_5_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PB_15, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_MISO[] = { + {PA_6, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_11, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_4, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_4_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PB_14, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_SCLK[] = { + {PA_1, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_5, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_3, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_3_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PB_10, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PB_13, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_SSEL[] = { + {PA_4, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_4_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PA_15, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_15_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PB_0, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_9, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PB_12, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {NC, NP, 0} +}; +#endif + +//*** CAN *** + +#if defined(HAL_CAN_MODULE_ENABLED) || defined(HAL_CAN_LEGACY_MODULE_ENABLED) +WEAK const PinMap PinMap_CAN_RD[] = { + {PA_11, CAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, + {PB_8, CAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, + {NC, NP, 0} +}; +#endif + +#if defined(HAL_CAN_MODULE_ENABLED) || defined(HAL_CAN_LEGACY_MODULE_ENABLED) +WEAK const PinMap PinMap_CAN_TD[] = { + {PA_12, CAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, + {PB_9, CAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, + {NC, NP, 0} +}; +#endif + +//*** No ETHERNET *** + +//*** QUADSPI *** + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_DATA0[] = { + {PB_1, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO0 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_DATA1[] = { + {PB_0, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO1 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_DATA2[] = { + {PA_7, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO2 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_DATA3[] = { + {PA_6, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO3 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_SCLK[] = { + {PA_3, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_CLK + {PB_10, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_CLK + {NC, NP, 0} +}; +#endif + +#ifdef HAL_QSPI_MODULE_ENABLED +WEAK const PinMap PinMap_QUADSPI_SSEL[] = { + {PA_2, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_NCS + {PB_11, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_NCS + {NC, NP, 0} +}; +#endif + +//*** No USB *** + +//*** No SD *** + +#endif /* !CUSTOM_PERIPHERAL_PINS */ diff --git a/variants/MicroNode/PinNamesVar.h b/variants/MicroNode/PinNamesVar.h new file mode 100644 index 0000000..aaf98ed --- /dev/null +++ b/variants/MicroNode/PinNamesVar.h @@ -0,0 +1,42 @@ +/* Alternate pin name */ +PA_1_ALT1 = PA_1 | ALT1, +PA_2_ALT1 = PA_2 | ALT1, +PA_3_ALT1 = PA_3 | ALT1, +PA_4_ALT1 = PA_4 | ALT1, +PA_6_ALT1 = PA_6 | ALT1, +PA_15_ALT1 = PA_15 | ALT1, +PB_1_ALT1 = PB_1 | ALT1, +PB_3_ALT1 = PB_3 | ALT1, +PB_4_ALT1 = PB_4 | ALT1, +PB_5_ALT1 = PB_5 | ALT1, +PB_13_ALT1 = PB_13 | ALT1, +PB_14_ALT1 = PB_14 | ALT1, +PB_15_ALT1 = PB_15 | ALT1, + +/* SYS_WKUP */ +#ifdef PWR_WAKEUP_PIN1 + SYS_WKUP1 = PA_0, +#endif +#ifdef PWR_WAKEUP_PIN2 + SYS_WKUP2 = PC_13, +#endif +#ifdef PWR_WAKEUP_PIN3 + SYS_WKUP3 = NC, +#endif +#ifdef PWR_WAKEUP_PIN4 + SYS_WKUP4 = PA_2, +#endif +#ifdef PWR_WAKEUP_PIN5 + SYS_WKUP5 = NC, +#endif +#ifdef PWR_WAKEUP_PIN6 + SYS_WKUP6 = NC, +#endif +#ifdef PWR_WAKEUP_PIN7 + SYS_WKUP7 = NC, +#endif +#ifdef PWR_WAKEUP_PIN8 + SYS_WKUP8 = NC, +#endif + +/* No USB */ diff --git a/variants/MicroNode/README.md b/variants/MicroNode/README.md new file mode 100644 index 0000000..9419cf5 --- /dev/null +++ b/variants/MicroNode/README.md @@ -0,0 +1,5 @@ +# BR Micro Node Variant + +This project contains the files required for platformio to work for the Beyond Robotix Micro CAN node ! + +See https://github.com/BeyondRobotix/Arduino-DroneCAN diff --git a/variants/MicroNode/boards_entry.txt b/variants/MicroNode/boards_entry.txt new file mode 100644 index 0000000..976755c --- /dev/null +++ b/variants/MicroNode/boards_entry.txt @@ -0,0 +1,41 @@ +# This file help to add generic board entry. +# upload.maximum_size and product_line have to be verified +# and changed if needed. +# See: https://github.com/stm32duino/Arduino_Core_STM32/wiki/Add-a-new-variant-%28board%29 + +# Generic L431CBTx +GenL4.menu.pnum.GENERIC_L431CBTX=Generic L431CBTx +GenL4.menu.pnum.GENERIC_L431CBTX.upload.maximum_size=131072 +GenL4.menu.pnum.GENERIC_L431CBTX.upload.maximum_data_size=65536 +GenL4.menu.pnum.GENERIC_L431CBTX.build.board=GENERIC_L431CBTX +GenL4.menu.pnum.GENERIC_L431CBTX.build.product_line=STM32L431xx +GenL4.menu.pnum.GENERIC_L431CBTX.build.variant=STM32L4xx/L431C(B-C)(T-U) +GenL4.menu.pnum.GENERIC_L431CBTX.debug.svd_file={runtime.tools.STM32_SVD.path}/svd/STM32L4xx/STM32L4x1.svd + +# Generic L431CCTx +GenL4.menu.pnum.GENERIC_L431CCTX=Generic L431CCTx +GenL4.menu.pnum.GENERIC_L431CCTX.upload.maximum_size=262144 +GenL4.menu.pnum.GENERIC_L431CCTX.upload.maximum_data_size=65536 +GenL4.menu.pnum.GENERIC_L431CCTX.build.board=GENERIC_L431CCTX +GenL4.menu.pnum.GENERIC_L431CCTX.build.product_line=STM32L431xx +GenL4.menu.pnum.GENERIC_L431CCTX.build.variant=STM32L4xx/L431C(B-C)(T-U) +GenL4.menu.pnum.GENERIC_L431CCTX.debug.svd_file={runtime.tools.STM32_SVD.path}/svd/STM32L4xx/STM32L4x1.svd + +# Generic L431CBUx +GenL4.menu.pnum.GENERIC_L431CBUX=Generic L431CBUx +GenL4.menu.pnum.GENERIC_L431CBUX.upload.maximum_size=131072 +GenL4.menu.pnum.GENERIC_L431CBUX.upload.maximum_data_size=65536 +GenL4.menu.pnum.GENERIC_L431CBUX.build.board=GENERIC_L431CBUX +GenL4.menu.pnum.GENERIC_L431CBUX.build.product_line=STM32L431xx +GenL4.menu.pnum.GENERIC_L431CBUX.build.variant=STM32L4xx/L431C(B-C)(T-U) +GenL4.menu.pnum.GENERIC_L431CBUX.debug.svd_file={runtime.tools.STM32_SVD.path}/svd/STM32L4xx/STM32L4x1.svd + +# Generic L431CCUx +GenL4.menu.pnum.GENERIC_L431CCUX=Generic L431CCUx +GenL4.menu.pnum.GENERIC_L431CCUX.upload.maximum_size=262144 +GenL4.menu.pnum.GENERIC_L431CCUX.upload.maximum_data_size=65536 +GenL4.menu.pnum.GENERIC_L431CCUX.build.board=GENERIC_L431CCUX +GenL4.menu.pnum.GENERIC_L431CCUX.build.product_line=STM32L431xx +GenL4.menu.pnum.GENERIC_L431CCUX.build.variant=STM32L4xx/L431C(B-C)(T-U) +GenL4.menu.pnum.GENERIC_L431CCUX.debug.svd_file={runtime.tools.STM32_SVD.path}/svd/STM32L4xx/STM32L4x1.svd + diff --git a/variants/MicroNode/generic_clock.c b/variants/MicroNode/generic_clock.c new file mode 100644 index 0000000..d954a50 --- /dev/null +++ b/variants/MicroNode/generic_clock.c @@ -0,0 +1,64 @@ +/* + ******************************************************************************* + * Copyright (c) 2020-2021, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +#if defined(ARDUINO_GENERIC_L431CBTX) || defined(ARDUINO_GENERIC_L431CBUX) ||\ + defined(ARDUINO_GENERIC_L431CCTX) || defined(ARDUINO_GENERIC_L431CCUX) +#include "pins_arduino.h" + +/** + * @brief System Clock Configuration + * @param None + * @retval None + */ +WEAK void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {}; + + /** Configure the main internal regulator output voltage + */ + if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK) { + Error_Handler(); + } + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; + RCC_OscInitStruct.PLL.PLLM = 1; + RCC_OscInitStruct.PLL.PLLN = 10; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV7; + RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2; + RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK + | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) { + Error_Handler(); + } +} + +#endif /* ARDUINO_GENERIC_* */ diff --git a/variants/MicroNode/ldscript-no-bootloader.ld b/variants/MicroNode/ldscript-no-bootloader.ld new file mode 100644 index 0000000..5604432 --- /dev/null +++ b/variants/MicroNode/ldscript-no-bootloader.ld @@ -0,0 +1,190 @@ +/* +****************************************************************************** +** +** @file : LinkerScript.ld +** +** @author : Auto-generated by STM32CubeIDE +** +** @brief : Linker script for STM32L431CCUx Device from STM32L4 series +** 256KBytes FLASH +** 64KBytes RAM +** 16KBytes RAM2 +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used +** +** Target : STMicroelectronics STM32 +** +** Distribution: The file is distributed as is, without any warranty +** of any kind. +** +****************************************************************************** +** @attention +** +** Copyright (c) 2024 STMicroelectronics. +** All rights reserved. +** +** This software is licensed under terms that can be found in the LICENSE file +** in the root directory of this software component. +** If no LICENSE file comes with this software, it is provided AS-IS. +** +****************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of "RAM" Ram type memory */ + +_Min_Heap_Size = 0x200; /* required amount of heap */ +_Min_Stack_Size = 0x400; /* required amount of stack */ + +/* Memories definition */ +MEMORY +{ + BOOTCOM (rw) : ORIGIN = 0x20000000, LENGTH = 0x100 + RAM (xrw) : ORIGIN = 0x20000100, LENGTH = LD_MAX_DATA_SIZE - 0x100 + RAM2 (xrw) : ORIGIN = 0x10000000, LENGTH = 16K + FLASH (rx) : ORIGIN = 0x8000000 + LD_FLASH_OFFSET, LENGTH = LD_MAX_SIZE - LD_FLASH_OFFSET +} + +/* Sections */ +SECTIONS +{ + /* The startup code into "FLASH" Rom type memory */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data into "FLASH" Rom type memory */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data into "FLASH" Rom type memory */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + *(.ARM.extab* .gnu.linkonce.armextab.*) + . = ALIGN(4); + } >FLASH + + .ARM (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + . = ALIGN(4); + } >FLASH + + .preinit_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + . = ALIGN(4); + } >FLASH + + .init_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + . = ALIGN(4); + } >FLASH + + .fini_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + . = ALIGN(4); + } >FLASH + + /* Used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections into "RAM" Ram type memory */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + *(.RamFunc) /* .RamFunc sections */ + *(.RamFunc*) /* .RamFunc* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + + } >RAM AT> FLASH + + /* Uninitialized data section into "RAM" Ram type memory */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss section */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough "RAM" Ram type memory left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM + + /* Remove information from the compiler libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/variants/MicroNode/ldscript.ld b/variants/MicroNode/ldscript.ld new file mode 100644 index 0000000..bf76f96 --- /dev/null +++ b/variants/MicroNode/ldscript.ld @@ -0,0 +1,190 @@ +/* +****************************************************************************** +** +** @file : LinkerScript.ld +** +** @author : Auto-generated by STM32CubeIDE +** +** @brief : Linker script for STM32L431CCUx Device from STM32L4 series +** 256KBytes FLASH +** 64KBytes RAM +** 16KBytes RAM2 +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used +** +** Target : STMicroelectronics STM32 +** +** Distribution: The file is distributed as is, without any warranty +** of any kind. +** +****************************************************************************** +** @attention +** +** Copyright (c) 2024 STMicroelectronics. +** All rights reserved. +** +** This software is licensed under terms that can be found in the LICENSE file +** in the root directory of this software component. +** If no LICENSE file comes with this software, it is provided AS-IS. +** +****************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of "RAM" Ram type memory */ + +_Min_Heap_Size = 0x200; /* required amount of heap */ +_Min_Stack_Size = 0x400; /* required amount of stack */ + +/* Memories definition */ +MEMORY +{ + BOOTCOM (rw) : ORIGIN = 0x20000000, LENGTH = 0x100 + RAM (xrw) : ORIGIN = 0x20000100, LENGTH = LD_MAX_DATA_SIZE - 0x100 + RAM2 (xrw) : ORIGIN = 0x10000000, LENGTH = 16K + FLASH (rx) : ORIGIN = 0x800A000 + LD_FLASH_OFFSET, LENGTH = LD_MAX_SIZE - LD_FLASH_OFFSET +} + +/* Sections */ +SECTIONS +{ + /* The startup code into "FLASH" Rom type memory */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data into "FLASH" Rom type memory */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data into "FLASH" Rom type memory */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + *(.ARM.extab* .gnu.linkonce.armextab.*) + . = ALIGN(4); + } >FLASH + + .ARM (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + . = ALIGN(4); + } >FLASH + + .preinit_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + . = ALIGN(4); + } >FLASH + + .init_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + . = ALIGN(4); + } >FLASH + + .fini_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + . = ALIGN(4); + } >FLASH + + /* Used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections into "RAM" Ram type memory */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + *(.RamFunc) /* .RamFunc sections */ + *(.RamFunc*) /* .RamFunc* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + + } >RAM AT> FLASH + + /* Uninitialized data section into "RAM" Ram type memory */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss section */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough "RAM" Ram type memory left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM + + /* Remove information from the compiler libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/variants/MicroNode/variant_MICRONODE.cpp b/variants/MicroNode/variant_MICRONODE.cpp new file mode 100644 index 0000000..0f88262 --- /dev/null +++ b/variants/MicroNode/variant_MICRONODE.cpp @@ -0,0 +1,145 @@ +/* + ******************************************************************************* + * Copyright (c) 2020, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +#include "pins_arduino.h" +#include "Arduino.h" + +// Digital PinName array +const PinName digitalPin[] = { + PA_0, // D0/A0 + PA_1, // D1/A1 + PA_2, // D2/A2 + PA_3, // D3/A3 + PA_4, // D4/A4 + PA_5, // D5/A5 + PA_6, // D6/A6 + PA_7, // D7/A7 + PA_8, // D8 + PA_9, // D9 + PA_10, // D10 + PA_11, // D11 + PA_12, // D12 + PA_13, // D13 + PA_14, // D14 + PA_15, // D15 + PB_0, // D16/A8 + PB_1, // D17/A9 + PB_2, // D18 + PB_3, // D19 + PB_4, // D20 + PB_5, // D21 + PB_6, // D22 + PB_7, // D23 + PB_8, // D24 + PB_9, // D25 + PB_10, // D26 + PB_11, // D27 + PB_12, // D28 + PB_13, // D29 + PB_14, // D30 + PB_15, // D31 + PC_13, // D32 + PC_14, // D33 + PC_15, // D34 + PH_0, // D35 + PH_1, // D36 + PH_3 // D37 +}; + +// Analog (Ax) pin number array +const uint32_t analogInputPin[] = { + 0, // A0, PA0 + 1, // A1, PA1 + 2, // A2, PA2 + 3, // A3, PA3 + 4, // A4, PA4 + 5, // A5, PA5 + 6, // A6, PA6 + 7, // A7, PA7 + 16, // A8, PB0 + 17 // A9, PB1 +}; + +// --- SERIAL PORT INSTANTIATION --- +// This forces the creation of the Serial objects using the pins defined in variant_MICRONODE.h + +// Serial1 (USART1: RX=PB7, TX=PB6) +#if defined(PIN_SERIAL1_RX) && defined(PIN_SERIAL1_TX) +HardwareSerial Serial1(PIN_SERIAL1_RX, PIN_SERIAL1_TX); +#endif + +// Serial3 (USART3: RX=PB11, TX=PB10) +#if defined(PIN_SERIAL3_RX) && defined(PIN_SERIAL3_TX) +HardwareSerial Serial3(PIN_SERIAL3_RX, PIN_SERIAL3_TX); +#endif +// --------------------------------- + +#ifdef __cplusplus +extern "C" +{ +#endif + WEAK void SystemClock_Config(void) + { + RCC_OscInitTypeDef RCC_OscInitStruct = {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {}; + + /** Configure the main internal regulator output voltage + */ + if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; + RCC_OscInitStruct.HSIState = RCC_HSI_ON; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; + RCC_OscInitStruct.PLL.PLLM = 1; + RCC_OscInitStruct.PLL.PLLN = 10; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV7; + RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2; + RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) + { + Error_Handler(); + } + + // Reinitialize SysTick to enable the delay() function + // Configure SysTick to generate an interrupt every 1ms (SystemCoreClock / 1000) + if (SysTick_Config(SystemCoreClock / 1000)) + { + // If SysTick configuration fails, handle the error + Error_Handler(); + } + NVIC_EnableIRQ(SysTick_IRQn); + } + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/variants/MicroNode/variant_MICRONODE.h b/variants/MicroNode/variant_MICRONODE.h new file mode 100644 index 0000000..122e775 --- /dev/null +++ b/variants/MicroNode/variant_MICRONODE.h @@ -0,0 +1,189 @@ +/* + ******************************************************************************* + * Copyright (c) 2020, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +#pragma once + +/*---------------------------------------------------------------------------- + * STM32 pins number + *----------------------------------------------------------------------------*/ +#define PA0 PIN_A0 +#define PA1 PIN_A1 +#define PA2 PIN_A2 +#define PA3 PIN_A3 +#define PA4 PIN_A4 +#define PA5 PIN_A5 +#define PA6 PIN_A6 +#define PA7 PIN_A7 +#define PA8 8 +#define PA9 9 +#define PA10 10 +#define PA11 11 +#define PA12 12 +#define PA13 13 +#define PA14 14 +#define PA15 15 +#define PB0 PIN_A8 +#define PB1 PIN_A9 +#define PB2 18 +#define PB3 19 +#define PB4 20 +#define PB5 21 +#define PB6 22 +#define PB7 23 +#define PB8 24 +#define PB9 25 +#define PB10 26 +#define PB11 27 +#define PB12 28 +#define PB13 29 +#define PB14 30 +#define PB15 31 +#define PC13 32 +#define PC14 33 +#define PC15 34 +#define PH0 35 +#define PH1 36 +#define PH3 37 + +// Alternate pins number +#define PA1_ALT1 (PA1 | ALT1) +#define PA2_ALT1 (PA2 | ALT1) +#define PA3_ALT1 (PA3 | ALT1) +#define PA4_ALT1 (PA4 | ALT1) +#define PA6_ALT1 (PA6 | ALT1) +#define PA15_ALT1 (PA15 | ALT1) +#define PB1_ALT1 (PB1 | ALT1) +#define PB3_ALT1 (PB3 | ALT1) +#define PB4_ALT1 (PB4 | ALT1) +#define PB5_ALT1 (PB5 | ALT1) +#define PB13_ALT1 (PB13 | ALT1) +#define PB14_ALT1 (PB14 | ALT1) +#define PB15_ALT1 (PB15 | ALT1) + +#define NUM_DIGITAL_PINS 38 +#define NUM_ANALOG_INPUTS 10 + +// On-board LED pin number +#ifndef LED_BUILTIN + #define LED_BUILTIN PNUM_NOT_DEFINED +#endif + +// On-board user button +#ifndef USER_BTN + #define USER_BTN PNUM_NOT_DEFINED +#endif + +// SPI definitions +#ifndef PIN_SPI_SS + #define PIN_SPI_SS PA4 +#endif +#ifndef PIN_SPI_SS1 + #define PIN_SPI_SS1 PA15 +#endif +#ifndef PIN_SPI_SS2 + #define PIN_SPI_SS2 PB0 +#endif +#ifndef PIN_SPI_SS3 + #define PIN_SPI_SS3 PNUM_NOT_DEFINED +#endif +#ifndef PIN_SPI_MOSI + #define PIN_SPI_MOSI PA7 +#endif +#ifndef PIN_SPI_MISO + #define PIN_SPI_MISO PA6 +#endif +#ifndef PIN_SPI_SCK + #define PIN_SPI_SCK PA1 +#endif + +// I2C definitions +#ifndef PIN_WIRE_SDA + #define PIN_WIRE_SDA PB14 +#endif +#ifndef PIN_WIRE_SCL + #define PIN_WIRE_SCL PB13 +#endif + +// Timer Definitions +// Use TIM6/TIM7 when possible as servo and tone don't need GPIO output pin +#ifndef TIMER_TONE + #define TIMER_TONE TIM6 +#endif +#ifndef TIMER_SERVO + #define TIMER_SERVO TIM7 +#endif + +// UART Definitions +#ifndef SERIAL_UART_INSTANCE + #define SERIAL_UART_INSTANCE 101 +#endif + +// Default pin used for generic 'Serial' instance +// Mandatory for Firmata +#ifndef PIN_SERIAL_RX + #define PIN_SERIAL_RX PA3 +#endif +#ifndef PIN_SERIAL_TX + #define PIN_SERIAL_TX PA2 +#endif + +// Serial 1 Definitions +#ifndef PIN_SERIAL1_RX + #define PIN_SERIAL1_RX PB7 +#endif +#ifndef PIN_SERIAL1_TX + #define PIN_SERIAL1_TX PB6 +#endif + +// Serial 3 Definitions +#ifndef PIN_SERIAL3_RX + #define PIN_SERIAL3_RX PB11 +#endif +#ifndef PIN_SERIAL3_TX + #define PIN_SERIAL3_TX PB10 +#endif + +// Extra HAL modules +#if !defined(HAL_DAC_MODULE_DISABLED) + #define HAL_DAC_MODULE_ENABLED +#endif +#if !defined(HAL_QSPI_MODULE_DISABLED) + #define HAL_QSPI_MODULE_ENABLED +#endif + +/*---------------------------------------------------------------------------- + * Arduino objects - C++ only + *----------------------------------------------------------------------------*/ + +#ifdef __cplusplus + // These serial port names are intended to allow libraries and architecture-neutral + // sketches to automatically default to the correct port name for a particular type + // of use. For example, a GPS module would normally connect to SERIAL_PORT_HARDWARE_OPEN, + // the first hardware serial port whose RX/TX pins are not dedicated to another use. + // + // SERIAL_PORT_MONITOR Port which normally prints to the Arduino Serial Monitor + // + // SERIAL_PORT_USBVIRTUAL Port which is USB virtual serial + // + // SERIAL_PORT_LINUXBRIDGE Port which connects to a Linux system via Bridge library + // + // SERIAL_PORT_HARDWARE Hardware serial port, physical RX & TX pins. + // + // SERIAL_PORT_HARDWARE_OPEN Hardware serial ports which are open for use. Their RX & TX + // pins are NOT connected to anything by default. + #ifndef SERIAL_PORT_MONITOR + #define SERIAL_PORT_MONITOR Serial + #endif + #ifndef SERIAL_PORT_HARDWARE + #define SERIAL_PORT_HARDWARE Serial + #endif +#endif diff --git a/variants/MicroNodePlus/CMakeLists.txt b/variants/MicroNodePlus/CMakeLists.txt new file mode 100644 index 0000000..7f8fe0d --- /dev/null +++ b/variants/MicroNodePlus/CMakeLists.txt @@ -0,0 +1,32 @@ +# v3.21 implemented semantic changes regarding $ +# See https://cmake.org/cmake/help/v3.21/command/target_link_libraries.html#linking-object-libraries-via-target-objects +cmake_minimum_required(VERSION 3.21) + +add_library(variant INTERFACE) +add_library(variant_usage INTERFACE) + +target_include_directories(variant_usage INTERFACE + . +) + + +target_link_libraries(variant_usage INTERFACE + base_config +) + +target_link_libraries(variant INTERFACE variant_usage) + + + +add_library(variant_bin STATIC EXCLUDE_FROM_ALL + generic_clock.c + PeripheralPins.c + variant_generic.cpp + variant_NUCLEO_H723ZG.cpp +) +target_link_libraries(variant_bin PUBLIC variant_usage) + +target_link_libraries(variant INTERFACE + variant_bin +) + diff --git a/variants/MicroNodePlus/PeripheralPins.c b/variants/MicroNodePlus/PeripheralPins.c new file mode 100644 index 0000000..c6483fc --- /dev/null +++ b/variants/MicroNodePlus/PeripheralPins.c @@ -0,0 +1,768 @@ +/* + ******************************************************************************* + * Copyright (c) 2020, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +/* + * Automatically generated from STM32H723ZETx.xml, STM32H723ZGTx.xml + * STM32H730ZBTx.xml, STM32H733ZGTx.xml + * CubeMX DB release 6.0.120 + */ +#if !defined(CUSTOM_PERIPHERAL_PINS) +#include "Arduino.h" +#include "PeripheralPins.h" + +/* ===== + * Notes: + * - The pins mentioned Px_y_ALTz are alternative possibilities which use other + * HW peripheral instances. You can use them the same way as any other "normal" + * pin (i.e. analogWrite(PA7_ALT1, 128);). + * + * - Commented lines are alternative possibilities which are not used per default. + * If you change them, you will have to know what you do + * ===== + */ + +//*** ADC *** + +#ifdef HAL_ADC_MODULE_ENABLED +WEAK const PinMap PinMap_ADC[] = { + {PA_0, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 16, 0)}, // ADC1_INP16 + {PA_1, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 17, 0)}, // ADC1_INP17 + {PA_2, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 14, 0)}, // ADC1_INP14 + {PA_2_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 14, 0)}, // ADC2_INP14 + {PA_3, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC1_INP15 + {PA_3_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC2_INP15 + {PA_4, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 18, 0)}, // ADC1_INP18 + {PA_4_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 18, 0)}, // ADC2_INP18 + {PA_5, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 19, 0)}, // ADC1_INP19 + {PA_5_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 19, 0)}, // ADC2_INP19 + {PA_6, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC1_INP3 + {PA_6_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC2_INP3 + {PA_7, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC1_INP7 + {PA_7_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC2_INP7 + {PB_0, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC1_INP9 + {PB_0_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC2_INP9 + {PB_1, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC1_INP5 + {PB_1_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC2_INP5 + {PC_0, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC1_INP10 + {PC_0_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC2_INP10 + {PC_0_ALT2, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC3_INP10 + {PC_1, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC1_INP11 + {PC_1_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC2_INP11 + {PC_1_ALT2, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC3_INP11 + {PC_2_C, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 0, 0)}, // ADC3_INP0 + {PC_3_C, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // ADC3_INP1 + {PC_4, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC1_INP4 + {PC_4_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC2_INP4 + {PC_5, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC1_INP8 + {PC_5_ALT1, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC2_INP8 + {PF_3, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC3_INP5 + {PF_4, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC3_INP9 + {PF_5, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC3_INP4 + {PF_6, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC3_INP8 + {PF_7, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC3_INP3 + {PF_8, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC3_INP7 + {PF_9, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC3_INP2 + {PF_10, ADC3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC3_INP6 + {PF_11, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC1_INP2 + {PF_12, ADC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC1_INP6 + {PF_13, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC2_INP2 + {PF_14, ADC2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC2_INP6 + {NC, NP, 0} +}; +#endif + +//*** DAC *** + +#ifdef HAL_DAC_MODULE_ENABLED +WEAK const PinMap PinMap_DAC[] = { + {PA_4, DAC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // DAC1_OUT1 + {PA_5, DAC1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // DAC1_OUT2 + {NC, NP, 0} +}; +#endif + +//*** I2C *** + +#ifdef HAL_I2C_MODULE_ENABLED +WEAK const PinMap PinMap_I2C_SDA[] = { + {PB_7, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_7_ALT1, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C4)}, + {PB_9, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_9_ALT1, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C4)}, + {PB_11, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PC_9, I2C3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, + {PC_9_ALT1, I2C5, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C5)}, + {PC_10, I2C5, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C5)}, + {PD_13, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {PF_0, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PF_0_ALT1, I2C5, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C5)}, + {PF_15, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_I2C_MODULE_ENABLED +WEAK const PinMap PinMap_I2C_SCL[] = { + {PA_8, I2C3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, + {PA_8_ALT1, I2C5, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C5)}, + {PB_6, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_6_ALT1, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C4)}, + {PB_8, I2C1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, + {PB_8_ALT1, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C4)}, + {PB_10, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PC_11, I2C5, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C5)}, + {PD_12, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {PF_1, I2C2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, + {PF_1_ALT1, I2C5, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF6_I2C5)}, + {PF_14, I2C4, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C4)}, + {NC, NP, 0} +}; +#endif + +//*** No I3C *** + +//*** TIM *** + +#ifdef HAL_TIM_MODULE_ENABLED +WEAK const PinMap PinMap_TIM[] = { + {PA_0, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PA_0_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 1, 0)}, // TIM5_CH1 + {PA_1, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 + {PA_1_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 2, 0)}, // TIM5_CH2 + {PA_1_ALT2, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 1, 1)}, // TIM15_CH1N + {PA_2, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 + {PA_2_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 3, 0)}, // TIM5_CH3 + {PA_2_ALT2, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 1, 0)}, // TIM15_CH1 + {PA_3, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 4, 0)}, // TIM2_CH4 + {PA_3_ALT1, TIM5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 4, 0)}, // TIM5_CH4 + {PA_3_ALT2, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 2, 0)}, // TIM15_CH2 + {PA_5, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PA_5_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 1)}, // TIM8_CH1N + {PA_6, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 + {PA_6_ALT1, TIM13, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM13, 1, 0)}, // TIM13_CH1 + {PA_7, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N + {PA_7_ALT1, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 + {PA_7_ALT2, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 1)}, // TIM8_CH1N + {PA_7_ALT3, TIM14, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM14, 1, 0)}, // TIM14_CH1 + {PA_8, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 0)}, // TIM1_CH1 + {PA_9, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 0)}, // TIM1_CH2 + {PA_10, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 0)}, // TIM1_CH3 + {PA_11, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 4, 0)}, // TIM1_CH4 + {PA_15, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 + {PB_0, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N + {PB_0_ALT1, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 3, 0)}, // TIM3_CH3 + {PB_0_ALT2, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 1)}, // TIM8_CH2N + {PB_1, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N + {PB_1_ALT1, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 4, 0)}, // TIM3_CH4 + {PB_1_ALT2, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 1)}, // TIM8_CH3N + {PB_3, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 + {PB_4, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 + {PB_5, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 + {PB_6, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 1, 0)}, // TIM4_CH1 + {PB_6_ALT1, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM16, 1, 1)}, // TIM16_CH1N + {PB_7, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 2, 0)}, // TIM4_CH2 + {PB_7_ALT1, TIM17, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM17, 1, 1)}, // TIM17_CH1N + {PB_8, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 3, 0)}, // TIM4_CH3 + {PB_8_ALT1, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM16, 1, 0)}, // TIM16_CH1 + {PB_9, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 4, 0)}, // TIM4_CH4 + {PB_9_ALT1, TIM17, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM17, 1, 0)}, // TIM17_CH1 + {PB_10, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 + {PB_11, TIM2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 4, 0)}, // TIM2_CH4 + {PB_13, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N + {PB_14, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N + {PB_14_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 1)}, // TIM8_CH2N + {PB_14_ALT2, TIM12, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM12, 1, 0)}, // TIM12_CH1 + {PB_15, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N + {PB_15_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 1)}, // TIM8_CH3N + {PB_15_ALT2, TIM12, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM12, 2, 0)}, // TIM12_CH2 + {PC_6, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 + {PC_6_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 0)}, // TIM8_CH1 + {PC_7, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 + {PC_7_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 0)}, // TIM8_CH2 + {PC_8, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 3, 0)}, // TIM3_CH3 + {PC_8_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 0)}, // TIM8_CH3 + {PC_9, TIM3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 4, 0)}, // TIM3_CH4 + {PC_9_ALT1, TIM8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 4, 0)}, // TIM8_CH4 + {PC_12, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM15, 1, 0)}, // TIM15_CH1 + {PD_12, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 1, 0)}, // TIM4_CH1 + {PD_13, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 2, 0)}, // TIM4_CH2 + {PD_14, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 3, 0)}, // TIM4_CH3 + {PD_15, TIM4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 4, 0)}, // TIM4_CH4 + {PE_4, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 1, 1)}, // TIM15_CH1N + {PE_5, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 1, 0)}, // TIM15_CH1 + {PE_6, TIM15, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_TIM15, 2, 0)}, // TIM15_CH2 + {PE_8, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N + {PE_9, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 0)}, // TIM1_CH1 + {PE_10, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N + {PE_11, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 0)}, // TIM1_CH2 + {PE_12, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N + {PE_13, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 0)}, // TIM1_CH3 + {PE_14, TIM1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 4, 0)}, // TIM1_CH4 + {PF_0, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 1, 0)}, // TIM23_CH1 + {PF_1, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 2, 0)}, // TIM23_CH2 + {PF_2, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 3, 0)}, // TIM23_CH3 + {PF_3, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 4, 0)}, // TIM23_CH4 + {PF_6, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM16, 1, 0)}, // TIM16_CH1 + {PF_6_ALT1, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 1, 0)}, // TIM23_CH1 + {PF_7, TIM17, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM17, 1, 0)}, // TIM17_CH1 + {PF_7_ALT1, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 2, 0)}, // TIM23_CH2 + {PF_8, TIM13, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM13, 1, 0)}, // TIM13_CH1 + {PF_8_ALT1, TIM16, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM16, 1, 1)}, // TIM16_CH1N + {PF_8_ALT2, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 3, 0)}, // TIM23_CH3 + {PF_9, TIM14, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM14, 1, 0)}, // TIM14_CH1 + {PF_9_ALT1, TIM17, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM17, 1, 1)}, // TIM17_CH1N + {PF_9_ALT2, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 4, 0)}, // TIM23_CH4 + {PF_11, TIM24, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM24, 1, 0)}, // TIM24_CH1 + {PF_12, TIM24, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM24, 2, 0)}, // TIM24_CH2 + {PF_13, TIM24, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM24, 3, 0)}, // TIM24_CH3 + {PF_14, TIM24, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_TIM24, 4, 0)}, // TIM24_CH4 + {PG_12, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 1, 0)}, // TIM23_CH1 + {PG_13, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 2, 0)}, // TIM23_CH2 + {PG_14, TIM23, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF13_TIM23, 3, 0)}, // TIM23_CH3 + {NC, NP, 0} +}; +#endif + +//*** UART *** + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_TX[] = { + {PA_0, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PA_2, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_9, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_LPUART)}, + {PA_9_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PA_12, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_UART4)}, + {PA_15, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART7)}, + {PB_4, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART7)}, + {PB_6, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART)}, + {PB_6_ALT1, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_UART5)}, + {PB_6_ALT2, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_9, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_10, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PB_13, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_UART5)}, + {PB_14, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_USART1)}, + {PC_6, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PC_10, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PC_10_ALT1, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PC_12, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, + {PD_1, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PD_5, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PD_8, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PD_15, UART9, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART9)}, + {PE_1, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {PE_3, USART10, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_USART10)}, + {PE_8, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PF_7, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PG_1, UART9, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART9)}, + {PG_12, USART10, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_USART10)}, + {PG_14, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_RX[] = { + {PA_1, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PA_3, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_8, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART7)}, + {PA_10, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_LPUART)}, + {PA_10_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PA_11, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_UART4)}, + {PB_3, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART7)}, + {PB_5, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_UART5)}, + {PB_7, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_LPUART)}, + {PB_7_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_8, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_11, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PB_12, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_UART5)}, + {PB_15, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_USART1)}, + {PC_7, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PC_11, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PC_11_ALT1, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PD_0, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PD_2, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, + {PD_6, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PD_9, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PD_14, UART9, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART9)}, + {PE_0, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {PE_2, USART10, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_USART10)}, + {PE_7, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PF_6, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PG_0, UART9, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART9)}, + {PG_9, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PG_11, USART10, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_USART10)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_RTS[] = { + {PA_1, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_12, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_LPUART)}, + {PA_12_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PA_15, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_14, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_14_ALT1, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PC_8, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, + {PD_4, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PD_12, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PD_13, UART9, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART9)}, + {PD_15, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {PE_9, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PF_8, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PG_8, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PG_12, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PG_14, USART10, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_USART10)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_UART_MODULE_ENABLED +WEAK const PinMap PinMap_UART_CTS[] = { + {PA_0, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PA_11, LPUART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_LPUART)}, + {PA_11_ALT1, USART1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, + {PB_0, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PB_13, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PB_15, UART4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, + {PC_9, UART5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, + {PD_0, UART9, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_UART9)}, + {PD_3, USART2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, + {PD_11, USART3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, + {PD_14, UART8, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART8)}, + {PE_10, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PF_9, UART7, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_UART7)}, + {PG_13, USART10, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_USART10)}, + {PG_13_ALT1, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {PG_15, USART6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART6)}, + {NC, NP, 0} +}; +#endif + +//*** SPI *** + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_MOSI[] = { + {PA_7, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_7_ALT1, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_2, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_SPI3)}, + {PB_5, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_5_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_SPI3)}, + {PB_5_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_15, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_1, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_3_C, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_12, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PD_6, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI3)}, + {PD_7, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PE_6, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PE_14, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PF_9, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PF_11, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PG_14, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_MISO[] = { + {PA_6, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_6_ALT1, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_4, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_4_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PB_4_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_14, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_2_C, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_11, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PE_5, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PE_13, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PF_8, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PG_9, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PG_12, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_SCLK[] = { + {PA_5, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_5_ALT1, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PA_9, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PA_12, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PB_3, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PB_3_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PB_3_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PB_10, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PB_13, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PC_10, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PC_12, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {PD_3, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PE_2, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PE_12, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PF_7, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PG_11, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PG_13, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SPI_MODULE_ENABLED +WEAK const PinMap PinMap_SPI_SSEL[] = { + {PA_0, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {PA_4, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_4_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PA_4_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_SPI6)}, + {PA_11, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PA_15, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {PA_15_ALT1, SPI3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_SPI3)}, + {PA_15_ALT2, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_SPI6)}, + {PB_4, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_SPI2)}, + {PB_9, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PB_12, SPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI2)}, + {PE_4, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PE_11, SPI4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI4)}, + {PF_6, SPI5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI5)}, + {PG_8, SPI6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI6)}, + {PG_10, SPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF5_SPI1)}, + {NC, NP, 0} +}; +#endif + +//*** FDCAN *** + +#ifdef HAL_FDCAN_MODULE_ENABLED +WEAK const PinMap PinMap_CAN_RD[] = { + {PA_11, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PB_5, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN2)}, + {PB_8, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PB_12, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN2)}, + {PD_0, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PD_12, FDCAN3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_FDCAN3)}, + {PF_6, FDCAN3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF2_FDCAN3)}, + {PG_10, FDCAN3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF2_FDCAN3)}, + {NC, NP, 0} +}; +#endif + +#ifdef HAL_FDCAN_MODULE_ENABLED +WEAK const PinMap PinMap_CAN_TD[] = { + {PA_12, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PB_6, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN2)}, + {PB_9, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PB_13, FDCAN2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN2)}, + {PD_1, FDCAN1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_FDCAN1)}, + {PD_13, FDCAN3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_FDCAN3)}, + {PF_7, FDCAN3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF2_FDCAN3)}, + {PG_9, FDCAN3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF2_FDCAN3)}, + {NC, NP, 0} +}; +#endif + +//*** ETHERNET *** + +#if defined(HAL_ETH_MODULE_ENABLED) || defined(HAL_ETH_LEGACY_MODULE_ENABLED) +WEAK const PinMap PinMap_Ethernet[] = { + {PA_0, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_CRS + {PA_1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_REF_CLK + {PA_1_ALT1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RX_CLK + {PA_2, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_MDIO + {PA_3, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_COL + {PA_7, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_CRS_DV + {PA_7_ALT1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RX_DV + {PA_9, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_ER + {PB_0, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD2 + {PB_1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD3 + {PB_2, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_ER + {PB_5, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_PPS_OUT + {PB_8, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD3 + {PB_10, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RX_ER + {PB_11, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_EN + {PB_12, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD0 + {PB_13, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD1 + {PC_1, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_MDC + {PC_2_C, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD2 + {PC_3_C, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_CLK + {PC_4, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD0 + {PC_5, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_RXD1 + {PE_2, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD3 + {PG_8, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_PPS_OUT + {PG_11, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TX_EN + {PG_12, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD1 + {PG_13, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD0 + {PG_14, ETH, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_ETH)}, // ETH_TXD1 + {NC, NP, 0} +}; +#endif + +//*** OCTOSPI *** + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_DATA0[] = { + {PA_2, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO0 + {PB_1, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO0 + {PB_12, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO0 + {PC_3_C, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO0 + {PC_9, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO0 + {PD_11, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO0 + {PF_0, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P2)}, // OCTOSPIM_P2_IO0 + {PF_8, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO0 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_DATA1[] = { + {PB_0, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO1 + {PC_10, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO1 + {PD_12, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO1 + {PF_1, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P2)}, // OCTOSPIM_P2_IO1 + {PF_9, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO1 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_DATA2[] = { + {PA_3, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO2 + {PA_7, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO2 + {PB_13, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO2 + {PC_2_C, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO2 + {PE_2, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO2 + {PF_2, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P2)}, // OCTOSPIM_P2_IO2 + {PF_7, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO2 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_DATA3[] = { + {PA_1, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO3 + {PA_6, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF6_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO3 + {PD_13, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO3 + {PF_3, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P2)}, // OCTOSPIM_P2_IO3 + {PF_6, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO3 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_DATA4[] = { + {PC_1, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO4 + {PD_4, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO4 + {PE_7, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO4 + {PG_0, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P2)}, // OCTOSPIM_P2_IO4 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_DATA5[] = { + {PC_2_C, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO5 + {PD_5, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO5 + {PE_8, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO5 + {PG_1, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P2)}, // OCTOSPIM_P2_IO5 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_DATA6[] = { + {PC_3_C, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF4_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO6 + {PD_6, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO6 + {PE_9, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO6 + {PG_9, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO6 + {PG_10, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_OCTOSPIM_P2)}, // OCTOSPIM_P2_IO6 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_DATA7[] = { + {PD_7, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO7 + {PE_10, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO7 + {PG_11, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P2)}, // OCTOSPIM_P2_IO7 + {PG_14, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_IO7 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_SCLK[] = { + {PA_3, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_OCTOSPIM_P1)}, // OCTOSPIM_P1_CLK + {PB_2, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_CLK + {PF_4, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P2)}, // OCTOSPIM_P2_CLK + {PF_10, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_CLK + {NC, NP, 0} +}; +#endif + +#ifdef HAL_OSPI_MODULE_ENABLED +WEAK const PinMap PinMap_OCTOSPI_SSEL[] = { + {PB_6, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_NCS + {PB_10, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_NCS + {PC_11, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_OCTOSPIM_P1)}, // OCTOSPIM_P1_NCS + {PE_11, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_OCTOSPIM_P1)}, // OCTOSPIM_P1_NCS + {PG_6, OCTOSPI1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OCTOSPIM_P1)}, // OCTOSPIM_P1_NCS + {PG_12, OCTOSPI2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_OCTOSPIM_P2)}, // OCTOSPIM_P2_NCS + {NC, NP, 0} +}; +#endif + +//*** USB *** + +#if defined(HAL_PCD_MODULE_ENABLED) || defined(HAL_HCD_MODULE_ENABLED) +WEAK const PinMap PinMap_USB_OTG_HS[] = { +#ifdef USE_USB_HS_IN_FS + {PA_8, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_SOF + {PA_9, USB_OTG_HS, STM_PIN_DATA(STM_MODE_INPUT, GPIO_NOPULL, GPIO_AF_NONE)}, // USB_OTG_HS_VBUS + {PA_10, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ID + {PA_11, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF_NONE)}, // USB_OTG_HS_DM + {PA_12, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF_NONE)}, // USB_OTG_HS_DP +#else + {PA_3, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_D0 + {PA_5, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_CK + {PB_0, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_D1 + {PB_1, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_D2 + {PB_5, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_D7 + {PB_10, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_D3 + {PB_11, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_D4 + {PB_12, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_D5 + {PB_13, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_D6 + {PC_0, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_STP + {PC_2_C, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_DIR + {PC_3_C, USB_OTG_HS, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_OTG1_HS)}, // USB_OTG_HS_ULPI_NXT +#endif /* USE_USB_HS_IN_FS */ + {NC, NP, 0} +}; +#endif + +//*** SD *** + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_CMD[] = { + {PA_0, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_SDMMC2)}, // SDMMC2_CMD + {PD_2, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF12_SDMMC1)}, // SDMMC1_CMD + {PD_7, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_SDMMC2)}, // SDMMC2_CMD + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_CK[] = { + {PC_1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_SDMMC2)}, // SDMMC2_CK + {PC_12, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF12_SDMMC1)}, // SDMMC1_CK + {PD_6, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF11_SDMMC2)}, // SDMMC2_CK + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA0[] = { + {PB_13, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDMMC1)}, // SDMMC1_D0 + {PB_14, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_SDMMC2)}, // SDMMC2_D0 + {PC_8, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDMMC1)}, // SDMMC1_D0 + {PG_9, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_SDMMC2)}, // SDMMC2_D0 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA1[] = { + {PB_15, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_SDMMC2)}, // SDMMC2_D1 + {PC_9, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDMMC1)}, // SDMMC1_D1 + {PG_10, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF11_SDMMC2)}, // SDMMC2_D1 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA2[] = { + {PB_3, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_SDMMC2)}, // SDMMC2_D2 + {PC_10, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDMMC1)}, // SDMMC1_D2 + {PG_11, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDMMC2)}, // SDMMC2_D2 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA3[] = { + {PB_4, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_SDMMC2)}, // SDMMC2_D3 + {PC_11, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDMMC1)}, // SDMMC1_D3 + {PG_12, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDMMC2)}, // SDMMC2_D3 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA4[] = { + {PB_8, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDMMC1)}, // SDMMC1_D4 + {PB_8_ALT1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDMMC2)}, // SDMMC2_D4 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA5[] = { + {PB_9, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDMMC1)}, // SDMMC1_D5 + {PB_9_ALT1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDMMC2)}, // SDMMC2_D5 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA6[] = { + {PC_6, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDMMC1)}, // SDMMC1_D6 + {PC_6_ALT1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDMMC2)}, // SDMMC2_D6 + {PG_13, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDMMC2)}, // SDMMC2_D6 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_DATA7[] = { + {PC_7, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_SDMMC1)}, // SDMMC1_D7 + {PC_7_ALT1, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDMMC2)}, // SDMMC2_D7 + {PG_14, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_SDMMC2)}, // SDMMC2_D7 + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_CKIN[] = { + {PB_8, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF7_SDMMC1)}, // SDMMC1_CKIN + {PC_4, SDMMC2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF10_SDMMC2)}, // SDMMC2_CKIN + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_CDIR[] = { + {PB_9, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF7_SDMMC1)}, // SDMMC1_CDIR + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_D0DIR[] = { + {PC_6, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF8_SDMMC1)}, // SDMMC1_D0DIR + {NC, NP, 0} +}; +#endif + +#ifdef HAL_SD_MODULE_ENABLED +WEAK const PinMap PinMap_SD_D123DIR[] = { + {PC_7, SDMMC1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF8_SDMMC1)}, // SDMMC1_D123DIR + {NC, NP, 0} +}; +#endif + +#endif /* !CUSTOM_PERIPHERAL_PINS */ diff --git a/variants/MicroNodePlus/PinNamesVar.h b/variants/MicroNodePlus/PinNamesVar.h new file mode 100644 index 0000000..cfce515 --- /dev/null +++ b/variants/MicroNodePlus/PinNamesVar.h @@ -0,0 +1,113 @@ +/* Dual pad pin name */ +PC_2_C = PC_2 | PDUAL, +PC_3_C = PC_3 | PDUAL, + +/* Alternate pin name */ +PA_0_ALT1 = PA_0 | ALT1, +PA_1_ALT1 = PA_1 | ALT1, +PA_1_ALT2 = PA_1 | ALT2, +PA_2_ALT1 = PA_2 | ALT1, +PA_2_ALT2 = PA_2 | ALT2, +PA_3_ALT1 = PA_3 | ALT1, +PA_3_ALT2 = PA_3 | ALT2, +PA_4_ALT1 = PA_4 | ALT1, +PA_4_ALT2 = PA_4 | ALT2, +PA_5_ALT1 = PA_5 | ALT1, +PA_6_ALT1 = PA_6 | ALT1, +PA_7_ALT1 = PA_7 | ALT1, +PA_7_ALT2 = PA_7 | ALT2, +PA_7_ALT3 = PA_7 | ALT3, +PA_8_ALT1 = PA_8 | ALT1, +PA_9_ALT1 = PA_9 | ALT1, +PA_10_ALT1 = PA_10 | ALT1, +PA_11_ALT1 = PA_11 | ALT1, +PA_12_ALT1 = PA_12 | ALT1, +PA_15_ALT1 = PA_15 | ALT1, +PA_15_ALT2 = PA_15 | ALT2, +PB_0_ALT1 = PB_0 | ALT1, +PB_0_ALT2 = PB_0 | ALT2, +PB_1_ALT1 = PB_1 | ALT1, +PB_1_ALT2 = PB_1 | ALT2, +PB_3_ALT1 = PB_3 | ALT1, +PB_3_ALT2 = PB_3 | ALT2, +PB_4_ALT1 = PB_4 | ALT1, +PB_4_ALT2 = PB_4 | ALT2, +PB_5_ALT1 = PB_5 | ALT1, +PB_5_ALT2 = PB_5 | ALT2, +PB_6_ALT1 = PB_6 | ALT1, +PB_6_ALT2 = PB_6 | ALT2, +PB_7_ALT1 = PB_7 | ALT1, +PB_8_ALT1 = PB_8 | ALT1, +PB_9_ALT1 = PB_9 | ALT1, +PB_14_ALT1 = PB_14 | ALT1, +PB_14_ALT2 = PB_14 | ALT2, +PB_15_ALT1 = PB_15 | ALT1, +PB_15_ALT2 = PB_15 | ALT2, +PC_0_ALT1 = PC_0 | ALT1, +PC_0_ALT2 = PC_0 | ALT2, +PC_1_ALT1 = PC_1 | ALT1, +PC_1_ALT2 = PC_1 | ALT2, +PC_4_ALT1 = PC_4 | ALT1, +PC_5_ALT1 = PC_5 | ALT1, +PC_6_ALT1 = PC_6 | ALT1, +PC_7_ALT1 = PC_7 | ALT1, +PC_8_ALT1 = PC_8 | ALT1, +PC_9_ALT1 = PC_9 | ALT1, +PC_10_ALT1 = PC_10 | ALT1, +PC_11_ALT1 = PC_11 | ALT1, +PF_0_ALT1 = PF_0 | ALT1, +PF_1_ALT1 = PF_1 | ALT1, +PF_6_ALT1 = PF_6 | ALT1, +PF_7_ALT1 = PF_7 | ALT1, +PF_8_ALT1 = PF_8 | ALT1, +PF_8_ALT2 = PF_8 | ALT2, +PF_9_ALT1 = PF_9 | ALT1, +PF_9_ALT2 = PF_9 | ALT2, +PG_13_ALT1 = PG_13 | ALT1, + +/* SYS_WKUP */ +#ifdef PWR_WAKEUP_PIN1 + SYS_WKUP1 = PA_0, +#endif +#ifdef PWR_WAKEUP_PIN2 + SYS_WKUP2 = PA_2, +#endif +#ifdef PWR_WAKEUP_PIN3 + SYS_WKUP3 = NC, +#endif +#ifdef PWR_WAKEUP_PIN4 + SYS_WKUP4 = PC_13, +#endif +#ifdef PWR_WAKEUP_PIN5 + SYS_WKUP5 = NC, +#endif +#ifdef PWR_WAKEUP_PIN6 + SYS_WKUP6 = PC_1, +#endif +#ifdef PWR_WAKEUP_PIN7 + SYS_WKUP7 = NC, +#endif +#ifdef PWR_WAKEUP_PIN8 + SYS_WKUP8 = NC, +#endif + +/* USB */ +#ifdef USBCON + USB_OTG_HS_DM = PA_11, + USB_OTG_HS_DP = PA_12, + USB_OTG_HS_ID = PA_10, + USB_OTG_HS_SOF = PA_8, + USB_OTG_HS_ULPI_CK = PA_5, + USB_OTG_HS_ULPI_D0 = PA_3, + USB_OTG_HS_ULPI_D1 = PB_0, + USB_OTG_HS_ULPI_D2 = PB_1, + USB_OTG_HS_ULPI_D3 = PB_10, + USB_OTG_HS_ULPI_D4 = PB_11, + USB_OTG_HS_ULPI_D5 = PB_12, + USB_OTG_HS_ULPI_D6 = PB_13, + USB_OTG_HS_ULPI_D7 = PB_5, + USB_OTG_HS_ULPI_DIR = PC_2_C, + USB_OTG_HS_ULPI_NXT = PC_3_C, + USB_OTG_HS_ULPI_STP = PC_0, + USB_OTG_HS_VBUS = PA_9, +#endif diff --git a/variants/MicroNodePlus/boards_entry.txt b/variants/MicroNodePlus/boards_entry.txt new file mode 100644 index 0000000..fe44627 --- /dev/null +++ b/variants/MicroNodePlus/boards_entry.txt @@ -0,0 +1,41 @@ +# This file help to add generic board entry. +# upload.maximum_size and product_line have to be verified +# and changed if needed. +# See: https://github.com/stm32duino/Arduino_Core_STM32/wiki/Add-a-new-variant-%28board%29 + +# Generic H723ZETx +GenH7.menu.pnum.GENERIC_H723ZETX=Generic H723ZETx +GenH7.menu.pnum.GENERIC_H723ZETX.upload.maximum_size=524288 +GenH7.menu.pnum.GENERIC_H723ZETX.upload.maximum_data_size=577536 +GenH7.menu.pnum.GENERIC_H723ZETX.build.board=GENERIC_H723ZETX +GenH7.menu.pnum.GENERIC_H723ZETX.build.product_line=STM32H723xx +GenH7.menu.pnum.GENERIC_H723ZETX.build.variant=STM32H7xx/H723Z(E-G)T_H730ZBT_H733ZGT +GenH7.menu.pnum.GENERIC_H723ZETX.debug.svd_file={runtime.tools.STM32_SVD.path}/svd/STM32H7xx/STM32H723.svd + +# Generic H723ZGTx +GenH7.menu.pnum.GENERIC_H723ZGTX=Generic H723ZGTx +GenH7.menu.pnum.GENERIC_H723ZGTX.upload.maximum_size=1048576 +GenH7.menu.pnum.GENERIC_H723ZGTX.upload.maximum_data_size=577536 +GenH7.menu.pnum.GENERIC_H723ZGTX.build.board=GENERIC_H723ZGTX +GenH7.menu.pnum.GENERIC_H723ZGTX.build.product_line=STM32H723xx +GenH7.menu.pnum.GENERIC_H723ZGTX.build.variant=STM32H7xx/H723Z(E-G)T_H730ZBT_H733ZGT +GenH7.menu.pnum.GENERIC_H723ZGTX.debug.svd_file={runtime.tools.STM32_SVD.path}/svd/STM32H7xx/STM32H723.svd + +# Generic H730ZBTx +GenH7.menu.pnum.GENERIC_H730ZBTX=Generic H730ZBTx +GenH7.menu.pnum.GENERIC_H730ZBTX.upload.maximum_size=131072 +GenH7.menu.pnum.GENERIC_H730ZBTX.upload.maximum_data_size=577536 +GenH7.menu.pnum.GENERIC_H730ZBTX.build.board=GENERIC_H730ZBTX +GenH7.menu.pnum.GENERIC_H730ZBTX.build.product_line=STM32H730xx +GenH7.menu.pnum.GENERIC_H730ZBTX.build.variant=STM32H7xx/H723Z(E-G)T_H730ZBT_H733ZGT +GenH7.menu.pnum.GENERIC_H730ZBTX.debug.svd_file={runtime.tools.STM32_SVD.path}/svd/STM32H7xx/STM32H730.svd + +# Generic H733ZGTx +GenH7.menu.pnum.GENERIC_H733ZGTX=Generic H733ZGTx +GenH7.menu.pnum.GENERIC_H733ZGTX.upload.maximum_size=1048576 +GenH7.menu.pnum.GENERIC_H733ZGTX.upload.maximum_data_size=577536 +GenH7.menu.pnum.GENERIC_H733ZGTX.build.board=GENERIC_H733ZGTX +GenH7.menu.pnum.GENERIC_H733ZGTX.build.product_line=STM32H733xx +GenH7.menu.pnum.GENERIC_H733ZGTX.build.variant=STM32H7xx/H723Z(E-G)T_H730ZBT_H733ZGT +GenH7.menu.pnum.GENERIC_H733ZGTX.debug.svd_file={runtime.tools.STM32_SVD.path}/svd/STM32H7xx/STM32H733.svd + diff --git a/variants/MicroNodePlus/generic_clock.c b/variants/MicroNodePlus/generic_clock.c new file mode 100644 index 0000000..bc12a2c --- /dev/null +++ b/variants/MicroNodePlus/generic_clock.c @@ -0,0 +1,116 @@ +/* + ******************************************************************************* + * Copyright (c) 2020, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +#if defined(ARDUINO_GENERIC_H723ZETX) || defined(ARDUINO_GENERIC_H723ZGTX) ||\ + defined(ARDUINO_GENERIC_H730ZBTX) || defined(ARDUINO_GENERIC_H733ZGTX) +#include "pins_arduino.h" + +/** + * @brief System Clock Configuration + * @param None + * @retval None + */ +WEAK void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {}; + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {}; + RCC_CRSInitTypeDef RCC_CRSInitStruct = {}; + + /** Supply configuration update enable + */ + HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY); + + /** Configure the main internal regulator output voltage + */ + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0); + + while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {} + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_HSI; + RCC_OscInitStruct.HSIState = RCC_HSI_DIV1; + RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; + RCC_OscInitStruct.HSI48State = RCC_HSI48_ON; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; + RCC_OscInitStruct.PLL.PLLM = 4; + RCC_OscInitStruct.PLL.PLLN = 34; + RCC_OscInitStruct.PLL.PLLP = 1; + RCC_OscInitStruct.PLL.PLLQ = 4; + RCC_OscInitStruct.PLL.PLLR = 2; + RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3; + RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; + RCC_OscInitStruct.PLL.PLLFRACN = 0; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK + | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2 + | RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; + RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; + RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; + RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK) { + Error_Handler(); + } + + /** Initializes the peripherals clock + */ + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_SDMMC | RCC_PERIPHCLK_USB; + PeriphClkInitStruct.PLL2.PLL2M = 32; + PeriphClkInitStruct.PLL2.PLL2N = 96; + PeriphClkInitStruct.PLL2.PLL2P = 2; + PeriphClkInitStruct.PLL2.PLL2Q = 2; + PeriphClkInitStruct.PLL2.PLL2R = 4; + PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_1; + PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE; + PeriphClkInitStruct.PLL2.PLL2FRACN = 0.0; + PeriphClkInitStruct.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL2; + PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_HSI48; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) { + Error_Handler(); + } + + /*Configure the clock recovery system (CRS)**********************************/ + + /*Enable CRS Clock*/ + __HAL_RCC_CRS_CLK_ENABLE(); + + /* Default Synchro Signal division factor (not divided) */ + RCC_CRSInitStruct.Prescaler = RCC_CRS_SYNC_DIV1; + + /* Set the SYNCSRC[1:0] bits according to CRS_Source value */ + RCC_CRSInitStruct.Source = RCC_CRS_SYNC_SOURCE_USB1; + + /* HSI48 is synchronized with USB SOF at 1KHz rate */ + RCC_CRSInitStruct.ReloadValue = RCC_CRS_RELOADVALUE_DEFAULT; + RCC_CRSInitStruct.ErrorLimitValue = RCC_CRS_ERRORLIMIT_DEFAULT; + RCC_CRSInitStruct.Polarity = RCC_CRS_SYNC_POLARITY_RISING; + + /* Set the TRIM[5:0] to the default value */ + RCC_CRSInitStruct.HSI48CalibrationValue = RCC_CRS_HSI48CALIBRATION_DEFAULT; + + /* Start automatic synchronization */ + HAL_RCCEx_CRSConfig(&RCC_CRSInitStruct); +} +#endif /* ARDUINO_GENERIC_* */ diff --git a/variants/MicroNodePlus/ldscript.ld b/variants/MicroNodePlus/ldscript.ld new file mode 100644 index 0000000..c121c58 --- /dev/null +++ b/variants/MicroNodePlus/ldscript.ld @@ -0,0 +1,174 @@ +/* +****************************************************************************** +** +** File : LinkerScript.ld +** +** Author : STM32CubeIDE +** +** Abstract : Linker script for STM32H7 series +** 1024Kbytes FLASH and 560Kbytes RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Distribution: The file is distributed as is, without any warranty +** of any kind. +** +***************************************************************************** +** @attention +** +** Copyright (c) 2022 STMicroelectronics. +** All rights reserved. +** +** This software is licensed under terms that can be found in the LICENSE file +** in the root directory of this software component. +** If no LICENSE file comes with this software, it is provided AS-IS. +** +**************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(RAM_D1) + LENGTH(RAM_D1); /* end of RAM */ +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0x200 ; /* required amount of heap */ +_Min_Stack_Size = 0x400 ; /* required amount of stack */ + +/* Specify the memory areas */ +MEMORY +{ + ITCMRAM (xrw) : ORIGIN = 0x00000000, LENGTH = 64K + DTCMRAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K + FLASH (rx) : ORIGIN = 0x08000000 + LD_FLASH_OFFSET, LENGTH = LD_MAX_SIZE - LD_FLASH_OFFSET + RAM_D1 (xrw) : ORIGIN = 0x24000000, LENGTH = LD_MAX_DATA_SIZE + RAM_D2 (xrw) : ORIGIN = 0x30000000, LENGTH = 32K + RAM_D3 (xrw) : ORIGIN = 0x38000000, LENGTH = 16K +} + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data goes into FLASH */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH + .ARM (READONLY) : { + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + } >FLASH + + .preinit_array (READONLY) : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + + .init_array (READONLY) : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + + .fini_array (READONLY) : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + *(.RamFunc) /* .RamFunc sections */ + *(.RamFunc*) /* .RamFunc* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM_D1 AT> FLASH + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss section */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM_D1 + + /* User_heap_stack section, used to check that there is enough RAM left */ + ._user_heap_stack : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM_D1 + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/variants/MicroNodePlus/st_nucleo_h723zg.cfg b/variants/MicroNodePlus/st_nucleo_h723zg.cfg new file mode 100644 index 0000000..01be788 --- /dev/null +++ b/variants/MicroNodePlus/st_nucleo_h723zg.cfg @@ -0,0 +1,10 @@ +# This is an ST NUCLEO-H743ZI board with single STM32H743ZI chip. +# http://www.st.com/en/evaluation-tools/nucleo-h743zi.html + +source [find interface/stlink.cfg] + +transport select hla_swd + +source [find target/stm32h7x_dual_bank.cfg] + +reset_config none diff --git a/variants/MicroNodePlus/variant_MICRONODEPLUS.cpp b/variants/MicroNodePlus/variant_MICRONODEPLUS.cpp new file mode 100644 index 0000000..8cc9fc9 --- /dev/null +++ b/variants/MicroNodePlus/variant_MICRONODEPLUS.cpp @@ -0,0 +1,279 @@ +/* + ******************************************************************************* + * Copyright (c) 2022, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +#if defined(ARDUINO_NUCLEO_H723ZG) +#include "pins_arduino.h" + +// Pin number +const PinName digitalPin[] = { + PB_7, + PB_6, + PG_14, + PE_13, + PE_14, + PE_11, + PE_9, + PG_12, + PF_3, + PD_15, + PD_14, + PB_5, + PA_6, + PA_5, + PB_9, + PB_8, + PC_6, + PB_15, + PB_13, + PB_12, + PA_15, + PC_7, + PB_5, + PB_3, + PA_4, + PB_4, + PG_6, + PB_2, + PD_13, + PD_12, + PD_11, + PE_2, + PA_0, + PB_0, + PE_0, + PB_11, + PB_10, + PE_15, + PE_6, + PE_12, + PE_10, + PE_7, + PE_8, + PC_8, + PC_9, + PC_10, + PC_11, + PC_12, + PD_2, + PG_2, + PG_3, + PD_7, + PD_6, + PD_5, + PD_4, + PD_3, + PE_2, + PE_4, + PE_5, + PE_6, + PE_3, + PF_8, + PF_7, + PF_9, + PG_1, + PG_0, + PD_1, + PD_0, + PF_0, + PF_1, + PF_2, + PE_9, + PB_2, + PA_3, + PC_0, + PC_3_C, + PB_1, + PC_2_C, + PF_10, + PF_4, + PF_5, + PF_6, + PF_11, + PA_1, + PA_2, + PA_7, + PA_8, + PA_9, + PA_10, + PA_11, + PA_12, + PA_13, + PA_14, + PB_14, + PC_1, + PC_4, + PC_5, + PC_13, + PC_14, + PC_15, + PD_8, + PD_9, + PD_10, + PE_1, + PF_12, + PF_13, + PF_14, + PF_15, + PG_4, + PG_5, + PG_7, + PG_8, + PG_9, + PG_10, + PG_11, + PG_13, + PG_15, + PH_0, + PH_1 +}; + +// Analog (Ax) pin number array +const uint32_t analogInputPin[] = { + 73, // A0 + 74, // A1 + 75, // A2 + 76, // A3 + 77, // A4 + 78, // A5 + 79, // A6 + 80, // A7 + 81, // A8 + 82, // A9 + 83, // A10 + 84, // A11 + 85, // A12 + 94, // A13 + 95, // A14 + 96, // A15 + 104, // A16 + 105, // A17 + 106, // A18 + 8, // A19 + 12, // A20 + 13, // A21 + 24, // A22 + 32, // A23 + 33, // A24 + 61, // A25 + 62, // A26 + 63 // A27 +}; + +// ---------------------------------------------------------------------------- + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief System Clock Configuration + * @param None + * @retval None + */ +WEAK void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {}; + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {}; + RCC_CRSInitTypeDef RCC_CRSInitStruct = {}; + + /** Supply configuration update enable + */ + HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY); + + /** Configure the main internal regulator output voltage + */ + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0); + + while (!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {} + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI48 | RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS; + RCC_OscInitStruct.HSI48State = RCC_HSI48_ON; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; + RCC_OscInitStruct.PLL.PLLM = 4; + RCC_OscInitStruct.PLL.PLLN = 275; + RCC_OscInitStruct.PLL.PLLP = 1; + RCC_OscInitStruct.PLL.PLLQ = 4; + RCC_OscInitStruct.PLL.PLLR = 2; + RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_1; + RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE; + RCC_OscInitStruct.PLL.PLLFRACN = 0; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK + | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2 + | RCC_CLOCKTYPE_D3PCLK1 | RCC_CLOCKTYPE_D1PCLK1; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2; + RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2; + RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2; + RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK) { + Error_Handler(); + } + + /** Initializes the peripherals clock + */ + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_SDMMC | RCC_PERIPHCLK_USB; + PeriphClkInitStruct.PLL2.PLL2M = 1; + PeriphClkInitStruct.PLL2.PLL2N = 24; + PeriphClkInitStruct.PLL2.PLL2P = 2; + PeriphClkInitStruct.PLL2.PLL2Q = 2; + PeriphClkInitStruct.PLL2.PLL2R = 4; + PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_3; + PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE; + PeriphClkInitStruct.PLL2.PLL2FRACN = 0; + PeriphClkInitStruct.SdmmcClockSelection = RCC_SDMMCCLKSOURCE_PLL2; + PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_HSI48; + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) { + Error_Handler(); + } + + /*Configure the clock recovery system (CRS)**********************************/ + + /*Enable CRS Clock*/ + __HAL_RCC_CRS_CLK_ENABLE(); + + /* Default Synchro Signal division factor (not divided) */ + RCC_CRSInitStruct.Prescaler = RCC_CRS_SYNC_DIV1; + + /* Set the SYNCSRC[1:0] bits according to CRS_Source value */ + RCC_CRSInitStruct.Source = RCC_CRS_SYNC_SOURCE_USB1; + + /* HSI48 is synchronized with USB SOF at 1KHz rate */ + RCC_CRSInitStruct.ReloadValue = RCC_CRS_RELOADVALUE_DEFAULT; + RCC_CRSInitStruct.ErrorLimitValue = RCC_CRS_ERRORLIMIT_DEFAULT; + RCC_CRSInitStruct.Polarity = RCC_CRS_SYNC_POLARITY_RISING; + + /* Set the TRIM[5:0] to the default value */ + RCC_CRSInitStruct.HSI48CalibrationValue = RCC_CRS_HSI48CALIBRATION_DEFAULT; + + /* Start automatic synchronization */ + HAL_RCCEx_CRSConfig(&RCC_CRSInitStruct); +} + +#ifdef __cplusplus +} +#endif +#endif /* ARDUINO_ */ diff --git a/variants/MicroNodePlus/variant_MICRONODEPLUS.h b/variants/MicroNodePlus/variant_MICRONODEPLUS.h new file mode 100644 index 0000000..7105d7b --- /dev/null +++ b/variants/MicroNodePlus/variant_MICRONODEPLUS.h @@ -0,0 +1,294 @@ +/* + ******************************************************************************* + * Copyright (c) 2022, STMicroelectronics + * All rights reserved. + * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ******************************************************************************* + */ +#pragma once + +/*---------------------------------------------------------------------------- + * STM32 pins number + *----------------------------------------------------------------------------*/ +// CN7 right Arduino Uno connector +#define PB7 0 // USART_A_RX +#define PB6 1 // USART_A_TX +#define PG14 2 +#define PE13 3 // TIMER_A_PWM3 +#define PE14 4 +#define PE11 5 // TIMER_A_PWM2 +#define PE9 6 // TIMER_A_PWM1 +#define PG12 7 +#define PF3 PIN_A19 +#define PD15 9 // TIMER_B_PWM2 +#define PD14 10 // SPI_A_CS/ TIM_B_PWM3 +#define PB5 11 // SPI_A_MOSI/ PA7 if SB33 ON and SB35 OFF +#define PA6 PIN_A20 // SPI_A_MISO +#define PA5 PIN_A21 // SPI_A_SCK +#define PB9 14 // I2C_A_SDA +#define PB8 15 // I2C_A_SCL +// CN7 left +#define PC6 16 // I2S_A_MCK +#define PB15 17 // I2S_A_SD +#define PB13 18 // I2S_A_CK / RMII TXD1 - JP6 ON (default) +#define PB12 19 // I2S_A_WS +#define PA15 20 // I2S_B_WS +#define PC7 21 // I2S_B_MCK +// 22 is PB5 (11) +#define PB3 23 // I2S_B_CK/SPI_B_SCK - SWO +#define PA4 PIN_A22 // SPI_B_NSS +#define PB4 25 // SPI_B_MISO +#define PG6 26 // QSPI_CS +#define PB2 27 // QSPI_CLK +#define PD13 28 // QSPI_BK1_IO3 +#define PD12 29 // QSPI_BK1_IO1 +#define PD11 30 // QSPI_BK1_IO0 +#define PE2 31 // SAI_A_MCLK/QSPI_BK1_IO2 +#define PA0 PIN_A23 // TIMER_C_PWM1 +#define PB0 PIN_A24 // TIMER_D_PWM1 - LD1 LED_GREEN +#define PE0 34 // TIMER_B_ETR +// CN7 right +#define PB11 35 // TIMER_C_PWM3 +#define PB10 36 // TIMER_C_PWM2 +#define PE15 37 // TIMER_A_BKIN1 +#define PE6 38 +#define PE12 39 // TIMER_A_PWM3N +#define PE10 40 // TIMER_A_PWM2N +#define PE7 41 // TIMER_A_ETR +#define PE8 42 // TIMER_A_PWM1N +// CN8 right +#define PC8 43 // SDMMC1_D0 +#define PC9 44 // SDMMC1_D1/I2S_A_CKIN +#define PC10 45 // SDMMC1_D2 +#define PC11 46 // SDMMC1_D3 +#define PC12 47 // SDMMC1_CK +#define PD2 48 // SDMMC1_CMD +#define PG2 49 +#define PG3 50 +#define PD7 51 // USART_B_SCLK +#define PD6 52 // USART_B_RX +#define PD5 53 // USART_B_TX +#define PD4 54 // USART_B_RTS +#define PD3 55 // USART_B_CTS +// 56 is PE2 (31) +#define PE4 57 // SAI_A_FS +#define PE5 58 // SAI_A_SCK +// 59 is PE6 (38) +#define PE3 60 // SAI_B_SD +#define PF8 PIN_A25 // SAI_B_SCK +#define PF7 PIN_A26 // SAI_B_MCLK +#define PF9 PIN_A27 // SAI_B_FS +#define PG1 64 +// CN8 left +#define PG0 65 +#define PD1 66 // CAN_TX +#define PD0 67 // CAN_RX +#define PF0 68 // I2C_B_SDA +#define PF1 69 // I2C_B_SCL +#define PF2 70 // I2C_B_SMBA +// 71 is PE9 (6) +// 72 is PB2 (27) +#define PA3 PIN_A0 +#define PC0 PIN_A1 +#define PC3_C PIN_A2 +#define PB1 PIN_A3 +#define PC2_C PIN_A4 +#define PF10 PIN_A5 +// CN7 left (analog part) +#define PF4 PIN_A6 +#define PF5 PIN_A7 +#define PF6 PIN_A8 +// ST morpho +#define PF11 PIN_A9 +#define PA1 PIN_A10 // RMII Reference Clock - SB57 ON (default) +#define PA2 PIN_A11 // RMII MDIO - SB72 ON (default) +#define PA7 PIN_A12 // RMII RX Data Valid - SB31 ON (default) +#define PA8 86 +#define PA9 87 +#define PA10 88 +#define PA11 89 +#define PA12 90 +#define PA13 91 +#define PA14 92 +#define PB14 93 // LD3 LED_RED +#define PC1 PIN_A13 +#define PC4 PIN_A14 // RMII RXD0 - SB36 ON (default) +#define PC5 PIN_A15 // RMII RXD1 - SB29 ON (default) +#define PC13 97 +#define PC14 98 +#define PC15 99 // USER_BTN +#define PD8 100 +#define PD9 101 +#define PD10 102 // Serial Tx +#define PE1 103 // Serial Rx +#define PF12 PIN_A16 +#define PF13 PIN_A17 // LD2 LED_BLUE +#define PF14 PIN_A18 +#define PF15 107 +#define PG4 108 +#define PG5 109 +#define PG7 110 +#define PG8 111 +#define PG9 112 +#define PG10 113 +#define PG11 114 // RMII TX Enable - SB27 ON (default) +#define PG13 115 // RXII TXD0 - SB30 ON (default) +#define PG15 116 +#define PH0 117 +#define PH1 118 + +// Alternate pins number +#define PA0_ALT1 (PA0 | ALT1) +#define PA1_ALT1 (PA1 | ALT1) +#define PA1_ALT2 (PA1 | ALT2) +#define PA2_ALT1 (PA2 | ALT1) +#define PA2_ALT2 (PA2 | ALT2) +#define PA3_ALT1 (PA3 | ALT1) +#define PA3_ALT2 (PA3 | ALT2) +#define PA4_ALT1 (PA4 | ALT1) +#define PA4_ALT2 (PA4 | ALT2) +#define PA5_ALT1 (PA5 | ALT1) +#define PA6_ALT1 (PA6 | ALT1) +#define PA7_ALT1 (PA7 | ALT1) +#define PA7_ALT2 (PA7 | ALT2) +#define PA7_ALT3 (PA7 | ALT3) +#define PA8_ALT1 (PA8 | ALT1) +#define PA9_ALT1 (PA9 | ALT1) +#define PA10_ALT1 (PA10 | ALT1) +#define PA11_ALT1 (PA11 | ALT1) +#define PA12_ALT1 (PA12 | ALT1) +#define PA15_ALT1 (PA15 | ALT1) +#define PA15_ALT2 (PA15 | ALT2) +#define PB0_ALT1 (PB0 | ALT1) +#define PB0_ALT2 (PB0 | ALT2) +#define PB1_ALT1 (PB1 | ALT1) +#define PB1_ALT2 (PB1 | ALT2) +#define PB3_ALT1 (PB3 | ALT1) +#define PB3_ALT2 (PB3 | ALT2) +#define PB4_ALT1 (PB4 | ALT1) +#define PB4_ALT2 (PB4 | ALT2) +#define PB5_ALT1 (PB5 | ALT1) +#define PB5_ALT2 (PB5 | ALT2) +#define PB6_ALT1 (PB6 | ALT1) +#define PB6_ALT2 (PB6 | ALT2) +#define PB7_ALT1 (PB7 | ALT1) +#define PB8_ALT1 (PB8 | ALT1) +#define PB8_ALT2 (PB8 | ALT2) +#define PB9_ALT1 (PB9 | ALT1) +#define PB9_ALT2 (PB9 | ALT2) +#define PB14_ALT1 (PB14 | ALT1) +#define PB14_ALT2 (PB14 | ALT2) +#define PB15_ALT1 (PB15 | ALT1) +#define PB15_ALT2 (PB15 | ALT2) +#define PC0_ALT1 (PC0 | ALT1) +#define PC0_ALT2 (PC0 | ALT2) +#define PC1_ALT1 (PC1 | ALT1) +#define PC1_ALT2 (PC1 | ALT2) +#define PC4_ALT1 (PC4 | ALT1) +#define PC5_ALT1 (PC5 | ALT1) +#define PC6_ALT1 (PC6 | ALT1) +#define PC6_ALT2 (PC6 | ALT2) +#define PC7_ALT1 (PC7 | ALT1) +#define PC7_ALT2 (PC7 | ALT2) +#define PC8_ALT1 (PC8 | ALT1) +#define PC9_ALT1 (PC9 | ALT1) +#define PC10_ALT1 (PC10 | ALT1) +#define PC11_ALT1 (PC11 | ALT1) +#define PF0_ALT1 (PF0 | ALT1) +#define PF1_ALT1 (PF1 | ALT1) +#define PF6_ALT1 (PF6 | ALT1) +#define PF7_ALT1 (PF7 | ALT1) +#define PF8_ALT1 (PF8 | ALT1) +#define PF8_ALT2 (PF8 | ALT2) +#define PF9_ALT1 (PF9 | ALT1) +#define PF9_ALT2 (PF9 | ALT2) +#define PG13_ALT1 (PG13 | ALT1) + +#define NUM_DIGITAL_PINS 119 +#define NUM_DUALPAD_PINS 2 +#define NUM_ANALOG_INPUTS 28 + +// On-board LED pin number +#define LED_GREEN PB0 // LD1 +#define LED_YELLOW PE1 // LD2 +#define LED_RED PB14 // LD3 +#ifndef LED_BUILTIN + #define LED_BUILTIN LED_GREEN +#endif + +// On-board user button +#ifndef USER_BTN + #define USER_BTN PC13 +#endif + +// Timer Definitions +// Use TIM6/TIM7 when possible as servo and tone don't need GPIO output pin +#ifndef TIMER_TONE + #define TIMER_TONE TIM6 +#endif +#ifndef TIMER_SERVO + #define TIMER_SERVO TIM7 +#endif + +// UART Definitions +#ifndef SERIAL_UART_INSTANCE + #define SERIAL_UART_INSTANCE 3 //Connected to ST-Link +#endif + +// Serial pin used for console (ex: ST-Link) +// Required by Firmata +#ifndef PIN_SERIAL_RX + #define PIN_SERIAL_RX PD9 +#endif +#ifndef PIN_SERIAL_TX + #define PIN_SERIAL_TX PD8 +#endif + +// HSE default value is 25MHz in HAL +// By default HSE_BYPASS is based on HSI/2 from STLink +#ifndef HSE_BYPASS_NOT_USED + #define HSE_VALUE 8000000 +#endif + +// Extra HAL modules +#if !defined(HAL_DAC_MODULE_DISABLED) + #define HAL_DAC_MODULE_ENABLED +#endif +#if !defined(HAL_ETH_MODULE_DISABLED) + #define HAL_ETH_MODULE_ENABLED +#endif +#if !defined(HAL_OSPI_MODULE_DISABLED) + #define HAL_OSPI_MODULE_ENABLED +#endif +#if !defined(HAL_SD_MODULE_DISABLED) + #define HAL_SD_MODULE_ENABLED +#endif + +/*---------------------------------------------------------------------------- + * Arduino objects - C++ only + *----------------------------------------------------------------------------*/ + +#ifdef __cplusplus + // These serial port names are intended to allow libraries and architecture-neutral + // sketches to automatically default to the correct port name for a particular type + // of use. For example, a GPS module would normally connect to SERIAL_PORT_HARDWARE_OPEN, + // the first hardware serial port whose RX/TX pins are not dedicated to another use. + // + // SERIAL_PORT_MONITOR Port which normally prints to the Arduino Serial Monitor + // + // SERIAL_PORT_USBVIRTUAL Port which is USB virtual serial + // + // SERIAL_PORT_LINUXBRIDGE Port which connects to a Linux system via Bridge library + // + // SERIAL_PORT_HARDWARE Hardware serial port, physical RX & TX pins. + // + // SERIAL_PORT_HARDWARE_OPEN Hardware serial ports which are open for use. Their RX & TX + // pins are NOT connected to anything by default. + #define SERIAL_PORT_MONITOR Serial + #define SERIAL_PORT_HARDWARE Serial +#endif diff --git a/variants/README.md b/variants/README.md new file mode 100644 index 0000000..3d596b2 --- /dev/null +++ b/variants/README.md @@ -0,0 +1,3 @@ +# Variants + +