mcpwm2.c

/*
	Copyright 2016 Benjamin Vedder	benjamin@vedder.se

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include "ch.h"
#include "hal.h"
#include "stm32f4xx_conf.h"
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "mcpwm.h"
#include "mcpwm2.h"
#include "mc_interface.h"
#include "digital_filter.h"
#include "utils.h"
#include "ledpwm.h"
#include "terminal.h"
#include "encoder.h"

// Structs
typedef struct {
	volatile bool updated;
	volatile unsigned int top;
	volatile unsigned int duty;
	volatile unsigned int val_sample;
	volatile unsigned int curr1_sample;
	volatile unsigned int curr2_sample;
#ifdef HW_HAS_3_SHUNTS
	volatile unsigned int curr3_sample;
#endif
} mc_timer_struct;

// Private variables
static volatile int comm_step; // Range [1 6]
static volatile int detect_step; // Range [0 5]
static volatile int direction;
static volatile float dutycycle_set;
static volatile float dutycycle_now;
static volatile float rpm_now;
static volatile float speed_pid_set_rpm;
static volatile float pos_pid_set_pos;
static volatile float current_set;
static volatile int tachometer;
static volatile int tachometer_abs;
static volatile int tachometer_for_direction;
static volatile int curr0_sum;
static volatile int curr1_sum;
static volatile int curr_start_samples;
static volatile int curr0_offset;
static volatile int curr1_offset;
static volatile mc_state state;
static volatile mc_control_mode control_mode;
static volatile float last_current_sample;
static volatile float last_current_sample_filtered;
static volatile float mcpwm_detect_currents_avg[6];
static volatile float mcpwm_detect_avg_samples[6];
static volatile float switching_frequency_now;
static volatile int ignore_iterations;
static volatile mc_timer_struct timer_struct;
static volatile int curr_samp_volt; // Use the voltage-synchronized samples for this current sample
static int hall_to_phase_table[16];
static volatile unsigned int slow_ramping_cycles;
static volatile int has_commutated;
static volatile mc_rpm_dep_struct rpm_dep2;
static volatile float cycle_integrator_sum;
static volatile float cycle_integrator_iterations;
static volatile mc_configuration *conf;
static volatile float pwm_cycles_sum;
static volatile int pwm_cycles;
static volatile float last_pwm_cycles_sum;
static volatile float last_pwm_cycles_sums[6];
static volatile bool dccal_done;
static volatile bool sensorless_now;
static volatile int hall_detect_table[8][7];
static volatile bool init_done;
static volatile mc_comm_mode comm_mode_next;

#ifdef HW_HAS_3_SHUNTS
static volatile int curr2_sum;
static volatile int curr2_offset;
#endif

// KV FIR filter
#define KV_FIR_TAPS_BITS		7
#define KV_FIR_LEN				(1 << KV_FIR_TAPS_BITS)
#define KV_FIR_FCUT				0.02
static volatile float kv_fir_coeffs[KV_FIR_LEN];
static volatile float kv_fir_samples[KV_FIR_LEN];
static volatile int kv_fir_index = 0;

// Amplitude FIR filter
#define AMP_FIR_TAPS_BITS		7
#define AMP_FIR_LEN				(1 << AMP_FIR_TAPS_BITS)
#define AMP_FIR_FCUT			0.02
static volatile float amp_fir_coeffs[AMP_FIR_LEN];
static volatile float amp_fir_samples[AMP_FIR_LEN];
static volatile int amp_fir_index = 0;

// Current FIR filter
#define CURR_FIR_TAPS_BITS		4
#define CURR_FIR_LEN			(1 << CURR_FIR_TAPS_BITS)
#define CURR_FIR_FCUT			0.15
static volatile float current_fir_coeffs[CURR_FIR_LEN];
static volatile float current_fir_samples[CURR_FIR_LEN];
static volatile int current_fir_index = 0;

static volatile float last_adc_isr_duration;
static volatile float last_inj_adc_isr_duration;

// Global variables
volatile float mcpwm_detect_currents2[6];
volatile float mcpwm_detect_voltages2[6];
volatile float mcpwm_detect_currents_diff2[6];
volatile int mcpwm_vzero2;

// Private functions
static void set_duty_cycle_hl(float dutyCycle);
static void set_duty_cycle_ll(float dutyCycle);
static void set_duty_cycle_hw(float dutyCycle);
static void stop_pwm_ll(void);
static void stop_pwm_hw(void);
static void full_brake_ll(void);
static void full_brake_hw(void);
static void run_pid_control_speed(void);
static void run_pid_control_pos(float dt);
static void set_next_comm_step(int next_step);
static void update_rpm_tacho2(void);
static void update_sensor_mode(void);
static int read_hall(void);
//static void update_adc_sample_pos(mc_timer_struct *timer_tmp);
static void commutate(int steps);
static void set_next_timer_settings(mc_timer_struct *settings);
//static void update_timer_attempt(void);
//static void set_switching_frequency(float frequency);


// Defines
#define IS_DETECTING()			(state == MC_STATE_DETECTING)

// Threads
static THD_WORKING_AREA(timer_thread2_wa, 2048);
static THD_FUNCTION(timer_thread2, arg);
static THD_WORKING_AREA(rpm_thread2_wa, 1024);
static THD_FUNCTION(rpm_thread2, arg);
static volatile bool timer_thd_stop;
static volatile bool rpm_thd_stop;

void mcpwm_init2(volatile mc_configuration *configuration) {
	utils_sys_lock_cnt();

	init_done= false;

	//TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	//TIM_OCInitTypeDef  TIM_OCInitStructure;
	//TIM_BDTRInitTypeDef TIM_BDTRInitStructure;

	conf = configuration;

	// Initialize variables
	comm_step = 1;
	detect_step = 0;
	direction = 1;
	rpm_now = 0.0;
	dutycycle_set = 0.0;
	dutycycle_now = 0.0;
	speed_pid_set_rpm = 0.0;
	pos_pid_set_pos = 0.0;
	current_set = 0.0;
	tachometer = 0;
	tachometer_abs = 0;
	tachometer_for_direction = 0;
	state = MC_STATE_OFF;
	control_mode = CONTROL_MODE_NONE;
	last_current_sample = 0.0;
	last_current_sample_filtered = 0.0;
	switching_frequency_now = conf->m_bldc_f_sw_max;
	ignore_iterations = 0;
	curr_samp_volt = 0;
	slow_ramping_cycles = 0;
	has_commutated = 0;
	memset((void*)&rpm_dep2, 0, sizeof(rpm_dep2));
	cycle_integrator_sum = 0.0;
	cycle_integrator_iterations = 0.0;
	pwm_cycles_sum = 0.0;
	pwm_cycles = 0;
	last_pwm_cycles_sum = 0.0;
	memset((float*)last_pwm_cycles_sums, 0, sizeof(last_pwm_cycles_sums));
	dccal_done = false;
	memset((void*)hall_detect_table, 0, sizeof(hall_detect_table[0][0]) * 8 * 7);
	update_sensor_mode();
	comm_mode_next = conf->comm_mode;

	mcpwm_init_hall_table2((int8_t*)conf->hall_table);

	// Create KV FIR filter
	filter_create_fir_lowpass((float*)kv_fir_coeffs, KV_FIR_FCUT, KV_FIR_TAPS_BITS, 1);

	// Create amplitude FIR filter
	filter_create_fir_lowpass((float*)amp_fir_coeffs, AMP_FIR_FCUT, AMP_FIR_TAPS_BITS, 1);

	// Create current FIR filter
	filter_create_fir_lowpass((float*)current_fir_coeffs, CURR_FIR_FCUT, CURR_FIR_TAPS_BITS, 1);

    stop_pwm_hw();
    mc_timer_struct timer_tmp;
    timer_tmp.top = TIM8->ARR;
    timer_tmp.duty = TIM8->ARR / 2;
    //TIM2->CCR2 = 5;
    //update_adc_sample_pos(&timer_tmp);
    set_next_timer_settings(&timer_tmp);
    utils_sys_unlock_cnt();
	// Reset tachometers again
    timer_thd_stop = false;
    rpm_thd_stop = false;
    chThdCreateStatic(timer_thread2_wa, sizeof(timer_thread2_wa), NORMALPRIO, timer_thread2, NULL);
    chThdCreateStatic(rpm_thread2_wa, sizeof(rpm_thread2_wa), NORMALPRIO, rpm_thread2, NULL);

	tachometer = 0;
	tachometer_abs = 0;

	init_done = true;
}

void mcpwm_deinit2(void) {
	init_done = false;
	timer_thd_stop = true;
	rpm_thd_stop = true;

	while (timer_thd_stop || rpm_thd_stop) {
		chThdSleepMilliseconds(1);
	}
}

bool mcpwm_init_done2(void) {
	return init_done;
}

void mcpwm_set_configuration2(volatile mc_configuration *configuration) {
	// Stop everything first to be safe
	control_mode = CONTROL_MODE_NONE;
	stop_pwm_ll();

	utils_sys_lock_cnt();
	conf = configuration;
	comm_mode_next = conf->comm_mode;
	mcpwm_init_hall_table2((int8_t*)conf->hall_table);
	update_sensor_mode();
	utils_sys_unlock_cnt();
}

/**
 * Initialize the hall sensor lookup table
 *
 * @param table
 * The commutations corresponding to the hall sensor states in the forward direction-
 */
void mcpwm_init_hall_table2(int8_t *table) {
	const int fwd_to_rev[7] = {-1,1,6,5,4,3,2};

	for (int i = 0;i < 8;i++) {
		hall_to_phase_table[8 + i] = table[i];
		int ind_now = hall_to_phase_table[8 + i];

		if (ind_now < 1) {
			hall_to_phase_table[i] = ind_now;
			continue;
		}

		hall_to_phase_table[i] = fwd_to_rev[ind_now];
	}
}


/**
 * Use duty cycle control. Absolute values less than MCPWM_MIN_DUTY_CYCLE will
 * stop the motor.
 *
 * @param dutyCycle
 * The duty cycle to use.
 */
void mcpwm_set_duty2(float dutyCycle) {
	control_mode = CONTROL_MODE_DUTY;
	set_duty_cycle_hl(dutyCycle);
}

/**
 * Use duty cycle control. Absolute values less than MCPWM_MIN_DUTY_CYCLE will
 * stop the motor.
 *
 * WARNING: This function does not use ramping. A too large step with a large motor
 * can destroy hardware.
 *
 * @param dutyCycle
 * The duty cycle to use.
 */
void mcpwm_set_duty_noramp2(float dutyCycle) {
	control_mode = CONTROL_MODE_DUTY;

	if (state != MC_STATE_RUNNING) {
		set_duty_cycle_hl(dutyCycle);
	} else {
		dutycycle_set = dutyCycle;
		dutycycle_now = dutyCycle;
		set_duty_cycle_ll(dutyCycle);
	}
}


/**
 * Use PID rpm control. Note that this value has to be multiplied by half of
 * the number of motor poles.
 *
 * @param rpm
 * The electrical RPM goal value to use.
 */
void mcpwm_set_pid_speed2(float rpm) {
	control_mode = CONTROL_MODE_SPEED;
	speed_pid_set_rpm = rpm;
}

/**
 * Use PID position control. Note that this only works when encoder support
 * is enabled.
 *
 * @param pos
 * The desired position of the motor in degrees.
 */
void mcpwm_set_pid_pos2(float pos) {
	control_mode = CONTROL_MODE_POS;
	pos_pid_set_pos = pos;

	if (state != MC_STATE_RUNNING) {
		set_duty_cycle_hl(conf->l_min_duty);
	}
}

/**
 * Use current control and specify a goal current to use. The sign determines
 * the direction of the torque. Absolute values less than
 * conf->cc_min_current will release the motor.
 *
 * @param current
 * The current to use.
 */
void mcpwm_set_current2(float current) {
	if (fabsf(current) < conf->cc_min_current) {
		control_mode = CONTROL_MODE_NONE;
		stop_pwm_ll();
		return;
	}

	utils_truncate_number(&current, -conf->l_current_max, conf->l_current_max);

	control_mode = CONTROL_MODE_CURRENT;
	current_set = current;

	if (state != MC_STATE_RUNNING) {
		set_duty_cycle_hl(SIGN(current) * conf->l_min_duty);
	}
}
void mcpwm_set_current_dccal2(int off0, int off1) {
  curr0_offset = off0;
  curr1_offset = off1;
  dccal_done = true;
}

/**
 * Brake the motor with a desired current. Absolute values less than
 * conf->cc_min_current will release the motor.
 *7122
 * @param current
 * The current to use. Positive and negative values give the same effect.
 */
void mcpwm_set_brake_current2(float current) {
	if (fabsf(current) < conf->cc_min_current) {
		control_mode = CONTROL_MODE_NONE;
		stop_pwm_ll();
		return;
	}

	utils_truncate_number(&current, -fabsf(conf->l_current_min), fabsf(conf->l_current_min));

	control_mode = CONTROL_MODE_CURRENT_BRAKE;
	current_set = current;

	if (state != MC_STATE_RUNNING && state != MC_STATE_FULL_BRAKE) {
		// In case the motor is already spinning, set the state to running
		// so that it can be ramped down before the full brake is applied.
		if (conf->motor_type == MOTOR_TYPE_DC) {
			if (fabsf(dutycycle_now) > 0.1) {
				state = MC_STATE_RUNNING;
			} else {
				full_brake_ll();
			}
		} else {
			if (fabsf(rpm_now) > conf->l_max_erpm_fbrake) {
				state = MC_STATE_RUNNING;
			} else {
				full_brake_ll();
			}
		}
	}
}

/**
 * Get the electrical position (or commutation step) of the motor.
 *
 * @return
 * The current commutation step. Range [1 6]
 */
int mcpwm_get_comm_step2(void) {
	return comm_step;
}

float mcpwm_get_duty_cycle_set2(void) {
	return dutycycle_set;
}

float mcpwm_get_duty_cycle_now2(void) {
	return dutycycle_now;
}

/**
 * Get the current switching frequency.
 *
 * @return
 * The switching frequency in Hz.
 */
float mcpwm_get_switching_frequency_now2(void) {
	return switching_frequency_now;
}

/**
 * Calculate the current RPM of the motor. This is a signed value and the sign
 * depends on the direction the motor is rotating in. Note that this value has
 * to be divided by half the number of motor poles.
 *
 * @return
 * The RPM value.
 */
float mcpwm_get_rpm2(void) {
	return direction ? rpm_now : -rpm_now;
}

mc_state mcpwm_get_state2(void) {
	return state;
}

/**
 * Calculate the KV (RPM per volt) value for the motor. This function has to
 * be used while the motor is moving. Note that the return value has to be
 * divided by half the number of motor poles.
 *
 * @return
 * The KV value.
 */
float mcpwm_get_kv2(void) {
	return rpm_now / (GET_INPUT_VOLTAGE() * fabsf(dutycycle_now));
}

/**
 * Calculate the FIR-filtered KV (RPM per volt) value for the motor. This
 * function has to be used while the motor is moving. Note that the return
 * value has to be divided by half the number of motor poles.
 *
 * @return
 * The filtered KV value.
 */
float mcpwm_get_kv_filtered2(void) {
	float value = filter_run_fir_iteration((float*)kv_fir_samples,
			(float*)kv_fir_coeffs, KV_FIR_TAPS_BITS, kv_fir_index);

	return value;
}

/**
 * Get the motor current. The sign of this value will
 * represent whether the motor is drawing (positive) or generating
 * (negative) current.
 *
 * @return
 * The motor current.
 */
float mcpwm_get_tot_current2(void) {
	return last_current_sample;
}

/**
 * Get the FIR-filtered motor current. The sign of this value will
 * represent whether the motor is drawing (positive) or generating
 * (negative) current.
 *
 * @return
 * The filtered motor current.
 */
float mcpwm_get_tot_current_filtered2(void) {
	return last_current_sample_filtered;
}

/**
 * Get the motor current. The sign of this value represents the direction
 * in which the motor generates torque.
 *
 * @return
 * The motor current.
 */
float mcpwm_get_tot_current_directional2(void) {
	const float retval = mcpwm_get_tot_current2();
	return dutycycle_now > 0.0 ? retval : -retval;
}

/**
 * Get the filtered motor current. The sign of this value represents the
 * direction in which the motor generates torque.
 *
 * @return
 * The filtered motor current.
 */
float mcpwm_get_tot_current_directional_filtered2(void) {
	const float retval = mcpwm_get_tot_current_filtered2();
	return dutycycle_now > 0.0 ? retval : -retval;
}

/**
 * Get the input current to the motor controller.
 *
 * @return
 * The input current.
 */
float mcpwm_get_tot_current_in2(void) {
	return mcpwm_get_tot_current2() * fabsf(dutycycle_now);
}

/**
 * Get the FIR-filtered input current to the motor controller.
 *
 * @return
 * The filtered input current.
 */
float mcpwm_get_tot_current_in_filtered2(void) {
	return mcpwm_get_tot_current_filtered2() * fabsf(dutycycle_now);
}

/**
 * Read the number of steps the motor has rotated. This number is signed and
 * will return a negative number when the motor is rotating backwards.
 *
 * @param reset
 * If true, the tachometer counter will be reset after this call.
 *
 * @return
 * The tachometer value in motor steps. The number of motor revolutions will
 * be this number divided by (3 * MOTOR_POLE_NUMBER).
 */
int mcpwm_get_tachometer_value2(bool reset) {
	int val = tachometer;

	if (reset) {
		tachometer = 0;
	}

	return val;
}

/**
 * Read the absolute number of steps the motor has rotated.
 *
 * @param reset
 * If true, the tachometer counter will be reset after this call.
 *
 * @return
 * The tachometer value in motor steps. The number of motor revolutions will
 * be this number divided by (3 * MOTOR_POLE_NUMBER).
 */
int mcpwm_get_tachometer_abs_value2(bool reset) {
	int val = tachometer_abs;

	if (reset) {
		tachometer_abs = 0;
	}

	return val;
}

/**
 * Switch off all FETs.
 */
void mcpwm_stop_pwm2(void) {
	control_mode = CONTROL_MODE_NONE;
	stop_pwm_ll();
}

static void stop_pwm_ll(void) {
	state = MC_STATE_OFF;
	ignore_iterations = MCPWM_CMD_STOP_TIME;
	stop_pwm_hw();
}

static void stop_pwm_hw(void) {
#ifdef HW_HAS_DRV8313
	DISABLE_BR();
#endif

	TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_ForcedAction_InActive);
	TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
	TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Disable);

	TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_ForcedAction_InActive);
	TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
	TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Disable);

	TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_ForcedAction_InActive);
	TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
	TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Disable);

	TIM_GenerateEvent(TIM8, TIM_EventSource_COM);

	//set_switching_frequency(conf->m_bldc_f_sw_max);
}

static void full_brake_ll(void) {
	state = MC_STATE_FULL_BRAKE;
	ignore_iterations = MCPWM_CMD_STOP_TIME;
	full_brake_hw();
}

static void full_brake_hw(void) {
#ifdef HW_HAS_DRV8313
	ENABLE_BR();
#endif

	TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_ForcedAction_InActive);
	TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
	TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Enable);

	TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_ForcedAction_InActive);
	TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
	TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Enable);

	TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_ForcedAction_InActive);
	TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
	TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Enable);

	TIM_GenerateEvent(TIM8, TIM_EventSource_COM);

	//set_switching_frequency(conf->m_bldc_f_sw_max);
}

/**
 * High-level duty cycle setter. Will set the ramping goal of the duty cycle.
 * If motor is not running, it will be started in different ways depending on
 * whether it is moving or not.
 *
 * @param dutyCycle
 * The duty cycle in the range [-MCPWM_MAX_DUTY_CYCLE MCPWM_MAX_DUTY_CYCLE]
 * If the absolute value of the duty cycle is less than MCPWM_MIN_DUTY_CYCLE,
 * the motor phases will be shorted to brake the motor.
 */
static void set_duty_cycle_hl(float dutyCycle) {
	utils_truncate_number(&dutyCycle, -conf->l_max_duty, conf->l_max_duty);

	if (state == MC_STATE_DETECTING) {
		stop_pwm_ll();
		return;
	}

	dutycycle_set = dutyCycle;

	if (state != MC_STATE_RUNNING) {
		if (fabsf(dutyCycle) >= conf->l_min_duty) {
			// dutycycle_now is updated by the back-emf detection. If the motor already
			// is spinning, it will be non-zero.
			if (fabsf(dutycycle_now) < conf->l_min_duty) {
				dutycycle_now = SIGN(dutyCycle) * conf->l_min_duty;
			}

			set_duty_cycle_ll(dutycycle_now);
		} else {
			// In case the motor is already spinning, set the state to running
			// so that it can be ramped down before the full brake is applied.
			if (conf->motor_type == MOTOR_TYPE_DC) {
				if (fabsf(dutycycle_now) > 0.1) {
					state = MC_STATE_RUNNING;
				} else {
					full_brake_ll();
				}
			} else {
				if (fabsf(rpm_now) > conf->l_max_erpm_fbrake) {
					state = MC_STATE_RUNNING;
				} else {
					full_brake_ll();
				}
			}
		}
	}
}

/**
 * Low-level duty cycle setter. Will update the state of the application
 * and the motor direction accordingly.
 *
 * This function should be used with care. Ramping together with current
 * limiting should be used.
 *
 * @param dutyCycle
 * The duty cycle in the range [-MCPWM_MAX_DUTY_CYCLE MCPWM_MAX_DUTY_CYCLE]
 * If the absolute value of the duty cycle is less than MCPWM_MIN_DUTY_CYCLE,
 * the motor will be switched off.
 */
static void set_duty_cycle_ll(float dutyCycle) {
	if (dutyCycle >= conf->l_min_duty) {
		direction = 1;
	} else if (dutyCycle <= -conf->l_min_duty) {
		dutyCycle = -dutyCycle;
		direction = 0;
	}

	if (dutyCycle < conf->l_min_duty) {
		float max_erpm_fbrake;
#if BLDC_SPEED_CONTROL_CURRENT
		if (control_mode == CONTROL_MODE_CURRENT ||
				control_mode == CONTROL_MODE_CURRENT_BRAKE ||
				control_mode == CONTROL_MODE_SPEED) {
#else
		if (control_mode == CONTROL_MODE_CURRENT || control_mode == CONTROL_MODE_CURRENT_BRAKE) {
#endif
			max_erpm_fbrake = conf->l_max_erpm_fbrake_cc;
		} else {
			max_erpm_fbrake = conf->l_max_erpm_fbrake;
		}

		switch (state) {
		case MC_STATE_RUNNING:
			// TODO!!!
			if (fabsf(rpm_now) > max_erpm_fbrake) {
				dutyCycle = conf->l_min_duty;
			} else {
				full_brake_ll();
				return;
			}
			break;

		case MC_STATE_DETECTING:
			stop_pwm_ll();
			return;
			break;

		default:
			return;
		}
	} else if (dutyCycle > conf->l_max_duty) {
		dutyCycle = conf->l_max_duty;
	}

	set_duty_cycle_hw(dutyCycle);

	if (conf->motor_type == MOTOR_TYPE_DC) {
		state = MC_STATE_RUNNING;
		set_next_comm_step(comm_step);
		commutate(1);
	} else {
		if (sensorless_now) {
			if (state != MC_STATE_RUNNING) {
				if (state == MC_STATE_OFF) {
					state = MC_STATE_RUNNING;

					if (fabsf(rpm_now) < conf->sl_min_erpm) {
						commutate(1);
					}
				} else if (state == MC_STATE_FULL_BRAKE) {
					if (fabsf(rpm_now) < conf->sl_min_erpm && mcpwm_get_tot_current_filtered2() < conf->sl_max_fullbreak_current_dir_change) {
						state = MC_STATE_RUNNING;
						commutate(1);
					}
				}
			}
		} else {
			if (state != MC_STATE_RUNNING) {
				state = MC_STATE_RUNNING;
				comm_step = mcpwm_read_hall_phase2();
				set_next_comm_step(comm_step);
				commutate(1);
			}
		}
	}
}

/**
 * Lowest level (hardware) duty cycle setter. Will set the hardware timer to
 * the specified duty cycle and update the ADC sampling positions.
 *
 * @param dutyCycle
 * The duty cycle in the range [MCPWM_MIN_DUTY_CYCLE  MCPWM_MAX_DUTY_CYCLE]
 * (Only positive)
 */
static void set_duty_cycle_hw(float dutyCycle) {
	mc_timer_struct timer_tmp;

	utils_sys_lock_cnt();
	timer_tmp = timer_struct;
	utils_sys_unlock_cnt();

	utils_truncate_number(&dutyCycle, conf->l_min_duty, conf->l_max_duty);
	timer_tmp.top = mcpwm_get_timer_top();
	if (conf->motor_type == MOTOR_TYPE_BLDC && conf->pwm_mode == PWM_MODE_BIPOLAR && !IS_DETECTING()) {
		timer_tmp.duty = (uint16_t) (((float) timer_tmp.top / 2.0) * dutyCycle
				+ ((float) timer_tmp.top / 2.0));
	} else {
		timer_tmp.duty = (uint16_t)((float)timer_tmp.top * dutyCycle);
	}

	//update_adc_sample_pos(&timer_tmp);
	set_next_timer_settings(&timer_tmp);
}

static void run_pid_control_speed(void) {
	static float i_term = 0;
	static float prev_error = 0;
	float p_term;
	float d_term;

	// PID is off. Return.
	if (control_mode != CONTROL_MODE_SPEED) {
#if BLDC_SPEED_CONTROL_CURRENT
		i_term = 0.0;
#else
		i_term = dutycycle_now;
#endif
		prev_error = 0.0;
		return;
	}

	const float rpm = mcpwm_get_rpm2();
	float error = speed_pid_set_rpm - rpm;

	// Too low RPM set. Stop and return.
	if (fabsf(speed_pid_set_rpm) < conf->s_pid_min_erpm) {
		i_term = dutycycle_now;
		prev_error = error;
		mcpwm_set_duty2(0.0);
		return;
	}

#if BLDC_SPEED_CONTROL_CURRENT
	// Compute parameters
	p_term = error * conf->s_pid_kp * (1.0 / 20.0);
	i_term += error * (conf->s_pid_ki * MCPWM_PID_TIME_K) * (1.0 / 20.0);
	d_term = (error - prev_error) * (conf->s_pid_kd / MCPWM_PID_TIME_K) * (1.0 / 20.0);

	// I-term wind-up protection
	utils_truncate_number(&i_term, -1.0, 1.0);

	// Store previous error
	prev_error = error;

	// Some d_term filtering
	static float d_filtered = 0.0;
	UTILS_LP_FAST(d_filtered, d_term, 0.1);
	d_term = d_filtered;

	// Calculate output
	float output = p_term + i_term + d_term;
	utils_truncate_number(&output, -1.0, 1.0);

	// Optionally disable braking
	if (!conf->s_pid_allow_braking) {
		if (rpm > 0.0 && output < 0.0) {
			output = 0.0;
		}

		if (rpm < 0.0 && output > 0.0) {
			output = 0.0;
		}
	}

	current_set = output * conf->lo_current_max;

	if (state != MC_STATE_RUNNING) {
		set_duty_cycle_hl(SIGN(output) * conf->l_min_duty);
	}
#else
	// Compensation for supply voltage variations
	float scale = 1.0 / GET_INPUT_VOLTAGE();

	// Compute parameters
	p_term = error * conf->s_pid_kp * scale;
	i_term += error * (conf->s_pid_ki * MCPWM_PID_TIME_K) * scale;
	d_term = (error - prev_error) * (conf->s_pid_kd / MCPWM_PID_TIME_K) * scale;

	// I-term wind-up protection
	utils_truncate_number(&i_term, -1.0, 1.0);

	// Store previous error
	prev_error = error;

	// Calculate output
	float output = p_term + i_term + d_term;

	// Make sure that at least minimum output is used
	if (fabsf(output) < conf->l_min_duty) {
		output = SIGN(output) * conf->l_min_duty;
	}

	// Do not output in reverse direction to oppose too high rpm
	if (speed_pid_set_rpm > 0.0 && output < 0.0) {
		output = conf->l_min_duty;
		i_term = 0.0;
	} else if (speed_pid_set_rpm < 0.0 && output > 0.0) {
		output = -conf->l_min_duty;
		i_term = 0.0;
	}

	set_duty_cycle_hl(output);
#endif
}

static void run_pid_control_pos(float dt) {
	static float i_term = 0;
	static float prev_error = 0;
	float p_term;
	float d_term;

	// PID is off. Return.
	if (control_mode != CONTROL_MODE_POS) {
		i_term = 0;
		prev_error = 0;
		return;
	}

	// Compute error
	float error = utils_angle_difference(encoder_read_deg(), pos_pid_set_pos);

	// Compute parameters
	p_term = error * conf->p_pid_kp;
	i_term += error * (conf->p_pid_ki * dt);
	d_term = (error - prev_error) * (conf->p_pid_kd / dt);

	// I-term wind-up protection
	utils_truncate_number(&i_term, -1.0, 1.0);

	// Store previous error
	prev_error = error;

	// Calculate output
	float output = p_term + i_term + d_term;
	utils_truncate_number(&output, -1.0, 1.0);

	current_set = output * conf->lo_current_max;
}

static THD_FUNCTION(rpm_thread2, arg) {
	(void)arg;

	chRegSetThreadName("rpm timer2");

	for (;;) {
		if (rpm_thd_stop) {
			rpm_thd_stop = false;
			return;
		}

		if (rpm_dep2.comms != 0) {
			utils_sys_lock_cnt();
			const float comms = (float)rpm_dep2.comms;
			const float time_at_comm = (float)rpm_dep2.time_at_comm;
			rpm_dep2.comms = 0;
			rpm_dep2.time_at_comm = 0;
			utils_sys_unlock_cnt();

			rpm_now = (comms * MCPWM_RPM_TIMER_FREQ * 60.0) / (time_at_comm * 6.0);
		} else {
			// In case we have slowed down
			float rpm_tmp = (MCPWM_RPM_TIMER_FREQ * 60.0) / ((float) TIM14 ->CNT * 6.0);

			if (fabsf(rpm_tmp) < fabsf(rpm_now)) {
				rpm_now = rpm_tmp;
			}
		}

		// Some low-pass filtering
		static float rpm_filtered = 0.0;
		UTILS_LP_FAST(rpm_filtered, rpm_now, 0.1);
		rpm_now = rpm_filtered;
		const float rpm_abs = fabsf(rpm_now);

		// Update the cycle integrator limit
		rpm_dep2.cycle_int_limit = conf->sl_cycle_int_limit;
		rpm_dep2.cycle_int_limit_running = rpm_dep2.cycle_int_limit + (float)ADC_Value[ADC_IND_VIN_SENS] *
				conf->sl_bemf_coupling_k / (rpm_abs > conf->sl_min_erpm ? rpm_abs : conf->sl_min_erpm);
		rpm_dep2.cycle_int_limit_running = utils_map(rpm_abs, 0,
				conf->sl_cycle_int_rpm_br, rpm_dep2.cycle_int_limit_running,
				rpm_dep2.cycle_int_limit_running * conf->sl_phase_advance_at_br);
		rpm_dep2.cycle_int_limit_max = rpm_dep2.cycle_int_limit + (float)ADC_Value[ADC_IND_VIN_SENS] *
				conf->sl_bemf_coupling_k / conf->sl_min_erpm_cycle_int_limit;

		if (rpm_dep2.cycle_int_limit_running < 1.0) {
			rpm_dep2.cycle_int_limit_running = 1.0;
		}

		if (rpm_dep2.cycle_int_limit_running > rpm_dep2.cycle_int_limit_max) {
			rpm_dep2.cycle_int_limit_running = rpm_dep2.cycle_int_limit_max;
		}

		rpm_dep2.comm_time_sum = conf->m_bldc_f_sw_max / ((rpm_abs / 60.0) * 6.0);
		rpm_dep2.comm_time_sum_min_rpm = conf->m_bldc_f_sw_max / ((conf->sl_min_erpm / 60.0) * 6.0);

		run_pid_control_speed();

		chThdSleepMilliseconds(1);
	}
}

static THD_FUNCTION(timer_thread2, arg) {
	(void)arg;

	chRegSetThreadName("mcpwm timer2");

	float amp;
	float min_s;
	float max_s;

	for(;;) {
		if (timer_thd_stop) {
			timer_thd_stop = false;
			return;
		}

		if (state == MC_STATE_OFF) {
			// Track the motor back-emf and follow it with dutycycle_now. Also track
			// the direction of the motor.
			amp = filter_run_fir_iteration((float*)amp_fir_samples,
					(float*)amp_fir_coeffs, AMP_FIR_TAPS_BITS, amp_fir_index);

			// Direction tracking
			if (conf->motor_type == MOTOR_TYPE_DC) {
				if (amp > 0) {
					direction = 0;
				} else {
					direction = 1;
					amp = -amp;
				}
			} else {
				if (sensorless_now) {
					min_s = 9999999999999.0;
					max_s = 0.0;

					for (int i = 0;i < 6;i++) {
						if (last_pwm_cycles_sums[i] < min_s) {
							min_s = last_pwm_cycles_sums[i];
						}

						if (last_pwm_cycles_sums[i] > max_s) {
							max_s = last_pwm_cycles_sums[i];
						}
					}

					// If the relative difference between the longest and shortest commutation is
					// too large, we probably got the direction wrong. In that case, try the other
					// direction.
					//
					// The tachometer_for_direction value is used to make sure that the samples
					// have enough time after a direction change to get stable before trying to
					// change direction again.

					if ((max_s - min_s) / ((max_s + min_s) / 2.0) > 1.2) {
						if (tachometer_for_direction > 12) {
							if (direction == 1) {
								direction = 0;
							} else {
								direction = 1;
							}
							tachometer_for_direction = 0;
						}
					} else {
						tachometer_for_direction = 0;
					}
				} else {
					// If the direction tachometer is counting backwards, the motor is
					// not moving in the direction we think it is.
					if (tachometer_for_direction < -3) {
						if (direction == 1) {
							direction = 0;
						} else {
							direction = 1;
						}
						tachometer_for_direction = 0;
					} else if (tachometer_for_direction > 0) {
						tachometer_for_direction = 0;
					}
				}
			}

			if (direction == 1) {
				dutycycle_now = amp / (float)ADC_Value[ADC_IND_VIN_SENS];
			} else {
				dutycycle_now = -amp / (float)ADC_Value[ADC_IND_VIN_SENS];
			}
			utils_truncate_number((float*)&dutycycle_now, -conf->l_max_duty, conf->l_max_duty);
		} else {
			tachometer_for_direction = 0;
		}

		// Fill KV filter vector at 100Hz
		static int cnt_tmp = 0;
		cnt_tmp++;
		if (cnt_tmp >= 10) {
			cnt_tmp = 0;
			if (state == MC_STATE_RUNNING) {
				filter_add_sample((float*)kv_fir_samples, mcpwm_get_kv2(),
						KV_FIR_TAPS_BITS, (uint32_t*)&kv_fir_index);
			} else if (state == MC_STATE_OFF) {
				if (dutycycle_now >= conf->l_min_duty) {
					filter_add_sample((float*)kv_fir_samples, mcpwm_get_kv2(),
							KV_FIR_TAPS_BITS, (uint32_t*)&kv_fir_index);
				}
			}
		}

		chThdSleepMilliseconds(1);
	}
}

void mcpwm_adc_inj_int_handler2(int curr0, int curr1) {
	float curr0_currsamp = curr0;
	float curr1_currsamp = curr1;

	curr0_sum += curr0;
	curr1_sum += curr1;

	curr_start_samples++;

	curr0_currsamp -= curr0_offset;
	curr1_currsamp -= curr1_offset;
	curr0 -= curr0_offset;
	curr1 -= curr1_offset;

	ADC_curr_norm_value[0] = curr0;
	ADC_curr_norm_value[1] = curr1;

	ADC_curr_norm_value[2] = -(ADC_curr_norm_value[0] + ADC_curr_norm_value[1]);


	float curr_tot_sample = 0;
	if (conf->motor_type == MOTOR_TYPE_DC) {
		if (direction) {
			curr_tot_sample = -(float)(ADC_Value[ADC_IND_CURR4] - curr1_offset);
		} else {
			curr_tot_sample = -(float)(ADC_Value[ADC_IND_CURR3] - curr0_offset);
		}
	} else {
		static int detect_now = 0;

		/*
		 * Commutation Steps FORWARDS
		 * STEP		BR1		BR2		BR3
		 * 1		0		+		-
		 * 2		+		0		-
		 * 3		+		-		0
		 * 4		0		-		+
		 * 5		-		0		+
		 * 6		-		+		0
		 *
		 * Commutation Steps REVERSE (switch phase 2 and 3)
		 * STEP		BR1		BR2		BR3
		 * 1		0		-		+
		 * 2		+		-		0
		 * 3		+		0		-
		 * 4		0		+		-
		 * 5		-		+		0
		 * 6		-		0		+
		 */

		if (state == MC_STATE_FULL_BRAKE) {
			float c0 = (float)ADC_curr_norm_value[0];
			float c1 = (float)ADC_curr_norm_value[1];
			float c2 = (float)ADC_curr_norm_value[2];
			curr_tot_sample = sqrtf((c0*c0 + c1*c1 + c2*c2) / 1.5);
		} else {
			if (direction) {
				switch (comm_step) {
				case 1: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
				case 2: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
				case 3: curr_tot_sample = (float)ADC_curr_norm_value[0]; break;
				case 4: curr_tot_sample = (float)ADC_curr_norm_value[1]; break;
				case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
				case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
				default: break;
				}
			} else {
				switch (comm_step) {
				case 1: curr_tot_sample = (float)ADC_curr_norm_value[1]; break;
				case 2: curr_tot_sample = (float)ADC_curr_norm_value[0]; break;
				case 3: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
				case 4: curr_tot_sample = -(float)ADC_curr_norm_value[1]; break;
				case 5: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
				case 6: curr_tot_sample = -(float)ADC_curr_norm_value[0]; break;
				default: break;
				}
			}

			const float tot_sample_tmp = curr_tot_sample;
			static int comm_step_prev = 1;
			static float prev_tot_sample = 0.0;
			if (comm_step != comm_step_prev) {
				curr_tot_sample = prev_tot_sample;
			}
			comm_step_prev = comm_step;
			prev_tot_sample = tot_sample_tmp;
		}

		if (detect_now == 4) {
			const float a = fabsf(ADC_curr_norm_value[0]);
			const float b = fabsf(ADC_curr_norm_value[1]);

			if (a > b) {
				mcpwm_detect_currents2[detect_step] = a;
			} else {
				mcpwm_detect_currents2[detect_step] = b;
			}

			if (detect_step > 0) {
				mcpwm_detect_currents_diff2[detect_step] =
						mcpwm_detect_currents2[detect_step - 1] - mcpwm_detect_currents2[detect_step];
			} else {
				mcpwm_detect_currents_diff2[detect_step] =
						mcpwm_detect_currents2[5] - mcpwm_detect_currents2[detect_step];
			}

			const int vzero = ADC_V_ZERO;
			//			const int vzero = (ADC_V_L1 + ADC_V_L2 + ADC_V_L3) / 3;

			switch (comm_step) {
			case 1:
			case 4:
				mcpwm_detect_voltages2[detect_step] = ADC_V_L4 - vzero;
				break;

			case 2:
			case 5:
				mcpwm_detect_voltages2[detect_step] = ADC_V_L5 - vzero;
				break;

			case 3:
			case 6:
				mcpwm_detect_voltages2[detect_step] = ADC_V_L6 - vzero;
				break;

			default:
				break;
			}

			mcpwm_detect_currents_avg[detect_step] += mcpwm_detect_currents2[detect_step];
			mcpwm_detect_avg_samples[detect_step]++;

			stop_pwm_hw();
		}

		if (detect_now) {
			detect_now--;
		}

		if (IS_DETECTING() && detect_now == 0) {
			detect_now = 5;

			set_duty_cycle_hw(0.2);

			detect_step++;
			if (detect_step > 5) {
				detect_step = 0;
			}

			comm_step = detect_step + 1;

			set_next_comm_step(comm_step);
			TIM_GenerateEvent(TIM8, TIM_EventSource_COM);
		}
	}

	last_current_sample = curr_tot_sample * FAC_CURRENT;

	// Filter out outliers
	if (fabsf(last_current_sample) > (conf->l_abs_current_max * 1.2)) {
		last_current_sample = SIGN(last_current_sample) * conf->l_abs_current_max * 1.2;
	}

	filter_add_sample((float*) current_fir_samples, last_current_sample,
			CURR_FIR_TAPS_BITS, (uint32_t*) &current_fir_index);
	last_current_sample_filtered = filter_run_fir_iteration(
			(float*) current_fir_samples, (float*) current_fir_coeffs,
			CURR_FIR_TAPS_BITS, current_fir_index);
}

/*
 * New ADC samples ready. Do commutation!
 */
void mcpwm_adc_int_handler2(void) {
	// Set the next timer settings if an update is far enough away
    //update_timer_attempt();
	const float input_voltage = GET_INPUT_VOLTAGE();
	int ph1, ph2, ph3;
	int ph1_raw, ph2_raw, ph3_raw;

	static int direction_before = 1;
	if (!(state == MC_STATE_RUNNING && direction == direction_before)) {
		has_commutated = 0;
	}
	direction_before = direction;

	if (conf->motor_type == MOTOR_TYPE_BLDC) {

		/*
		 * Calculate the virtual ground, depending on the state.
		 */

		if (has_commutated && fabsf(dutycycle_now) > 0.2) {
			mcpwm_vzero2 = ADC_V_ZERO;
		} else {
			mcpwm_vzero2 = (ADC_V_L4 + ADC_V_L5 + ADC_V_L6) / 3;
		}

		if (direction) {
			ph1 = ADC_V_L4 - mcpwm_vzero2;
			ph2 = ADC_V_L5 - mcpwm_vzero2;
			ph3 = ADC_V_L6 - mcpwm_vzero2;
			ph1_raw = ADC_V_L4;
			ph2_raw = ADC_V_L5;
			ph3_raw = ADC_V_L6;
		} else {
			ph1 = ADC_V_L4 - mcpwm_vzero2;
			ph2 = ADC_V_L6 - mcpwm_vzero2;
			ph3 = ADC_V_L5 - mcpwm_vzero2;
			ph1_raw = ADC_V_L4;
			ph2_raw = ADC_V_L6;
			ph3_raw = ADC_V_L5;
		}
		float amp = 0.0;

		if (has_commutated) {
			amp = fabsf(dutycycle_now) * (float)ADC_Value[ADC_IND_VIN_SENS];
		} else {
			amp = sqrtf((float)(ph1*ph1 + ph2*ph2 + ph3*ph3)) * sqrtf(2.0);
		}

		filter_add_sample((float*)amp_fir_samples, amp,
				AMP_FIR_TAPS_BITS, (uint32_t*)&amp_fir_index);


		if (sensorless_now) {
			static float cycle_integrator = 0;

			if (pwm_cycles_sum >= rpm_dep2.comm_time_sum_min_rpm) {
				if (state == MC_STATE_RUNNING) {
					if (conf->comm_mode == COMM_MODE_INTEGRATE) {
						// This means that the motor is stuck. If this commutation does not
						// produce any torque because of misalignment at start, two
						// commutations ahead should produce full torque.
						commutate(2);
					} else if (conf->comm_mode == COMM_MODE_DELAY) {
						commutate(1);
					}

					cycle_integrator = 0.0;
				}
			}

			if ((state == MC_STATE_RUNNING && pwm_cycles >= 2) || state == MC_STATE_OFF) {
				int v_diff = 0;
				int ph_now_raw = 0;

				switch (comm_step) {
				case 1:
					v_diff = ph1;
					ph_now_raw = ph1_raw;
					break;
				case 2:
					v_diff = -ph2;
					ph_now_raw = ph2_raw;
					break;
				case 3:
					v_diff = ph3;
					ph_now_raw = ph3_raw;
					break;
				case 4:
					v_diff = -ph1;
					ph_now_raw = ph1_raw;
					break;
				case 5:
					v_diff = ph2;
					ph_now_raw = ph2_raw;
					break;
				case 6:
					v_diff = -ph3;
					ph_now_raw = ph3_raw;
					break;
				default:
					break;
				}

				// Collect hall sensor samples in the first half of the commutation cycle. This is
				// because positive timing is much better than negative timing in case they are
				// mis-aligned.
				if (v_diff < 50) {
					hall_detect_table[read_hall()][comm_step]++;
				}

				// Don't commutate while the motor is standing still and the signal only consists
				// of weak noise.
				if (abs(v_diff) < 10) {
					v_diff = 0;
				}

				if (v_diff > 0) {
					// TODO!
					//					const int min = 100;
					int min = (int)((1.0 - fabsf(dutycycle_now)) * (float)ADC_Value[ADC_IND_VIN_SENS] * 0.3);
					if (min > ADC_Value[ADC_IND_VIN_SENS] / 4) {
						min = ADC_Value[ADC_IND_VIN_SENS] / 4;
					}

					if (pwm_cycles_sum > (last_pwm_cycles_sum / 2.0) ||
							!has_commutated || (ph_now_raw > min && ph_now_raw < (ADC_Value[ADC_IND_VIN_SENS] - min))) {
						cycle_integrator += (float)v_diff / switching_frequency_now;
					}
				}

				static float cycle_sum = 0.0;
				if (conf->comm_mode == COMM_MODE_INTEGRATE) {
					float limit;
					if (has_commutated) {
						limit = rpm_dep2.cycle_int_limit_running * (0.0005 * VDIV_CORR);
					} else {
						limit = rpm_dep2.cycle_int_limit * (0.0005 * VDIV_CORR);
					}

					if (cycle_integrator >= (rpm_dep2.cycle_int_limit_max * (0.0005 * VDIV_CORR)) ||
							cycle_integrator >= limit) {
						commutate(1);
						cycle_integrator = 0.0;
						cycle_sum = 0.0;
					}
				} else if (conf->comm_mode == COMM_MODE_DELAY) {
					if (v_diff > 0) {
						cycle_sum += conf->m_bldc_f_sw_max / switching_frequency_now;

						if (cycle_sum >= utils_map(fabsf(rpm_now), 0,
								conf->sl_cycle_int_rpm_br, rpm_dep2.comm_time_sum / 2.0,
								(rpm_dep2.comm_time_sum / 2.0) * conf->sl_phase_advance_at_br)) {
							commutate(1);
							cycle_integrator_sum += cycle_integrator * (1.0 / (0.0005 * VDIV_CORR));
							cycle_integrator_iterations += 1.0;
							cycle_integrator = 0.0;
							cycle_sum = 0.0;
						}
					} else {
						cycle_integrator = 0.0;
						cycle_sum = 0.0;
					}
				}
			} else {
				cycle_integrator = 0.0;
			}

			pwm_cycles_sum += conf->m_bldc_f_sw_max / switching_frequency_now;
			pwm_cycles++;
		} else {
			const int hall_phase = mcpwm_read_hall_phase2();
			if (comm_step != hall_phase) {
				comm_step = hall_phase;

				update_rpm_tacho2();

				if (state == MC_STATE_RUNNING) {
					set_next_comm_step(comm_step);
					commutate(0);
				}
			} else if (state == MC_STATE_RUNNING && !has_commutated) {
				set_next_comm_step(comm_step);
				commutate(0);
			}
		}
	} else {
		float amp = 0.0;

		if (has_commutated) {
			amp = dutycycle_now * (float)ADC_Value[ADC_IND_VIN_SENS];
		} else {
			amp = ADC_V_L6 - ADC_V_L4;
		}

		// Fill the amplitude FIR filter
		filter_add_sample((float*)amp_fir_samples, amp,
				AMP_FIR_TAPS_BITS, (uint32_t*)&amp_fir_index);

		if (state == MC_STATE_RUNNING && !has_commutated) {
			set_next_comm_step(comm_step);
			commutate(0);
		}
	}

	const float current_nofilter = mcpwm_get_tot_current2();
	const float current_in_nofilter = current_nofilter * fabsf(dutycycle_now);

	if (state == MC_STATE_RUNNING && has_commutated) {
		// Compensation for supply voltage variations
		const float voltage_scale = 20.0 / input_voltage;
		float ramp_step = conf->m_duty_ramp_step / (switching_frequency_now / 1000.0);
		float ramp_step_no_lim = ramp_step;
		const float rpm = mcpwm_get_rpm2();

		if (slow_ramping_cycles) {
			slow_ramping_cycles--;
			ramp_step *= 0.1;
		}

		float dutycycle_now_tmp = dutycycle_now;

#if BLDC_SPEED_CONTROL_CURRENT
		if (control_mode == CONTROL_MODE_CURRENT ||
				control_mode == CONTROL_MODE_POS ||
				control_mode == CONTROL_MODE_SPEED) {
#else
		if (control_mode == CONTROL_MODE_CURRENT || control_mode == CONTROL_MODE_POS) {
#endif
			// Compute error
			const float error = current_set - (direction ? current_nofilter : -current_nofilter);
			float step = error * conf->cc_gain * voltage_scale;
			const float start_boost = conf->cc_startup_boost_duty * voltage_scale;

			// Do not ramp too much
			utils_truncate_number(&step, -conf->cc_ramp_step_max, conf->cc_ramp_step_max);

			// Switching frequency correction
			step /= switching_frequency_now / 1000.0;

			if (slow_ramping_cycles) {
				slow_ramping_cycles--;
				step *= 0.1;
			}

			// Optionally apply startup boost.
			if (fabsf(dutycycle_now_tmp) < start_boost) {
				utils_step_towards(&dutycycle_now_tmp,
						current_set > 0.0 ?
								start_boost :
								-start_boost, ramp_step);
			} else {
				dutycycle_now_tmp += step;
			}

			// Upper truncation
			utils_truncate_number((float*)&dutycycle_now_tmp, -conf->l_max_duty, conf->l_max_duty);

			// Lower truncation
			if (fabsf(dutycycle_now_tmp) < conf->l_min_duty) {
				if (dutycycle_now_tmp < 0.0 && current_set > 0.0) {
					dutycycle_now_tmp = conf->l_min_duty;
				} else if (dutycycle_now_tmp > 0.0 && current_set < 0.0) {
					dutycycle_now_tmp = -conf->l_min_duty;
				}
			}

			// The set dutycycle should be in the correct direction in case the output is lower
			// than the minimum duty cycle and the mechanism below gets activated.
			dutycycle_set = dutycycle_now_tmp >= 0.0 ? conf->l_min_duty : -conf->l_min_duty;
		} else if (control_mode == CONTROL_MODE_CURRENT_BRAKE) {
			// Compute error
			const float error = -fabsf(current_set) - current_nofilter;
			float step = error * conf->cc_gain * voltage_scale;

			// Do not ramp too much
			utils_truncate_number(&step, -conf->cc_ramp_step_max, conf->cc_ramp_step_max);

			// Switching frequency correction
			step /= switching_frequency_now / 1000.0;

			if (slow_ramping_cycles) {
				slow_ramping_cycles--;
				step *= 0.1;
			}

			dutycycle_now_tmp += SIGN(dutycycle_now_tmp) * step;

			// Upper truncation
			utils_truncate_number((float*)&dutycycle_now_tmp, -conf->l_max_duty, conf->l_max_duty);

			// Lower truncation
			if (fabsf(dutycycle_now_tmp) < conf->l_min_duty) {
				if (fabsf(rpm_now) < conf->l_max_erpm_fbrake_cc) {
					dutycycle_now_tmp = 0.0;
					dutycycle_set = dutycycle_now_tmp;
				} else {
					dutycycle_now_tmp = SIGN(dutycycle_now_tmp) * conf->l_min_duty;
					dutycycle_set = dutycycle_now_tmp;
				}
			}
		} else {
			utils_step_towards((float*)&dutycycle_now_tmp, dutycycle_set, ramp_step);
		}

		static int limit_delay = 0;

		// Apply limits in priority order
		if (current_nofilter > conf->lo_current_max) {
			utils_step_towards((float*) &dutycycle_now, 0.0,
					ramp_step_no_lim * fabsf(current_nofilter - conf->lo_current_max) * conf->m_current_backoff_gain);
			limit_delay = 1;
		} else if (current_nofilter < conf->lo_current_min) {
			utils_step_towards((float*) &dutycycle_now, direction ? conf->l_max_duty : -conf->l_max_duty,
					ramp_step_no_lim * fabsf(current_nofilter - conf->lo_current_min) * conf->m_current_backoff_gain);
			limit_delay = 1;
		} else if (current_in_nofilter > conf->lo_in_current_max) {
			utils_step_towards((float*) &dutycycle_now, 0.0,
					ramp_step_no_lim * fabsf(current_in_nofilter - conf->lo_in_current_max) * conf->m_current_backoff_gain);
			limit_delay = 1;
		} else if (current_in_nofilter < conf->lo_in_current_min) {
			utils_step_towards((float*) &dutycycle_now, direction ? conf->l_max_duty : -conf->l_max_duty,
					ramp_step_no_lim * fabsf(current_in_nofilter - conf->lo_in_current_min) * conf->m_current_backoff_gain);
			limit_delay = 1;
		}

		if (limit_delay > 0) {
			limit_delay--;
		} else {
			dutycycle_now = dutycycle_now_tmp;
		}

		// When the set duty cycle is in the opposite direction, make sure that the motor
		// starts again after stopping completely
		if (fabsf(dutycycle_now) < conf->l_min_duty) {
			if (dutycycle_set >= conf->l_min_duty) {
				dutycycle_now = conf->l_min_duty;
			} else if (dutycycle_set <= -conf->l_min_duty) {
				dutycycle_now = -conf->l_min_duty;
			}
		}

		// Don't start in the opposite direction when the RPM is too high even if the current is low enough.
		if (dutycycle_now >= conf->l_min_duty && rpm < -conf->l_max_erpm_fbrake) {
			dutycycle_now = -conf->l_min_duty;
		} else if (dutycycle_now <= -conf->l_min_duty && rpm > conf->l_max_erpm_fbrake) {
			dutycycle_now = conf->l_min_duty;
		}

		set_duty_cycle_ll(dutycycle_now);
	}

	if (encoder_is_configured()) {
		run_pid_control_pos(1.0 / switching_frequency_now);
	}
}

void mcpwm_set_detect2(void) {
	if (mc_interface_try_input()) {
		return;
	}

	control_mode = CONTROL_MODE_NONE;
	stop_pwm_hw();

	//set_switching_frequency(conf->m_bldc_f_sw_max);

	for(int i = 0;i < 6;i++) {
		mcpwm_detect_currents2[i] = 0;
		mcpwm_detect_currents_avg[i] = 0;
		mcpwm_detect_avg_samples[i] = 0;
	}

	state = MC_STATE_DETECTING;
}

float mcpwm_get_detect_pos2(void) {
	float v[6];
	v[0] = mcpwm_detect_currents_avg[0] / mcpwm_detect_avg_samples[0];
	v[1] = mcpwm_detect_currents_avg[1] / mcpwm_detect_avg_samples[1];
	v[2] = mcpwm_detect_currents_avg[2] / mcpwm_detect_avg_samples[2];
	v[3] = mcpwm_detect_currents_avg[3] / mcpwm_detect_avg_samples[3];
	v[4] = mcpwm_detect_currents_avg[4] / mcpwm_detect_avg_samples[4];
	v[5] = mcpwm_detect_currents_avg[5] / mcpwm_detect_avg_samples[5];

	for(int i = 0;i < 6;i++) {
		mcpwm_detect_currents_avg[i] = 0;
		mcpwm_detect_avg_samples[i] = 0;
	}

	float v0 = v[0] + v[3];
	float v1 = v[1] + v[4];
	float v2 = v[2] + v[5];

	float offset = (v0 + v1 + v2) / 3.0;
	v0 -= offset;
	v1 -= offset;
	v2 -= offset;

	float amp = sqrtf((v0*v0 + v1*v1 + v2*v2) / 1.5);
	v0 /= amp;
	v1 /= amp;
	v2 /= amp;

	float ph[1];
	ph[0] = asinf(v0) * 180.0 / M_PI;

	float res = ph[0];
	if (v1 < v2) {
		res = 180 - ph[0];
	}

	utils_norm_angle(&res);

	return res;
}

float mcpwm_read_reset_avg_cycle_integrator2(void) {
	float res = cycle_integrator_sum / cycle_integrator_iterations;
	cycle_integrator_sum = 0;
	cycle_integrator_iterations = 0;
	return res;
}

/**
 * Set the commutation mode for sensorless commutation.
 *
 * @param mode
 * COMM_MODE_INTEGRATE: More robust, but requires many parameters.
 * COMM_MODE_DELAY: Like most hobby ESCs. Requires less parameters,
 * but has worse startup and is less robust.
 *
 */
void mcpwm_set_comm_mode2(mc_comm_mode mode) {
	conf->comm_mode = mode;
}

mc_comm_mode mcpwm_get_comm_mode2(void) {
	return conf->comm_mode;
}

float mcpwm_get_last_adc_isr_duration2(void) {
	return last_adc_isr_duration;
}

float mcpwm_get_last_inj_adc_isr_duration2(void) {
	return last_inj_adc_isr_duration;
}

mc_rpm_dep_struct mcpwm_get_rpm_dep2(void) {
	return rpm_dep2;
}

bool mcpwm_is_dccal_done2(void) {
	return dccal_done;
}

void mcpwm_switch_comm_mode2(mc_comm_mode next) {
	comm_mode_next = next;
}

/**
 * Reset the hall sensor detection table
 */
void mcpwm_reset_hall_detect_table2(void) {
	memset((void*)hall_detect_table, 0, sizeof(hall_detect_table[0][0]) * 8 * 7);
}

/**
 * Get the current detected hall sensor table
 *
 * @param table
 * Pointer to a table where the result should be stored
 *
 * @return
 * 0: OK
 * -1: Invalid hall sensor output
 * -2: WS2811 enabled
 * -3: Encoder enabled
 */
int mcpwm_get_hall_detect_result2(int8_t *table) {
	if (WS2811_ENABLE) {
		return -2;
	} else if (conf->m_sensor_port_mode != SENSOR_PORT_MODE_HALL) {
		return -3;
	}

	for (int i = 0;i < 8;i++) {
		int samples = 0;
		int res = -1;
		for (int j = 1;j < 7;j++) {
			if (hall_detect_table[i][j] > samples) {
				samples = hall_detect_table[i][j];
				if (samples > 15) {
					res = j;
				}
			}
			table[i] = res;
		}
	}

	int invalid_samp_num = 0;
	int nums[7] = {0, 0, 0, 0, 0, 0, 0};
	int tot_nums = 0;
	for (int i = 0;i < 8;i++) {
		if (table[i] == -1) {
			invalid_samp_num++;
		} else {
			if (!nums[table[i]]) {
				nums[table[i]] = 1;
				tot_nums++;
			}
		}
	}

	if (invalid_samp_num == 2 && tot_nums == 6) {
		return 0;
	} else {
		return -1;
	}
}

/**
 * Read the current phase of the motor using hall effect sensors
 * @return
 * The phase read.
 */
int mcpwm_read_hall_phase2(void) {
	return hall_to_phase_table[read_hall() + (direction ? 8 : 0)];
}

static int read_hall(void) {
	return READ_HALL4() | (READ_HALL5() << 1) | (READ_HALL6() << 2);
}

/*
 * Commutation Steps FORWARDS
 * STEP		BR1		BR2		BR3
 * 1		0		+		-
 * 2		+		0		-
 * 3		+		-		0
 * 4		0		-		+
 * 5		-		0		+
 * 6		-		+		0
 *
 * Commutation Steps REVERSE (switch phase 2 and 3)
 * STEP		BR1		BR2		BR3
 * 1		0		-		+
 * 2		+		-		0
 * 3		+		0		-
 * 4		0		+		-
 * 5		-		+		0
 * 6		-		0		+
 */
/*
static void update_adc_sample_pos(mc_timer_struct *timer_tmp) {
  volatile uint32_t duty = timer_tmp->duty;
  volatile uint32_t top = timer_tmp->top;
  volatile uint32_t val_sample = timer_tmp->val_sample;
  volatile uint32_t curr1_sample = timer_tmp->curr1_sample;

  if (duty > (uint32_t)((float)top * conf->l_max_duty)) {
      duty = (uint32_t)((float)top * conf->l_max_duty);
  }

  curr_samp_volt = 0;

      // Sample the ADC at an appropriate time during the pwm cycle
      if (IS_DETECTING()) {
          // Voltage samples
          val_sample =  350; //duty / 2;

          // Current samples
          curr1_sample = duty + (top - duty) / 2;
      } else {
                  // Voltage samples
              val_sample =  350;//duty / 2;

              // Current samples
              curr1_sample = duty + (top - duty) / 2;
              //if (curr1_sample > (top - 70)) {
              //    curr1_sample = top - 70;
              //}
          }

  timer_tmp->val_sample = val_sample;
  timer_tmp->curr1_sample = curr1_sample;
}
*/

static void update_rpm_tacho2(void) {
	int step = comm_step - 1;
	static int last_step = 0;
	int tacho_diff = (step - last_step) % 6;
	last_step = step;

	if (tacho_diff > 3) {
		tacho_diff -= 6;
	} else if (tacho_diff < -2) {
		tacho_diff += 6;
	}

	if (tacho_diff != 0) {
		rpm_dep2.comms += tacho_diff;
		rpm_dep2.time_at_comm +=  TIM14->CNT;
		TIM14->CNT = 0;
	}

	// Tachometers
	tachometer_for_direction += tacho_diff;
	tachometer_abs += tacho_diff;

	if (direction) {
		tachometer += tacho_diff;
	} else {
		tachometer -= tacho_diff;
	}
}

static void update_sensor_mode(void) {
	if (conf->sensor_mode == SENSOR_MODE_SENSORLESS ||
			(conf->sensor_mode == SENSOR_MODE_HYBRID &&
					fabsf(mcpwm_get_rpm2()) > conf->hall_sl_erpm)) {
		sensorless_now = true;
	} else {
		sensorless_now = false;
	}
}

static void commutate(int steps) {
	last_pwm_cycles_sum = pwm_cycles_sum;
	last_pwm_cycles_sums[comm_step - 1] = pwm_cycles_sum;
	pwm_cycles_sum = 0;
	pwm_cycles = 0;

	if (conf->motor_type == MOTOR_TYPE_BLDC && sensorless_now) {
		comm_step += steps;
		while (comm_step > 6) {
			comm_step -= 6;
		}
		while (comm_step < 1) {
			comm_step += 6;
		}

		update_rpm_tacho2();

		if (!(state == MC_STATE_RUNNING)) {
			update_sensor_mode();
			return;
		}

		set_next_comm_step(comm_step);
	}

	TIM_GenerateEvent(TIM8, TIM_EventSource_COM);
	has_commutated = 1;

	mc_timer_struct timer_tmp;

	utils_sys_lock_cnt();
	timer_tmp = timer_struct;
	utils_sys_unlock_cnt();

	//update_adc_sample_pos(&timer_tmp);
	set_next_timer_settings(&timer_tmp);
	update_sensor_mode();
	conf->comm_mode = comm_mode_next;
}

static void set_next_timer_settings(mc_timer_struct *settings) {
	utils_sys_lock_cnt();
	timer_struct = *settings;
	timer_struct.updated = false;
	utils_sys_unlock_cnt();
}

unsigned int mcpwm_get_timer_duty2(void){
  return timer_struct.duty;
}
/*
static void set_switching_frequency(float frequency) {
	switching_frequency_now = frequency;
	mc_timer_struct timer_tmp;

	utils_sys_lock_cnt();
	timer_tmp = timer_struct;
	utils_sys_unlock_cnt();

	timer_tmp.top = SYSTEM_CORE_CLOCK / (int)switching_frequency_now;
	//update_adc_sample_pos(&timer_tmp);
	set_next_timer_settings(&timer_tmp);
}
*/

static void set_next_comm_step(int next_step) {
	if (conf->motor_type == MOTOR_TYPE_DC) {
		// 0
		TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_OCMode_Inactive);
		TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
		TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Disable);

		if (direction) {
			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_OCMode_PWM1);
			TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Enable);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Enable);
		} else {
			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_OCMode_PWM1);
			TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Enable);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Enable);
		}

		return;
	}

	uint16_t positive_oc_mode = TIM_OCMode_PWM1;
	uint16_t negative_oc_mode = TIM_OCMode_Inactive;

	uint16_t positive_highside = TIM_CCx_Enable;
	uint16_t positive_lowside = TIM_CCxN_Enable;

	uint16_t negative_highside = TIM_CCx_Enable;
	uint16_t negative_lowside = TIM_CCxN_Enable;

	if (!IS_DETECTING()) {
		switch (conf->pwm_mode) {
		case PWM_MODE_NONSYNCHRONOUS_HISW:
			positive_lowside = TIM_CCxN_Disable;
			break;

		case PWM_MODE_SYNCHRONOUS:
			break;

		case PWM_MODE_BIPOLAR:
			negative_oc_mode = TIM_OCMode_PWM2;
			break;
		}
	}

	if (next_step == 1) {
		if (direction) {
#ifdef HW_HAS_DRV8313
			DISABLE_BR1();
			ENABLE_BR2();
			ENABLE_BR3();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_2, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_2, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_3, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_3, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, negative_lowside);
		} else {
#ifdef HW_HAS_DRV8313
			DISABLE_BR1();
			ENABLE_BR3();
			ENABLE_BR2();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_3, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_3, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_2, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_2, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, negative_lowside);
		}
	} else if (next_step == 2) {
		if (direction) {
#ifdef HW_HAS_DRV8313
			DISABLE_BR2();
			ENABLE_BR1();
			ENABLE_BR3();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_1, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_1, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_3, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_3, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, negative_lowside);
		} else {
#ifdef HW_HAS_DRV8313
			DISABLE_BR3();
			ENABLE_BR1();
			ENABLE_BR2();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_1, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_1, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_2, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_2, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, negative_lowside);
		}
	} else if (next_step == 3) {
		if (direction) {
#ifdef HW_HAS_DRV8313
			DISABLE_BR3();
			ENABLE_BR1();
			ENABLE_BR2();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_1, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_1, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_2, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_2, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, negative_lowside);
		} else {
#ifdef HW_HAS_DRV8313
			DISABLE_BR2();
			ENABLE_BR1();
			ENABLE_BR3();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_1, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_1, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_3, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_3, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, negative_lowside);
		}
	} else if (next_step == 4) {
		if (direction) {
#ifdef HW_HAS_DRV8313
			DISABLE_BR1();
			ENABLE_BR3();
			ENABLE_BR2();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_3, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_3, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_2, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_2, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, negative_lowside);
		} else {
#ifdef HW_HAS_DRV8313
			DISABLE_BR1();
			ENABLE_BR2();
			ENABLE_BR3();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_2, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_2, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_3, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_3, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, negative_lowside);
		}
	} else if (next_step == 5) {
		if (direction) {
#ifdef HW_HAS_DRV8313
			DISABLE_BR2();
			ENABLE_BR3();
			ENABLE_BR1();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_3, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_3, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_1, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_1, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, negative_lowside);
		} else {
#ifdef HW_HAS_DRV8313
			DISABLE_BR3();
			ENABLE_BR2();
			ENABLE_BR1();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_2, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_2, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_1, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_1, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, negative_lowside);
		}
	} else if (next_step == 6) {
		if (direction) {
#ifdef HW_HAS_DRV8313
			DISABLE_BR3();
			ENABLE_BR2();
			ENABLE_BR1();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_2, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_2, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_1, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_1, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, negative_lowside);
		} else {
#ifdef HW_HAS_DRV8313
			DISABLE_BR2();
			ENABLE_BR3();
			ENABLE_BR1();
#endif
			// 0
			TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_OCMode_Inactive);
			TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
			TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Disable);

			// +
			TIM_SelectOCxM(TIM8, TIM_Channel_3, positive_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_3, positive_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_3, positive_lowside);

			// -
			TIM_SelectOCxM(TIM8, TIM_Channel_1, negative_oc_mode);
			TIM_CCxCmd(TIM8, TIM_Channel_1, negative_highside);
			TIM_CCxNCmd(TIM8, TIM_Channel_1, negative_lowside);
		}
	} else {
#ifdef HW_HAS_DRV8313
		DISABLE_BR1();
		DISABLE_BR2();
		DISABLE_BR3();
#endif
		// Invalid phase.. stop PWM!
		TIM_SelectOCxM(TIM8, TIM_Channel_1, TIM_ForcedAction_InActive);
		TIM_CCxCmd(TIM8, TIM_Channel_1, TIM_CCx_Enable);
		TIM_CCxNCmd(TIM8, TIM_Channel_1, TIM_CCxN_Disable);

		TIM_SelectOCxM(TIM8, TIM_Channel_2, TIM_ForcedAction_InActive);
		TIM_CCxCmd(TIM8, TIM_Channel_2, TIM_CCx_Enable);
		TIM_CCxNCmd(TIM8, TIM_Channel_2, TIM_CCxN_Disable);

		TIM_SelectOCxM(TIM8, TIM_Channel_3, TIM_ForcedAction_InActive);
		TIM_CCxCmd(TIM8, TIM_Channel_3, TIM_CCx_Enable);
		TIM_CCxNCmd(TIM8, TIM_Channel_3, TIM_CCxN_Disable);
	}
}