forked from Archive/PX4-Autopilot
Compare commits
1 Commits
main
...
pr-hrt_clo
| Author | SHA1 | Date |
|---|---|---|
Daniel Agar
|
0d3daf3efd |
|
|
@ -33,7 +33,6 @@ CONFIG_DRIVERS_RC_INPUT=y
|
|||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
|
|
|
|||
|
|
@ -35,7 +35,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
|||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
|
|
|
|||
|
|
@ -36,7 +36,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
|||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_DRIVERS_ADC_ADS1115=n
|
||||
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
|
||||
CONFIG_DRIVERS_IRLOCK=n
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_DRIVERS_HEATER=n
|
||||
CONFIG_DRIVERS_UAVCAN=n
|
||||
CONFIG_CYPHAL_BMS_SUBSCRIBER=y
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_BAROMETERS=n
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
|
||||
CONFIG_COMMON_MAGNETOMETER=n
|
||||
|
|
|
|||
|
|
@ -40,7 +40,6 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
|||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=6
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_BAROMETERS=n
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=n
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_BAROMETERS=n
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=n
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_HYGROMETERS=n
|
||||
CONFIG_COMMON_OSD=n
|
||||
CONFIG_DRIVERS_ADC_ADS1115=n
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_COMMON_BAROMETERS=n
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
|
||||
CONFIG_COMMON_HYGROMETERS=n
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@ CONFIG_DRIVERS_PX4IO=y
|
|||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
|
|
|
|||
|
|
@ -42,7 +42,6 @@ CONFIG_DRIVERS_RC_INPUT=y
|
|||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
|
|
|
|||
|
|
@ -1,4 +1,3 @@
|
|||
# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
|
||||
CONFIG_DRIVERS_UAVCAN=n
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=n
|
||||
CONFIG_MODULES_ZENOH=y
|
||||
|
|
|
|||
|
|
@ -333,10 +333,6 @@ if(EXISTS ${BOARD_DEFCONFIG})
|
|||
set(config_uavcan_num_ifaces ${UAVCAN_INTERFACES} CACHE INTERNAL "UAVCAN interfaces" FORCE)
|
||||
endif()
|
||||
|
||||
if(UAVCAN_TIMER_OVERRIDE)
|
||||
set(config_uavcan_timer_override ${UAVCAN_TIMER_OVERRIDE} CACHE INTERNAL "UAVCAN TIMER OVERRIDE" FORCE)
|
||||
endif()
|
||||
|
||||
# OPTIONS
|
||||
|
||||
if(CONSTRAINED_FLASH)
|
||||
|
|
|
|||
|
|
@ -244,6 +244,8 @@
|
|||
# error HRT_TIMER_CHANNEL must be a value between 1 and 4
|
||||
#endif
|
||||
|
||||
static volatile hrt_abstime base_time;
|
||||
|
||||
/*
|
||||
* Queue of callout entries.
|
||||
*/
|
||||
|
|
@ -276,6 +278,112 @@ static void hrt_call_reschedule(void);
|
|||
static void hrt_call_invoke(void);
|
||||
|
||||
|
||||
#define HRT_TIME_SYNC
|
||||
|
||||
#if defined(HRT_TIME_SYNC)
|
||||
|
||||
#include <math.h>
|
||||
|
||||
static bool sync_set = false;
|
||||
static bool sync_locked = false;
|
||||
static uint32_t sync_jump_cnt = 0;
|
||||
|
||||
static float sync_prev_adj = 0;
|
||||
static float sync_rel_rate_ppm = 0;
|
||||
static float sync_rel_rate_error_integral = 0;
|
||||
static int32_t sync_accumulated_correction_nsec = 0;
|
||||
static int32_t sync_correction_nsec_per_overflow = 0;
|
||||
static hrt_abstime prev_sync_adj_at;
|
||||
|
||||
static const float sync_offset_p = 0.01f; ///< PPM per one usec error
|
||||
static const float sync_rate_i = 0.02f; ///< PPM per one PPM error for second
|
||||
static const float sync_rate_error_corner_freq = 0.01f;
|
||||
static const float sync_max_rate_correction_ppm = 300.0f;
|
||||
static const float sync_lock_thres_rate_ppm = 2.f;
|
||||
static const hrt_abstime sync_lock_thres_offset = 4000; ///< usec
|
||||
static const hrt_abstime sync_min_jump = 10000; ///< Min error to jump rather than change rate
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(float adj_usec)
|
||||
{
|
||||
const hrt_abstime ts = hrt_absolute_time();
|
||||
const float dt = (ts - prev_sync_adj_at) / 1e6f;
|
||||
prev_sync_adj_at = ts;
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * sync_offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (sync_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
sync_prev_adj = adj_usec;
|
||||
sync_rel_rate_ppm = lowpass(sync_rel_rate_ppm, new_rel_rate_ppm, sync_rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - sync_rel_rate_ppm;
|
||||
|
||||
if (dt > 10) {
|
||||
sync_rel_rate_error_integral = 0;
|
||||
|
||||
} else {
|
||||
sync_rel_rate_error_integral += rel_rate_error * dt * sync_rate_i;
|
||||
sync_rel_rate_error_integral = fmaxf(sync_rel_rate_error_integral, -sync_max_rate_correction_ppm);
|
||||
sync_rel_rate_error_integral = fminf(sync_rel_rate_error_integral, sync_max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
/* Rate controller */
|
||||
float total_rate_correction_ppm = rel_rate_error + sync_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = fmaxf(total_rate_correction_ppm, -sync_max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = fminf(total_rate_correction_ppm, sync_max_rate_correction_ppm);
|
||||
|
||||
sync_correction_nsec_per_overflow = (HRT_COUNTER_PERIOD * 1000) * (total_rate_correction_ppm / 1e6f);
|
||||
|
||||
// syslog(LOG_INFO, "$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// (double)adj_usec, (double)sync_rel_rate_ppm, (double)sync_rel_rate_error_integral, (double)target_rel_rate_ppm,
|
||||
// (double)total_rate_correction_ppm);
|
||||
}
|
||||
|
||||
void hrt_absolute_time_adjust(int64_t adjustment)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
|
||||
if (fabsf((float)adjustment) > (float)sync_min_jump || !sync_set) {
|
||||
|
||||
const hrt_abstime time_prev = base_time;
|
||||
base_time += adjustment;
|
||||
syslog(LOG_NOTICE, "HRT: resetting %llu -> %llu\n", time_prev, base_time);
|
||||
|
||||
sync_set = true;
|
||||
sync_locked = false;
|
||||
sync_jump_cnt++;
|
||||
sync_prev_adj = 0;
|
||||
sync_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!sync_locked) {
|
||||
sync_locked =
|
||||
(fabsf(sync_rel_rate_ppm) < sync_lock_thres_rate_ppm) &&
|
||||
(fabsf(sync_prev_adj) < sync_lock_thres_offset);
|
||||
}
|
||||
}
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
#endif // HRT_TIME_SYNC
|
||||
|
||||
|
||||
|
||||
int hrt_ioctl(unsigned int cmd, unsigned long arg);
|
||||
/*
|
||||
* Specific registers and bits used by PPM sub-functions
|
||||
|
|
@ -665,7 +773,6 @@ hrt_absolute_time(void)
|
|||
* pair. Discourage the compiler from moving loads/stores
|
||||
* to these outside of the protected range.
|
||||
*/
|
||||
static volatile hrt_abstime base_time;
|
||||
static volatile uint32_t last_count;
|
||||
|
||||
/* prevent re-entry */
|
||||
|
|
@ -683,12 +790,34 @@ hrt_absolute_time(void)
|
|||
*/
|
||||
if (count < last_count) {
|
||||
base_time += HRT_COUNTER_PERIOD;
|
||||
|
||||
#if defined(HRT_TIME_SYNC)
|
||||
|
||||
if (sync_set) {
|
||||
sync_accumulated_correction_nsec += sync_correction_nsec_per_overflow;
|
||||
|
||||
if (abs(sync_accumulated_correction_nsec) >= 1000) {
|
||||
base_time += sync_accumulated_correction_nsec / 1000;
|
||||
sync_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (sync_correction_nsec_per_overflow > 0) {
|
||||
sync_correction_nsec_per_overflow--;
|
||||
|
||||
} else if (sync_correction_nsec_per_overflow < 0) {
|
||||
sync_correction_nsec_per_overflow++;
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* HRT_TIME_SYNC */
|
||||
}
|
||||
|
||||
/* save the count for next time */
|
||||
last_count = count;
|
||||
|
||||
/* compute the current time */
|
||||
|
||||
abstime = HRT_COUNTER_SCALE(base_time + count);
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
|
|
|
|||
|
|
@ -131,6 +131,11 @@ typedef struct latency_boardctl {
|
|||
*/
|
||||
__EXPORT extern hrt_abstime hrt_absolute_time(void);
|
||||
|
||||
/**
|
||||
* Adjust the synchronized clock.
|
||||
*/
|
||||
__EXPORT extern void hrt_absolute_time_adjust(int64_t adjustment);
|
||||
|
||||
/**
|
||||
* Convert a timespec to absolute time.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -42,22 +42,12 @@ set(UAVCAN_PLATFORM "generic")
|
|||
if(CONFIG_ARCH_CHIP)
|
||||
if(${CONFIG_NET_CAN} MATCHES "y")
|
||||
set(UAVCAN_DRIVER "socketcan")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
|
||||
set(UAVCAN_DRIVER "kinetis")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "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")
|
||||
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()
|
||||
|
||||
|
|
@ -74,7 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
|
|||
add_definitions(
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
|
||||
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
|
||||
|
|
|
|||
|
|
@ -96,8 +96,3 @@ depends on DRIVERS_UAVCAN
|
|||
string "UAVCAN peripheral firmware"
|
||||
help
|
||||
list of UAVCAN peripheral firmware to build and embed
|
||||
|
||||
menuconfig BOARD_UAVCAN_TIMER_OVERRIDE
|
||||
depends on DRIVERS_UAVCAN
|
||||
int "UAVCAN timer override"
|
||||
default 0
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@ and the following commandline defines:
|
|||
| Setting | Description |
|
||||
|-------------------|-----------------------------------------------------------------------------------|
|
||||
|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:
|
||||
1. Build time command line configuration of Mailbox/FIFO and filters
|
||||
|
|
|
|||
|
|
@ -18,12 +18,3 @@
|
|||
#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"
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -46,57 +46,6 @@ uavcan::UtcTime getUtc();
|
|||
*/
|
||||
void adjustUtc(uavcan::UtcDuration adjustment);
|
||||
|
||||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F),
|
||||
rate_i(0.02F),
|
||||
rate_error_corner_freq(0.01F),
|
||||
max_rate_correction_ppm(300.0F),
|
||||
lock_thres_rate_ppm(2.0F),
|
||||
lock_thres_offset(uavcan::UtcDuration::fromMSec(4)),
|
||||
min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Clock rate error.
|
||||
* Positive if the hardware timer is slower than reference time.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
float getUtcRateCorrectionPPM();
|
||||
|
||||
/**
|
||||
* Number of non-gradual adjustments performed so far.
|
||||
* Ideally should be zero.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
uavcan::uint32_t getUtcJumpCount();
|
||||
|
||||
/**
|
||||
* Whether UTC is synchronized and locked.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
bool isUtcLocked();
|
||||
|
||||
/**
|
||||
* UTC sync params get/set.
|
||||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -7,31 +7,7 @@
|
|||
#include <uavcan_kinetis/thread.hpp>
|
||||
#include "internal.hpp"
|
||||
|
||||
#if UAVCAN_KINETIS_TIMER_NUMBER
|
||||
# include <cassert>
|
||||
# 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);
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
namespace uavcan_kinetis
|
||||
{
|
||||
|
|
@ -40,30 +16,12 @@ namespace clock
|
|||
namespace
|
||||
{
|
||||
|
||||
const uavcan::uint32_t CountsPerPeriod = (TIMX_INPUT_CLOCK / TIMX_INTERRUPT_FREQ);
|
||||
const uavcan::uint32_t CountsPerUs = (TIMX_INPUT_CLOCK / 1000000);
|
||||
const uavcan::uint32_t USecPerOverflow = (1000000 / TIMX_INTERRUPT_FREQ);
|
||||
|
||||
Mutex mutex;
|
||||
|
||||
bool initialized = false;
|
||||
|
||||
bool utc_set = false;
|
||||
bool utc_locked = false;
|
||||
uavcan::uint32_t utc_jump_cnt = 0;
|
||||
UtcSyncParams utc_sync_params;
|
||||
float utc_prev_adj = 0;
|
||||
float utc_rel_rate_ppm = 0;
|
||||
float utc_rel_rate_error_integral = 0;
|
||||
uavcan::int32_t utc_accumulated_correction_nsec = 0;
|
||||
uavcan::int32_t utc_correction_nsec_per_overflow = 0;
|
||||
uavcan::MonotonicTime prev_utc_adj_at;
|
||||
|
||||
uavcan::uint64_t time_mono = 0;
|
||||
uavcan::uint64_t time_utc = 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void init()
|
||||
{
|
||||
CriticalSectionLocker lock;
|
||||
|
|
@ -73,231 +31,34 @@ void init()
|
|||
}
|
||||
|
||||
initialized = true;
|
||||
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
|
||||
// Power-on Clock
|
||||
modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT);
|
||||
|
||||
// Enable module
|
||||
putreg32(0, KINETIS_PIT_MCR);
|
||||
|
||||
// Start the timer
|
||||
|
||||
putreg32(CountsPerPeriod - 1, TMR_REG(KINETIS_PIT_LDVAL_OFFSET));
|
||||
putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET)); // Start
|
||||
|
||||
// Prioritize and Enable IRQ
|
||||
|
||||
#if 0
|
||||
// This has to be off or uses the default priority
|
||||
// Without the ability to point the vector
|
||||
// Directly to this ISR this will reenter the
|
||||
// exception_common and cause the interrupt
|
||||
// Stack pointer to be reset
|
||||
up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
|
||||
#endif
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
}
|
||||
|
||||
void setUtc(uavcan::UtcTime time)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
|
||||
static uavcan::uint64_t sampleUtcFromCriticalSection()
|
||||
{
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE);
|
||||
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_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 / CountsPerUs);
|
||||
// DO NOTHING
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
return hrt_absolute_time();
|
||||
}
|
||||
|
||||
uavcan::MonotonicTime getMonotonic()
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
|
||||
usec = time + (cnt / CountsPerUs);
|
||||
|
||||
} // End Scope Critical section
|
||||
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
|
||||
uavcan::UtcTime getUtc()
|
||||
{
|
||||
if (utc_set) {
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
return uavcan::UtcTime();
|
||||
}
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
|
||||
if (dt > 10) {
|
||||
utc_rel_rate_error_integral = 0;
|
||||
|
||||
} else {
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
|
||||
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
|
||||
// total_rate_correction_ppm);
|
||||
}
|
||||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
|
||||
time_utc = 1;
|
||||
|
||||
} else {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!utc_locked) {
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float getUtcRateCorrectionPPM()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getUtcJumpCount()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
}
|
||||
|
||||
bool isUtcLocked()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
}
|
||||
|
||||
UtcSyncParams getUtcSyncParams()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
}
|
||||
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
hrt_absolute_time_adjust(adj_usec);
|
||||
}
|
||||
|
||||
} // namespace clock
|
||||
|
|
@ -321,45 +82,5 @@ SystemClock &SystemClock::instance()
|
|||
return *ptr;
|
||||
}
|
||||
|
||||
} // namespace uavcan_kinetis
|
||||
} // namespace uavcan_stm32
|
||||
|
||||
|
||||
/**
|
||||
* Timer interrupt handler
|
||||
*/
|
||||
|
||||
extern "C"
|
||||
UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler)
|
||||
{
|
||||
putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET));
|
||||
|
||||
using namespace uavcan_kinetis::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
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -17,12 +17,3 @@
|
|||
#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"
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -45,56 +45,6 @@ uavcan::UtcTime getUtc();
|
|||
*/
|
||||
void adjustUtc(uavcan::UtcDuration adjustment);
|
||||
|
||||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F)
|
||||
, rate_i(0.02F)
|
||||
, rate_error_corner_freq(0.01F)
|
||||
, max_rate_correction_ppm(300.0F)
|
||||
, lock_thres_rate_ppm(2.0F)
|
||||
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
|
||||
, min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{ }
|
||||
};
|
||||
|
||||
/**
|
||||
* Clock rate error.
|
||||
* Positive if the hardware timer is slower than reference time.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
float getUtcRateCorrectionPPM();
|
||||
|
||||
/**
|
||||
* Number of non-gradual adjustments performed so far.
|
||||
* Ideally should be zero.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
uavcan::uint32_t getUtcJumpCount();
|
||||
|
||||
/**
|
||||
* Whether UTC is synchronized and locked.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
bool isUtcLocked();
|
||||
|
||||
/**
|
||||
* UTC sync params get/set.
|
||||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -6,49 +6,7 @@
|
|||
#include <uavcan_stm32/thread.hpp>
|
||||
#include "internal.hpp"
|
||||
|
||||
#if UAVCAN_STM32_TIMER_NUMBER
|
||||
|
||||
#include <cassert>
|
||||
#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);
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
namespace uavcan_stm32
|
||||
{
|
||||
|
|
@ -57,26 +15,9 @@ namespace clock
|
|||
namespace
|
||||
{
|
||||
|
||||
const uavcan::uint32_t USecPerOverflow = 65536;
|
||||
|
||||
Mutex mutex;
|
||||
|
||||
bool initialized = false;
|
||||
|
||||
bool utc_set = false;
|
||||
bool utc_locked = false;
|
||||
uavcan::uint32_t utc_jump_cnt = 0;
|
||||
UtcSyncParams utc_sync_params;
|
||||
float utc_prev_adj = 0;
|
||||
float utc_rel_rate_ppm = 0;
|
||||
float utc_rel_rate_error_integral = 0;
|
||||
uavcan::int32_t utc_accumulated_correction_nsec = 0;
|
||||
uavcan::int32_t utc_correction_nsec_per_overflow = 0;
|
||||
uavcan::MonotonicTime prev_utc_adj_at;
|
||||
|
||||
uavcan::uint64_t time_mono = 0;
|
||||
uavcan::uint64_t time_utc = 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -89,246 +30,34 @@ void init()
|
|||
}
|
||||
|
||||
initialized = true;
|
||||
|
||||
|
||||
# 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)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
|
||||
static uavcan::uint64_t sampleUtcFromCriticalSection()
|
||||
{
|
||||
# if UAVCAN_STM32_NUTTX
|
||||
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
|
||||
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
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
|
||||
// DO NOTHING
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
return hrt_absolute_time();
|
||||
}
|
||||
|
||||
uavcan::MonotonicTime getMonotonic()
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
// 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
|
||||
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
uavcan::UtcTime getUtc()
|
||||
{
|
||||
if (utc_set) {
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
return uavcan::UtcTime();
|
||||
}
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
|
||||
if (dt > 10) {
|
||||
utc_rel_rate_error_integral = 0;
|
||||
|
||||
} else {
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
|
||||
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
|
||||
// total_rate_correction_ppm);
|
||||
}
|
||||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
|
||||
time_utc = 1;
|
||||
|
||||
} else {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!utc_locked) {
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float getUtcRateCorrectionPPM()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getUtcJumpCount()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
}
|
||||
|
||||
bool isUtcLocked()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
}
|
||||
|
||||
UtcSyncParams getUtcSyncParams()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
}
|
||||
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
hrt_absolute_time_adjust(adj_usec);
|
||||
}
|
||||
|
||||
} // namespace clock
|
||||
|
|
@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
|
|||
|
||||
} // 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
|
||||
|
|
|
|||
|
|
@ -17,12 +17,3 @@
|
|||
#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"
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -45,56 +45,6 @@ uavcan::UtcTime getUtc();
|
|||
*/
|
||||
void adjustUtc(uavcan::UtcDuration adjustment);
|
||||
|
||||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F)
|
||||
, rate_i(0.02F)
|
||||
, rate_error_corner_freq(0.01F)
|
||||
, max_rate_correction_ppm(300.0F)
|
||||
, lock_thres_rate_ppm(2.0F)
|
||||
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
|
||||
, min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{ }
|
||||
};
|
||||
|
||||
/**
|
||||
* Clock rate error.
|
||||
* Positive if the hardware timer is slower than reference time.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
float getUtcRateCorrectionPPM();
|
||||
|
||||
/**
|
||||
* Number of non-gradual adjustments performed so far.
|
||||
* Ideally should be zero.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
uavcan::uint32_t getUtcJumpCount();
|
||||
|
||||
/**
|
||||
* Whether UTC is synchronized and locked.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
bool isUtcLocked();
|
||||
|
||||
/**
|
||||
* UTC sync params get/set.
|
||||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -6,49 +6,7 @@
|
|||
#include <uavcan_stm32h7/thread.hpp>
|
||||
#include "internal.hpp"
|
||||
|
||||
#if UAVCAN_STM32H7_TIMER_NUMBER
|
||||
|
||||
#include <cassert>
|
||||
#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.
|
||||
* 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_STM32H7_TIMX_INPUT_CLOCK=STM32_HCLK
|
||||
*
|
||||
* Alternatively, the new clock rate can be specified directly.
|
||||
*/
|
||||
# ifdef UAVCAN_STM32H7_TIMX_INPUT_CLOCK
|
||||
# undef TIMX_INPUT_CLOCK
|
||||
# define TIMX_INPUT_CLOCK UAVCAN_STM32H7_TIMX_INPUT_CLOCK
|
||||
# endif
|
||||
|
||||
extern "C" UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler);
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
namespace uavcan_stm32h7
|
||||
{
|
||||
|
|
@ -57,26 +15,9 @@ namespace clock
|
|||
namespace
|
||||
{
|
||||
|
||||
const uavcan::uint32_t USecPerOverflow = 65536;
|
||||
|
||||
Mutex mutex;
|
||||
|
||||
bool initialized = false;
|
||||
|
||||
bool utc_set = false;
|
||||
bool utc_locked = false;
|
||||
uavcan::uint32_t utc_jump_cnt = 0;
|
||||
UtcSyncParams utc_sync_params;
|
||||
float utc_prev_adj = 0;
|
||||
float utc_rel_rate_ppm = 0;
|
||||
float utc_rel_rate_error_integral = 0;
|
||||
uavcan::int32_t utc_accumulated_correction_nsec = 0;
|
||||
uavcan::int32_t utc_correction_nsec_per_overflow = 0;
|
||||
uavcan::MonotonicTime prev_utc_adj_at;
|
||||
|
||||
uavcan::uint64_t time_mono = 0;
|
||||
uavcan::uint64_t time_utc = 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -89,246 +30,34 @@ void init()
|
|||
}
|
||||
|
||||
initialized = true;
|
||||
|
||||
|
||||
# if UAVCAN_STM32H7_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)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
|
||||
static uavcan::uint64_t sampleUtcFromCriticalSection()
|
||||
{
|
||||
# if UAVCAN_STM32H7_NUTTX
|
||||
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
|
||||
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
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
|
||||
// DO NOTHING
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
return hrt_absolute_time();
|
||||
}
|
||||
|
||||
uavcan::MonotonicTime getMonotonic()
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
|
||||
# if UAVCAN_STM32H7_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
|
||||
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
uavcan::UtcTime getUtc()
|
||||
{
|
||||
if (utc_set) {
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
uavcan::uint64_t usec = hrt_absolute_time();
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
return uavcan::UtcTime();
|
||||
}
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
|
||||
if (dt > 10) {
|
||||
utc_rel_rate_error_integral = 0;
|
||||
|
||||
} else {
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
|
||||
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
|
||||
// total_rate_correction_ppm);
|
||||
}
|
||||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
|
||||
time_utc = 1;
|
||||
|
||||
} else {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!utc_locked) {
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float getUtcRateCorrectionPPM()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getUtcJumpCount()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
}
|
||||
|
||||
bool isUtcLocked()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
}
|
||||
|
||||
UtcSyncParams getUtcSyncParams()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
}
|
||||
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
hrt_absolute_time_adjust(adj_usec);
|
||||
}
|
||||
|
||||
} // namespace clock
|
||||
|
|
@ -354,47 +83,3 @@ SystemClock &SystemClock::instance()
|
|||
|
||||
} // namespace uavcan_stm32h7
|
||||
|
||||
|
||||
/**
|
||||
* Timer interrupt handler
|
||||
*/
|
||||
|
||||
extern "C"
|
||||
UAVCAN_STM32H7_IRQ_HANDLER(TIMX_IRQHandler)
|
||||
{
|
||||
UAVCAN_STM32H7_IRQ_PROLOGUE();
|
||||
|
||||
# if UAVCAN_STM32H7_NUTTX
|
||||
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
|
||||
# endif
|
||||
|
||||
using namespace uavcan_stm32h7::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_STM32H7_IRQ_EPILOGUE();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -42,22 +42,12 @@ set(UAVCAN_PLATFORM "generic")
|
|||
if(CONFIG_ARCH_CHIP)
|
||||
if(${CONFIG_NET_CAN} MATCHES "y")
|
||||
set(UAVCAN_DRIVER "socketcan")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
|
||||
set(UAVCAN_DRIVER "kinetis")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "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")
|
||||
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()
|
||||
|
||||
|
|
@ -74,7 +64,6 @@ string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
|
|||
add_definitions(
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
|
||||
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
|
||||
|
|
|
|||
Loading…
Reference in New Issue