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);
}
