/**
* @brief Responsible for sending out the TIME_SYNC pulse
*
* Logs the time of the strobe and strobes. If the requested iteration
* count is reached we transition to the TIMESYNC_STATE_ACTIVE state or
* the TIMESYNC_STATE_DEBUT_INIT state depending on CONFIG_ARCH_TIMESYNC_DEBUG.
*
*/
static int timesync_strobe_handler(int irq, void *context, void *priv) {
irqstate_t flags;
uint64_t strobe_time;
flags = irqsave();
STM32_TIM_ACKINT(timesync_strobe_timer, irq);
timesync_strobe(timesync_pin_strobe_mask, &strobe_time);
timesync_log_frame_time(strobe_time);
if (timesync_strobe_index == timesync_strobe_count) {
STM32_TIM_DISABLEINT(timesync_strobe_timer, 0);
STM32_TIM_SETMODE(timesync_strobe_timer, STM32_TIM_MODE_DISABLED);
STM32_TIM_SETCLOCK(timesync_strobe_timer, 0);
#ifdef CONFIG_ARCH_TIMESYNC_DEBUG
timesync_set_state(TIMESYNC_STATE_DEBUG_INIT);
sem_post(&dbg_thread_sem);
#else
timesync_set_state(TIMESYNC_STATE_ACTIVE);
#endif
}
irqrestore(flags);
return 0;
}
int filter_interrupt(int irq, FAR void *context)
{
STM32_TIM_ACKINT(timer2, 0);
// Previous wheel speeds for filtering
static float previous_speeds_filtered[4] = {0, 0, 0, 0};
// filter parameter alpha
float alpha = 0.2;
int sensor;
float delta_t;
// For every sensor (1-4)
for(sensor = 0 ; sensor < 4 ; sensor++)
{
// Check for stand still
delta_t = ((float)(getreg32(STM32_TIM5_BASE + STM32_GTIM_CNT_OFFSET) - previous_time[sensor]))/1000000.0;
// If more than one second between transitions -> set speed to 0
if(delta_t > 0.3)
{
speeds[sensor] = 0;
}
// Low pass filter
speeds_filtered[sensor] = alpha*speeds[sensor] + (1-alpha)*previous_speeds_filtered[sensor];
previous_speeds_filtered[sensor] = speeds_filtered[sensor];
}
// Publish speeds
}
/**
* @brief Responsible for handling roll-over of the lower 32 bit integer into the 64 bit frame-time
*
* Handily the amount to append to the frame-time will always be 0x100000000
*/
static int timesync_frame_time_rollover_handler(int irq, void *context,
void *priv) {
STM32_TIM_ACKINT(timesync_rollover_slave_timer, irq);
timesync_frame_time += TIMESYNC_ROLLOVER_CASCADE_TOTAL;
return 0;
}
static int sif_anout_isr(int irq, void *context)
{
STM32_TIM_ACKINT(vsn_sif.tim8, 0);
test++;
teirq = irq;
return OK;
}
static int up_lcdextcominisr(int irq, void *context)
{
STM32_TIM_ACKINT(tim, 0);
if (g_isr == NULL)
{
lcddbg("error, irq not attached, disabled\n");
STM32_TIM_DISABLEINT(tim, 0);
return OK;
}
return g_isr(irq, context);
}
static int up_lcdextcominisr(int irq, void *context, void *arg)
{
STM32_TIM_ACKINT(tim, ATIM_SR_UIF);
if (g_isr == NULL)
{
lcderr("ERROR: error, irq not attached, disabled\n");
STM32_TIM_DISABLEINT(tim, 0);
return OK;
}
return g_isr(irq, context, arg);
}
/**
* @brief Enable TimeSync at the given reference frame_time
*
* Setup TIM8 as master clock cascading into TIM4 see DocID018909 Rev 10 page 612
* https://www.rapitasystems.com/blog/chaining-two-16-bit-timers-together-stm32f4
* 622/1728 DocID018909 Rev 10 - details which timers can be cascaded
* TIM8 has been selected since we can clock that @ 96MHz and divide down by 5 to
* get to 19.2MHz with just a little work @ the PLL configuration.
*/
int timesync_enable(uint8_t strobe_count, uint64_t frame_time,
uint32_t strobe_delay, uint32_t refclk) {
if (!strobe_count || strobe_count > GB_TIMESYNC_MAX_STROBES) {
return -EINVAL;
}
if (!strobe_delay) {
return -EINVAL;
}
if (!refclk || STM32_TIM18_FREQUENCY % refclk) {
lldbg("Error Time-Sync clock %dHz doesn't divide APB clock %dHz evenly\n",
refclk, STM32_TIM18_FREQUENCY);
return -ENODEV;
}
timesync_refclk = refclk;
timesync_strobe_count = strobe_count;
timesync_strobe_index = 0;
timesync_strobe_delay = strobe_delay;
timesync_set_state(TIMESYNC_STATE_SYNCING);
timesync_frame_time = frame_time;
/* Disable timers */
timesync_disable_timers();
/*******************************************************************
* Enable TIM8 against the hardware input clock output on TIM8_TRGO
*******************************************************************/
/* Configure MMS=010 in TIM8_CR2 output TIM8_TRGO on rollover (master mode) */
STM32_TIM_SETMASTER_MODE(timesync_rollover_master_timer, STM32_TIM_MASTER_MODE_UPDATE);
/* Configures TIM8_ARR - auto reload register */
STM32_TIM_SETPERIOD(timesync_rollover_master_timer, TIMESYNC_ROLLOVER_TIMER_PERIOD);
/* Configures TIM8_PSC - prescaler value to get our desired master clock */
STM32_TIM_SETCLOCK(timesync_rollover_master_timer, timesync_refclk);
/*********************************************************************
* Enable TIM4 with clock input source ITR2 (TIM8_TRGO), no pre-scaler
* interrupt on over-flow
*********************************************************************/
/* Configure ITR0 as internal trigger TIM4_SMCR:TS=011(TIM8) external clock mode TIM4_SMCR:SMS=111 (slave mode) */
STM32_TIM_SETSLAVE_MODE(timesync_rollover_slave_timer, STM32_TIM_SLAVE_EXTERNAL_MODE, STM32_TIM_SLAVE_INTERNAL_TRIGGER3);
/* Configures TIM4_ARR - auto reload register */
STM32_TIM_SETPERIOD(timesync_rollover_slave_timer, TIMESYNC_ROLLOVER_TIMER_PERIOD);
/* Configures TIM4_PSC - set to STM32_TIM18_FREQUENCY to get a prescaler value of 0 */
STM32_TIM_SETCLOCK(timesync_rollover_slave_timer, STM32_TIM18_FREQUENCY);
/****************************************************************************
* Enable TIM6 as a simple up-counter at the strobe_delay specified by the AP
* strobe_delay is expressed in microseconds.
****************************************************************************/
/* Delay is expressed in microseconds so apply directly to TIM6_ARR */
STM32_TIM_SETPERIOD(timesync_strobe_timer, strobe_delay);
/* Clock is simply the fundamental input Hz (clocks per second) / 1000 */
STM32_TIM_SETCLOCK(timesync_strobe_timer, 1000000UL);
/***************
* Enable timers
***************/
/* Configures TIM8_CR1 - mode up-counter */
STM32_TIM_SETMODE(timesync_rollover_master_timer, STM32_TIM_MODE_UP);
/* Configures TIM4_CR1 - mode up-counter */
STM32_TIM_SETMODE(timesync_rollover_slave_timer, STM32_TIM_MODE_UP);
/* Configures TIM6_CR1 - mode up-counter */
STM32_TIM_SETMODE(timesync_strobe_timer, STM32_TIM_MODE_UP);
/* Enable roll-over timer interrupt */
STM32_TIM_ACKINT(timesync_rollover_slave_timer, timesync_rollover_timer_irq);
STM32_TIM_ENABLEINT(timesync_rollover_slave_timer, 0);
/* Enable strobe timer interrupt */
STM32_TIM_ACKINT(timesync_strobe_timer, timesync_strobe_timer_irq);
STM32_TIM_ENABLEINT(timesync_strobe_timer, 0);
dbg_verbose("ref-clk-freq=%dHz timer-freq=%dHz period=%d\n",
STM32_TIM18_FREQUENCY, timesync_refclk,
TIMESYNC_ROLLOVER_TIMER_PERIOD);
dbg_verbose("strobe-clk=%dHz strobe-pin-mask=0x%08lx period=%d\n",
STM32_TIM18_FREQUENCY / 1000UL, timesync_pin_strobe_mask, strobe_delay);
return 0;
}
