/**
* 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;
}
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;
}
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();
}
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);
}
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;
}
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");
}
}
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);
}
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();
}
static void stop_from_isr(void)
{
_state.flags = 0;
motor_timer_cancel();
motor_pwm_set_freewheeling();
}
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();
}
