void Servo::writeMicroseconds(int value)
{
uint8_t channel, instance;
uint8_t pin = servos[this->servoIndex].Pin.nbr;
//instance of pwm module is MSB - look at VWariant.h
instance=(g_APinDescription[pin].ulPWMChannel & 0xF0)/16;
//index of pwm channel is LSB - look at VWariant.h
channel=g_APinDescription[pin].ulPWMChannel & 0x0F;
group_pins[instance][channel]=g_APinDescription[pin].ulPin;
NRF_PWM_Type * PWMInstance = instance == 0 ? NRF_PWM0 : (instance == 1 ? NRF_PWM1 : NRF_PWM2);
//configure pwm instance and enable it
seq_values[instance][channel]= value | 0x8000;
nrf_pwm_sequence_t const seq={
seq_values[instance],
NRF_PWM_VALUES_LENGTH(seq_values),
0,
0
};
nrf_pwm_pins_set(PWMInstance, group_pins[instance]);
nrf_pwm_enable(PWMInstance);
nrf_pwm_configure(PWMInstance, NRF_PWM_CLK_125kHz, NRF_PWM_MODE_UP, 2500); // 20ms - 50Hz
nrf_pwm_decoder_set(PWMInstance, NRF_PWM_LOAD_INDIVIDUAL, NRF_PWM_STEP_AUTO);
nrf_pwm_sequence_set(PWMInstance, 0, &seq);
nrf_pwm_loop_set(PWMInstance, 0UL);
nrf_pwm_task_trigger(PWMInstance, NRF_PWM_TASK_SEQSTART0);
}
nrfx_err_t nrfx_pwm_init(nrfx_pwm_t const * const p_instance,
nrfx_pwm_config_t const * p_config,
nrfx_pwm_handler_t handler)
{
NRFX_ASSERT(p_config);
nrfx_err_t err_code;
pwm_control_block_t * p_cb = &m_cb[p_instance->drv_inst_idx];
if (p_cb->state != NRFX_DRV_STATE_UNINITIALIZED)
{
err_code = NRFX_ERROR_INVALID_STATE;
NRFX_LOG_WARNING("Function: %s, error code: %s.",
__func__,
NRFX_LOG_ERROR_STRING_GET(err_code));
return err_code;
}
p_cb->handler = handler;
configure_pins(p_instance, p_config);
nrf_pwm_enable(p_instance->p_registers);
nrf_pwm_configure(p_instance->p_registers,
p_config->base_clock, p_config->count_mode, p_config->top_value);
nrf_pwm_decoder_set(p_instance->p_registers,
p_config->load_mode, p_config->step_mode);
nrf_pwm_shorts_set(p_instance->p_registers, 0);
nrf_pwm_int_set(p_instance->p_registers, 0);
nrf_pwm_event_clear(p_instance->p_registers, NRF_PWM_EVENT_LOOPSDONE);
nrf_pwm_event_clear(p_instance->p_registers, NRF_PWM_EVENT_SEQEND0);
nrf_pwm_event_clear(p_instance->p_registers, NRF_PWM_EVENT_SEQEND1);
nrf_pwm_event_clear(p_instance->p_registers, NRF_PWM_EVENT_STOPPED);
// The workaround for nRF52 Anomaly 109 "protects" DMA transfers by handling
// interrupts generated on SEQEND0 and SEQEND1 events (this ensures that
// the 64 MHz clock is ready when data for the next sequence to be played
// is read). Therefore, the PWM interrupt must be enabled even if the event
// handler is not used.
#if defined(USE_DMA_ISSUE_WORKAROUND)
NRFX_IRQ_PRIORITY_SET(DMA_ISSUE_EGU_IRQn, p_config->irq_priority);
NRFX_IRQ_ENABLE(DMA_ISSUE_EGU_IRQn);
#else
if (p_cb->handler)
#endif
{
NRFX_IRQ_PRIORITY_SET(nrfx_get_irq_number(p_instance->p_registers),
p_config->irq_priority);
NRFX_IRQ_ENABLE(nrfx_get_irq_number(p_instance->p_registers));
}
p_cb->state = NRFX_DRV_STATE_INITIALIZED;
err_code = NRFX_SUCCESS;
NRFX_LOG_INFO("Function: %s, error code: %s.", __func__, NRFX_LOG_ERROR_STRING_GET(err_code));
return err_code;
}
ret_code_t nrf_drv_pwm_init(nrf_drv_pwm_t const * const p_instance,
nrf_drv_pwm_config_t const * p_config,
nrf_drv_pwm_handler_t handler)
{
ASSERT(p_config)
pwm_control_block_t * p_cb = &m_cb[p_instance->drv_inst_idx];
if (p_cb->state != NRF_DRV_STATE_UNINITIALIZED)
{
return NRF_ERROR_INVALID_STATE;
}
p_cb->handler = handler;
configure_pins(p_instance, p_config);
nrf_pwm_enable(p_instance->p_registers);
nrf_pwm_configure(p_instance->p_registers,
p_config->base_clock, p_config->count_mode, p_config->top_value);
nrf_pwm_decoder_set(p_instance->p_registers,
p_config->load_mode, p_config->step_mode);
nrf_pwm_shorts_set(p_instance->p_registers, 0);
nrf_pwm_int_set(p_instance->p_registers, 0);
nrf_pwm_event_clear(p_instance->p_registers, NRF_PWM_EVENT_LOOPSDONE);
nrf_pwm_event_clear(p_instance->p_registers, NRF_PWM_EVENT_SEQEND0);
nrf_pwm_event_clear(p_instance->p_registers, NRF_PWM_EVENT_SEQEND1);
nrf_pwm_event_clear(p_instance->p_registers, NRF_PWM_EVENT_STOPPED);
if (p_cb->handler)
{
nrf_drv_common_irq_enable(nrf_drv_get_IRQn(p_instance->p_registers),
p_config->irq_priority);
}
p_cb->state = NRF_DRV_STATE_INITIALIZED;
return NRF_SUCCESS;
}
