fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-14 11:11:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

[WIP] uavcan use HRT as monotonic source

Browse Source
This commit is contained in:
Daniel Agar
2022-07-07 21:29:42 -04:00
parent 2f486c37fc
commit 72f7f0d787
22 changed files with 42 additions and 271 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/cuav/nora/default.px4board
-1
View File
@@ -40,7 +40,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
CONFIG_COMMON_TELEMETRY=y CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_BATTERY_STATUS=y
boards/cuav/x7pro/default.px4board
-1
View File
@@ -41,7 +41,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
CONFIG_COMMON_TELEMETRY=y CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_BATTERY_STATUS=y
boards/modalai/fc-v2/default.px4board
-1
View File
@@ -36,7 +36,6 @@ CONFIG_DRIVERS_RPM=y
CONFIG_COMMON_TELEMETRY=y CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_BOARD_UAVCAN_INTERFACES=1
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_CAMERA_FEEDBACK=y
boards/px4/fmu-v5/cyphal.px4board
-1
View File
@@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_DRIVERS_HEATER=n CONFIG_DRIVERS_HEATER=n
CONFIG_DRIVERS_OSD=n CONFIG_DRIVERS_OSD=n
CONFIG_DRIVERS_UAVCAN=n CONFIG_DRIVERS_UAVCAN=n
boards/px4/fmu-v5/debug.px4board
-1
View File
@@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_BAROMETERS=n CONFIG_COMMON_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_MAGNETOMETER=n CONFIG_COMMON_MAGNETOMETER=n
boards/px4/fmu-v5/default.px4board
-1
View File
@@ -43,7 +43,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
CONFIG_COMMON_TELEMETRY=y CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_BATTERY_STATUS=y
boards/px4/fmu-v5/protected.px4board
-1
View File
@@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_BAROMETERS=n CONFIG_COMMON_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_DISTANCE_SENSOR=n
boards/px4/fmu-v5/stackcheck.px4board
-1
View File
@@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_HYGROMETERS=n CONFIG_COMMON_HYGROMETERS=n
CONFIG_COMMON_TELEMETRY=n CONFIG_COMMON_TELEMETRY=n
CONFIG_DRIVERS_BATT_SMBUS=n CONFIG_DRIVERS_BATT_SMBUS=n
boards/px4/fmu-v5/uavcanv0periph.px4board
-1
View File
@@ -1,4 +1,3 @@
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
CONFIG_COMMON_BAROMETERS=n CONFIG_COMMON_BAROMETERS=n
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
CONFIG_COMMON_HYGROMETERS=n CONFIG_COMMON_HYGROMETERS=n
boards/px4/fmu-v6c/default.px4board
-1
View File
@@ -29,7 +29,6 @@ CONFIG_DRIVERS_PX4IO=y
CONFIG_COMMON_TELEMETRY=y CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_BATTERY_STATUS=y
CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_CAMERA_FEEDBACK=y
boards/px4/fmu-v6x/default.px4board
-1
View File
@@ -36,7 +36,6 @@ CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_SAFETY_BUTTON=y
CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_AIRSPEED_SELECTOR=y
CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_CAMERA_FEEDBACK=y
CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_COMMANDER=y
cmake/kconfig.cmake
-4
View File
@@ -298,10 +298,6 @@ if(EXISTS ${BOARD_DEFCONFIG})
set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE) set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE)
endif() endif()
if(UAVCAN_TIMER_OVERRIDE)
set(config_uavcan_timer_override ${UAVCAN_TIMER_OVERRIDE} CACHE INTERNAL "UAVCAN TIMER OVERRIDE" FORCE)
endif()
# OPTIONS # OPTIONS
if(CONSTRAINED_FLASH) if(CONSTRAINED_FLASH)
src/drivers/uavcan/CMakeLists.txt
-10
View File
@@ -42,19 +42,10 @@ set(UAVCAN_PLATFORM "generic")
if(CONFIG_ARCH_CHIP) if(CONFIG_ARCH_CHIP)
if(${CONFIG_ARCH_CHIP} MATCHES "kinetis") if(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
set(UAVCAN_DRIVER "kinetis") set(UAVCAN_DRIVER "kinetis")
set(UAVCAN_TIMER 1)
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7") elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
set(UAVCAN_DRIVER "stm32h7") set(UAVCAN_DRIVER "stm32h7")
set(UAVCAN_TIMER 5) # The default timer is TIM5
if (DEFINED config_uavcan_timer_override)
set (UAVCAN_TIMER ${config_uavcan_timer_override})
endif()
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32") elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
set(UAVCAN_DRIVER "stm32") set(UAVCAN_DRIVER "stm32")
set(UAVCAN_TIMER 6) # The default timer is TIM6
if (DEFINED config_uavcan_timer_override)
set (UAVCAN_TIMER ${config_uavcan_timer_override})
endif()
endif() endif()
endif() endif()
@@ -71,7 +62,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
add_definitions( add_definitions(
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1 -DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER} -DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
src/drivers/uavcan/Kconfig
-5
View File
@@ -17,8 +17,3 @@ depends on DRIVERS_UAVCAN
string "UAVCAN peripheral firmware" string "UAVCAN peripheral firmware"
help help
list of UAVCAN peripheral firmware to build and embed list of UAVCAN peripheral firmware to build and embed
menuconfig BOARD_UAVCAN_TIMER_OVERRIDE
depends on DRIVERS_UAVCAN
int "UAVCAN timer override"
default 0
src/drivers/uavcan/uavcan_drivers/kinetis/README.md
-1
View File
@@ -15,7 +15,6 @@ and the following commandline defines:
| Setting | Description | | Setting | Description |
|-------------------|-----------------------------------------------------------------------------------| |-------------------|-----------------------------------------------------------------------------------|
|UAVCAN_KINETIS_NUM_IFACES | - {1..2} Sets the number of CAN interfaces the SW will support | |UAVCAN_KINETIS_NUM_IFACES | - {1..2} Sets the number of CAN interfaces the SW will support |
|UAVCAN_KINETIS_TIMER_NUMBER | - {0..3} Sets the Periodic Interrupt Timer (PITn) channel |
Things that could be improved: Things that could be improved:
1. Build time command line configuartion of Mailbox/FIFO and filters 1. Build time command line configuartion of Mailbox/FIFO and filters
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/build_config.hpp
-9
View File
@@ -18,12 +18,3 @@
#if !defined(UAVCAN_KINETIS_NUM_IFACES) || (UAVCAN_KINETIS_NUM_IFACES != 1 && UAVCAN_KINETIS_NUM_IFACES != 2) #if !defined(UAVCAN_KINETIS_NUM_IFACES) || (UAVCAN_KINETIS_NUM_IFACES != 1 && UAVCAN_KINETIS_NUM_IFACES != 2)
# error "UAVCAN_KINETIS_NUM_IFACES must be set to either 1 or 2" # error "UAVCAN_KINETIS_NUM_IFACES must be set to either 1 or 2"
#endif #endif
/**
* Any PIT timer channel (PIT0-PIT3)
* e.g. -DUAVCAN_KINETIS_TIMER_NUMBER=2
*/
#ifndef UAVCAN_KINETIS_TIMER_NUMBER
// In this case the clock driver should be implemented by the application
# define UAVCAN_KINETIS_TIMER_NUMBER 0
#endif
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp
-23
View File
@@ -7,30 +7,9 @@
#include <uavcan_kinetis/thread.hpp> #include <uavcan_kinetis/thread.hpp>
#include "internal.hpp" #include "internal.hpp"
#if UAVCAN_KINETIS_TIMER_NUMBER
# include <cassert> # include <cassert>
# include <math.h> # include <math.h>
/*
* Timer instance
* todo:Consider using Lifetime Timer support
*/
# define TIMX_IRQHandler UAVCAN_KINETIS_GLUE3(PIT, UAVCAN_KINETIS_TIMER_NUMBER, _IRQHandler)
# define TIMX (KINETIS_PIT_BASE + (UAVCAN_KINETIS_TIMER_NUMBER << 4))
# define TMR_REG(o) (TIMX + (o))
# define TIMX_INPUT_CLOCK BOARD_BUS_FREQ
# define TIMX_INTERRUPT_FREQ 16
# define TIMX_IRQn UAVCAN_KINETIS_GLUE2(KINETIS_IRQ_PITCH, UAVCAN_KINETIS_TIMER_NUMBER)
# if UAVCAN_KINETIS_TIMER_NUMBER >= 0 && UAVCAN_KINETIS_TIMER_NUMBER <= 3
# define KINETIS_PIT_LDVAL_OFFSET KINETIS_PIT_LDVAL0_OFFSET
# define KINETIS_PIT_CVAL_OFFSET KINETIS_PIT_CVAL0_OFFSET
# define KINETIS_PIT_TCTRL_OFFSET KINETIS_PIT_TCTRL0_OFFSET
# define KINETIS_PIT_TFLG_OFFSET KINETIS_PIT_TFLG0_OFFSET
# else
# error "This UAVCAN_KINETIS_TIMER_NUMBER is not supported yet"
# endif
extern "C" UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler); extern "C" UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler);
namespace uavcan_kinetis namespace uavcan_kinetis
@@ -361,5 +340,3 @@ UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler)
return 0; return 0;
} }
#endif
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp
-9
View File
@@ -17,12 +17,3 @@
#if !defined(UAVCAN_STM32_NUM_IFACES) || (UAVCAN_STM32_NUM_IFACES != 1 && UAVCAN_STM32_NUM_IFACES != 2) #if !defined(UAVCAN_STM32_NUM_IFACES) || (UAVCAN_STM32_NUM_IFACES != 1 && UAVCAN_STM32_NUM_IFACES != 2)
# error "UAVCAN_STM32_NUM_IFACES must be set to either 1 or 2" # error "UAVCAN_STM32_NUM_IFACES must be set to either 1 or 2"
#endif #endif
/**
* Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5)
* e.g. -DUAVCAN_STM32_TIMER_NUMBER=2
*/
#ifndef UAVCAN_STM32_TIMER_NUMBER
// In this case the clock driver should be implemented by the application
# define UAVCAN_STM32_TIMER_NUMBER 0
#endif
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+42 -156
View File
@@ -6,50 +6,11 @@
#include <uavcan_stm32/thread.hpp> #include <uavcan_stm32/thread.hpp>
#include "internal.hpp" #include "internal.hpp"
#if UAVCAN_STM32_TIMER_NUMBER #include <drivers/drv_hrt.h>
#include <cassert> #include <cassert>
#include <math.h> #include <math.h>
/*
* Timer instance
*/
# if UAVCAN_STM32_NUTTX
# define TIMX UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE)
# define TMR_REG(o) (TIMX + (o))
# define TIMX_INPUT_CLOCK UAVCAN_STM32_GLUE3(STM32_APB1_TIM, UAVCAN_STM32_TIMER_NUMBER, _CLKIN)
# define TIMX_IRQn UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER)
# endif
# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
# define TIMX_RCC_ENR RCC->APB1ENR
# define TIMX_RCC_RSTR RCC->APB1RSTR
# define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN)
# define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
# else
# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
# endif
/**
* UAVCAN_STM32_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed.
* This is useful at least with certain versions of ChibiOS which do not support the bit
* RCC_DKCFGR.TIMPRE that is available in newer models of STM32. In that case, if TIMPRE is active,
* the auto-detected value of TIMX_INPUT_CLOCK will be twice lower than the actual clock speed.
* Read this for additional context: http://www.chibios.com/forum/viewtopic.php?f=35&t=3870
* A normal way to use the override feature is to provide an alternative macro, e.g.:
*
* -DUAVCAN_STM32_TIMX_INPUT_CLOCK=STM32_HCLK
*
* Alternatively, the new clock rate can be specified directly.
*/
# ifdef UAVCAN_STM32_TIMX_INPUT_CLOCK
# undef TIMX_INPUT_CLOCK
# define TIMX_INPUT_CLOCK UAVCAN_STM32_TIMX_INPUT_CLOCK
# endif
extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler);
namespace uavcan_stm32 namespace uavcan_stm32
{ {
namespace clock namespace clock
@@ -77,8 +38,46 @@ uavcan::MonotonicTime prev_utc_adj_at;
uavcan::uint64_t time_mono = 0; uavcan::uint64_t time_mono = 0;
uavcan::uint64_t time_utc = 0; uavcan::uint64_t time_utc = 0;
int32_t hrt_to_utc_offset = 0;
} }
/**
* Timer interrupt handler
*/
static void timer_callback(void *arg)
{
using namespace uavcan_stm32::clock;
UAVCAN_ASSERT(initialized);
time_mono += USecPerOverflow;
if (utc_set) {
time_utc += USecPerOverflow;
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
utc_accumulated_correction_nsec %= 1000;
}
hrt_to_utc_offset = (int64_t)time_mono - (int64_t)time_utc;
// Correction decay - 1 nsec per 65536 usec
if (utc_correction_nsec_per_overflow > 0) {
utc_correction_nsec_per_overflow--;
} else if (utc_correction_nsec_per_overflow < 0) {
utc_correction_nsec_per_overflow++;
} else {
; // Zero
}
}
}
static struct hrt_call timer_call {};
void init() void init()
{ {
@@ -90,34 +89,7 @@ void init()
initialized = true; initialized = true;
hrt_call_every(&timer_call, USecPerOverflow, USecPerOverflow, timer_callback, nullptr);
# if UAVCAN_STM32_NUTTX
// Attach IRQ
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
// Power-on and reset
modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
// Start the timer
putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
putreg16(((TIMX_INPUT_CLOCK / 1000000) - 1), TMR_REG(STM32_BTIM_PSC_OFFSET));
putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
// Prioritize and Enable IRQ
// todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
// need to investigate
// up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
up_enable_irq(TIMX_IRQn);
# endif
} }
void setUtc(uavcan::UtcTime time) void setUtc(uavcan::UtcTime time)
@@ -139,23 +111,9 @@ void setUtc(uavcan::UtcTime time)
static uavcan::uint64_t sampleUtcFromCriticalSection() static uavcan::uint64_t sampleUtcFromCriticalSection()
{ {
# if UAVCAN_STM32_NUTTX
UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(initialized);
UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
volatile uavcan::uint64_t time = time_utc; return hrt_absolute_time() - hrt_to_utc_offset;
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
}
return time + cnt;
# endif
} }
uavcan::uint64_t getUtcUSecFromCanInterrupt() uavcan::uint64_t getUtcUSecFromCanInterrupt()
@@ -165,34 +123,7 @@ uavcan::uint64_t getUtcUSecFromCanInterrupt()
uavcan::MonotonicTime getMonotonic() uavcan::MonotonicTime getMonotonic()
{ {
uavcan::uint64_t usec = 0; return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
// Scope Critical section
{
CriticalSectionLocker locker;
volatile uavcan::uint64_t time = time_mono;
# if UAVCAN_STM32_NUTTX
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
# endif
time += USecPerOverflow;
}
usec = time + cnt;
# ifndef NDEBUG
static uavcan::uint64_t prev_usec = 0; // Self-test
UAVCAN_ASSERT(prev_usec <= usec);
(void)prev_usec;
prev_usec = usec;
# endif
} // End Scope Critical section
return uavcan::MonotonicTime::fromUSec(usec);
} }
uavcan::UtcTime getUtc() uavcan::UtcTime getUtc()
@@ -353,48 +284,3 @@ SystemClock &SystemClock::instance()
} }
} // namespace uavcan_stm32 } // namespace uavcan_stm32
/**
* Timer interrupt handler
*/
extern "C"
UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
# if UAVCAN_STM32_NUTTX
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
# endif
using namespace uavcan_stm32::clock;
UAVCAN_ASSERT(initialized);
time_mono += USecPerOverflow;
if (utc_set) {
time_utc += USecPerOverflow;
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
utc_accumulated_correction_nsec %= 1000;
}
// Correction decay - 1 nsec per 65536 usec
if (utc_correction_nsec_per_overflow > 0) {
utc_correction_nsec_per_overflow--;
} else if (utc_correction_nsec_per_overflow < 0) {
utc_correction_nsec_per_overflow++;
} else {
; // Zero
}
}
UAVCAN_STM32_IRQ_EPILOGUE();
}
#endif
src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/build_config.hpp
-9
View File
@@ -17,12 +17,3 @@
#if !defined(UAVCAN_STM32H7_NUM_IFACES) || (UAVCAN_STM32H7_NUM_IFACES != 1 && UAVCAN_STM32H7_NUM_IFACES != 2) #if !defined(UAVCAN_STM32H7_NUM_IFACES) || (UAVCAN_STM32H7_NUM_IFACES != 1 && UAVCAN_STM32H7_NUM_IFACES != 2)
# error "UAVCAN_STM32H7_NUM_IFACES must be set to either 1 or 2" # error "UAVCAN_STM32H7_NUM_IFACES must be set to either 1 or 2"
#endif #endif
/**
* Any General-Purpose timer (TIM2, TIM3, TIM4, TIM5)
* e.g. -DUAVCAN_STM32H7_TIMER_NUMBER=2
*/
#ifndef UAVCAN_STM32H7_TIMER_NUMBER
// In this case the clock driver should be implemented by the application
# define UAVCAN_STM32H7_TIMER_NUMBER 0
#endif
src/drivers/uavcan/uavcan_drivers/stm32h7/driver/src/uc_stm32h7_clock.cpp
-24
View File
@@ -6,31 +6,9 @@
#include <uavcan_stm32h7/thread.hpp> #include <uavcan_stm32h7/thread.hpp>
#include "internal.hpp" #include "internal.hpp"
#if UAVCAN_STM32H7_TIMER_NUMBER
#include <cassert> #include <cassert>
#include <math.h> #include <math.h>
/*
* Timer instance
*/
# if UAVCAN_STM32H7_NUTTX
# define TIMX UAVCAN_STM32H7_GLUE3(STM32_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _BASE)
# define TMR_REG(o) (TIMX + (o))
# define TIMX_INPUT_CLOCK UAVCAN_STM32H7_GLUE3(STM32_APB1_TIM, UAVCAN_STM32H7_TIMER_NUMBER, _CLKIN)
# define TIMX_IRQn UAVCAN_STM32H7_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32H7_TIMER_NUMBER)
# endif
# if UAVCAN_STM32H7_TIMER_NUMBER >= 2 && UAVCAN_STM32H7_TIMER_NUMBER <= 7
# define TIMX_RCC_ENR RCC->APB1ENR
# define TIMX_RCC_RSTR RCC->APB1RSTR
# define TIMX_RCC_ENR_MASK UAVCAN_STM32H7_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32H7_TIMER_NUMBER, EN)
# define TIMX_RCC_RSTR_MASK UAVCAN_STM32H7_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32H7_TIMER_NUMBER, RST)
# else
# error "This UAVCAN_STM32H7_TIMER_NUMBER is not supported yet"
# endif
/** /**
* UAVCAN_STM32H7_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed. * UAVCAN_STM32H7_TIMX_INPUT_CLOCK can be used to manually override the auto-detected timer clock speed.
* This is useful at least with certain versions of ChibiOS which do not support the bit * This is useful at least with certain versions of ChibiOS which do not support the bit
@@ -396,5 +374,3 @@ UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler)
UAVCAN_STM32H7_IRQ_EPILOGUE(); UAVCAN_STM32H7_IRQ_EPILOGUE();
} }
#endif
src/drivers/uavcannode/CMakeLists.txt
-10
View File
@@ -42,19 +42,10 @@ set(UAVCAN_PLATFORM "generic")
if(CONFIG_ARCH_CHIP) if(CONFIG_ARCH_CHIP)
if(${CONFIG_ARCH_CHIP} MATCHES "kinetis") if(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
set(UAVCAN_DRIVER "kinetis") set(UAVCAN_DRIVER "kinetis")
set(UAVCAN_TIMER 1)
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7") elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")
set(UAVCAN_DRIVER "stm32h7") set(UAVCAN_DRIVER "stm32h7")
set(UAVCAN_TIMER 5) # The default timer is TIM5
if (DEFINED config_uavcan_timer_override)
set (UAVCAN_TIMER ${config_uavcan_timer_override})
endif()
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32") elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
set(UAVCAN_DRIVER "stm32") set(UAVCAN_DRIVER "stm32")
set(UAVCAN_TIMER 5) # The default timer is TIM5
if (DEFINED config_uavcan_timer_override)
set (UAVCAN_TIMER ${config_uavcan_timer_override})
endif()
endif() endif()
endif() endif()
@@ -71,7 +62,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
add_definitions( add_definitions(
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1 -DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER} -DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1