static uint64_t spinup_wait_zc(const uint64_t step_deadline) { const int min_samples_past_zc = (_state.comm_period / _params.adc_sampling_period) / 128 + 1; int num_samples_past_zc = 0; uint64_t zc_timestamp = 0; while (motor_timer_hnsec() <= step_deadline) { const bool past_zc = is_past_zc(spinup_sample_bemf()); if (past_zc) { if (zc_timestamp == 0) { zc_timestamp = motor_timer_hnsec(); } num_samples_past_zc++; if (num_samples_past_zc >= min_samples_past_zc) { break; } } else { zc_timestamp = 0; if (num_samples_past_zc > 0) { num_samples_past_zc--; } } } return zc_timestamp; }
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_timer_hndelay(int hnsecs) { static const int OVERHEAD_HNSEC = 1 * HNSEC_PER_USEC; if (hnsecs > OVERHEAD_HNSEC) { hnsecs -= OVERHEAD_HNSEC; } else { hnsecs = 0; } const uint64_t deadline = motor_timer_hnsec() + hnsecs; while (motor_timer_hnsec() < deadline) { } }
void motor_timer_set_absolute(uint64_t timestamp_hnsec) { const uint64_t current_timestamp = motor_timer_hnsec(); if (timestamp_hnsec > current_timestamp) { motor_timer_set_relative(timestamp_hnsec - current_timestamp); } else { motor_timer_set_relative(0); } }
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_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(); }