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);
}
}
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_callback(uint64_t timestamp_hnsec)
{
if (!(_state.flags & FLAG_ACTIVE)) {
return;
}
motor_timer_set_relative(_state.comm_period);
// Next comm step
_state.current_comm_step++;
if (_state.current_comm_step >= MOTOR_NUM_COMMUTATION_STEPS) {
_state.current_comm_step = 0;
}
bool stop_now = false;
switch (_state.zc_detection_result) {
case ZC_DETECTED: {
engage_current_comm_step();
register_good_step();
_state.flags &= ~FLAG_SYNC_RECOVERY;
break;
}
case ZC_DESATURATION: {
engage_current_comm_step();
fake_missed_zc_detection(timestamp_hnsec);
_state.flags |= FLAG_SYNC_RECOVERY;
_state.immediate_desaturations++;
if (_state.immediate_desaturations >= _params.zc_failures_max) {
stop_now = true;
}
break;
}
case ZC_NOT_DETECTED:
case ZC_FAILED: {
if ((_state.flags & FLAG_SPINUP) && !(_state.flags & FLAG_SYNC_RECOVERY)) {
engage_current_comm_step();
} else {
motor_pwm_set_freewheeling();
}
fake_missed_zc_detection(timestamp_hnsec);
register_bad_step(&stop_now);
_state.flags |= FLAG_SYNC_RECOVERY;
break;
}
default: {
assert(0);
stop_now = true;
}
}
if (stop_now) {
stop_from_isr(); // No bounce no play
return;
}
_state.zc_detection_result = ZC_NOT_DETECTED;
_state.blank_time_deadline = timestamp_hnsec + _params.comm_blank_hnsec;
prepare_zc_detector_for_next_step();
motor_adc_enable_from_isr();
}
