void mb_servo_set_ns(uint32_t duration_ns) { /* set Match5 value (pulse duration )*/ PWMMR5 = cpu_ticks_of_nsec(duration_ns); /* commit PWMMRx changes */ PWMLER = PWMLER_LATCH5; }
static void sys_tick_handler(void) { /* get current time */ struct timespec now; clock_gettime(CLOCK_MONOTONIC, &now); /* time difference to startup */ time_t d_sec = now.tv_sec - startup_time.tv_sec; long d_nsec = now.tv_nsec - startup_time.tv_nsec; /* wrap if negative nanoseconds */ if (d_nsec < 0) { d_sec -= 1; d_nsec += 1000000000L; } #ifdef SYS_TIME_LED if (d_sec > sys_time.nb_sec) { LED_TOGGLE(SYS_TIME_LED); } #endif sys_time.nb_sec = d_sec; sys_time.nb_sec_rem = cpu_ticks_of_nsec(d_nsec); sys_time.nb_tick = sys_time_ticks_of_sec(d_sec) + sys_time_ticks_of_usec(d_nsec / 1000); /* advance virtual timers */ for (unsigned int i = 0; i < SYS_TIME_NB_TIMER; i++) { if (sys_time.timer[i].in_use && sys_time.nb_tick >= sys_time.timer[i].end_time) { sys_time.timer[i].end_time += sys_time.timer[i].duration; sys_time.timer[i].elapsed = true; /* call registered callbacks, WARNING: they will be executed in the sys_time thread! */ if (sys_time.timer[i].cb) { sys_time.timer[i].cb(i); } } } }
/* external interrupt for drdy pin */ exti_select_source(EXTI5, GPIOB); exti_set_trigger(EXTI5, EXTI_TRIGGER_RISING); exti_enable_request(EXTI5); nvic_set_priority(NVIC_EXTI9_5_IRQ, 0x0f); nvic_enable_irq(NVIC_EXTI9_5_IRQ); } void ms2100_reset_cb( struct spi_transaction * t __attribute__ ((unused)) ) { // set RESET pin high for at least 100 nsec // busy wait should not harm Ms2100Set(); // FIXME, make nanosleep funcion uint32_t dt_ticks = cpu_ticks_of_nsec(110); int32_t end_cpu_ticks = systick_get_value() - dt_ticks; if (end_cpu_ticks < 0) end_cpu_ticks += systick_get_reload(); while (systick_get_value() > (uint32_t)end_cpu_ticks) ; Ms2100Reset(); } void exti9_5_isr(void) { ms2100.status = MS2100_GOT_EOC; exti_reset_request(EXTI5); }