/** * @brief Interrupt handler * * This function is invoked in the IRQ service routine. * source: when calling advance_time_tick, ticks advance. */ void interrupt_handler() { if (INT_REG(INT_ICIP) & BIT26) { TMR_REG(TMR_OSCR) = 0x00; advance_time_tick(); TMR_REG(TMR_OSSR) = BIT0; } }
/** * @brief Initialize CuRT Timer * * Once OS Timer is initialized, the timer is about to work. * Note: the multi-tasking environment (setup by start_curt) will be invoked * after this routine. */ void init_os_timer() { INT_REG(INT_ICLR) &= ~BIT26; TMR_REG(TMR_OSMR0) = PXA255_TMR_CLK / OS_TICKS_PER_SEC; TMR_REG(TMR_OSMR1) = 0x3FFFFFFF; TMR_REG(TMR_OSMR2) = 0x7FFFFFFF; TMR_REG(TMR_OSMR3) = 0xBFFFFFFF; TMR_REG(TMR_OSCR) = 0x00; TMR_REG(TMR_OSSR) = BIT0; TMR_REG(TMR_OIER) = BIT0; INT_REG(INT_ICMR) |= BIT26; }
static uavcan::uint64_t sampleUtcFromCriticalSection() { # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE); volatile uavcan::uint64_t time = time_utc; volatile uavcan::uint32_t cnt = TIMX->CNT; if (TIMX->SR & TIM_SR_UIF) { cnt = TIMX->CNT; 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 # 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 }
void rgb_led(int r, int g , int b, int freqs) { long fosc = TMR_FREQUENCY; long prescale = 2048; long p1s = fosc / prescale; long p0p5s = p1s / 2; uint16_t val; static bool once = 0; if (!once) { once = 1; /* Enabel Clock to Block */ modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_TIM3EN); /* Reload */ val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET)); val |= ATIM_EGR_UG; putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET)); /* Set Prescaler STM32_TIM_SETCLOCK */ putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET)); /* Enable STM32_TIM_SETMODE*/ putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET)); putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE | (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET)); putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET)); putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P | ATIM_CCER_CC2E | ATIM_CCER_CC2P | ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET)); stm32_configgpio(GPIO_TIM3_CH1OUT); stm32_configgpio(GPIO_TIM3_CH2OUT); stm32_configgpio(GPIO_TIM3_CH3OUT); } long p = freqs == 0 ? p1s : p1s / freqs; putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET)); p = freqs == 0 ? p1s + 1 : p0p5s / freqs; putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET)); putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET)); putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET)); val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET)); if (freqs == 0) { val &= ~ATIM_CR1_CEN; } else { val |= ATIM_CR1_CEN; } putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET)); }
uavcan::MonotonicTime getMonotonic() { uavcan::uint64_t usec = 0; // Scope Critical section { CriticalSectionLocker locker; volatile uavcan::uint64_t time = time_mono; # if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL volatile uavcan::uint32_t cnt = TIMX->CNT; if (TIMX->SR & TIM_SR_UIF) { cnt = TIMX->CNT; # endif # 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() { if (utc_set) { uavcan::uint64_t usec = 0; { CriticalSectionLocker locker; usec = sampleUtcFromCriticalSection(); } 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)); // lowsyslog("$ 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) < utc_sync_params.lock_thres_offset.toUSec()); } } }