CanDriver.cpp

Go to the documentation of this file.

/* Copyright (c) 2018 Skyward Experimental Rocketry
 * Author: Luca Erbetta
 *
 * 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.
 */
 
#include "CanDriver.h"
 
#include <kernel/scheduler/scheduler.h>
#include <utils/ClockUtils.h>
#include <utils/KernelTime.h>
 
#include <algorithm>
#include <cmath>
 
#include "CanInterrupt.h"
#include "diagnostic/PrintLogger.h"
 
using namespace std::chrono;
 
namespace Boardcore
{
 
namespace Canbus
{
 
PrintLogger l = Logging::getLogger("canbus");
 
CanbusDriver::CanbusDriver(CAN_TypeDef* can, CanbusConfig config,
                           AutoBitTiming bitTiming)
    : CanbusDriver(can, config, calcBitTiming(bitTiming))
{
}
 
CanbusDriver::CanbusDriver(CAN_TypeDef* can, CanbusConfig config,
                           BitTiming bitTiming)
    : can(can)
{
    if (can == CAN2)
    {
        // CAN2 also need the CAN1 clock
        ClockUtils::enablePeripheralClock(CAN1);
    }
    // Enable the peripheral clock
    ClockUtils::enablePeripheralClock(can);
 
    // Enter init mode
    can->MCR &= ~CAN_MCR_SLEEP;
    can->MCR |= CAN_MCR_INRQ;
 
    while ((can->MSR & CAN_MSR_INAK) == 0)
        ;
 
    // Automatic wakeup when a new packet is available
    if (config.awum)
        can->MCR |= CAN_MCR_AWUM;
 
    // Automatically recover from Bus-Off mode
    if (config.abom)
        can->MCR |= CAN_MCR_ABOM;
 
    // Disable automatic retransmission
    if (config.nart)
        can->MCR |= CAN_MCR_NART;
 
    // Bit timing configuration
    can->BTR &= ~CAN_BTR_BRP;
    can->BTR &= ~CAN_BTR_TS1;
    can->BTR &= ~CAN_BTR_TS2;
    can->BTR &= ~CAN_BTR_SJW;
 
    can->BTR |= bitTiming.BRP & 0x3FF;
    can->BTR |= ((bitTiming.BS1 - 1) & 0xF) << 16;
    can->BTR |= ((bitTiming.BS2 - 1) & 0x7) << 20;
    can->BTR |= ((bitTiming.SJW - 1) & 0x3) << 24;
 
    if (config.loopback)
        can->BTR |= CAN_BTR_LBKM;
 
    // Enter filter initialization mode
    can->FMR |= CAN_FMR_FINIT;
 
    if (can == CAN1)
        canDrivers[0] = this;
    else
        canDrivers[1] = this;
 
    // Enable interrupts
    can->IER |= CAN_IER_FMPIE0 | CAN_IER_FMPIE1 | CAN_IER_TMEIE;
 
    // Enable the corresponding interrupts
    if (can == CAN1)
    {
        NVIC_EnableIRQ(CAN1_RX0_IRQn);
        NVIC_SetPriority(CAN1_RX0_IRQn, 14);
 
        NVIC_EnableIRQ(CAN1_RX1_IRQn);
        NVIC_SetPriority(CAN1_RX1_IRQn, 14);
 
        NVIC_EnableIRQ(CAN1_TX_IRQn);
        NVIC_SetPriority(CAN1_TX_IRQn, 14);
    }
    else if (can == CAN2)
    {
        NVIC_EnableIRQ(CAN2_RX0_IRQn);
        NVIC_SetPriority(CAN2_RX0_IRQn, 14);
 
        NVIC_EnableIRQ(CAN2_RX1_IRQn);
        NVIC_SetPriority(CAN2_RX1_IRQn, 14);
 
        NVIC_EnableIRQ(CAN2_TX_IRQn);
        NVIC_SetPriority(CAN2_TX_IRQn, 14);
    }
    else
    {
        PrintLogger ls = l.getChild("constructor");
        LOG_ERR(ls, "Unsupported peripheral");
    }
}
 
CanbusDriver::~CanbusDriver()
{
    ClockUtils::disablePeripheralClock(can);
    if (can == CAN2)
        ClockUtils::disablePeripheralClock(CAN1);
}
 
CanbusDriver::BitTiming CanbusDriver::calcBitTiming(AutoBitTiming autoBt)
{
    PrintLogger ls = l.getChild("bittiming");
 
    BitTiming cfgOpt;
    float costOpt = 1000;
    uint8_t NOpt  = 5;
 
    BitTiming cfgIter;
    cfgIter.SJW     = 0;
    uint32_t apbclk = ClockUtils::getAPBPeripheralsClock(ClockUtils::APB::APB1);
 
    // Iterate over the possible number of quanta in a bit to find the best
    // settings
    for (uint8_t N = 3; N <= 25; N++)
    {
        // Calc optimal baud rate prescaler
        cfgIter.BRP = std::max(
            std::min((int)roundf(apbclk * 1.0f / (autoBt.baudRate * N) - 1),
                     1 << 10),
            1);
 
        // Given N, calculate BS1 and BS2 that result in a sample time as
        // close as possible to the target one
        cfgIter.BS1 =
            std::min(std::max((int)roundf(autoBt.samplePoint * N - 1), 1),
                     std::min(N - 2, 16));
 
        cfgIter.BS2 = N - cfgIter.BS1 - 1;
 
        float brErrPercent =
            fabs(apbclk * 1.0f / (N * (cfgIter.BRP + 1)) - autoBt.baudRate) /
            autoBt.baudRate;
        float sp = (1 + cfgIter.BS1) * 1.0f / (1 + cfgIter.BS1 + cfgIter.BS2);
 
        float spErrPercent = fabs(sp - autoBt.samplePoint) / autoBt.samplePoint;
 
        // Calculate the cost function over N
        float cost = BR_ERR_WEIGHT * brErrPercent +
                     SP_ERR_WEIGHT * spErrPercent +
                     N_ERR_WEIGHT * fabs(N - 10) / 25;
 
        // Find config that minimizes the cost function
        if (cost < costOpt)
        {
            cfgOpt  = cfgIter;
            costOpt = cost;
            NOpt    = N;
        }
    }
 
    cfgOpt.SJW = fminf(ceilf(NOpt * 1.0f / 5), 4);
 
    LOG_DEBUG(ls,
              "Optimal Bit Timing Registers: BRP={}, BS1={}, BS2={}, SJW={}",
              cfgOpt.BRP, cfgOpt.BS1, cfgOpt.BS2, cfgOpt.SJW);
 
    float brTrue = apbclk * 1.0f / ((cfgOpt.BRP + 1) * NOpt);
    float spTrue = (1 + cfgOpt.BS1) * 1.0f / (1 + cfgOpt.BS1 + cfgOpt.BS2);
 
    LOG_DEBUG(ls,
              "Optimal Bit Timing: BR_true={:.2f}, spTrue:{:.2f}%, "
              "BR_error={:.2f}%, SP_error={:.2f}%",
              brTrue / 1000, spTrue * 100,
              fabsf(brTrue - autoBt.baudRate) / autoBt.baudRate * 100,
              fabs(spTrue - autoBt.samplePoint) / autoBt.samplePoint * 100);
 
    return cfgOpt;
}
 
bool CanbusDriver::init(std::chrono::milliseconds timeout)
{
    if (isInit)
        return true;
 
    PrintLogger ls = l.getChild("init");
 
    can->FMR &= ~CAN_FMR_FINIT;  // Exit filter init mode
    can->MCR &= ~CAN_MCR_INRQ;   // Enable canbus
 
    LOG_DEBUG(ls, "Waiting for CAN bus synchronization...");
 
    auto timeoutTs = timeout == milliseconds::zero()
                         ? steady_clock::time_point::max()
                         : steady_clock::now() + timeout;
 
    // Wait until the can peripheral synchronizes with the bus
    while ((can->MSR & CAN_MSR_INAK) > 0)
    {
        if (steady_clock::now() > timeoutTs)
        {
            LOG_ERR(ls, "CAN bus synchronization timed out after {} ms",
                    timeout.count());
            return false;
        }
        Thread::sleep(1);
    }
 
    LOG_INFO(ls, "CAN bus synchronized, init done!");
 
    isInit = true;
    return true;
}
 
bool CanbusDriver::addFilter(FilterBank filter)
{
    PrintLogger ls = l.getChild("addfilter");
    uint8_t index  = filterIndex;
 
    // CAN2 filters start from the 15th position
    if (can == CAN2)
        index = index + 14;
 
    if (isInit)
    {
        LOG_ERR(ls, "Cannot add filter: CAN bus already initialized");
        return false;
    }
 
    if (index >= NUM_FILTER_BANKS)
    {
        LOG_ERR(ls, "Cannot add filter: no more filter banks available");
        return false;
    }
 
    // NOTE: the filters are set in CAN1 peripheral because the filter registers
    // between the peripherals are in common.
    CAN1->sFilterRegister[index].FR1 = filter.FR1;
    CAN1->sFilterRegister[index].FR2 = filter.FR2;
 
    CAN1->FM1R |= (filter.mode == FilterMode::MASK ? 0 : 1) << index;
    CAN1->FS1R |= (filter.scale == FilterScale::DUAL16 ? 0 : 1) << index;
    CAN1->FFA1R |= (filter.fifo & 0x1) << index;
 
    // Enable the filter
    CAN1->FA1R |= 1 << index;
 
    ++filterIndex;
 
    return true;
}
 
uint32_t CanbusDriver::send(CanPacket packet)
{
    PrintLogger ls = l.getChild("send");
 
    if (!isInit)
    {
        LOG_ERR(ls, "CAN bus is not initialized!");
        return 0;
    }
 
    bool didWait = false;
 
    {
        miosix::FastInterruptDisableLock d;
 
        // Wait until there is an empty mailbox available to use
        while ((can->TSR & CAN_TSR_TME) == 0)
        {
            didWait = true;
            waiting = Thread::IRQgetCurrentThread();
            Thread::IRQwait();
            {
                miosix::FastInterruptEnableLock e(d);
                Thread::yield();
            }
        }
    }
 
    if (didWait)
    {
        // Warn that the function blocked. We are probably transmitting too fast
        LOG_WARN_ASYNC(ls, "Had to wait for an empty mailbox!");
    }
 
    // Index of first empty mailbox
    uint8_t mbxCode = (can->TSR & CAN_TSR_CODE) >> 24;
 
    if (mbxCode > 2)
    {
        LOG_ERR(ls, "Invalid TSR_CODE!");
        return 0;
    }
 
    uint32_t seq          = txSeq++;
    txMailboxSeq[mbxCode] = seq;
 
    CAN_TxMailBox_TypeDef* mailbox = &can->sTxMailBox[mbxCode];
 
    can->sTxMailBox[mbxCode].TIR &= CAN_TI0R_TXRQ;
    if (packet.ext)
    {
        can->sTxMailBox[mbxCode].TIR |= ((packet.id & 0x1FFFFFFF) << 3);
        can->sTxMailBox[mbxCode].TIR |= 1 << 2;
    }
    else
    {
        can->sTxMailBox[mbxCode].TIR |= ((packet.id & 0x7FF) << 21);
    }
 
    can->sTxMailBox[mbxCode].TIR |= (packet.rtr ? 1 : 0) << 1;
 
    mailbox->TDTR = (packet.length & 0xF);
 
    // Reset data registers
    mailbox->TDLR = 0;
    mailbox->TDHR = 0;
 
    // Fill data registers
    for (uint8_t i = 0; i < packet.length; ++i)
        if (i < 4)
            mailbox->TDLR |= packet.data[i] << i * 8;
        else
            mailbox->TDHR |= packet.data[i] << (i - 4) * 8;
 
    // Finally send the packet
    can->sTxMailBox[mbxCode].TIR |= CAN_TI0R_TXRQ;
 
    return seq;
}
 
void CanbusDriver::handleRXInterrupt(int fifo)
{
    CanRXStatus status;
    status.fifo = fifo;
    volatile uint32_t* RFR;
    CAN_FIFOMailBox_TypeDef* mailbox;
 
    mailbox = &can->sFIFOMailBox[fifo];
    if (fifo == 0)
        RFR = &can->RF0R;
    else
        RFR = &can->RF1R;
 
    status.fifoOverrun = (*RFR & CAN_RF0R_FOVR0) > 0;
    status.fifoFull    = (*RFR & CAN_RF0R_FULL0) > 0;
 
    CanPacket p;
    bool hppw = false;
 
    // Note: Bit position definitions are the same for both FIFOs
    // eg: CAN_RF0R_FMP0 == CAN_RF1R_FMP1
 
    if ((*RFR & CAN_RF0R_FMP0) > 0)
    {
        p.timestamp = Kernel::IRQgetOldTick();
 
        status.rxStatus     = *RFR & (CAN_RF0R_FULL0 | CAN_RF0R_FOVR0) >> 3;
        status.errCode      = (can->ESR | CAN_ESR_LEC) >> 4;
        status.rxErrCounter = (can->ESR | CAN_ESR_REC) >> 24;
 
        p.ext = (mailbox->RIR & CAN_RI0R_IDE) > 0;
        p.rtr = (mailbox->RIR & CAN_RI0R_RTR) > 0;
 
        if (p.ext)
            p.id = (mailbox->RIR >> 3) & 0x1FFFFFFF;
        else
            p.id = (mailbox->RIR >> 21) & 0x7FF;
 
        p.length = mailbox->RDTR & CAN_RDT0R_DLC;
 
        for (uint8_t i = 0; i < p.length; i++)
            if (i < 4)  // Low register
                p.data[i] = (mailbox->RDLR >> (i * 8)) & 0xFF;
            else  // High register
                p.data[i] = (mailbox->RDHR >> ((i - 4) * 8)) & 0xFF;
 
        *RFR |= CAN_RF0R_RFOM0;
 
        // Put the message into the queue
        bufRxPackets.IRQput(CanRXPacket{p, status}, hppw);
    }
 
    if (hppw)
        miosix::Scheduler::IRQfindNextThread();
}
 
void CanbusDriver::wakeTXThread()
{
    if (waiting)
    {
        waiting->IRQwakeup();
        waiting = 0;
    }
}
 
}  // namespace Canbus
 
}  // namespace Boardcore