Control Algorithm for Sensorless FOC control

https://github.com/vedderb/

In firmware documentation mcpwm_foc.c file is containing that. 

But the code is a little bit confused my mind. 

Is there a block diagram?

That makes the more understandable the algorithm.

https://github.com/vedderb/bldc/blob/master/mcpwm_foc.c

Watch some of the youtube videos, they explain it somewhat. https://www.youtube.com/user/BenjaminsRobotics

Guys, do you maybe know the function that is used during the FOC detection at the FOC wizard? I have this issue that my motor would run just fine during detection but then never again. I've been playing with the parameters since 3 weeks now and still no chance :/ I would appreciate your help.

This is where I got stuck:

The utility function that the "Run Detection" button from the wizard calls is here:

QString res = Utility::detectAllFoc(mVesc, true,
                                        ui->maxPowerLossBox->value(),
                                        ui->currentInMinBox->value(),
                                        ui->currentInMaxBox->value(),
                                        ui->openloopErpmBox->value(),
                                        ui->sensorlessErpmBox->value());

And this I believe is the one that emits the command to the firmware:

void Commands::detectAllFoc(bool detect_can, double max_power_loss, double min_current_in,
                            double max_current_in, double openloop_rpm, double sl_erpm)
{
    VByteArray vb;
    vb.vbAppendInt8(COMM_DETECT_APPLY_ALL_FOC); 
    vb.vbAppendInt8(detect_can);
    vb.vbAppendDouble32(max_power_loss, 1e3);
    vb.vbAppendDouble32(min_current_in, 1e3);
    vb.vbAppendDouble32(max_current_in, 1e3);
    vb.vbAppendDouble32(openloop_rpm, 1e3);
    vb.vbAppendDouble32(sl_erpm, 1e3);
    emitData(vb);
}

Then on the firmware side I find this function:

void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted)

The wizard starts the detection with resistance measurement but the above function doesn't start with measuring the resistance neither does it call: mcpwm_foc_measure_resistance() 

So I'm confused.

Any help would be appreciated!!!

I think I found it in the conf_general.c:

I'd still appreciate a confirmation if anybody is sure about it!

int conf_general_detect_apply_all_foc(float max_power_loss,
		bool store_mcconf_on_success, bool send_mcconf_on_success) {
	int result = -1;

	mcconf = *mc_interface_get_configuration();
	mcconf_old = mcconf;

	mcconf.motor_type = MOTOR_TYPE_FOC;
	mcconf.foc_sensor_mode = FOC_SENSOR_MODE_SENSORLESS;
	mcconf.foc_f_sw = 10000.0; // Lower f_sw => less dead-time distortion
	mcconf.foc_current_kp = 0.0005;
	mcconf.foc_current_ki = 1.0;
	mcconf.l_current_max = MCCONF_L_CURRENT_MAX;
	mcconf.l_current_min = MCCONF_L_CURRENT_MIN;
	mcconf.l_current_max_scale = MCCONF_L_CURRENT_MAX_SCALE;
	mcconf.l_current_min_scale = MCCONF_L_CURRENT_MIN_SCALE;
	mcconf.l_watt_max = MCCONF_L_WATT_MAX;
	mcconf.l_watt_min = MCCONF_L_WATT_MIN;
	mcconf.l_max_erpm = MCCONF_L_RPM_MAX;
	mcconf.l_min_erpm = MCCONF_L_RPM_MIN;
	mc_interface_set_configuration(&mcconf);

	// Wait maximum 5s for fault code to disappear
	for (int i = 0;i < 500;i++) {
		if (mc_interface_get_fault() == FAULT_CODE_NONE) {
			break;
		}
		chThdSleepMilliseconds(10);
	}

	if (mc_interface_get_fault() != FAULT_CODE_NONE) {
		mc_interface_set_configuration(&mcconf_old);
		return -1;
	}

	// Wait one second for things to get ready after
	// the fault disappears.
	chThdSleepMilliseconds(1000);

	// Disable timeout
	systime_t tout = timeout_get_timeout_msec();
	float tout_c = timeout_get_brake_current();
	timeout_reset();
	timeout_configure(60000, 0.0);

	mc_interface_lock();

	float current_start = mcconf.l_current_max / 50;
	if (current_start < (mcconf.cc_min_current * 1.1)) {
		current_start = mcconf.cc_min_current * 1.1;
	}

	float i_last = 0.0;
	for (float i = current_start;i < mcconf.l_current_max;i *= 1.5) {
		float res_tmp = mcpwm_foc_measure_resistance(i, 5);
		i_last = i;

		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
			timeout_configure(tout, tout_c);
			mc_interface_unlock();
			mc_interface_release_motor();
			mc_interface_set_configuration(&mcconf_old);
			return -11;
		}

		if ((i * i * res_tmp) >= (max_power_loss / 3.0)) {
			break;
		}
	}

	float r = mcpwm_foc_measure_resistance(i_last, 100);
	float l = mcpwm_foc_measure_inductance_current(i_last, 100, 0, 0) * 1e-6;
	float i_max = sqrtf(max_power_loss / r);
	utils_truncate_number(&i_max, HW_LIM_CURRENT);

	// Increase switching frequency for flux linkage measurement
	// as dead-time distortion has less effect at higher modulation.
	// Having a smooth rotation is more important.
	mcconf.foc_f_sw = 20000.0;
	mc_interface_set_configuration(&mcconf);

	float lambda = 0.0;
	int res = conf_general_measure_flux_linkage_openloop(i_max / 2.5, 0.3, 1800, r, &lambda);

	mc_motor_type old_type = mcconf_old.motor_type;
	float old_r = mcconf_old.foc_motor_r;
	float old_l = mcconf_old.foc_motor_l;
	float old_flux_linkage = mcconf_old.foc_motor_flux_linkage;
	float old_kp = mcconf_old.foc_current_kp;
	float old_ki = mcconf_old.foc_current_ki;
	float old_observer_gain = mcconf_old.foc_observer_gain;
	bool old_temp_comp = mcconf_old.foc_temp_comp;
	float old_temp_comp_base_temp = mcconf_old.foc_temp_comp_base_temp;

	if (res) {
		mcconf_old.l_current_max = i_max;
		mcconf_old.l_current_min = -i_max;

		float tc = 1000.0;
		float bw = 1.0 / (tc * 1e-6);
		float kp = l * bw;
		float ki = r * bw;
		float gain = (0.00001 / r) / (lambda * lambda);

		mcconf_old.motor_type = MOTOR_TYPE_FOC;
		mcconf_old.foc_motor_r = r;
		mcconf_old.foc_motor_l = l;
		mcconf_old.foc_motor_flux_linkage = lambda;
		mcconf_old.foc_current_kp = kp;
		mcconf_old.foc_current_ki = ki;
		mcconf_old.foc_observer_gain = gain * 1e6;

		// Temperature compensation
		// Skip temperature compensation for now, as it seems to make
		// things worse on some setups.
//		if (mc_interface_temp_motor_filtered() > 0.0) {
//			mcconf_old.foc_temp_comp = true;
//			mcconf_old.foc_temp_comp_base_temp = mc_interface_temp_motor_filtered();
//		} else {
//			mcconf_old.foc_temp_comp = false;
//		}
	} else {
		result = -10;
	}

	timeout_configure(tout, tout_c);
	mc_interface_unlock();
	mc_interface_release_motor();
	mc_interface_set_configuration(&mcconf_old);

	// Restore initial settings on sensor detection failure
	if (res) {
		// Wait for motor to stop
		chThdSleepMilliseconds(100);
		for (int i = 0;i < 1000;i++) {
			if (fabsf(mc_interface_get_rpm()) > 100.0) {
				chThdSleepMilliseconds(10);
			} else {
				break;
			}
		}

		// This will also store the settings to emulated eeprom and send them to vesc tool
		result = conf_general_autodetect_apply_sensors_foc(i_max / 3.0,
				store_mcconf_on_success, send_mcconf_on_success);
	} else {
		mcconf_old.motor_type = old_type;
		mcconf_old.foc_motor_r = old_r;
		mcconf_old.foc_motor_l = old_l;
		mcconf_old.foc_motor_flux_linkage = old_flux_linkage;
		mcconf_old.foc_current_kp = old_kp;
		mcconf_old.foc_current_ki = old_ki;
		mcconf_old.foc_observer_gain = old_observer_gain;
		mcconf_old.foc_temp_comp = old_temp_comp;
		mcconf_old.foc_temp_comp_base_temp = old_temp_comp_base_temp;
		mc_interface_set_configuration(&mcconf_old);
	}

	return result;
}