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);
}
int motor_init(void)
{
_watchdog_id = watchdog_create(WATCHDOG_TIMEOUT_MSEC);
if (_watchdog_id < 0) {
return _watchdog_id;
}
int ret = motor_rtctl_init();
if (ret) {
return ret;
}
chMtxInit(&_mutex);
chEvtInit(&_setpoint_update_event);
configure();
init_filters();
if (_state.input_voltage < MIN_VALID_INPUT_VOLTAGE || _state.input_voltage > MAX_VALID_INPUT_VOLTAGE) {
lowsyslog("Motor: Invalid input voltage: %f\n", _state.input_voltage);
return -1;
}
ret = rpmctl_init();
if (ret) {
return ret;
}
motor_rtctl_stop();
assert_always(chThdCreateStatic(_wa_control_thread, sizeof(_wa_control_thread),
HIGHPRIO, control_thread, NULL));
return 0;
}
int motor_init(void)
{
_watchdog_id = watchdogCreate(WATCHDOG_TIMEOUT_MSEC);
if (_watchdog_id < 0) {
return _watchdog_id;
}
int ret = motor_rtctl_init();
if (ret) {
return ret;
}
configure();
init_filters();
if (_state.input_voltage < MIN_VALID_INPUT_VOLTAGE || _state.input_voltage > MAX_VALID_INPUT_VOLTAGE) {
printf("Motor: Invalid input voltage: %f\n", _state.input_voltage);
return -1;
}
ret = rpmctl_init();
if (ret) {
return ret;
}
motor_rtctl_stop();
if (!chThdCreateStatic(_wa_control_thread, sizeof(_wa_control_thread), HIGHPRIO, control_thread, NULL)) {
abort();
}
return 0;
}
static void stop(bool expected)
{
motor_rtctl_stop();
_state.limit_mask = 0;
_state.dc_actual = 0.0;
_state.dc_openloop_setpoint = 0.0;
_state.rpm_setpoint = 0;
_state.setpoint_ttl_ms = 0;
_state.rtctl_state = motor_rtctl_get_state();
if (expected) {
_state.num_unexpected_stops = 0;
} else {
_state.num_unexpected_stops++;
}
rpmctl_reset();
}
int motor_rtctl_init(void)
{
int ret = motor_pwm_init();
if (ret) {
return ret;
}
motor_timer_init();
ret = motor_adc_init();
if (ret) {
return ret;
}
motor_forced_rotation_detector_init();
configure();
motor_rtctl_stop();
return 0;
}
