// init hardware
void initHardware(unsigned long freq) {
// set frequency
// DIVI is the integer part of the divisor
// the fractional part (DIVF) drops clock cycles to get the output frequency, bad for servo motors
// 320 bits for one cycle of 20 milliseconds = 62.5 us per bit = 16 kHz
int idiv = (int) (19200000.0f / freq);
if (idiv < 1 || idiv > 0x1000) {
printf("idiv out of range: 1 < 0x%x < 0x1000\n", idiv);
exit(-1);
}
printf("Clock set to %d Hz\n", 19200000/idiv);
// mmap register space
setupRegisterMemoryMappings();
// stop clock and waiting for busy flag doesn't work, so kill clock
*(clk + PWMCLK_CNTL) = 0x5A000000 | (1 << 5);
usleep(10);
*(clk + PWMCLK_DIV) = 0x5A000000 | (idiv<<12);
// source=osc and enable clock
*(clk + PWMCLK_CNTL) = 0x5A000011;
}
// init hardware
void initHardware()
{
// mmap register space
setupRegisterMemoryMappings();
// set PWM alternate function for GPIO18
SET_GPIO_ALT(12, 0);
// stop clock and waiting for busy flag doesn't work, so kill clock
*(clk + PWMCLK_CNTL) = 0x5A000000 | (1 << 5);
usleep(10);
// set frequency
// DIVI is the integer part of the divisor
// the fractional part (DIVF) drops clock cycles to get the output frequency, bad for servo motors
// 320 bits for one cycle of 20 milliseconds = 62.5 us per bit = 16 kHz
int idiv = (int) (19200000.0f / 16000.0f);
if (idiv < 1 || idiv > 0x1000) {
printf("idiv out of range: %x\n", idiv);
exit(-1);
}
*(clk + PWMCLK_DIV) = 0x5A000000 | (idiv<<12);
// source=osc and enable clock
*(clk + PWMCLK_CNTL) = 0x5A000011;
// disable PWM
*(pwm + PWM_CTL) = 0;
// needs some time until the PWM module gets disabled, without the delay the PWM module crashs
usleep(10);
// filled with 0 for 20 milliseconds = 320 bits
*(pwm + PWM_RNG1) = 320;
// 32 bits = 2 milliseconds, init with 1 millisecond
setServo(0);
// start PWM1 in serializer mode
*(pwm + PWM_CTL) = 3;
}
