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_rtctl_start(float duty_cycle, bool reverse)
{
motor_rtctl_stop(); // Just in case
if (!_initialization_confirmed) {
return; // Go home you're drunk
}
if (duty_cycle <= 0) {
assert(0);
return;
}
/*
* Initialize the structs
*/
memset(&_diag, 0, sizeof(_diag));
memset(&_state, 0, sizeof(_state)); // Mighty reset
motor_forced_rotation_detector_reset();
_diag.started_at = motor_timer_hnsec();
_state.comm_table = reverse ? COMMUTATION_TABLE_REVERSE : COMMUTATION_TABLE_FORWARD;
_state.pwm_val_after_spinup = motor_pwm_compute_pwm_val(duty_cycle);
init_adc_filters();
/*
* Initial spinup.
*/
const tprio_t orig_priority = chThdSetPriority(HIGHPRIO);
motor_pwm_prepare_to_start();
const bool started = do_bemf_spinup(duty_cycle);
/*
* Engage the normal mode if started
* At this point, if the spinup was OK, we're sutiated RIGHT AFTER THE DETECTED ZERO CROSS, engaged.
*/
if (started) {
_state.blank_time_deadline = motor_timer_hnsec() + _params.comm_blank_hnsec;
_state.zc_detection_result = ZC_DETECTED;
_state.flags = FLAG_ACTIVE | FLAG_SPINUP;
motor_timer_set_relative(_state.comm_period / 3);
lowsyslog("Motor: Spinup OK, comm period: %u usec\n", (unsigned)(_state.comm_period / HNSEC_PER_USEC));
} else {
lowsyslog("Motor: Spinup failed\n");
motor_rtctl_stop();
}
chThdSetPriority(orig_priority);
}
void motor_rtctl_set_duty_cycle(float duty_cycle)
{
// We don't need a critical section to write an integer
_state.pwm_val = motor_pwm_compute_pwm_val(duty_cycle);
}
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;
}
