/**
* Update the override limits for a configuration based on MOSFET temperature etc.
*
* @param conf
* The configaration to update.
*/
static void update_override_limits(volatile mc_configuration *conf) {
const float temp = NTC_TEMP(ADC_IND_TEMP_MOS1);
const float v_in = GET_INPUT_VOLTAGE();
// Temperature
if (temp < conf->l_temp_fet_start) {
conf->lo_current_min = conf->l_current_min;
conf->lo_current_max = conf->l_current_max;
} else if (temp > conf->l_temp_fet_end) {
conf->lo_current_min = 0.0;
conf->lo_current_max = 0.0;
mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_FET);
} else {
float maxc = fabsf(conf->l_current_max);
if (fabsf(conf->l_current_min) > maxc) {
maxc = fabsf(conf->l_current_min);
}
maxc = utils_map(temp, conf->l_temp_fet_start, conf->l_temp_fet_end, maxc, 0.0);
if (fabsf(conf->l_current_max) > maxc) {
conf->lo_current_max = SIGN(conf->l_current_max) * maxc;
}
if (fabsf(conf->l_current_min) > maxc) {
conf->lo_current_min = SIGN(conf->l_current_min) * maxc;
}
}
// Battery cutoff
if (v_in > conf->l_battery_cut_start) {
conf->lo_in_current_max = conf->l_in_current_max;
} else if (v_in < conf->l_battery_cut_end) {
conf->lo_in_current_max = 0.0;
} else {
conf->lo_in_current_max = utils_map(v_in, conf->l_battery_cut_start,
conf->l_battery_cut_end, conf->l_in_current_max, 0.0);
}
conf->lo_in_current_min = conf->l_in_current_min;
}
static void set_output(float output) {
output /= (1.0 - HYST);
if (output > HYST) {
output -= HYST;
} else if (output < -HYST) {
output += HYST;
} else {
output = 0.0;
}
const float rpm = mcpwm_get_rpm();
if (output > 0.0 && rpm > -MCPWM_MIN_RPM) {
float current = output * MCPWM_CURRENT_MAX;
// Soft RPM limit
if (rpm > RPM_MAX_2) {
current = -MCPWM_CURRENT_CONTROL_MIN;
} else if (rpm > RPM_MAX_1) {
current = utils_map(rpm, RPM_MAX_1, RPM_MAX_2, current, -MCPWM_CURRENT_CONTROL_MIN);
}
// Some low-pass filtering
static float current_p1 = 0.0;
static float current_p2 = 0.0;
current = (current + current_p1 + current_p2) / 3;
current_p2 = current_p1;
current_p1 = current;
if (fabsf(current) < MCPWM_CURRENT_CONTROL_MIN) {
current = -MCPWM_CURRENT_CONTROL_MIN;
}
mcpwm_set_current(current);
} else {
mcpwm_set_brake_current(output * MCPWM_CURRENT_MIN);
}
}
static void adc_int_handler(void *p, uint32_t flags) {
(void)p;
(void)flags;
uint32_t t_start = timer_time_now();
// Reset the watchdog
timeout_feed_WDT(THREAD_MCPWM);
int curr0 = GET_CURRENT1();
int curr1 = GET_CURRENT2();
#ifdef HW_HAS_3_SHUNTS
int curr2 = GET_CURRENT3();
#endif
m_curr0_sum += curr0;
m_curr1_sum += curr1;
#ifdef HW_HAS_3_SHUNTS
m_curr2_sum += curr2;
#endif
curr0 -= m_curr0_offset;
curr1 -= m_curr1_offset;
#ifdef HW_HAS_3_SHUNTS
curr2 -= m_curr2_offset;
#endif
m_curr_samples++;
// Update current
#ifdef HW_HAS_3_SHUNTS
float i1 = -(float)curr2;
#else
float i1 = -(float)curr1;
#endif
float i2 = (float)curr0;
m_current_now = utils_max_abs(i1, i2) * FAC_CURRENT;
UTILS_LP_FAST(m_current_now_filtered, m_current_now, m_conf->gpd_current_filter_const);
// Check for most critical faults here, as doing it in mc_interface can be too slow
// for high switching frequencies.
const float input_voltage = GET_INPUT_VOLTAGE();
static int wrong_voltage_iterations = 0;
if (input_voltage < m_conf->l_min_vin ||
input_voltage > m_conf->l_max_vin) {
wrong_voltage_iterations++;
if ((wrong_voltage_iterations >= 8)) {
mc_interface_fault_stop(input_voltage < m_conf->l_min_vin ?
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE);
}
} else {
wrong_voltage_iterations = 0;
}
if (m_conf->l_slow_abs_current) {
if (fabsf(m_current_now) > m_conf->l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
} else {
if (fabsf(m_current_now_filtered) > m_conf->l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
}
// Buffer handling
static bool buffer_was_empty = true;
static int interpol = 0;
static float buffer_last = 0.0;
static float buffer_next = 0.0;
interpol++;
if (interpol > m_conf->gpd_buffer_interpol) {
interpol = 0;
if (m_sample_buffer.read != m_sample_buffer.write) {
buffer_last = buffer_next;
buffer_next = m_sample_buffer.buffer[m_sample_buffer.read++];
m_sample_buffer.read %= SAMPLE_BUFFER_SIZE;
m_output_now = buffer_last;
m_is_running = true;
buffer_was_empty = false;
} else {
if (!buffer_was_empty) {
stop_pwm_hw();
}
buffer_was_empty = true;
}
} else if (!buffer_was_empty) {
m_output_now = utils_map((float)interpol,
0.0, (float)m_conf->gpd_buffer_interpol + 1.0,
buffer_last, buffer_next);
m_is_running = true;
}
if (m_is_running) {
gpdrive_output_sample(m_output_now);
if (m_output_mode == GPD_OUTPUT_MODE_CURRENT) {
float v_in = GET_INPUT_VOLTAGE();
float err = m_current_state.current_set - m_current_now_filtered;
m_current_state.voltage_now = m_current_state.voltage_int + err * m_conf->gpd_current_kp;
m_current_state.voltage_int += err * m_conf->gpd_current_ki * (1.0 / m_fsw_now);
utils_truncate_number_abs((float*)&m_current_state.voltage_int, v_in);
set_modulation(m_current_state.voltage_now / v_in);
}
}
ledpwm_update_pwm();
m_last_adc_isr_duration = timer_seconds_elapsed_since(t_start);
}
static msg_t adc_thread(void *arg) {
(void)arg;
chRegSetThreadName("APP_ADC");
// Set servo pin as an input with pullup
palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_INPUT_PULLUP);
for(;;) {
// Sleep for a time according to the specified rate
systime_t sleep_time = CH_FREQUENCY / config.update_rate_hz;
// At least one tick should be slept to not block the other threads
if (sleep_time == 0) {
sleep_time = 1;
}
chThdSleep(sleep_time);
// Read the external ADC pin and convert the value to a voltage.
float pwr = (float)ADC_Value[ADC_IND_EXT];
pwr /= 4095;
pwr *= V_REG;
read_voltage = pwr;
// Optionally apply a mean value filter
if (config.use_filter) {
static float filter_buffer[FILTER_SAMPLES];
static int filter_ptr = 0;
filter_buffer[filter_ptr++] = pwr;
if (filter_ptr >= FILTER_SAMPLES) {
filter_ptr = 0;
}
pwr = 0.0;
for (int i = 0; i < FILTER_SAMPLES; i++) {
pwr += filter_buffer[i];
}
pwr /= FILTER_SAMPLES;
}
// Map and truncate the read voltage
pwr = utils_map(pwr, config.voltage_start, config.voltage_end, 0.0, 1.0);
utils_truncate_number(&pwr, 0.0, 1.0);
// Optionally invert the read voltage
if (config.voltage_inverted) {
pwr = 1.0 - pwr;
}
decoded_level = pwr;
// Read the servo pin and optionally invert it.
bool button_val = !palReadPad(HW_ICU_GPIO, HW_ICU_PIN);
if (config.button_inverted) {
button_val = !button_val;
}
switch (config.ctrl_type) {
case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
case ADC_CTRL_TYPE_DUTY_REV_CENTER:
// Scale the voltage and set 0 at the center
pwr *= 2.0;
pwr -= 1.0;
break;
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
// Invert the voltage if the button is pressed
if (button_val) {
pwr = -pwr;
}
break;
default:
break;
}
// Apply a deadband
utils_deadband(&pwr, config.hyst, 1.0);
float current = 0;
bool current_mode = false;
bool current_mode_brake = false;
const volatile mc_configuration *mcconf = mcpwm_get_configuration();
bool send_duty = false;
// Use the filtered and mapped voltage for control according to the configuration.
switch (config.ctrl_type) {
case ADC_CTRL_TYPE_CURRENT:
case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
case ADC_CTRL_TYPE_CURRENT_REV_BUTTON:
current_mode = true;
if (pwr >= 0.0) {
current = pwr * mcconf->l_current_max;
} else {
current = pwr * fabsf(mcconf->l_current_min);
}
if (fabsf(pwr) < 0.001) {
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_FREQUENCY;
}
break;
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_BUTTON:
current_mode = true;
if (pwr >= 0.0) {
current = pwr * mcconf->l_current_max;
} else {
current = fabsf(pwr * mcconf->l_current_min);
current_mode_brake = true;
}
if (pwr < 0.001) {
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_FREQUENCY;
}
break;
case ADC_CTRL_TYPE_DUTY:
case ADC_CTRL_TYPE_DUTY_REV_CENTER:
case ADC_CTRL_TYPE_DUTY_REV_BUTTON:
if (fabsf(pwr) < 0.001) {
ms_without_power += (1000.0 * (float)sleep_time) / (float)CH_FREQUENCY;
}
if (!(ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start)) {
mcpwm_set_duty(pwr);
send_duty = true;
}
break;
default:
continue;
}
// If safe start is enabled and the output has not been zero for long enough
if (ms_without_power < MIN_MS_WITHOUT_POWER && config.safe_start) {
static int pulses_without_power_before = 0;
if (ms_without_power == pulses_without_power_before) {
ms_without_power = 0;
}
pulses_without_power_before = ms_without_power;
mcpwm_set_brake_current(timeout_get_brake_current());
continue;
}
// Reset timeout
timeout_reset();
// Find lowest RPM (for traction control)
float rpm_local = mcpwm_get_rpm();
float rpm_lowest = rpm_local;
if (config.multi_esc) {
for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
float rpm_tmp = msg->rpm;
if (fabsf(rpm_tmp) < fabsf(rpm_lowest)) {
rpm_lowest = rpm_tmp;
}
}
}
}
// Optionally send the duty cycles to the other ESCs seen on the CAN-bus
if (send_duty && config.multi_esc) {
float duty = mcpwm_get_duty_cycle_now();
for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_duty(msg->id, duty);
}
}
}
if (current_mode) {
if (current_mode_brake) {
mcpwm_set_brake_current(current);
// Send brake command to all ESCs seen recently on the CAN bus
for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
comm_can_set_current_brake(msg->id, current);
}
}
} else {
// Apply soft RPM limit
if (rpm_lowest > config.rpm_lim_end && current > 0.0) {
current = mcconf->cc_min_current;
} else if (rpm_lowest > config.rpm_lim_start && current > 0.0) {
current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
} else if (rpm_lowest < -config.rpm_lim_end && current < 0.0) {
current = mcconf->cc_min_current;
} else if (rpm_lowest < -config.rpm_lim_start && current < 0.0) {
rpm_lowest = -rpm_lowest;
current = -current;
current = utils_map(rpm_lowest, config.rpm_lim_start, config.rpm_lim_end, current, mcconf->cc_min_current);
current = -current;
rpm_lowest = -rpm_lowest;
}
float current_out = current;
bool is_reverse = false;
if (current_out < 0.0) {
is_reverse = true;
current_out = -current_out;
current = -current;
rpm_local = -rpm_local;
rpm_lowest = -rpm_lowest;
}
// Traction control
if (config.multi_esc) {
for (int i = 0; i < CAN_STATUS_MSGS_TO_STORE; i++) {
can_status_msg *msg = comm_can_get_status_msg_index(i);
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
if (config.tc) {
float rpm_tmp = msg->rpm;
if (is_reverse) {
rpm_tmp = -rpm_tmp;
}
float diff = rpm_tmp - rpm_lowest;
current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
if (current_out < mcconf->cc_min_current) {
current_out = 0.0;
}
}
if (is_reverse) {
comm_can_set_current(msg->id, -current_out);
} else {
comm_can_set_current(msg->id, current_out);
}
}
}
if (config.tc) {
float diff = rpm_local - rpm_lowest;
current_out = utils_map(diff, 0.0, config.tc_max_diff, current, 0.0);
if (current_out < mcconf->cc_min_current) {
current_out = 0.0;
}
}
}
if (is_reverse) {
mcpwm_set_current(-current_out);
} else {
mcpwm_set_current(current_out);
}
}
}
}
return 0;
}
