rusefi
/
bldc
mirror of https://github.com/rusefi/bldc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Change roll comp math to avoid external deps

This commit is contained in:
mattbutlar 2021-03-30 23:20:35 -07:00
parent 4f74c179a2
commit b61049b18e
1 changed files with 7 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
applications/app_balance.c
View File

@ -91,8 +91,7 @@ static float proportional, integral, derivative;
static float last_proportional;
static float pid_value;
static float setpoint, setpoint_target, setpoint_target_interpolated;
static uint16_t wheel_diameter;
static float turn_radius, turn_circumference, turn_angle, abs_roll_angle, roll_compensation, roll_step_size;
static float abs_roll_angle, roll_angle_sin, roll_compensation, roll_step_size;
static SetpointAdjustmentType setpointAdjustmentType;
static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value, yaw_setpoint;
static systime_t current_time, last_time, diff_time;
@ -111,7 +110,6 @@ void app_balance_configure(balance_config *conf, imu_config *conf2) {
}
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
wheel_diameter = mc_interface_get_configuration()->si_wheel_diameter;
}
void app_balance_start(void) {
@ -134,11 +132,9 @@ void reset_vars(void){
setpoint_target = 0;
setpointAdjustmentType = CENTERING;
yaw_setpoint = 0;
roll_angle_sin = 0;
roll_step_size = 0;
roll_compensation = 0;
turn_radius = 0;
turn_circumference = 0;
turn_angle = 0;
state = RUNNING;
current_time = 0;
last_time = 0;
@ -287,15 +283,14 @@ void calculate_setpoint_target(void){
}
void calculate_roll_compensation(void){
if(setpointAdjustmentType != CENTERING && (state == RUNNING || state == RUNNING_TILTBACK_CONSTANT) && abs_roll_angle > balance_conf.yaw_ki && abs_erpm > balance_conf.roll_steer_erpm_kp){
turn_radius = cosf(abs_roll_angle * M_PI / 180.0f) * wheel_diameter;
turn_circumference = (2 * M_PI) * turn_radius;
turn_angle = ((((mc_interface_get_speed() * 1000) / balance_conf.hertz) / turn_circumference) * 360.0f) / 2.0f;
if(setpointAdjustmentType != CENTERING && (state == RUNNING || state == RUNNING_TILTBACK_CONSTANT) && abs_roll_angle > balance_conf.yaw_ki && abs_erpm > balance_conf.yaw_kd){
roll_angle_sin = sinf(abs_roll_angle * M_PI / 180.0f);
roll_compensation = (roll_angle_sin * balance_conf.yaw_current_clamp) + ((abs_erpm * balance_conf.roll_steer_erpm_kp) / 60);
if(erpm > 0){
roll_compensation = (balance_conf.roll_steer_kp * turn_angle);
roll_compensation = balance_conf.roll_steer_kp * roll_compensation;
} else{
roll_compensation = (-balance_conf.roll_steer_kp * turn_angle);
roll_compensation = -balance_conf.roll_steer_kp * roll_compensation;
}
if(roll_compensation > balance_conf.yaw_current_clamp){