HAL_ChibiOS: add support for CAN on STM32H7 boards

libraries/AP_HAL_ChibiOS/CAN.h

@@ -28,7 +28,11 @@
 
 #include "CANThread.h"
 #include "CANClock.h"
+#if defined(STM32H7XX)
+#include "CANFDIface.h"
+#else
 #include "CANIface.h"
+#endif
 
 #define MAX_NUMBER_OF_CAN_INTERFACES    2
 #define MAX_NUMBER_OF_CAN_DRIVERS       2

libraries/AP_HAL_ChibiOS/CANFDIface.h

@@ -0,0 +1,448 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ * Code by Siddharth Bharat Purohit
+ */
+
+#pragma once
+
+#include "AP_HAL_ChibiOS.h"
+
+#if HAL_WITH_UAVCAN
+
+#include "CANThread.h"
+#include "fdcan.hpp"
+
+class SLCANRouter;
+
+namespace ChibiOS_CAN
+{
+/**
+ * Driver error codes.
+ * These values can be returned from driver functions negated.
+ */
+//static const uavcan::int16_t ErrUnknown               = 1000; ///< Reserved for future use
+static const uavcan::int16_t ErrNotImplemented          = 1001; ///< Feature not implemented
+static const uavcan::int16_t ErrInvalidBitRate          = 1002; ///< Bit rate not supported
+static const uavcan::int16_t ErrLogic                   = 1003; ///< Internal logic error
+static const uavcan::int16_t ErrUnsupportedFrame        = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc)
+static const uavcan::int16_t ErrMsrInakNotSet           = 1005; ///< INAK bit of the MSR register is not 1
+static const uavcan::int16_t ErrMsrInakNotCleared       = 1006; ///< INAK bit of the MSR register is not 0
+static const uavcan::int16_t ErrBitRateNotDetected      = 1007; ///< Auto bit rate detection could not be finished
+static const uavcan::int16_t ErrFilterNumConfigs        = 1008; ///< Number of filters is more than supported
+
+/**
+ * RX queue item.
+ * The application shall not use this directly.
+ */
+struct CanRxItem {
+    uavcan::uint64_t utc_usec;
+    uavcan::CanFrame frame;
+    uavcan::CanIOFlags flags;
+    CanRxItem()
+        : utc_usec(0)
+        , flags(0)
+    { }
+};
+
+/**
+ * Single CAN iface.
+ * The application shall not use this directly.
+ */
+class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
+{
+    friend class ::SLCANRouter;
+    static SLCANRouter *_slcan_router;
+    class RxQueue
+    {
+        CanRxItem* const buf_;
+        const uavcan::uint8_t capacity_;
+        uavcan::uint8_t in_;
+        uavcan::uint8_t out_;
+        uavcan::uint8_t len_;
+        uavcan::uint32_t overflow_cnt_;
+        void registerOverflow();
+
+    public:
+        RxQueue(CanRxItem* buf, uavcan::uint8_t capacity)
+            : buf_(buf)
+            , capacity_(capacity)
+            , in_(0)
+            , out_(0)
+            , len_(0)
+            , overflow_cnt_(0)
+        { }
+
+        void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags);
+        void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags);
+
+        void reset();
+
+        unsigned getLength() const
+        {
+            return len_;
+        }
+
+        uavcan::uint32_t getOverflowCount() const
+        {
+            return overflow_cnt_;
+        }
+    };
+
+    struct MessageRAM {
+        uavcan::uint32_t StandardFilterSA;
+        uavcan::uint32_t ExtendedFilterSA;
+        uavcan::uint32_t RxFIFO0SA;
+        uavcan::uint32_t RxFIFO1SA;
+        uavcan::uint32_t TxFIFOQSA;
+        uavcan::uint32_t EndAddress;
+    } MessageRam_;
+
+    struct Timings {
+        uavcan::uint16_t prescaler;
+        uavcan::uint8_t sjw;
+        uavcan::uint8_t bs1;
+        uavcan::uint8_t bs2;
+
+        Timings()
+            : prescaler(0)
+            , sjw(0)
+            , bs1(0)
+            , bs2(0)
+        { }
+    };
+
+    struct TxItem {
+        uavcan::MonotonicTime deadline;
+        uavcan::CanFrame frame;
+        bool loopback;
+        bool abort_on_error;
+        uint8_t index;
+        TxItem() :
+            loopback(false),
+            abort_on_error(false)
+        { }
+    };
+
+    enum { NumTxMailboxes = 32 };
+
+    static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes];
+    static uint32_t FDCANMessageRAMOffset_;
+    RxQueue rx_queue_;
+    fdcan::CanType* const can_;
+    uavcan::uint64_t error_cnt_;
+    uavcan::uint32_t served_aborts_cnt_;
+    BusEvent& update_event_;
+    TxItem pending_tx_[NumTxMailboxes];
+    uavcan::uint8_t peak_tx_mailbox_index_;
+    const uavcan::uint8_t self_index_;
+    bool had_activity_;
+    int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
+
+    virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
+                                 uavcan::CanIOFlags flags);
+
+    virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+                                    uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags);
+
+    virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
+            uavcan::uint16_t num_configs);
+
+    virtual uavcan::uint16_t getNumFilters() const;
+
+    void setupMessageRam(void);
+
+    static uint32_t FDCAN2MessageRAMOffset_;
+public:
+    enum { MaxRxQueueCapacity = 254 };
+
+    enum OperatingMode {
+        NormalMode,
+        SilentMode
+    };
+
+    CanIface(fdcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index,
+             CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity);
+
+    /**
+     * Initializes the hardware CAN controller.
+     * Assumes:
+     *   - Iface clock is enabled
+     *   - Iface has been resetted via RCC
+     *   - Caller will configure NVIC by itself
+     */
+    int init(const uavcan::uint32_t bitrate, const OperatingMode mode);
+
+    void handleTxCompleteInterrupt(uavcan::uint64_t utc_usec);
+    void handleRxInterrupt(uavcan::uint8_t fifo_index);
+
+    bool readRxFIFO(uavcan::uint8_t fifo_index);
+    /**
+     * 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 pollErrorFlagsFromISR();
+
+    void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
+
+    bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const;
+    bool isRxBufferEmpty() const;
+
+    /**
+     * Number of RX frames lost due to queue overflow.
+     * This is an atomic read, it doesn't require a critical section.
+     */
+    uavcan::uint32_t getRxQueueOverflowCount() const
+    {
+        return rx_queue_.getOverflowCount();
+    }
+
+    /**
+     * 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.
+     */
+    virtual uavcan::uint64_t getErrorCount() const;
+
+    /**
+     * Number of times the driver exercised library's requirement to abort transmission on first error.
+     * This is an atomic read, it doesn't require a critical section.
+     * See @ref uavcan::CanIOFlagAbortOnError.
+     */
+    uavcan::uint32_t getVoluntaryTxAbortCount() const
+    {
+        return served_aborts_cnt_;
+    }
+
+    /**
+     * Returns the number of frames pending in the RX queue.
+     * This is intended for debug use only.
+     */
+    unsigned getRxQueueLength() const;
+
+    /**
+     * Whether this iface had at least one successful IO since the previous call of this method.
+     * This is designed for use with iface activity LEDs.
+     */
+    bool hadActivity();
+
+    /**
+     * Peak number of TX mailboxes used concurrently since initialization.
+     * Range is [1, 3].
+     * Value of 3 suggests that priority inversion could be taking place.
+     */
+    uavcan::uint8_t getPeakNumTxMailboxesUsed() const
+    {
+        return uavcan::uint8_t(peak_tx_mailbox_index_ + 1);
+    }
+
+    fdcan::CanType* can_reg(void)
+    {
+        return can_;
+    }
+};
+
+/**
+ * CAN driver, incorporates all available CAN ifaces.
+ * Please avoid direct use, prefer @ref CanInitHelper instead.
+ */
+class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
+{
+    BusEvent update_event_;
+    static bool clock_init_;
+    CanIface if0_;
+#if UAVCAN_STM32_NUM_IFACES > 1
+    CanIface if1_;
+#endif
+    bool initialized_by_me_[UAVCAN_STM32_NUM_IFACES];
+    uavcan::uint8_t num_ifaces_;
+    uavcan::uint8_t if_int_to_gl_index_[UAVCAN_STM32_NUM_IFACES];
+
+
+    virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
+                                   const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
+                                   uavcan::MonotonicTime blocking_deadline);
+
+    static void initOnce();
+    static void initOnce(uavcan::uint8_t can_number, bool enable_irqs);
+
+public:
+    template <unsigned RxQueueCapacity>
+    CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
+        : update_event_(*this)
+        , if0_(fdcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
+#if UAVCAN_STM32_NUM_IFACES > 1
+        , if1_(fdcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
+#endif
+    {
+        uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();
+    }
+
+    /**
+     * This function returns select masks indicating which interfaces are available for read/write.
+     */
+    uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const;
+
+    BusEvent* getUpdateEvent()
+    {
+        return &update_event_;
+    }
+    /**
+     * Whether there's at least one interface where receive() would return a frame.
+     */
+    bool hasReadableInterfaces() const;
+
+    /**
+     * Returns zero if OK.
+     * Returns negative value if failed (e.g. invalid bitrate).
+     */
+    int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
+    int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number);
+
+    virtual CanIface* getIface(uavcan::uint8_t iface_index);
+
+    virtual uavcan::uint8_t getNumIfaces() const
+    {
+        return num_ifaces_;
+    }
+
+    /**
+     * Whether at least one iface had at least one successful IO since previous call of this method.
+     * This is designed for use with iface activity LEDs.
+     */
+    bool hadActivity();
+};
+
+/**
+ * Helper class.
+ * Normally only this class should be used by the application.
+ * 145 usec per Extended CAN frame @ 1 Mbps, e.g. 32 RX slots * 145 usec --> 4.6 msec before RX queue overruns.
+ */
+template <unsigned RxQueueCapacity = 128>
+class CanInitHelper
+{
+    CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity];
+
+public:
+    enum { BitRateAutoDetect = 0 };
+
+    CanDriver driver;
+
+    CanInitHelper() :
+        driver(queue_storage_)
+    { }
+
+    /**
+     * This overload simply configures the provided bitrate.
+     * Auto bit rate detection will not be performed.
+     * Bitrate value must be positive.
+     * @return  Negative value on error; non-negative on success. Refer to constants Err*.
+     */
+    int init(uavcan::uint32_t bitrate)
+    {
+        return driver.init(bitrate, CanIface::NormalMode);
+    }
+
+    int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number)
+    {
+        return driver.init(bitrate, mode, can_number);
+    }
+
+    /**
+     * This function can either initialize the driver at a fixed bit rate, or it can perform
+     * automatic bit rate detection. For theory please refer to the CiA application note #801.
+     *
+     * @param delay_callable    A callable entity that suspends execution for strictly more than one second.
+     *                          The callable entity will be invoked without arguments.
+     *                          @ref getRecommendedListeningDelay().
+     *
+     * @param inout_bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
+     *                          If auto detection was used, the function will update the argument
+     *                          with established bit rate. In case of an error the value will be undefined.
+     *
+     * @return                  Negative value on error; non-negative on success. Refer to constants Err*.
+     */
+    template <typename DelayCallable>
+    int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect)
+    {
+        if (inout_bitrate > 0) {
+            return driver.init(inout_bitrate, CanIface::NormalMode);
+        } else {
+            static const uavcan::uint32_t StandardBitRates[] = {
+                1000000,
+                500000,
+                250000,
+                125000
+            };
+
+            for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) {
+                inout_bitrate = StandardBitRates[br];
+
+                const int res = driver.init(inout_bitrate, CanIface::SilentMode);
+
+                delay_callable();
+
+                if (res >= 0) {
+                    for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
+                        if (!driver.getIface(iface)->isRxBufferEmpty()) {
+                            // Re-initializing in normal mode
+                            return driver.init(inout_bitrate, CanIface::NormalMode);
+                        }
+                    }
+                }
+            }
+
+            return -ErrBitRateNotDetected;
+        }
+    }
+
+    /**
+     * Use this value for listening delay during automatic bit rate detection.
+     */
+    static uavcan::MonotonicDuration getRecommendedListeningDelay()
+    {
+        return uavcan::MonotonicDuration::fromMSec(1050);
+    }
+};
+
+}
+
+#include "CANSerialRouter.h"
+
+#endif //HAL_WITH_UAVCAN

libraries/AP_HAL_ChibiOS/CANIface.h

@@ -43,7 +43,9 @@
 #include "AP_HAL_ChibiOS.h"
 
 #if HAL_WITH_UAVCAN
-
+# if defined(STM32H7XX)
+#include "CANFDIface.h"
+#else
 #include "CANThread.h"
 #include "CANIface.h"
 #include "bxcan.hpp"
@@ -444,5 +446,5 @@ public:
 }
 
 #include "CANSerialRouter.h"
-
+#endif // defined(STM32H7XX)
 #endif //HAL_WITH_UAVCAN

libraries/AP_HAL_ChibiOS/CanIface.cpp

@@ -43,13 +43,15 @@
 #if HAL_WITH_UAVCAN
 #include <cassert>
 #include <cstring>
-#include "CANIface.h"
 #include "CANClock.h"
 #include "CANInternal.h"
 #include "CANSerialRouter.h"
 #include <AP_UAVCAN/AP_UAVCAN_SLCAN.h>
 # include <hal.h>
 
+# if !defined(STM32H7XX)
+#include "CANIface.h"
+
 #if CH_KERNEL_MAJOR == 2
 # if !(defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F3XX)  || defined(STM32F4XX))
 // IRQ numbers
@@ -1253,4 +1255,6 @@ UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler)
 
 } // extern "C"
 
+#endif //!defined(STM32H7XX)
+
 #endif //HAL_WITH_UAVCAN

libraries/AP_HAL_ChibiOS/bxcan.hpp

@@ -44,7 +44,7 @@
 
 #if HAL_WITH_UAVCAN
 
-#include <uavcan_stm32/build_config.hpp>
+# if !defined(STM32H7XX)
 
 #include <uavcan/uavcan.hpp>
 #include <stdint.h>
@@ -326,4 +326,5 @@ constexpr unsigned long FMR_FINIT =           (1U << 0); /* Bit 0:  Filter Init
 #if UAVCAN_CPP_VERSION < UAVCAN_CPP11
 # undef constexpr
 #endif
+#endif //!defined(STM32H7XX)
 #endif //HAL_WITH_UAVCAN

libraries/AP_HAL_ChibiOS/fdcan.hpp

@@ -0,0 +1,90 @@
+/*
+ * 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 <http://www.gnu.org/licenses/>.
+ *
+ * Code by Siddharth Bharat Purohit
+ */
+
+#pragma once
+
+#include "AP_HAL_ChibiOS.h"
+
+#if HAL_WITH_UAVCAN
+
+#if defined(STM32H7XX)
+
+#include <uavcan/uavcan.hpp>
+#include <stdint.h>
+
+#ifndef UAVCAN_CPP_VERSION
+# error UAVCAN_CPP_VERSION
+#endif
+
+#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
+// #undef'ed at the end of this file
+# define constexpr const
+#endif
+
+namespace ChibiOS_CAN
+{
+namespace fdcan
+{
+typedef FDCAN_GlobalTypeDef CanType;
+
+constexpr unsigned long IDE = (0x40000000U); // Identifier Extension
+constexpr unsigned long STID_MASK = (0x1FFC0000U); // Standard Identifier Mask
+constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask
+constexpr unsigned long RTR       = (0x20000000U); // Remote Transmission Request
+constexpr unsigned long DLC_MASK  = (0x000F0000U); // Data Length Code
+
+/**
+ * CANx register sets
+ */
+CanType* const Can[UAVCAN_STM32_NUM_IFACES] = {
+    reinterpret_cast<CanType*>(FDCAN1_BASE)
+#if UAVCAN_STM32_NUM_IFACES > 1
+    ,
+    reinterpret_cast<CanType*>(FDCAN2_BASE)
+#endif
+};
+}
+}
+
+#if UAVCAN_CPP_VERSION < UAVCAN_CPP11
+# undef constexpr
+#endif
+#endif //defined(STM32H7XX)
+#endif //HAL_WITH_UAVCAN

libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.dat

@@ -81,8 +81,8 @@ PB2 BOOT1 INPUT
 PB3 FMU_SW0 INPUT
 
 # This defines the pins for the 2nd CAN interface, if available.
-#PB6 CAN2_TX CAN2
-#PB12 CAN2_RX CAN2
+PB6 CAN2_TX CAN2
+PB12 CAN2_RX CAN2
 
 # Now the first I2C bus. The pin speeds are automatically setup
 # correctly, but can be overridden here if needed.
@@ -135,8 +135,8 @@ PD7 BARO_CS CS
 PE4 MPU_EXT_CS CS
 
 # the first CAN bus
-#PD0 CAN1_RX CAN1
-#PD1 CAN1_TX CAN1
+PD0 CAN1_RX CAN1
+PD1 CAN1_TX CAN1
 
 # Another USART, this one for telem1. This one has RTS and CTS lines.
 # USART2 serial2 telem1