29 lines
824 B
C++
29 lines
824 B
C++
|
#include "pico/stdlib.h"
|
||
|
#include "hardware/pwm.h"
|
||
|
|
||
|
#include "sidetone.h"
|
||
|
|
||
|
Sidetone::Sidetone(uint gpio) : m_gpio(gpio)
|
||
|
{
|
||
|
gpio_set_function(gpio, GPIO_FUNC_PWM);
|
||
|
uint slice_num = pwm_gpio_to_slice_num(gpio);
|
||
|
pwm_config config = pwm_get_default_config();
|
||
|
pwm_init(slice_num, &config, true);
|
||
|
}
|
||
|
|
||
|
void Sidetone::on(uint freq)
|
||
|
{
|
||
|
// Calculate and cofigure new clock divider according to the frequency
|
||
|
// This formula is assuming we are running at 125MHz.
|
||
|
// TODO: Make this work for any frequency
|
||
|
float clkdiv = (1.f / freq) * 2000.f;
|
||
|
uint slice_num = pwm_gpio_to_slice_num(m_gpio);
|
||
|
pwm_set_clkdiv(slice_num, clkdiv);
|
||
|
// Configure duty to 50% ((2**16)-1)/2) to generate a square wave
|
||
|
pwm_set_gpio_level(m_gpio, 32768U);
|
||
|
}
|
||
|
|
||
|
void Sidetone::off()
|
||
|
{
|
||
|
pwm_set_gpio_level(m_gpio, 0);
|
||
|
}
|