Beispiel #1
0
static void update_control_non_running(void)
{
	// Do not change anything while the motor is starting
	const enum motor_rtctl_state rtctl_state = motor_rtctl_get_state();
	if (rtctl_state == MOTOR_RTCTL_STATE_STARTING) {
		return;
	}

	// Start if necessary
	const float spinup_dc = _params.dc_min_voltage / _state.input_voltage;

	const bool need_start =
		(_state.mode == MOTOR_CONTROL_MODE_OPENLOOP && (_state.dc_openloop_setpoint > 0)) ||
		(_state.mode == MOTOR_CONTROL_MODE_RPM && (_state.rpm_setpoint > 0));

	if (need_start && (_state.num_unexpected_stops < _params.num_unexpected_stops_to_latch)) {
		const uint64_t timestamp = motor_rtctl_timestamp_hnsec();

		_state.dc_actual = spinup_dc;
		motor_rtctl_start(spinup_dc, _params.reverse);
		_state.rtctl_state = motor_rtctl_get_state();

		// This HACK prevents the setpoint TTL expiration in case of protracted startup
		const int elapsed_ms = (motor_rtctl_timestamp_hnsec() - timestamp) / HNSEC_PER_MSEC;
		_state.setpoint_ttl_ms += elapsed_ms;

		lowsyslog("Motor: Startup %i ms, DC %f, mode %i\n", elapsed_ms, spinup_dc, _state.mode);

		if (_state.rtctl_state == MOTOR_RTCTL_STATE_IDLE) {
			handle_unexpected_stop();
		}
	}
}
int motor_rtctl_test_hardware(void)
{
    if (motor_rtctl_get_state() != MOTOR_RTCTL_STATE_IDLE) {
        return -1;
    }

    motor_pwm_set_freewheeling();
    usleep(INITIAL_DELAY_MS * 1000);

    lowsyslog("Motor: Power stage test...\n");
    {
        int res = test_power_stage();
        if (res != 0) {
            return res;
        }
    }

    lowsyslog("Motor: Cross phase test...\n");
    {
        int res = test_cross_phase_conductivity();
        if (res != 0) {
            return res;
        }
    }

    lowsyslog("Motor: Sensors test...\n");
    {
        int res = test_sensors();
        if (res != 0) {
            return res;
        }
    }

    return 0;
}
Beispiel #3
0
bool motor_is_idle(void)
{
	chMtxLock(&_mutex);
	bool ret = motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE;
	chMtxUnlock();
	return ret;
}
Beispiel #4
0
bool motor_is_running(void)
{
	chMtxLock(&_mutex);
	bool ret = motor_rtctl_get_state() == MOTOR_RTCTL_STATE_RUNNING;
	chMtxUnlock();
	return ret;
}
Beispiel #5
0
enum motor_rtctl_forced_rotation motor_rtctl_get_forced_rotation_state(void)
{
    if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
        return motor_forced_rotation_detector_get_state();
    } else {
        return MOTOR_RTCTL_FORCED_ROT_NONE;
    }
}
Beispiel #6
0
uint32_t motor_rtctl_get_comm_period_hnsec(void)
{
    if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
        return 0;
    }

    const uint32_t val = _state.comm_period;
    return val;
}
Beispiel #7
0
void motor_beep(int frequency, int duration_msec)
{
	chMtxLock(&_mutex);

	if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
		_state.beep_frequency = frequency;
		_state.beep_duration_msec = duration_msec;
		chMtxUnlock();
		chEvtBroadcastFlags(&_setpoint_update_event, ALL_EVENTS); // Wake the control thread
	} else {
		chMtxUnlock();
	}
}
Beispiel #8
0
static void update_setpoint_ttl(int dt_ms)
{
	const enum motor_rtctl_state rtctl_state = motor_rtctl_get_state();
	if (_state.setpoint_ttl_ms <= 0 || rtctl_state != MOTOR_RTCTL_STATE_RUNNING) {
		return;
	}

	_state.setpoint_ttl_ms -= dt_ms;
	if (_state.setpoint_ttl_ms <= 0) {
		stop(true);
		lowsyslog("Motor: Setpoint TTL expired, stop\n");
	}
}
Beispiel #9
0
static void update_control(uint32_t comm_period, float dt)
{
	/*
	 * Start/stop management
	 */
	const enum motor_rtctl_state new_rtctl_state = motor_rtctl_get_state();

	const bool just_stopped =
		new_rtctl_state == MOTOR_RTCTL_STATE_IDLE &&
		_state.rtctl_state != MOTOR_RTCTL_STATE_IDLE;
	if (just_stopped) {
		handle_unexpected_stop();
	}

	_state.rtctl_state = new_rtctl_state;
	if (comm_period == 0 || _state.rtctl_state != MOTOR_RTCTL_STATE_RUNNING) {
		update_control_non_running();
		return;
	}

	/*
	 * Primary control logic; can return NAN to stop the motor
	 */
	float new_duty_cycle = nan("");
	if (_state.mode == MOTOR_CONTROL_MODE_OPENLOOP) {
		new_duty_cycle = update_control_open_loop(comm_period);
	}
	else if (_state.mode == MOTOR_CONTROL_MODE_RPM) {
		new_duty_cycle = update_control_rpm(comm_period, dt);
	}
	else { assert(0); }

	if (!isfinite(new_duty_cycle)) {
		stop(true);
		return;
	}

	/*
	 * Limiters
	 */
	new_duty_cycle = update_control_current_limit(new_duty_cycle);
	new_duty_cycle = update_control_dc_slope(new_duty_cycle, dt);

	/*
	 * Update
	 */
	_state.dc_actual = new_duty_cycle;
	motor_rtctl_set_duty_cycle(_state.dc_actual);
}
Beispiel #10
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();
}
Beispiel #11
0
static void update_filters(float dt)
{
	float voltage = 0, current = 0;
	motor_rtctl_get_input_voltage_current(&voltage, &current);

	if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
		// Current sensor offset calibration, corner frequency is much lower.
		const float offset_tau = _params.voltage_current_lowpass_tau * 100;
		_state.input_curent_offset = lowpass(_state.input_curent_offset, current, offset_tau, dt);
	}

	current -= _state.input_curent_offset;

	_state.input_voltage = lowpass(_state.input_voltage, voltage, _params.voltage_current_lowpass_tau, dt);
	_state.input_current = lowpass(_state.input_current, current, _params.voltage_current_lowpass_tau, dt);
}
int motor_rtctl_test_motor(void)
{
    if (motor_rtctl_get_state() != MOTOR_RTCTL_STATE_IDLE) {
        return -1;
    }

    const int threshold = ((1 << MOTOR_ADC_RESOLUTION) * ANALOG_TOLERANCE_PERCENT) / 100;
    struct motor_adc_sample sample;
    int result = 0;
    enum motor_pwm_phase_manip manip_cmd[MOTOR_NUM_PHASES] = {
        MOTOR_PWM_MANIP_LOW,
        MOTOR_PWM_MANIP_FLOATING,
        MOTOR_PWM_MANIP_FLOATING
    };

    motor_pwm_set_freewheeling();
    /*
     * Test with low level
     */
    manip_cmd[0] = MOTOR_PWM_MANIP_LOW;
    motor_pwm_manip(manip_cmd);
    usleep(SAMPLE_DELAY_MS * 1000);
    sample = motor_adc_get_last_sample();

    if (sample.phase_voltage_raw[1] > threshold || sample.phase_voltage_raw[2] > threshold) {
        result++;
    }

    /*
     * Test with high level
     */
    manip_cmd[0] = MOTOR_PWM_MANIP_HIGH;
    motor_pwm_manip(manip_cmd);
    usleep(SAMPLE_DELAY_MS * 1000);
    sample = motor_adc_get_last_sample();

    if (abs(sample.phase_voltage_raw[0] - sample.phase_voltage_raw[1]) > threshold) {
        result++;
    }
    if (abs(sample.phase_voltage_raw[0] - sample.phase_voltage_raw[2]) > threshold) {
        result++;
    }

    motor_pwm_set_freewheeling();
    return result;
}
Beispiel #13
0
static void poll_beep(void)
{
	const bool do_beep =
		(_state.beep_frequency > 0) &&
		(_state.beep_duration_msec > 0) &&
		(motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE);

	if (do_beep) {
		if (_state.beep_duration_msec > MAX_BEEP_DURATION_MSEC) {
			_state.beep_duration_msec = MAX_BEEP_DURATION_MSEC;
		}
		motor_rtctl_beep(_state.beep_frequency, _state.beep_duration_msec);
	}

	_state.beep_frequency = 0;
	_state.beep_duration_msec = 0;
}
Beispiel #14
0
void motor_rtctl_get_input_voltage_current(float* out_voltage, float* out_current)
{
	int volt = 0, curr = 0;

	if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
		const struct motor_adc_sample smpl = motor_adc_get_last_sample();
		volt = smpl.input_voltage;
		curr = smpl.input_current;
	} else {
		volt = _state.input_voltage;
		curr = _state.input_current;
	}

	if (out_voltage) {
		*out_voltage = motor_adc_convert_input_voltage(volt);
	}
	if (out_current) {
		*out_current = motor_adc_convert_input_current(curr);
	}
}