void motor_pwm_manip(const enum motor_pwm_phase_manip command[MOTOR_NUM_PHASES])
{
irq_primask_disable();
phase_reset_all_i();
irq_primask_enable();
for (int phase = 0; phase < MOTOR_NUM_PHASES; phase++) {
if (command[phase] == MOTOR_PWM_MANIP_HIGH) {
// We don't want to engage 100% duty cycle because the high side pump needs switching
const int pwm_val = motor_pwm_compute_pwm_val(0.80f);
irq_primask_disable();
phase_set_i(phase, pwm_val, false);
irq_primask_enable();
} else if (command[phase] == MOTOR_PWM_MANIP_HALF) {
irq_primask_disable();
phase_set_i(phase, _pwm_half_top, false);
irq_primask_enable();
} else if (command[phase] == MOTOR_PWM_MANIP_LOW) {
irq_primask_disable();
phase_set_i(phase, 0, false);
irq_primask_enable();
} else if (command[phase] == MOTOR_PWM_MANIP_FLOATING) {
// Nothing to do
} else {
assert(0);
}
}
adjust_adc_sync(_pwm_half_top); // Default for phase manip
apply_phase_config();
}
void motor_timer_set_relative(int delay_hnsec)
{
delay_hnsec -= 1 * HNSEC_PER_USEC;
if (delay_hnsec < 0) {
delay_hnsec = 0;
}
assert(_nanosec_per_tick > 0);
int delay_ticks = (delay_hnsec * 100) / _nanosec_per_tick;
if (delay_ticks > 0xFFFF) {
assert(0);
delay_ticks = 0xFFFF;
}
/*
* Interrupts must be disabled completely because the following
* sequence requires strict timing.
* No port_*() functions are allowed here!
*/
irq_primask_disable();
if (delay_hnsec > HNSEC_PER_USEC) {
TIMEVT->CCR1 = TIMEVT->CNT + delay_ticks;
TIMEVT->SR = ~TIM_SR_CC1IF; // Acknowledge IRQ
TIMEVT->DIER |= TIM_DIER_CC1IE; // Enable this compare match
} else {
// Force the update event immediately because the delay is too small
TIMEVT->DIER |= TIM_DIER_CC1IE; // Either here or at the next statement IRQ will be generated
TIMEVT->EGR = TIM_EGR_CC1G;
}
irq_primask_enable();
}
struct motor_adc_sample motor_adc_get_last_sample(void)
{
struct motor_adc_sample ret;
irq_primask_disable();
ret = _sample;
irq_primask_enable();
return ret;
}
void motor_pwm_set_freewheeling(void)
{
irq_primask_disable();
phase_reset_all_i();
adjust_adc_sync_default();
apply_phase_config();
irq_primask_enable();
}
uint64_t motor_timer_hnsec(void)
{
assert(_nanosec_per_tick > 0); // Make sure the timer was initialized
#if !NDEBUG
static volatile uint64_t prev_output;
irq_primask_disable();
const volatile uint64_t prev_output_sample = prev_output;
irq_primask_enable();
#endif
volatile uint64_t ticks = 0;
volatile uint_fast16_t sample = 0;
while (1) {
ticks = _raw_ticks;
sample = TIMSTP->CNT;
const volatile uint64_t ticks2 = _raw_ticks;
if (ticks == ticks2) {
if (TIMSTP->SR & TIM_SR_UIF) {
sample = TIMSTP->CNT;
ticks += TICKS_PER_OVERFLOW;
}
break;
}
}
const uint64_t output = ((ticks + sample) * _nanosec_per_tick) / 100;
#if !NDEBUG
irq_primask_disable();
// Make sure the prev output was not modified from another context.
if (prev_output_sample == prev_output) {
assert(prev_output <= output);
prev_output = output;
}
irq_primask_enable();
#endif
return output;
}
void motor_pwm_energize(const int polarity[MOTOR_NUM_PHASES])
{
irq_primask_disable();
phase_reset_all_i();
irq_primask_enable();
for (int phase = 0; phase < MOTOR_NUM_PHASES; phase++) {
const int pol = polarity[phase];
if (pol == 0) {
continue;
}
assert(pol == 1 || pol == -1);
irq_primask_disable();
phase_set_i(phase, (pol > 0) ? _pwm_top : 0, false);
irq_primask_enable();
}
adjust_adc_sync(_pwm_top);
apply_phase_config();
}
void motor_rtctl_stop(void)
{
_state.flags = 0;
motor_timer_cancel();
_state.flags = 0;
irq_primask_disable();
motor_adc_enable_from_isr(); // ADC should be enabled by default
irq_primask_enable();
motor_pwm_set_freewheeling();
}
void motor_rtctl_beep(int frequency, int duration_msec)
{
if (_state.flags & FLAG_ACTIVE) {
return;
}
irq_primask_disable();
motor_adc_disable_from_isr();
irq_primask_enable();
const tprio_t orig_priority = chThdSetPriority(HIGHPRIO);
motor_pwm_beep(frequency, duration_msec);
chThdSetPriority(orig_priority);
irq_primask_disable();
motor_adc_enable_from_isr();
irq_primask_enable();
/*
* Motor windings may get saturated after beeping, making immediately following spinup unreliable.
* This little delay fixes that, not in the best way though.
*/
usleep(10000);
}
static bool do_bemf_spinup(const float max_duty_cycle)
{
assert(chThdGetPriority() == HIGHPRIO); // Mandatory
// Make sure we're not going to underflow during time calculations
while (motor_timer_hnsec() < _params.spinup_start_comm_period) {
;
}
const uint64_t deadline = motor_timer_hnsec() + _params.spinup_timeout;
float dc = _params.spinup_duty_cycle_increment;
unsigned num_good_comms = 0;
_state.comm_period = _params.spinup_start_comm_period;
_state.prev_zc_timestamp = motor_timer_hnsec() - _state.comm_period / 2;
_state.pwm_val = motor_pwm_compute_pwm_val(dc);
while (motor_timer_hnsec() <= deadline) {
// Engage the current comm step
irq_primask_disable();
motor_pwm_set_step_from_isr(_state.comm_table + _state.current_comm_step, _state.pwm_val);
irq_primask_enable();
uint64_t step_deadline = motor_timer_hnsec() + _state.comm_period;
motor_timer_hndelay(_params.comm_blank_hnsec);
// Wait for the next zero crossing
const uint64_t zc_timestamp = spinup_wait_zc(step_deadline);
num_good_comms = (zc_timestamp > 0) ? (num_good_comms + 1) : 0;
// Compute the next duty cycle
dc += _params.spinup_duty_cycle_increment;
if (dc > max_duty_cycle) {
dc = max_duty_cycle;
}
_state.pwm_val = motor_pwm_compute_pwm_val(dc);
if (zc_timestamp > 0) {
assert(zc_timestamp > _state.prev_zc_timestamp);
// Update comm period
const uint32_t new_comm_period = zc_timestamp - _state.prev_zc_timestamp;
_state.prev_zc_timestamp = zc_timestamp;
_state.comm_period = (_state.comm_period + new_comm_period) / 2;
step_deadline = zc_timestamp + _state.comm_period / 2;
// Check the termination condition
const bool enough_good_comms = num_good_comms > _params.spinup_num_good_comms;
const bool fast_enough = _state.comm_period <= _params.spinup_end_comm_period;
if (enough_good_comms || fast_enough) {
break;
}
} else {
// If ZC was not detected, we need to fake the previous ZC timestamp now
_state.prev_zc_timestamp = step_deadline - _state.comm_period / 2;
}
// Wait till the end of the current step
while (motor_timer_hnsec() <= step_deadline) {
;
}
// Next step
_state.current_comm_step++;
if (_state.current_comm_step >= MOTOR_NUM_COMMUTATION_STEPS) {
_state.current_comm_step = 0;
}
}
return _state.comm_period < _params.comm_period_max;
}
void motor_rtctl_print_debug_info(void)
{
static const int ALIGNMENT = 25;
#define PRINT_INT(name, value) lowsyslog(" %-*s %li\n", ALIGNMENT, (name), (long)(value))
#define PRINT_FLT(name, value) lowsyslog(" %-*s %f\n", ALIGNMENT, (name), (float)(value))
/*
* Instant state
*/
irq_primask_disable();
const struct control_state state_copy = _state;
irq_primask_enable();
lowsyslog("Motor RTCTL state\n");
PRINT_INT("comm period", state_copy.comm_period / HNSEC_PER_USEC);
PRINT_INT("flags", state_copy.flags);
PRINT_INT("neutral voltage", state_copy.neutral_voltage);
PRINT_INT("input voltage", state_copy.input_voltage_raw);
PRINT_INT("input current", state_copy.input_current);
PRINT_INT("pwm val", state_copy.pwm_val);
/*
* Diagnostics
*/
irq_primask_disable();
const struct diag_info diag_copy = _diag;
irq_primask_enable();
lowsyslog("Motor RTCTL diag\n");
PRINT_INT("zc failures", diag_copy.zc_failures_since_start);
PRINT_INT("desaturations", diag_copy.desaturations);
PRINT_INT("bemf out of range", diag_copy.bemf_samples_out_of_range);
PRINT_INT("bemf premature zc", diag_copy.bemf_samples_premature_zc);
PRINT_INT("zc sol failures", diag_copy.zc_solution_failures);
PRINT_INT("zc sol num samples",diag_copy.zc_solution_num_samples);
PRINT_FLT("zc sol slope", diag_copy.zc_solution_slope / (float)LEAST_SQUARES_MULT);
PRINT_FLT("zc sol yintercept", diag_copy.zc_solution_yintercept / (float)LEAST_SQUARES_MULT);
/*
* ZC fitting
*/
#if DEBUG_BUILD
if (_diag.zc_solution_num_samples > 0) {
lowsyslog("Motor ZC solution data\n");
lowsyslog(" zc samples ");
for (int i = 0; i < _diag.zc_solution_num_samples; i++) {
lowsyslog("%-5i ", diag_copy.zc_solution_samples[i]);
}
lowsyslog("\n");
lowsyslog(" zc fitted ");
for (int i = 0; i < _diag.zc_solution_num_samples; i++) {
const int x = _params.adc_sampling_period * i;
const int y = (diag_copy.zc_solution_slope * x + diag_copy.zc_solution_yintercept) /
LEAST_SQUARES_MULT;
lowsyslog("%-5i ", y);
}
lowsyslog("\n");
}
#endif
#undef PRINT_INT
#undef PRINT_FLT
}
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();
}
