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(); }