/**
 * Sets high/low levels on the output FETs, reads ADC samples making sure that there are proper corellations.
 */
static int test_power_stage(void)
{
    int result = 0;
    int high_samples[MOTOR_NUM_PHASES];
    memset(high_samples, 0, sizeof(high_samples));

    const int threshold = ((1 << MOTOR_ADC_RESOLUTION) * ANALOG_TOLERANCE_PERCENT) / 100;

    motor_pwm_set_freewheeling();

    /*
     * Test phases at low level; collect high level readings
     */
    for (int phase = 0; phase < MOTOR_NUM_PHASES; phase++) {
        // Low level
        const int low = test_one_phase(phase, false);
        if (low > threshold) {
            lowsyslog("Motor: Selftest FAILURE at phase %i: low sample %i is above threshold %i\n",
                      phase, low, threshold);
            result++;
        }

        // High level
        const int high = test_one_phase(phase, true);
        high_samples[phase] = high;
        if (high < threshold) {
            lowsyslog("Motor: Selftest FAILURE at phase %i: high sample %i is below threshold %i\n",
                      phase, high, threshold);
            result++;
        }
        // It is not possible to check against the high threshold directly
        // because its value will depend on the supply voltage

        lowsyslog("Motor: Selftest phase %i: low %i, high %i\n", phase, low, high);
    }

    /*
     * Make sure that the high level readings are nearly identical
     */
    int high_samples_sorted[MOTOR_NUM_PHASES];
    memcpy(high_samples_sorted, high_samples, sizeof(high_samples));
    qsort(high_samples_sorted, MOTOR_NUM_PHASES, sizeof(int), compare_samples);
    const int high_median = high_samples_sorted[MOTOR_NUM_PHASES / 2];

    for (int phase = 0; phase < MOTOR_NUM_PHASES; phase++) {
        if (abs(high_samples[phase] - high_median) > threshold) {
            lowsyslog("Motor: Selftest FAILURE at phase %i: sample %i is far from median %i\n",
                      phase, high_samples[phase], high_median);
            result++;
        }
    }

    motor_pwm_set_freewheeling();
    return result;
}
Beispiel #2
0
static void init_adc_filters(void)
{
    struct motor_adc_sample smpl;
    enum motor_pwm_phase_manip manip_cmd[3];

    // Low phase
    for (int i = 0 ; i < MOTOR_NUM_PHASES; i++) {
        manip_cmd[i] = MOTOR_PWM_MANIP_LOW;
    }
    motor_pwm_manip(manip_cmd);
    smpl = motor_adc_get_last_sample();
    const int low = (smpl.phase_voltage_raw[0] + smpl.phase_voltage_raw[1] + smpl.phase_voltage_raw[2]) / 3;

    // High phase
    for (int i = 0 ; i < MOTOR_NUM_PHASES; i++) {
        manip_cmd[i] = MOTOR_PWM_MANIP_HIGH;
    }
    motor_pwm_manip(manip_cmd);
    smpl = motor_adc_get_last_sample();
    const int high = (smpl.phase_voltage_raw[0] + smpl.phase_voltage_raw[1] + smpl.phase_voltage_raw[2]) / 3;

    // Phase neutral
    motor_pwm_set_freewheeling();
    _state.neutral_voltage = (low + high) / 2;

    // Supply voltage and current
    _state.input_voltage_raw = smpl.input_voltage_raw;
    _state.temperature_raw = smpl.temperature_raw;
}
static int test_sensors(void)
{
    /*
     * Enable the synchronous PWM on all phases, obtain a sample and disable PWM again
     */
    const enum motor_pwm_phase_manip manip_cmd[MOTOR_NUM_PHASES] = {
        MOTOR_PWM_MANIP_HALF,
        MOTOR_PWM_MANIP_HALF,
        MOTOR_PWM_MANIP_HALF
    };
    motor_pwm_manip(manip_cmd);
    usleep(INITIAL_DELAY_MS * 1000);

    const struct motor_adc_sample sample = motor_adc_get_last_sample();

    motor_pwm_set_freewheeling();

    /*
     * Validate the obtained sample
     */
    const bool valid_voltage     = (sample.input_voltage_raw > 0) && (sample.input_voltage_raw < MOTOR_ADC_SAMPLE_MAX);
    const bool valid_temperature = (sample.temperature_raw > 0) && (sample.temperature_raw < MOTOR_ADC_SAMPLE_MAX);

    if (!valid_voltage || !valid_temperature) {
        lowsyslog("Motor: Invalid sensor readings: raw input voltage %i, raw temperature %i\n",
                  sample.input_voltage_raw, sample.temperature_raw);
        return 1;
    }

    lowsyslog("Motor: Raw phase voltages: %i %i %i\n",
              sample.phase_voltage_raw[0], sample.phase_voltage_raw[1], sample.phase_voltage_raw[2]);
    lowsyslog("Motor: Raw phase currents: %i %i\n", sample.phase_current_raw[0], sample.phase_current_raw[1]);
    lowsyslog("Motor: Raw input voltage, temperature: %i %i\n", sample.input_voltage_raw, sample.temperature_raw);
    return 0;
}
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;
}
/**
 * Detects cross-phase short circuit
 */
static int test_cross_phase_conductivity(void)
{
    int num_detects = 0;

    for (int phase = 0; phase < MOTOR_NUM_PHASES; phase++) {
        // Apply the high level voltage to the selected phase
        enum motor_pwm_phase_manip manip_cmd[MOTOR_NUM_PHASES] = {
            MOTOR_PWM_MANIP_FLOATING,
            MOTOR_PWM_MANIP_FLOATING,
            MOTOR_PWM_MANIP_FLOATING
        };
        manip_cmd[phase] = MOTOR_PWM_MANIP_HIGH;

        motor_pwm_set_freewheeling();
        usleep(SAMPLE_DELAY_MS * 1000);
        motor_pwm_manip(manip_cmd);
        usleep(SAMPLE_DELAY_MS * 1000);
        motor_pwm_set_freewheeling();

        // Read back the full ADC sample
        const struct motor_adc_sample sample = motor_adc_get_last_sample();

        // Make sure only one phase is at the high level; other must be at low level
        const int low_first  = (phase + 1) % MOTOR_NUM_PHASES;
        const int low_second = (phase + 2) % MOTOR_NUM_PHASES;
        assert((low_first != phase) && (low_second != phase) && (low_first != low_second));

        const int low_sum = sample.phase_voltage_raw[low_first] + sample.phase_voltage_raw[low_second];

        const bool valid = (low_sum * 2) < sample.phase_voltage_raw[phase];

        if (!valid) {
            num_detects++;
            lowsyslog("Motor: Phase %i cross conductivity: %i %i %i\n", phase,
                      sample.phase_voltage_raw[0], sample.phase_voltage_raw[1], sample.phase_voltage_raw[2]);
        }
    }

    motor_pwm_set_freewheeling();

    if (num_detects == MOTOR_NUM_PHASES) {
        lowsyslog("Motor: All phases are shorted, assuming the motor is connected\n");
        num_detects = 0;
    }

    return num_detects;
}
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 #7
0
void motor_rtctl_stop(void)
{
    _state.flags = 0;
    motor_timer_cancel();
    _state.flags = 0;

    irq_primask_disable();
    motor_adc_enable_from_isr(); // ADC should be enabled by default
    irq_primask_enable();

    motor_pwm_set_freewheeling();
}
Beispiel #8
0
void motor_pwm_prepare_to_start(void)
{
	// High side drivers cap precharge
	const enum motor_pwm_phase_manip cmd[3] = {
		MOTOR_PWM_MANIP_LOW,
		MOTOR_PWM_MANIP_LOW,
		MOTOR_PWM_MANIP_LOW
	};
	motor_pwm_manip(cmd);
	usleep(1000);
	motor_pwm_set_freewheeling();
	usleep(10000);
}
Beispiel #9
0
int motor_pwm_init(void)
{
	const int ret = init_constants(config_get("mot_pwm_hz"));
	if (ret) {
		return ret;
	}

	init_timers();
	start_timers();

	motor_pwm_set_freewheeling();
	return 0;
}
Beispiel #10
0
void motor_rtctl_execute_cli_command(int argc, const char* argv[])
{
	if (argc < 0 || argv == NULL) {
		printf("Invalid args\n");
		return;
	}

	const struct motor_adc_sample adc_sample = motor_adc_get_last_sample();

	printf("ADC raw phases:  %i  %i  %i\n",
		adc_sample.phase_values[0], adc_sample.phase_values[1], adc_sample.phase_values[2]);

	printf("ADC raw vtg/cur: V=%i  I=%i\n", adc_sample.input_voltage, adc_sample.input_current);

	if ((argc > 0) && !strcmp("enrg", argv[0])) {
		if (argc != 4) {
			printf("Invalid num args\n");
			return;
		}
		const int polarity[MOTOR_NUM_PHASES] = {
			atoi(argv[1]),
			atoi(argv[2]),
			atoi(argv[3])
		};
		printf("%i %i %i\n", polarity[0], polarity[1], polarity[2]);
		motor_pwm_energize(polarity);
	} else if ((argc >= 1) && (argc <= 3)) {
		const enum motor_pwm_phase_manip manip_cmd[MOTOR_NUM_PHASES] = {
			arg_to_pwm_manip(argv[0]),
			(argc > 1) ? arg_to_pwm_manip(argv[1]) : MOTOR_PWM_MANIP_FLOATING,
			(argc > 2) ? arg_to_pwm_manip(argv[2]) : MOTOR_PWM_MANIP_FLOATING
		};
		printf("Manip %i %i %i\n", (int)manip_cmd[0], (int)manip_cmd[1], (int)manip_cmd[2]);
		motor_pwm_manip(manip_cmd);
	} else {
		motor_pwm_set_freewheeling();
		printf("Freewheeling\n");
	}
}
Beispiel #11
0
void motor_adc_sample_callback(const struct motor_adc_sample* sample)
{
    const bool proceed =
        ((_state.flags & FLAG_ACTIVE) != 0) &&
        (_state.zc_detection_result == ZC_NOT_DETECTED) &&
        (sample->timestamp >= _state.blank_time_deadline);

    if (!proceed) {
        if ((_state.flags & FLAG_ACTIVE) == 0) {
            motor_forced_rotation_detector_update_from_adc_callback(COMMUTATION_TABLE_FORWARD, sample);
            // Params update - current is forced to zero
            update_input_voltage_current_temperature(sample->input_voltage_raw,
                    0, sample->temperature_raw);
        }
        return;
    }
    assert(_state.comm_table);

    /*
     * Normalization against the neutral voltage
     */
    update_neutral_voltage(sample);
    const struct motor_pwm_commutation_step* const step = _state.comm_table + _state.current_comm_step;
    const int bemf = sample->phase_voltage_raw[step->floating] - _state.neutral_voltage;

    const bool past_zc = is_past_zc(bemf);

    /*
     * BEMF/ZC validation
     */
    const int bemf_threshold = _state.neutral_voltage * _params.bemf_valid_range_pct128 / 128;
    if (!past_zc && (abs(bemf) > bemf_threshold)) {
        _diag.bemf_samples_out_of_range++;
        _state.zc_bemf_samples_acquired = 0;
        _state.zc_bemf_samples_acquired_past_zc = 0;
        return;
    }

    if (past_zc && (_state.zc_bemf_samples_acquired == 0) && !(_state.flags & FLAG_SPINUP)) {
        _diag.bemf_samples_premature_zc++;
        /*
         * BEMF signal may be affected by extreme saturation, which we can detect
         * as BEMF readings on the wrong side of neutral voltage past 30 degrees since
         * the last commutation. In this case we simply turn off the light and freewheel
         * towards the next scheduled commutation.
         * Usually, saturation occurs under low RPM with very high duty cycle
         * (because of large W * sec), as in case of rapid acceleration or high constant load.
         * In both cases the powerskipping naturally keeps the power within acceptable range for
         * the given motor.
         */
        const uint64_t deadline =
            _state.prev_zc_timestamp + _state.comm_period - _params.adc_sampling_period / 2;
        if (sample->timestamp >= deadline) {
            motor_pwm_set_freewheeling();
            _state.zc_detection_result = ZC_DESATURATION;
            _diag.desaturations++;
        }
        return;
    }

    /*
     * Here the BEMF sample is considered to be valid, and can be added to the solution.
     * Input voltage/current/temperature updates are synced with ZC - this way the noise can be reduced.
     * Note that update is always skipped when phase C is at low because it lacks a current sensor.
     */
    add_bemf_sample(bemf, sample->timestamp);

    if (past_zc && _state.comm_table[_state.current_comm_step].negative < 2) {
        const int current = -sample->phase_current_raw[_state.comm_table[_state.current_comm_step].negative];
        update_input_voltage_current_temperature(sample->input_voltage_raw, current, sample->temperature_raw);
    }

    /*
     * Is there enough samples collected?
     */
    bool enough_samples = false;
    if (past_zc) {
        _state.zc_bemf_samples_acquired_past_zc++;

        enough_samples =
            (_state.zc_bemf_samples_acquired_past_zc >= _state.zc_bemf_samples_optimal_past_zc) &&
            (_state.zc_bemf_samples_acquired > 1);

        if (step->floating == 0) {
            TESTPAD_ZC_SET();
        }
    }

    if (!enough_samples) {
        return;
    }

    /*
     * Find the exact ZC position using the collected samples
     */
    const uint64_t zc_timestamp = solve_zc_approximation();

    TESTPAD_ZC_CLEAR();

    if (zc_timestamp == 0) {
        // Sorry Mario
        motor_pwm_set_freewheeling();
        _state.zc_detection_result = ZC_FAILED;
        return;
    }
    handle_detected_zc(zc_timestamp);
}
Beispiel #12
0
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();
}
Beispiel #13
0
static void stop_from_isr(void)
{
    _state.flags = 0;
    motor_timer_cancel();
    motor_pwm_set_freewheeling();
}
Beispiel #14
0
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();
}