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 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;
}
