uint16 Servo::readMicroseconds() const {
if (!this->attached()) {
ASSERT(0);
return 0;
}
stm32_pin_info pin_info = PIN_MAP[this->pin];
uint16 compare = timer_get_compare(pin_info.timer_device,
pin_info.timer_channel);
return COMPARE_TO_US(compare);
}
/* mach_gettimeoffset returns the number of microseconds elapsed since the
start of the current tick */
unsigned long mach_gettimeoffset(void)
{
unsigned long tctr=timer_get_time(CONFIG_XILINX_TIMER_0_BASEADDR,0);
unsigned long tcmp=timer_get_compare(CONFIG_XILINX_TIMER_0_BASEADDR,0);
unsigned long offset;
/*
* Timer used in count up mode, so subtract load value from
* counter register to get elapsed ticks */
offset = (tctr-tcmp)/(CONFIG_XILINX_CPU_CLOCK_FREQ/1000000);
/*
* If we are still in the first half of the upcount and a
* timer interupt is pending, then add on a tick's worth of time.
*/
if (((offset * 2) < (1000000/HZ)) &&
(timer_get_csr(CONFIG_XILINX_TIMER_0_BASEADDR,0)&TIMER_INTERRUPT))
{
offset += 1000000/HZ;
}
return offset;
}
// Adafruit Modification to change PWM Period/Frequency
void pwmPeriod(uint8 pin, uint32 us)
{
if (pin >= BOARD_NR_GPIO_PINS) return;
timer_dev *dev = PIN_MAP[pin].timer_device;
if (dev == NULL || dev->type == TIMER_BASIC) return;
// Get timer's max speed in hz
uint32 max_speed = rcc_dev_timer_clk_speed(dev->clk_id);
// period in cpu cycles
uint32 cycle = us * (max_speed / 1000000UL);
uint16 prescaler = (uint16) (cycle / TIMER_MAX_RELOAD);
uint16 reload = (uint16) round(cycle / (prescaler+1));
// Re-map compare to preserve duty cycle
uint16 compare = timer_get_compare(dev, PIN_MAP[pin].timer_channel);
compare = map(compare, 0, timer_get_reload(dev), 0, reload);
timer_set_prescaler(dev, prescaler);
timer_set_reload(dev, reload);
timer_set_compare(dev, PIN_MAP[pin].timer_channel, compare);
}
uint16 HardwareTimer::getCompare(int channel) {
return timer_get_compare(this->dev, (uint8)channel);
}
