// Initialize timer 1 and the edge_array structure to output pulses.
//*********************************************************************
void pulse_and_edge_init ( void )
{
uint16_t temp_u16 ;
// Initial edge array setup.
// Init edge times for all on-edges.
edge_array [ 0 ] . edge_time = US_TO_CYCLES(BANK_TO_BANK_DELTA_US * 0);
edge_array [ 9 ] . edge_time = US_TO_CYCLES(BANK_TO_BANK_DELTA_US * 1);
edge_array [ 18 ] . edge_time = (uint16_t)US_TO_CYCLES(BANK_TO_BANK_DELTA_US * 2);
edge_array [ 27 ] . edge_time = (uint16_t)US_TO_CYCLES(BANK_TO_BANK_DELTA_US * 3);
// Init edge times for no-pulse edges (these edges mark time during
// the 10ms interval without pulses).
edge_array [ 36 ] . edge_time = (uint16_t)US_TO_CYCLES(BANK_TO_BANK_DELTA_US * 4);
edge_array [ 37 ] . edge_time = (uint16_t)US_TO_CYCLES(12000);
edge_array [ 38 ] . edge_time = (uint16_t)US_TO_CYCLES(16000);
edge_array [ 39 ] . edge_time = (uint16_t)US_TO_CYCLES(19900);
// Init the function pointers for the no-pulse edges
edge_array[36].function_ptr =
edge_array[37].function_ptr =
edge_array[38].function_ptr =
edge_array[39].function_ptr = (uint16_t)no_pulse;
// Init remaining edges based on no pulses
copy_pulses_to_sorted_edges ( 0, 0 ) ;
set_edge_data_and_function_ptr ( 0, 0 ) ;
copy_pulses_to_sorted_edges ( 8, 9 ) ;
set_edge_data_and_function_ptr ( 9, 2 ) ;
copy_pulses_to_sorted_edges ( 16, 18 ) ;
set_edge_data_and_function_ptr ( 18, 4 ) ;
copy_pulses_to_sorted_edges ( 24, 27 ) ;
set_edge_data_and_function_ptr ( 27, 6 ) ;
// First OCR1A is at the 10ms point, after all servo pulses are done
edge_data_ptr = (uint16_t) ( & edge_array [ 36 ] ) ;
// Init TCNT1 and OCR1A. Note that the order of accesses for high/low
// bytes of these 16-bit registers is important. High bytes must be
// written first. Assume interrupts are disabled.
temp_u16 = edge_array [ 36 ] . edge_time - NCYLES_ISR_PRE_EDGE ;
TCNT1H = temp_u16 >> 8 ;
TCNT1L = temp_u16 ;
OCR1AH = temp_u16 >> 8 ;
OCR1AL = temp_u16 ;
// Reset the output compare flag if set
TIFR1 = _BV(OCF1A);
// Enable the output compare interrupt
TIMSK1 |= _BV(OCIE1A);
}
void SERVO_CONTROLLER_Init (SERVO_CONTROLLER_Frequency frequency)
{
//Timers init
SC_TIM1_Init(frequency);
SC_TIM2_Init(frequency);
SC_TIM3_Init(frequency);
SC_TIM4_Init(frequency);
//Channels init
for (int i = 0; i < SERVO_TotalChannelsNum; i++)
{
SERVO_Channel *ch = &servo_channels[i];
ch->pulseWidth_us = DEGREES_0_PULSE_US;
ch->pulseCycles = US_TO_CYCLES(DEGREES_0_PULSE_US);
}
}
void SERVO_CONTROLLER_SetChannel (SERVO_ChannelId channel, uint16_t microseconds)
{
SERVO_Channel *ch = &servo_channels[channel];
TIM_HandleTypeDef *htim = tim_handlers_table[channel];
int32_t timChannel = tim_channels_table[channel];
if (!htim || timChannel < 0)
{
return;
}
uint16_t cycles = US_TO_CYCLES(microseconds);
ch->pulseWidth_us = microseconds;
ch->pulseCycles = cycles;
/*
* HAL driver set the Preload enable bit for all channels.
* So we can write value directly to CCRx register.
*/
switch (timChannel)
{
case TIM_CHANNEL_1:
htim->Instance->CCR1 = cycles;
break;
case TIM_CHANNEL_2:
htim->Instance->CCR2 = cycles;
break;
case TIM_CHANNEL_3:
htim->Instance->CCR3 = cycles;
break;
case TIM_CHANNEL_4:
htim->Instance->CCR4 = cycles;
break;
default:
break;
}
}
