static void set_modulation(float mod) {
utils_truncate_number_abs(&mod, m_conf->l_max_duty);
m_mod_now = mod;
if (m_output_mode == GPD_OUTPUT_MODE_NONE || mc_interface_get_fault() != FAULT_CODE_NONE) {
return;
}
if (m_conf->pwm_mode == PWM_MODE_BIPOLAR) {
uint32_t duty = (uint32_t) (((float)TIM1->ARR / 2.0) * mod + ((float)TIM1->ARR / 2.0));
TIM1->CCR1 = duty;
TIM1->CCR3 = duty;
// +
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM1);
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
// -
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_PWM2);
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
} else {
uint32_t duty = (uint32_t)((float)TIM1->ARR * fabsf(mod));
TIM1->CCR1 = duty;
TIM1->CCR3 = duty;
if (mod >= 0) {
// +
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM1);
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
// -
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive);
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
} else {
// +
TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_PWM1);
TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
// -
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive);
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
}
}
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
m_is_running = true;
}
void mr_control_run_iteration(float dt) {
bool lost_signal = true;
pos_get_pos(&m_pos_last);
if (mr_control_time_since_input_update() < INPUT_TIMEOUT_MS) {
lost_signal = false;
} else {
memset(&m_rc, 0, sizeof(MR_RC_STATE));
}
if (m_power_override_time > 0.0) {
m_power_override_time -= dt;
for (int i = 0;i < 4;i++) {
actuator_set_motor(i, m_power_override[i]);
}
if (m_power_override_time <= 0.0) {
memset(m_power_override, 0, sizeof(m_power_override));
}
return;
}
memset(&m_output, 0, sizeof(MR_OUTPUT));
if (!lost_signal) {
// Set roll, pitch and yaw goals to the current value if the throttle
// just got above the threshold.
static int was_below_tres = 0;
if (m_rc.throttle < MIN_THROTTLE) {
was_below_tres = 1;
actuator_set_motor(-1, 0.0);
return;
} else {
if (was_below_tres) {
m_ctrl.roll_goal = m_pos_last.roll;
m_ctrl.pitch_goal = m_pos_last.pitch;
m_ctrl.yaw_goal = m_pos_last.yaw;
m_ctrl.roll_integrator = 0.0;
m_ctrl.pitch_integrator = 0.0;
m_ctrl.yaw_integrator = 0.0;
}
was_below_tres = 0;
}
if (m_rc.throttle < THROTTLE_OVERRIDE_LIM || 1) {
// Manual control
update_rc_control(&m_ctrl, &m_rc, &m_pos_last, dt);
m_output.throttle = m_rc.throttle;
} else {
// TODO: autopilot
}
// Run attitude control
float roll_error = utils_angle_difference(m_ctrl.roll_goal, m_pos_last.roll);
float pitch_error = utils_angle_difference(m_ctrl.pitch_goal, m_pos_last.pitch);
float yaw_error = utils_angle_difference(m_ctrl.yaw_goal, m_pos_last.yaw);
roll_error /= 360.0;
pitch_error /= 360.0;
yaw_error /= 360.0;
// Run integration
m_ctrl.roll_integrator += roll_error * dt;
m_ctrl.pitch_integrator += pitch_error * dt;
m_ctrl.yaw_integrator += yaw_error * dt;
// Prevent wind-up
utils_truncate_number_abs(&m_ctrl.roll_integrator, 1.0 / main_config.mr.ctrl_gain_roll_i);
utils_truncate_number_abs(&m_ctrl.pitch_integrator, 1.0 / main_config.mr.ctrl_gain_pitch_i);
utils_truncate_number_abs(&m_ctrl.yaw_integrator, 1.0 / main_config.mr.ctrl_gain_yaw_i);
float d_roll_sample_process = -utils_angle_difference(m_pos_last.roll, m_ctrl.last_roll_process) / dt;
float d_roll_sample_error = (roll_error - m_ctrl.last_roll_error) / dt;
float d_pitch_sample_process = -utils_angle_difference(m_pos_last.pitch, m_ctrl.last_pitch_process) / dt;
float d_pitch_sample_error = (pitch_error - m_ctrl.last_pitch_error) / dt;
float d_yaw_sample_process = -utils_angle_difference(m_pos_last.yaw, m_ctrl.last_yaw_process) / dt;
float d_yaw_sample_error = (yaw_error - m_ctrl.last_yaw_error) / dt;
m_ctrl.last_roll_process = m_pos_last.roll;
m_ctrl.last_roll_error = roll_error;
m_ctrl.last_pitch_process = m_pos_last.pitch;
m_ctrl.last_pitch_error = pitch_error;
m_ctrl.last_yaw_process = m_pos_last.yaw;
m_ctrl.last_yaw_error = yaw_error;
d_roll_sample_process /= 360.0;
d_pitch_sample_process /= 360.0;
d_yaw_sample_process /= 360.0;
m_output.roll = roll_error * main_config.mr.ctrl_gain_roll_p +
m_ctrl.roll_integrator * main_config.mr.ctrl_gain_roll_i +
d_roll_sample_process * main_config.mr.ctrl_gain_roll_dp +
d_roll_sample_error * main_config.mr.ctrl_gain_roll_de;
m_output.pitch = pitch_error * main_config.mr.ctrl_gain_pitch_p +
m_ctrl.pitch_integrator * main_config.mr.ctrl_gain_pitch_i +
d_pitch_sample_process * main_config.mr.ctrl_gain_pitch_dp +
d_pitch_sample_error * main_config.mr.ctrl_gain_pitch_de;
m_output.yaw = yaw_error * main_config.mr.ctrl_gain_yaw_p +
m_ctrl.yaw_integrator * main_config.mr.ctrl_gain_yaw_i +
d_yaw_sample_process * main_config.mr.ctrl_gain_yaw_dp +
d_yaw_sample_error * main_config.mr.ctrl_gain_yaw_de;
utils_truncate_number_abs(&m_output.roll, 1.0);
utils_truncate_number_abs(&m_output.pitch, 1.0);
utils_truncate_number_abs(&m_output.yaw, 1.0);
// Compensate throttle for roll and pitch
const float tan_roll = tanf(m_pos_last.roll * M_PI / 180.0);
const float tan_pitch = tanf(m_pos_last.pitch * M_PI / 180.0);
const float tilt_comp_factor = sqrtf(tan_roll * tan_roll + tan_pitch * tan_pitch + 1);
m_output.throttle *= tilt_comp_factor;
utils_truncate_number(&m_output.throttle, 0.0, 1.0);
}
actuator_set_output(m_output.throttle, m_output.roll, m_output.pitch, m_output.yaw);
}
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);
}
