mirror of https://github.com/rusefi/bldc.git
Change roll comp math to avoid external deps
This commit is contained in:
parent
4f74c179a2
commit
b61049b18e
|
@ -91,8 +91,7 @@ static float proportional, integral, derivative;
|
||||||
static float last_proportional;
|
static float last_proportional;
|
||||||
static float pid_value;
|
static float pid_value;
|
||||||
static float setpoint, setpoint_target, setpoint_target_interpolated;
|
static float setpoint, setpoint_target, setpoint_target_interpolated;
|
||||||
static uint16_t wheel_diameter;
|
static float abs_roll_angle, roll_angle_sin, roll_compensation, roll_step_size;
|
||||||
static float turn_radius, turn_circumference, turn_angle, abs_roll_angle, roll_compensation, roll_step_size;
|
|
||||||
static SetpointAdjustmentType setpointAdjustmentType;
|
static SetpointAdjustmentType setpointAdjustmentType;
|
||||||
static float yaw_proportional, yaw_integral, yaw_derivative, yaw_last_proportional, yaw_pid_value, yaw_setpoint;
|
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;
|
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;
|
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
|
||||||
tiltback_step_size = balance_conf.tiltback_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) {
|
void app_balance_start(void) {
|
||||||
|
@ -134,11 +132,9 @@ void reset_vars(void){
|
||||||
setpoint_target = 0;
|
setpoint_target = 0;
|
||||||
setpointAdjustmentType = CENTERING;
|
setpointAdjustmentType = CENTERING;
|
||||||
yaw_setpoint = 0;
|
yaw_setpoint = 0;
|
||||||
|
roll_angle_sin = 0;
|
||||||
roll_step_size = 0;
|
roll_step_size = 0;
|
||||||
roll_compensation = 0;
|
roll_compensation = 0;
|
||||||
turn_radius = 0;
|
|
||||||
turn_circumference = 0;
|
|
||||||
turn_angle = 0;
|
|
||||||
state = RUNNING;
|
state = RUNNING;
|
||||||
current_time = 0;
|
current_time = 0;
|
||||||
last_time = 0;
|
last_time = 0;
|
||||||
|
@ -287,15 +283,14 @@ void calculate_setpoint_target(void){
|
||||||
}
|
}
|
||||||
|
|
||||||
void calculate_roll_compensation(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){
|
if(setpointAdjustmentType != CENTERING && (state == RUNNING || state == RUNNING_TILTBACK_CONSTANT) && abs_roll_angle > balance_conf.yaw_ki && abs_erpm > balance_conf.yaw_kd){
|
||||||
turn_radius = cosf(abs_roll_angle * M_PI / 180.0f) * wheel_diameter;
|
roll_angle_sin = sinf(abs_roll_angle * M_PI / 180.0f);
|
||||||
turn_circumference = (2 * M_PI) * turn_radius;
|
roll_compensation = (roll_angle_sin * balance_conf.yaw_current_clamp) + ((abs_erpm * balance_conf.roll_steer_erpm_kp) / 60);
|
||||||
turn_angle = ((((mc_interface_get_speed() * 1000) / balance_conf.hertz) / turn_circumference) * 360.0f) / 2.0f;
|
|
||||||
|
|
||||||
if(erpm > 0){
|
if(erpm > 0){
|
||||||
roll_compensation = (balance_conf.roll_steer_kp * turn_angle);
|
roll_compensation = balance_conf.roll_steer_kp * roll_compensation;
|
||||||
} else{
|
} 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){
|
if(roll_compensation > balance_conf.yaw_current_clamp){
|
||||||
|
|
Loading…
Reference in New Issue