<commit_before>#include "control.h"
#include "setpoint.h"
#include "core/systick.h"
#include "state.h"

static state_t _control_p_gains;
static state_t _control_i_gains;
static state_t _control_d_gains;

static uint32_t _control_last_update;
static state_t _control_setpoint_error_last;
static state_t _control_setpoint_error_integral;

void control_init(void) {
	// TODO
	// Set the gain values
}

void control_state_gains_multiply_to_motors(float *gains,
                                            float *error,
                                            float *motors) {
	int i, j;
	for(i = 0;i < 4;i++) {
		for(j = 0;j < 4;j++) {
			motors[i] += gains[j] * error[j];
		}
	}
}

void control_update(void) {
	state_t setpoint_error;
	state_t error_dt;
	state_t error_integral_slice;

	// Calculate dt and update last_ticks
	uint32_t d_msecs = systickGetTicks() - _control_last_update;
	float dt = d_msecs / 1000.0f;

	// Calculate error
	state_subtract(state_inertial_get(), setpoint_get(), &setpoint_error);

	// Calculate d error / dt
	state_subtract(&setpoint_error, &_control_setpoint_error_last, &error_dt);
	state_scale(&error_dt, 1 / d_msecs, &error_dt);

	// Calculate error integral
	state_scale(&setpoint_error, dt, &error_integral_slice);
	state_add(&error_integral_slice, &_control_setpoint_error_integral, &_control_setpoint_error_integral);

	// Update error_last
	state_copy(&setpoint_error, &_control_setpoint_error_last);

	// Accumulate gains * error
	float motor_accum[4];
	control_state_gains_multiply_to_motors((float*)&_control_p_gains,
	                                       (float*)&setpoint_error,
	                                       motor_accum);
}<commit_msg>make control.c under 80char width<commit_after>#include "control.h"
#include "setpoint.h"
#include "core/systick.h"
#include "state.h"

static state_t _control_p_gains;
static state_t _control_i_gains;
static state_t _control_d_gains;

static uint32_t _control_last_update;
static state_t _control_setpoint_error_last;
static state_t _control_setpoint_error_integral;

void control_init(void) {
	// TODO
	// Set the gain values
}

void control_state_gains_multiply_to_motors(float *gains,
                                            float *error,
                                            float *motors) {
	int i, j;
	for(i = 0;i < 4;i++) {
		for(j = 0;j < 4;j++) {
			motors[i] += gains[j] * error[j];
		}
	}
}

void control_update(void) {
	state_t setpoint_error;
	state_t error_dt;
	state_t error_integral_slice;

	// Calculate dt and update last_ticks
	uint32_t d_msecs = systickGetTicks() - _control_last_update;
	float dt = d_msecs / 1000.0f;

	// Calculate error
	state_subtract(state_inertial_get(), setpoint_get(), &setpoint_error);

	// Calculate d error / dt
	state_subtract(&setpoint_error, &_control_setpoint_error_last, &error_dt);
	state_scale(&error_dt, 1 / d_msecs, &error_dt);

	// Calculate error integral
	state_scale(&setpoint_error, dt, &error_integral_slice);
	state_add(&error_integral_slice,
	          &_control_setpoint_error_integral,
	          &_control_setpoint_error_integral);

	// Update error_last
	state_copy(&setpoint_error, &_control_setpoint_error_last);

	// Accumulate gains * error
	float motor_accum[4];
	control_state_gains_multiply_to_motors((float*)&_control_p_gains,
	                                       (float*)&setpoint_error,
	                                       motor_accum);
}
{'fraction_non_alphanumeric': 0.09825072886297376, 'fraction_numerical': 0.009329446064139942, 'mean_word_length': 3.3987179487179486, 'pattern_counts': {'":': 0, '<': 7, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 12, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '438244', 'n_tokens_mistral': 1264, 'n_tokens_neox': 1184, 'n_words': 224}