/**
* @brief ChibiOS/RT initialization.
* @details After executing this function the current instructions stream
* becomes the main thread.
* @pre Interrupts must be still disabled when @p chSysInit() is invoked
* and are internally enabled.
* @post The main thread is created with priority @p NORMALPRIO.
* @note This function has special, architecture-dependent, requirements,
* see the notes into the various port reference manuals.
*
* @special
*/
void chSysInit(void) {
static Thread mainthread;
port_init();
scheduler_init();
vt_init();
#if CH_USE_MEMCORE
core_init();
#endif
#if CH_USE_HEAP
heap_init();
#endif
#if CH_DBG_ENABLE_TRACE
trace_init();
#endif
/* Now this instructions flow becomes the main thread.*/
setcurrp(_thread_init(&mainthread, NORMALPRIO));
currp->p_state = THD_STATE_CURRENT;
chSysEnable();
#if !CH_NO_IDLE_THREAD
/* This thread has the lowest priority in the system, its role is just to
serve interrupts in its context while keeping the lowest energy saving
mode compatible with the system status.*/
chThdCreateStatic(_idle_thread_wa, sizeof(_idle_thread_wa), IDLEPRIO,
(tfunc_t)_idle_thread, NULL);
#endif
}
bool Flasher_::write(PageID page, volatile const Data *bufp) {
R2P_ASSERT(is_aligned(const_cast<const Data *>(bufp)));
if (!check_bounds(address_of(page), PAGE_SIZE, get_program_start(),
get_program_length())) {
return false;
}
volatile Data *const flashp = reinterpret_cast<volatile Data *>(
reinterpret_cast<uintptr_t>(address_of(page))
);
chSysDisable();
// Unlock flash for write access
if (!flash_unlock()) {
flash_lock();
chSysEnable();
return false;
}
flash_busy_wait();
for (size_t pos = 0; pos < PAGE_SIZE / WORD_ALIGNMENT; ++pos) {
// Enter flash programming mode
FLASH->CR |= FLASH_CR_PG;
// Write half-word to flash
flashp[pos] = bufp[pos];
flash_busy_wait();
// Exit flash programming mode
FLASH->CR &= ~FLASH_CR_PG;
// Check for flash error
if (flashp[pos] != bufp[pos]) {
flash_lock();
chSysEnable();
return false;
}
}
flash_lock();
chSysEnable();
return true;
}
void stepperSetDetails(Stepper* s)
{
if (!s->timerRunning && (s->position != s->destination) && (s->speed != 0)) {
s->timerRunning = true;
chSysDisable();
fasttimerStart(&s->fastTimer, s->speed, true);
chSysEnable();
}
else {
if ((s->timerRunning) && ((s->position == s->destination) || (s->speed == 0))) {
chSysDisable();
fasttimerStop(&s->fastTimer);
chSysEnable();
s->timerRunning = false;
}
}
}
/*
max_freq = 10000000
for prescaler in xrange(int(TIMEVT_INPUT_CLOCK / float(max_freq) + 0.5), 65535 + 1):
if TIMEVT_INPUT_CLOCK % prescaler:
continue
if int(1e9) % (TIMEVT_INPUT_CLOCK / prescaler):
continue
print prescaler, TIMEVT_INPUT_CLOCK / prescaler, int(1e9) / (TIMEVT_INPUT_CLOCK / prescaler)
*/
void motor_timer_init(void)
{
chSysDisable();
// Power-on and reset
TIMEVT_RCC_ENR |= TIMEVT_RCC_ENR_MASK;
TIMEVT_RCC_RSTR |= TIMEVT_RCC_RSTR_MASK;
TIMEVT_RCC_RSTR &= ~TIMEVT_RCC_RSTR_MASK;
TIMSTP_RCC_ENR |= TIMSTP_RCC_ENR_MASK;
TIMSTP_RCC_RSTR |= TIMSTP_RCC_RSTR_MASK;
TIMSTP_RCC_RSTR &= ~TIMSTP_RCC_RSTR_MASK;
chSysEnable();
// Find the optimal prescaler value
uint32_t prescaler = (uint32_t)(TIMEVT_INPUT_CLOCK / ((float)MAX_FREQUENCY)); // Initial value
if (prescaler < 1)
prescaler = 1;
for (;; prescaler++) {
ASSERT_ALWAYS(prescaler < 0xFFFF);
if (TIMEVT_INPUT_CLOCK % prescaler) {
continue;
}
const uint32_t prescaled_clock = TIMEVT_INPUT_CLOCK / prescaler;
if (INT_1E9 % prescaled_clock) {
continue;
}
break; // Ok, current prescaler value can divide the timer frequency with no remainder
}
_nanosec_per_tick = INT_1E9 / (TIMEVT_INPUT_CLOCK / prescaler);
ASSERT_ALWAYS(_nanosec_per_tick < 1000); // Make sure it is sane
printf("Motor: Timer resolution: %u nanosec\n", (unsigned)_nanosec_per_tick);
// Enable IRQ
nvicEnableVector(TIMEVT_IRQn, MOTOR_IRQ_PRIORITY_MASK);
nvicEnableVector(TIMSTP_IRQn, MOTOR_IRQ_PRIORITY_MASK);
// Start the event timer
TIMEVT->ARR = 0xFFFF;
TIMEVT->PSC = (uint16_t)(prescaler - 1);
TIMEVT->CR1 = TIM_CR1_URS;
TIMEVT->SR = 0;
TIMEVT->EGR = TIM_EGR_UG; // Reload immediately
TIMEVT->CR1 = TIM_CR1_CEN; // Start
// Start the timestamping timer
TIMSTP->ARR = 0xFFFF;
TIMSTP->PSC = (uint16_t)(prescaler - 1);
TIMSTP->CR1 = TIM_CR1_URS;
TIMSTP->SR = 0;
TIMSTP->EGR = TIM_EGR_UG; // Reload immediately
TIMSTP->DIER = TIM_DIER_UIE;
TIMSTP->CR1 = TIM_CR1_CEN; // Start
}
/**
Simply take a number of steps from wherever the motor is currently positioned.
This function will move the motor a given number of steps from the current position.
@param stepper Which stepper (0 or 1).
@param steps An integer specifying the number of steps. Can be negative to go in reverse.
@return status (0 = OK).
\b Example
\code
// take 1200 steps forward from our current position
stepperStep(0, 1200);
\endcode
*/
int stepperStep(int stepper, int steps)
{
Stepper* s = &steppers[stepper];
chSysDisable();
s->destination = (s->position + steps);
chSysEnable();
stepperSetDetails(s);
return CONTROLLER_OK;
}
/**
Set the destination position for a stepper motor.
This will start the stepper moving the given number of
steps at the current speed, as set by Stepper_SetSpeed().
While it's moving, you can call Stepper_GetPosition() to read
its current position.
@param stepper An integer specifying which stepper (0 or 1).
@param positionRequested An integer specifying the desired stepper position.
@return status (0 = OK).
\b Example
\code
// start moving stepper 0 1500 steps
stepperSetDestination(0, 1500);
\endcode
*/
int stepperSetDestination(int stepper, int positionRequested)
{
Stepper* s = &steppers[stepper];
chSysDisable();
s->destination = positionRequested;
chSysEnable();
stepperSetDetails(s);
return CONTROLLER_OK;
}
/**
Reset the position of the specified stepper motor.
Note that this will not ask the stepper to move. It will simply set the
stepper's current position as 0. To move the stepper, see
stepperSetDestination() or stepperStep().
@param stepper An integer specifying which stepper (0 or 1).
@param position An integer specifying the stepper position.
@return status (0 = OK).
\b Example
\code
// reset stepper 1 to call its current position 0
stepperSetPosition(1, 0);
\endcode
*/
int stepperResetPosition(int stepper, int position)
{
Stepper* s = &steppers[stepper];
chSysDisable();
s->position = position;
s->destination = position;
chSysEnable();
stepperSetDetails(s);
return CONTROLLER_OK;
}
/**
* @brief ChibiOS/RT initialization.
* @details After executing this function the current instructions stream
* becomes the main thread.
* @pre Interrupts must disabled before invoking this function.
* @post The main thread is created with priority @p NORMALPRIO and
* interrupts are enabled.
*
* @special
*/
void chSysInit(void) {
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
extern stkalign_t __main_thread_stack_base__;
#endif
port_init();
_scheduler_init();
_vt_init();
#if CH_CFG_USE_TM == TRUE
_tm_init();
#endif
#if CH_CFG_USE_MEMCORE == TRUE
_core_init();
#endif
#if CH_CFG_USE_HEAP == TRUE
_heap_init();
#endif
#if CH_DBG_STATISTICS == TRUE
_stats_init();
#endif
#if CH_DBG_ENABLE_TRACE == TRUE
_dbg_trace_init();
#endif
#if CH_CFG_NO_IDLE_THREAD == FALSE
/* Now this instructions flow becomes the main thread.*/
setcurrp(_thread_init(&ch.mainthread, NORMALPRIO));
#else
/* Now this instructions flow becomes the idle thread.*/
setcurrp(_thread_init(&ch.mainthread, IDLEPRIO));
#endif
currp->p_state = CH_STATE_CURRENT;
#if CH_DBG_ENABLE_STACK_CHECK == TRUE
/* This is a special case because the main thread thread_t structure is not
adjacent to its stack area.*/
currp->p_stklimit = &__main_thread_stack_base__;
#endif
chSysEnable();
/* Note, &ch_debug points to the string "main" if the registry is
active, else the parameter is ignored.*/
chRegSetThreadName((const char *)&ch_debug);
#if CH_CFG_NO_IDLE_THREAD == FALSE
/* This thread has the lowest priority in the system, its role is just to
serve interrupts in its context while keeping the lowest energy saving
mode compatible with the system status.*/
(void) chThdCreateStatic(ch.idle_thread_wa, sizeof(ch.idle_thread_wa),
IDLEPRIO, (tfunc_t)_idle_thread, NULL);
#endif
}
/**
Set the speed at which a stepper will move.
This is a number of ms per step, rather than the more common steps per second.
Arranging it this way makes it easier to express as an integer.
Fastest speed is 1ms / step (1000 steps per second) and slowest is many seconds.
@param stepper An integer specifying which stepper (0 or 1).
@param speed An integer specifying the stepper speed in ms per step
@return status (0 = OK).
\b Example
\code
// set the speed to 1ms / step (1000 steps per second)
stepperSetSpeed(0, 1);
\endcode
*/
int stepperSetSpeed(int stepper, int speed)
{
Stepper* s = &steppers[stepper];
s->speed = speed * 1000;
chSysDisable();
fasttimerStop(&s->fastTimer);
fasttimerStart(&s->fastTimer, s->speed, true);
chSysEnable();
stepperSetDetails(s);
return CONTROLLER_OK;
}
/**
Disable a stepper motor.
@param stepper Which stepper (0 or 1).
*/
void stepperDisable(int stepper)
{
Stepper* s = &steppers[stepper];
if (s->timerRunning) {
chSysDisable();
fasttimerStop(&s->fastTimer);
chSysEnable();
}
int i;
for (i = 0; i < 4; i++)
pinOff(s->pins[i]);
int pwm = stepper * 2;
pwmDisableChannel(pwm);
pwmDisableChannel(pwm + 1);
}
/**
* @brief Initializes the kernel.
* @details Initializes the kernel structures, the current instructions flow
* becomes the idle thread upon return. The idle thread must not
* invoke any kernel primitive able to change state to not runnable.
* @note This function assumes that the @p nil global variable has been
* zeroed by the runtime environment. If this is not the case then
* make sure to clear it before calling this function.
*
* @special
*/
void chSysInit(void) {
thread_t *tp;
const thread_config_t *tcp;
/* Port layer initialization.*/
port_init();
/* System initialization hook.*/
NIL_CFG_SYSTEM_INIT_HOOK();
/* Iterates through the list of defined threads.*/
tp = &nil.threads[0];
tcp = nil_thd_configs;
while (tp < &nil.threads[NIL_CFG_NUM_THREADS]) {
#if NIL_CFG_ENABLE_STACK_CHECK
tp->stklim = (stkalign_t *)tcp->wbase;
#endif
/* Port dependent thread initialization.*/
PORT_SETUP_CONTEXT(tp, tcp->wend, tcp->funcp, tcp->arg);
/* Initialization hook.*/
NIL_CFG_THREAD_EXT_INIT_HOOK(tp);
tp++;
tcp++;
}
#if NIL_CFG_ENABLE_STACK_CHECK
/* The idle thread is a special case because its stack is set up by the
runtime environment.*/
tp->stklim = THD_IDLE_BASE;
#endif
/* Runs the highest priority thread, the current one becomes the null
thread.*/
nil.current = nil.next = nil.threads;
port_switch(nil.current, tp);
/* Interrupts enabled for the idle thread.*/
chSysEnable();
}
int motor_adc_init(void)
{
_shunt_resistance = configGet("mot_i_shunt_mr") / 1000.0f;
chSysDisable();
RCC->AHBENR |= RCC_AHBENR_DMA1EN; // Never disabled
RCC->CFGR &= ~RCC_CFGR_ADCPRE;
#if STM32_PCLK2 == 72000000
// ADC clock 72 / 6 = 12 MHz
RCC->CFGR |= RCC_CFGR_ADCPRE_DIV6;
#else
# error "What's wrong with PCLK2?"
#endif
chSysEnable();
enable();
return 0;
}
/*
* @brief Used to terminate all running tasks and disable the OS. All
* deinit of drivers and modules should be placed here.
*/
void vSystemDeinit(void)
{
/* Starting the shutdown sequence. */
if (system_state == SYSTEM_RUNNING)
system_state = SYSTEM_TERMINATING;
else
osalSysHalt("System is not running, invalid operation");
/* Terminate critical tasks */
vSystemTerminateCriticalTasks();
/* Stop drivers */
vSystemDeinitList();
/* Stop SysTick */
chSysDisable();
vSystemDisableSystick();
chSysEnable();
system_state = SYSTEM_STOPPED;
}
/**
* @brief ChibiOS/RT initialization.
* @details After executing this function the current instructions stream
* becomes the main thread.
* @pre Interrupts must be still disabled when @p chSysInit() is invoked
* and are internally enabled.
* @post The main thread is created with priority @p NORMALPRIO.
* @note This function has special, architecture-dependent, requirements,
* see the notes into the various port reference manuals.
*
* @special
*/
void chSysInit(void) {
static Thread mainthread;
#if CH_DBG_ENABLE_STACK_CHECK
extern stkalign_t __main_thread_stack_base__;
#endif
port_init();
_scheduler_init();
_vt_init();
#if CH_USE_MEMCORE
_core_init();
#endif
#if CH_USE_HEAP
_heap_init();
#endif
#if CH_DBG_ENABLE_TRACE
_trace_init();
#endif
/* Now this instructions flow becomes the main thread.*/
setcurrp(_thread_init(&mainthread, NORMALPRIO));
currp->p_state = THD_STATE_CURRENT;
#if CH_DBG_ENABLE_STACK_CHECK
/* This is a special case because the main thread Thread structure is not
adjacent to its stack area.*/
currp->p_stklimit = &__main_thread_stack_base__;
#endif
chSysEnable();
chRegSetThreadName("main");
#if !CH_NO_IDLE_THREAD
/* This thread has the lowest priority in the system, its role is just to
serve interrupts in its context while keeping the lowest energy saving
mode compatible with the system status.*/
chThdCreateStatic(_idle_thread_wa, sizeof(_idle_thread_wa), IDLEPRIO,
(tfunc_t)_idle_thread, NULL);
#endif
}
static void enable(void)
{
// DMA
DMA1_Channel1->CCR = 0; // Deinitialize
DMA1_Channel1->CMAR = (uint32_t)_adc1_2_dma_buffer;
DMA1_Channel1->CNDTR = sizeof(_adc1_2_dma_buffer) / 4;
DMA1_Channel1->CPAR = (uint32_t)&ADC1->DR;
DMA1_Channel1->CCR =
DMA_CCR_PL_0 | DMA_CCR_PL_1 | // Max priority
DMA_CCR_MSIZE_1 | // 32 bit
DMA_CCR_PSIZE_1 | // 32 bit
DMA_CCR_MINC |
DMA_CCR_CIRC |
DMA_CCR_EN;
// ADC enable, reset
const uint32_t enr_mask = RCC_APB2ENR_ADC1EN | RCC_APB2ENR_ADC2EN;
const uint32_t rst_mask = RCC_APB2RSTR_ADC1RST | RCC_APB2RSTR_ADC2RST;
chSysDisable();
RCC->APB2ENR |= enr_mask;
RCC->APB2RSTR |= rst_mask;
RCC->APB2RSTR &= ~rst_mask;
chSysEnable();
usleep(5); // Sequence: enable ADC, wait 2+ cycles, poweron, calibrate?
// ADC calibration
ADC1->CR2 = ADC_CR2_ADON;
adc_calibrate(ADC1);
ADC2->CR2 = ADC_CR2_ADON;
adc_calibrate(ADC2);
/*
* ADC channel sampling:
* A B C A B C VOLT
* C A B C A B CURR
*/
ADC1->SQR1 = ADC_SQR1_L_1 | ADC_SQR1_L_2;
ADC1->SQR3 =
ADC_SQR3_SQ1_0 |
ADC_SQR3_SQ2_1 |
ADC_SQR3_SQ3_0 | ADC_SQR3_SQ3_1 |
ADC_SQR3_SQ4_0 |
ADC_SQR3_SQ5_1 |
ADC_SQR3_SQ6_0 | ADC_SQR3_SQ6_1;
ADC1->SQR2 = ADC_SQR2_SQ7_2;
ADC2->SQR1 = ADC1->SQR1;
ADC2->SQR3 =
ADC_SQR3_SQ1_0 | ADC_SQR3_SQ1_1 |
ADC_SQR3_SQ2_0 |
ADC_SQR3_SQ3_1 |
ADC_SQR3_SQ4_0 | ADC_SQR3_SQ4_1 |
ADC_SQR3_SQ5_0 |
ADC_SQR3_SQ6_1;
ADC2->SQR2 = ADC_SQR2_SQ7_2 | ADC_SQR2_SQ7_0;
// SMPR registers are not configured because they have right values by default
// ADC initialization
ADC1->CR1 = ADC_CR1_DUALMOD_1 | ADC_CR1_DUALMOD_2 | ADC_CR1_SCAN | ADC_CR1_EOCIE;
ADC1->CR2 = ADC_CR2_ADON | ADC_CR2_EXTTRIG | MOTOR_ADC1_2_TRIGGER | ADC_CR2_DMA;
ADC2->CR1 = ADC_CR1_DUALMOD_1 | ADC_CR1_DUALMOD_2 | ADC_CR1_SCAN;
ADC2->CR2 = ADC_CR2_ADON | ADC_CR2_EXTTRIG | ADC_CR2_EXTSEL_0 | ADC_CR2_EXTSEL_1 | ADC_CR2_EXTSEL_2;
// ADC IRQ (only ADC1 IRQ is used because ADC2 is configured in slave mode)
chSysDisable();
nvicEnableVector(ADC1_2_IRQn, MOTOR_IRQ_PRIORITY_MASK);
chSysEnable();
}
/**
* @brief ChibiOS/RT initialization.
* @details After executing this function the current instructions stream
* becomes the main thread.
* @pre Interrupts must disabled before invoking this function.
* @post The main thread is created with priority @p NORMALPRIO and
* interrupts are enabled.
*
* @special
*/
void chSysInit(void) {
extern stkalign_t __main_thread_stack_base__;
_scheduler_init();
_vt_init();
#if CH_DBG_SYSTEM_STATE_CHECK == TRUE
ch.dbg.isr_cnt = (cnt_t)0;
ch.dbg.lock_cnt = (cnt_t)0;
#endif
#if CH_CFG_USE_TM == TRUE
_tm_init();
#endif
#if CH_CFG_USE_MEMCORE == TRUE
_core_init();
#endif
#if CH_CFG_USE_HEAP == TRUE
_heap_init();
#endif
#if CH_DBG_STATISTICS == TRUE
_stats_init();
#endif
#if CH_DBG_TRACE_MASK != CH_DBG_TRACE_MASK_NONE
_dbg_trace_init();
#endif
#if CH_CFG_NO_IDLE_THREAD == FALSE
/* Now this instructions flow becomes the main thread.*/
#if CH_CFG_USE_REGISTRY == TRUE
currp = _thread_init(&ch.mainthread, (const char *)&ch_debug, NORMALPRIO);
#else
currp = _thread_init(&ch.mainthread, "main", NORMALPRIO);
#endif
#else
/* Now this instructions flow becomes the idle thread.*/
currp = _thread_init(&ch.mainthread, "idle", IDLEPRIO);
#endif
/* Setting up the base address of the static main thread stack.*/
currp->stklimit = &__main_thread_stack_base__;
/* Setting up the caller as current thread.*/
currp->state = CH_STATE_CURRENT;
/* Port layer initialization last because it depend on some of the
initializations performed before.*/
port_init();
#if CH_DBG_STATISTICS == TRUE
/* Starting measurement for this thread.*/
chTMStartMeasurementX(&currp->stats);
#endif
/* It is alive now.*/
chSysEnable();
#if CH_CFG_NO_IDLE_THREAD == FALSE
{
static const thread_descriptor_t idle_descriptor = {
"idle",
THD_WORKING_AREA_BASE(ch_idle_thread_wa),
THD_WORKING_AREA_END(ch_idle_thread_wa),
IDLEPRIO,
_idle_thread,
NULL
};
/* This thread has the lowest priority in the system, its role is just to
serve interrupts in its context while keeping the lowest energy saving
mode compatible with the system status.*/
(void) chThdCreate(&idle_descriptor);
}
#endif
}
~CriticalSectionLocker() { chSysEnable(); }
void motor_pwm_beep(int frequency, int duration_msec)
{
static const float DUTY_CYCLE = 0.01;
static const int ACTIVE_USEC_MAX = 20;
motor_pwm_set_freewheeling();
frequency = (frequency < 100) ? 100 : frequency;
frequency = (frequency > 5000) ? 5000 : frequency;
duration_msec = (duration_msec < 1) ? 1 : duration_msec;
duration_msec = (duration_msec > 5000) ? 5000 : duration_msec;
/*
* Timing constants
*/
const int half_period_hnsec = (HNSEC_PER_SEC / frequency) / 2;
int active_hnsec = half_period_hnsec * DUTY_CYCLE;
if (active_hnsec > ACTIVE_USEC_MAX * HNSEC_PER_USEC) {
active_hnsec = ACTIVE_USEC_MAX * HNSEC_PER_USEC;
}
const int idle_hnsec = half_period_hnsec - active_hnsec;
const uint64_t end_time = motor_timer_hnsec() + duration_msec * HNSEC_PER_MSEC;
/*
* FET round robin
* This way we can beep even if some FETs went bananas
*/
static unsigned _phase_sel;
const int low_phase_first = _phase_sel++ % MOTOR_NUM_PHASES;
const int low_phase_second = _phase_sel++ % MOTOR_NUM_PHASES;
const int high_phase = _phase_sel++ % MOTOR_NUM_PHASES;
_phase_sel++; // We need to increment it not by multiple of 3
assert(low_phase_first != high_phase && low_phase_second != high_phase);
/*
* Commutations
* No high side pumping
*/
phase_set_i(low_phase_first, 0, false);
phase_set_i(low_phase_second, 0, false);
phase_set_i(high_phase, 0, false);
apply_phase_config();
while (end_time > motor_timer_hnsec()) {
chSysSuspend();
irq_primask_disable();
phase_set_i(high_phase, _pwm_top, false);
apply_phase_config();
irq_primask_enable();
motor_timer_hndelay(active_hnsec);
irq_primask_disable();
phase_set_i(high_phase, 0, false);
apply_phase_config();
irq_primask_enable();
chSysEnable();
motor_timer_hndelay(idle_hnsec);
}
motor_pwm_set_freewheeling();
}
static void init_timers(void)
{
assert_always(_pwm_top > 0); // Make sure it was initialized
chSysDisable();
// TIM1
RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
RCC->APB2RSTR |= RCC_APB2RSTR_TIM1RST;
RCC->APB2RSTR &= ~RCC_APB2RSTR_TIM1RST;
// TIM2
RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
RCC->APB1RSTR |= RCC_APB1RSTR_TIM2RST;
RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM2RST;
chSysEnable();
// Reload value
TIM2->ARR = TIM1->ARR = _pwm_top;
// Left-aligned PWM, direction up (will be enabled later)
TIM2->CR1 = TIM1->CR1 = 0;
// Output idle state 0, buffered updates
TIM1->CR2 = TIM_CR2_CCUS | TIM_CR2_CCPC;
/*
* OC channels
* TIM1 CC1, CC2, CC3 are used to control the FETs; TIM1 CC4 is not used.
* TIM2 CC2 is used to trigger the ADC conversion.
*/
// Phase A, phase B
TIM1->CCMR1 =
TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 |
TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1;
// Phase C
TIM1->CCMR2 =
TIM_CCMR2_OC3PE | TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1;
// ADC sync
TIM2->CCMR1 =
TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_0;
// OC polarity (no inversion, all disabled except ADC sync)
TIM1->CCER = 0;
TIM2->CCER = TIM_CCER_CC2E;
/*
* Dead time generator setup.
* DTS clock divider set 0, hence fDTS = input clock.
* DTG bit 7 must be 0, otherwise it will change multiplier which is not supported yet.
* At 72 MHz one tick ~ 13.9 nsec, max 127 * 13.9 ~ 1.764 usec, which is large enough.
*/
const float pwm_dead_time = PWM_DEAD_TIME_NANOSEC / 1e9f;
const float pwm_dead_time_ticks_float = pwm_dead_time / (1.f / PWM_TIMER_FREQUENCY);
assert(pwm_dead_time_ticks_float > 0);
assert(pwm_dead_time_ticks_float < (_pwm_top * 0.2f));
uint16_t dead_time_ticks = (uint16_t)pwm_dead_time_ticks_float;
if (dead_time_ticks > 127) {
assert(0);
dead_time_ticks = 127;
}
lowsyslog("Motor: PWM dead time %u ticks\n", (unsigned)dead_time_ticks);
TIM1->BDTR = TIM_BDTR_AOE | TIM_BDTR_MOE | dead_time_ticks;
/*
* Default ADC sync config, will be adjusted dynamically
*/
TIM2->CCR2 = _pwm_top / 4;
// Timers are configured now but not started yet. Starting is tricky because of synchronization, see below.
TIM1->EGR = TIM_EGR_UG | TIM_EGR_COMG;
TIM2->EGR = TIM_EGR_UG | TIM_EGR_COMG;
}
