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

Merge pull request #272 from Mitchlol/balance_4 

Balance 4
This commit is contained in:
Benjamin Vedder 2021-03-30 09:03:35 +02:00 committed by GitHub
parent 7355ae7e14 9f41e4407f
commit 86e574aa1b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 93 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
appconf/appconf_default.h
View File

@ -352,9 +352,6 @@
#ifndef APPCONF_BALANCE_DEADZONE
#define APPCONF_BALANCE_DEADZONE 0.0
#endif
#ifndef APPCONF_BALANCE_CURRENT_BOOST
#define APPCONF_BALANCE_CURRENT_BOOST 0.0
#endif
#ifndef APPCONF_BALANCE_MULTI_ESC
#define APPCONF_BALANCE_MULTI_ESC false
#endif
@ -379,18 +376,33 @@
#ifndef APPCONF_BALANCE_YAW_CURRENT_CLAMP
#define APPCONF_BALANCE_YAW_CURRENT_CLAMP 0.0
#endif
#ifndef APPCONF_BALANCE_SETPOINT_PITCH_FILTER
#define APPCONF_BALANCE_SETPOINT_PITCH_FILTER 0.0
#endif
#ifndef APPCONF_BALANCE_SETPOINT_TARGET_FILTER
#define APPCONF_BALANCE_SETPOINT_TARGET_FILTER 1.0
#endif
#ifndef APPCONF_BALANCE_SETPOINT_FILTER_CLAMP
#define APPCONF_BALANCE_SETPOINT_FILTER_CLAMP 8.0
#endif
#ifndef APPCONF_BALANCE_KD_PT1_FREQUENCY
#define APPCONF_BALANCE_KD_PT1_FREQUENCY 0
#endif
#ifndef APPCONF_BALANCE_BOOSTER_ANGLE
#define APPCONF_BALANCE_BOOSTER_ANGLE 8
#endif
#ifndef APPCONF_BALANCE_BOOSTER_RAMP
#define APPCONF_BALANCE_BOOSTER_RAMP 1
#endif
#ifndef APPCONF_BALANCE_BOOSTER_CURRENT
#define APPCONF_BALANCE_BOOSTER_CURRENT 0
#endif
#ifndef APPCONF_BALANCE_TORQUETILT_START_CURRENT
#define APPCONF_BALANCE_TORQUETILT_START_CURRENT 10
#endif
#ifndef APPCONF_BALANCE_TORQUETILT_ANGLE_LIMIT
#define APPCONF_BALANCE_TORQUETILT_ANGLE_LIMIT 5
#endif
#ifndef APPCONF_BALANCE_TORQUETILT_SPEED
#define APPCONF_BALANCE_TORQUETILT_SPEED 5
#endif
#ifndef APPCONF_BALANCE_TORQUETILT_POWER
#define APPCONF_BALANCE_TORQUETILT_POWER 0.0
#endif
#ifndef APPCONF_BALANCE_TORQUETILT_FILTER
#define APPCONF_BALANCE_TORQUETILT_FILTER 0.95
#endif
// PAS app
#ifndef APPCONF_PAS_CTRL_TYPE

62
applications/app_balance.c
View File

@ -73,7 +73,7 @@ static thread_t *app_thread;
// Config values
static volatile balance_config balance_conf;
static volatile imu_config imu_conf;
static float startup_step_size, tiltback_step_size;
static float startup_step_size, tiltback_step_size, torquetilt_step_size;
// Runtime values read from elsewhere
static float pitch_angle, roll_angle;
@ -88,9 +88,9 @@ static SwitchState switch_state;
// Rumtime state values
static BalanceState state;
static float proportional, integral, derivative;
static float last_proportional;
static float last_proportional, abs_proportional;
static float pid_value;
static float setpoint, setpoint_target, setpoint_target_interpolated;
static float setpoint, setpoint_target, setpoint_target_interpolated, torquetilt_target, torquetilt_interpolated, torquetilt_filtered_current;
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;
@ -109,6 +109,7 @@ 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;
torquetilt_step_size = balance_conf.torquetilt_speed / balance_conf.hertz;
}
void app_balance_start(void) {
@ -129,6 +130,9 @@ void reset_vars(void){
setpoint = pitch_angle;
setpoint_target_interpolated = pitch_angle;
setpoint_target = 0;
torquetilt_target = 0;
torquetilt_interpolated = 0;
torquetilt_filtered_current = 0;
setpointAdjustmentType = CENTERING;
yaw_setpoint = 0;
state = RUNNING;
@ -291,6 +295,25 @@ void calculate_setpoint_interpolated(void){
}
}
void apply_torque_tilt(void){
// Filter current (Basic LPF)
torquetilt_filtered_current = ((1-balance_conf.torquetilt_filter) * motor_current) + (balance_conf.torquetilt_filter * torquetilt_filtered_current);
// Wat is this line O_o
// Take abs motor current, subtract start offset, and take the max of that with 0 to get the current above our start threshold (absolute).
// Then multiply it by "power" to get our desired angle, and min with the limit to respect boundaries.
// Finally multiply it by sign motor current to get directionality back
torquetilt_target = fminf(fmaxf((fabsf(torquetilt_filtered_current) - balance_conf.torquetilt_start_current), 0) * balance_conf.torquetilt_power, balance_conf.torquetilt_angle_limit) * SIGN(torquetilt_filtered_current);
if(fabsf(torquetilt_target - torquetilt_interpolated) < torquetilt_step_size){
torquetilt_interpolated = torquetilt_target;
}else if (torquetilt_target - torquetilt_interpolated > 0){
torquetilt_interpolated += torquetilt_step_size;
}else{
torquetilt_interpolated -= torquetilt_step_size;
}
setpoint += torquetilt_interpolated;
}
float apply_deadzone(float error){
if(balance_conf.deadzone == 0){
return error;
@ -440,24 +463,8 @@ static THD_FUNCTION(balance_thread, arg) {
// Calculate setpoint and interpolation
calculate_setpoint_target();
calculate_setpoint_interpolated();
// Apply setpoint filtering
if(setpointAdjustmentType == CENTERING){
// Ignore filtering during centering
setpoint = setpoint_target_interpolated;
}else{
setpoint = (setpoint * (1-balance_conf.setpoint_pitch_filter)) + (pitch_angle * balance_conf.setpoint_pitch_filter);
setpoint = (setpoint * (1-balance_conf.setpoint_target_filter)) + (setpoint_target_interpolated * balance_conf.setpoint_target_filter);
}
// Clamp setpoint
if(setpointAdjustmentType != CENTERING){
if(setpoint - setpoint_target_interpolated > balance_conf.setpoint_filter_clamp){
setpoint = setpoint_target_interpolated + balance_conf.setpoint_filter_clamp;
}else if (setpoint - setpoint_target_interpolated < -balance_conf.setpoint_filter_clamp){
setpoint = setpoint_target_interpolated - balance_conf.setpoint_filter_clamp;
}
}
setpoint = setpoint_target_interpolated;
apply_torque_tilt();
// Do PID maths
proportional = setpoint - pitch_angle;
@ -477,11 +484,14 @@ static THD_FUNCTION(balance_thread, arg) {
last_proportional = proportional;
// Apply current boost
if(pid_value > 0){
pid_value += balance_conf.current_boost;
}else if(pid_value < 0){
pid_value -= balance_conf.current_boost;
// Apply Booster
abs_proportional = fabsf(proportional);
if(abs_proportional > balance_conf.booster_angle){
if(abs_proportional - balance_conf.booster_angle < balance_conf.booster_ramp){
pid_value += (balance_conf.booster_current * SIGN(proportional)) * (abs_proportional - balance_conf.booster_angle);
}else{
pid_value += balance_conf.booster_current * SIGN(proportional);
}
}

36
confgenerator.c
View File

@ -287,7 +287,6 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_roll_tolerance, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.startup_speed, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.deadzone, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.current_boost, &ind);
buffer[ind++] = conf->app_balance_conf.multi_esc;
buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_kp, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_ki, &ind);
@ -296,10 +295,15 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
buffer_append_float32_auto(buffer, conf->app_balance_conf.roll_steer_erpm_kp, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.brake_current, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.yaw_current_clamp, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.setpoint_pitch_filter, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.setpoint_target_filter, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.setpoint_filter_clamp, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.kd_pt1_frequency, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.booster_angle, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.booster_ramp, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.booster_current, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.torquetilt_start_current, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.torquetilt_angle_limit, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.torquetilt_speed, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.torquetilt_power, &ind);
buffer_append_float32_auto(buffer, conf->app_balance_conf.torquetilt_filter, &ind);
buffer[ind++] = conf->app_pas_conf.ctrl_type;
buffer[ind++] = conf->app_pas_conf.sensor_type;
buffer_append_float16(buffer, conf->app_pas_conf.current_scaling, 1000, &ind);
@ -624,7 +628,6 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->app_balance_conf.startup_roll_tolerance = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.startup_speed = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.deadzone = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.current_boost = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.multi_esc = buffer[ind++];
conf->app_balance_conf.yaw_kp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.yaw_ki = buffer_get_float32_auto(buffer, &ind);
@ -633,10 +636,15 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
conf->app_balance_conf.roll_steer_erpm_kp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.brake_current = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.yaw_current_clamp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.setpoint_pitch_filter = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.setpoint_target_filter = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.setpoint_filter_clamp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.kd_pt1_frequency = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.booster_angle = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.booster_ramp = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.booster_current = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.torquetilt_start_current = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.torquetilt_angle_limit = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.torquetilt_speed = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.torquetilt_power = buffer_get_float32_auto(buffer, &ind);
conf->app_balance_conf.torquetilt_filter = buffer_get_float32_auto(buffer, &ind);
conf->app_pas_conf.ctrl_type = buffer[ind++];
conf->app_pas_conf.sensor_type = buffer[ind++];
conf->app_pas_conf.current_scaling = buffer_get_float16(buffer, 1000, &ind);
@ -945,7 +953,6 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->app_balance_conf.startup_roll_tolerance = APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE;
conf->app_balance_conf.startup_speed = APPCONF_BALANCE_STARTUP_SPEED;
conf->app_balance_conf.deadzone = APPCONF_BALANCE_DEADZONE;
conf->app_balance_conf.current_boost = APPCONF_BALANCE_CURRENT_BOOST;
conf->app_balance_conf.multi_esc = APPCONF_BALANCE_MULTI_ESC;
conf->app_balance_conf.yaw_kp = APPCONF_BALANCE_YAW_KP;
conf->app_balance_conf.yaw_ki = APPCONF_BALANCE_YAW_KI;
@ -954,10 +961,15 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
conf->app_balance_conf.roll_steer_erpm_kp = APPCONF_BALANCE_ROLL_STEER_ERPM_KP;
conf->app_balance_conf.brake_current = APPCONF_BALANCE_BRAKE_CURRENT;
conf->app_balance_conf.yaw_current_clamp = APPCONF_BALANCE_YAW_CURRENT_CLAMP;
conf->app_balance_conf.setpoint_pitch_filter = APPCONF_BALANCE_SETPOINT_PITCH_FILTER;
conf->app_balance_conf.setpoint_target_filter = APPCONF_BALANCE_SETPOINT_TARGET_FILTER;
conf->app_balance_conf.setpoint_filter_clamp = APPCONF_BALANCE_SETPOINT_FILTER_CLAMP;
conf->app_balance_conf.kd_pt1_frequency = APPCONF_BALANCE_KD_PT1_FREQUENCY;
conf->app_balance_conf.booster_angle = APPCONF_BALANCE_BOOSTER_ANGLE;
conf->app_balance_conf.booster_ramp = APPCONF_BALANCE_BOOSTER_RAMP;
conf->app_balance_conf.booster_current = APPCONF_BALANCE_BOOSTER_CURRENT;
conf->app_balance_conf.torquetilt_start_current = APPCONF_BALANCE_TORQUETILT_START_CURRENT;
conf->app_balance_conf.torquetilt_angle_limit = APPCONF_BALANCE_TORQUETILT_ANGLE_LIMIT;
conf->app_balance_conf.torquetilt_speed = APPCONF_BALANCE_TORQUETILT_SPEED;
conf->app_balance_conf.torquetilt_power = APPCONF_BALANCE_TORQUETILT_POWER;
conf->app_balance_conf.torquetilt_filter = APPCONF_BALANCE_TORQUETILT_FILTER;
conf->app_pas_conf.ctrl_type = APPCONF_PAS_CTRL_TYPE;
conf->app_pas_conf.sensor_type = APPCONF_PAS_SENSOR_TYPE;
conf->app_pas_conf.current_scaling = APPCONF_PAS_CURRENT_SCALING;

2
confgenerator.h
View File

@ -9,7 +9,7 @@
// Constants
#define MCCONF_SIGNATURE 2835115562
#define APPCONF_SIGNATURE 3264926020
#define APPCONF_SIGNATURE 3708020089
// Functions
int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

12
datatypes.h
View File

@ -690,7 +690,6 @@ typedef struct {
float startup_roll_tolerance;
float startup_speed;
float deadzone;
float current_boost;
bool multi_esc;
float yaw_kp;
float yaw_ki;
@ -699,10 +698,15 @@ typedef struct {
float roll_steer_erpm_kp;
float brake_current;
float yaw_current_clamp;
float setpoint_pitch_filter;
float setpoint_target_filter;
float setpoint_filter_clamp;
uint16_t kd_pt1_frequency;
float booster_angle;
float booster_ramp;
float booster_current;
float torquetilt_start_current;
float torquetilt_angle_limit;
float torquetilt_speed;
float torquetilt_power;
float torquetilt_filter;
} balance_config;
// CAN status modes