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

Merge pull request #299 from Mitchlol/balance_4.3 

Balance 4.3
This commit is contained in:
Benjamin Vedder 2021-05-18 23:40:19 +02:00 committed by GitHub
parent ad06b4cf5d b237cf4b9f
commit bfe1fa0a12
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 75 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
appconf/appconf_default.h
View File

@ -379,8 +379,11 @@
#ifndef APPCONF_BALANCE_YAW_CURRENT_CLAMP
#define APPCONF_BALANCE_YAW_CURRENT_CLAMP 0.0
#endif
#ifndef APPCONF_BALANCE_KD_PT1_FREQUENCY
#define APPCONF_BALANCE_KD_PT1_FREQUENCY 0
#ifndef APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY
#define APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY 0
#endif
#ifndef APPCONF_BALANCE_KD_PT1_HIGHPASS_FREQUENCY
#define APPCONF_BALANCE_KD_PT1_HIGHPASS_FREQUENCY 0
#endif
#ifndef APPCONF_BALANCE_BOOSTER_ANGLE
#define APPCONF_BALANCE_BOOSTER_ANGLE 8

92
applications/app_balance.c
View File

@ -37,20 +37,19 @@
// Can
#define MAX_CAN_AGE 0.1
// Data type
// Data type (Value 5 was removed, and can be reused at a later date, but i wanted to preserve the current value's numbers for UIs)
typedef enum {
STARTUP = 0,
RUNNING,
RUNNING_TILTBACK_DUTY,
RUNNING_TILTBACK_HIGH_VOLTAGE,
RUNNING_TILTBACK_LOW_VOLTAGE,
RUNNING_TILTBACK_CONSTANT,
FAULT_ANGLE_PITCH,
FAULT_ANGLE_ROLL,
FAULT_SWITCH_HALF,
FAULT_SWITCH_FULL,
FAULT_DUTY,
FAULT_STARTUP
RUNNING = 1,
RUNNING_TILTBACK_DUTY = 2,
RUNNING_TILTBACK_HIGH_VOLTAGE = 3,
RUNNING_TILTBACK_LOW_VOLTAGE = 4,
FAULT_ANGLE_PITCH = 6,
FAULT_ANGLE_ROLL = 7,
FAULT_SWITCH_HALF = 8,
FAULT_SWITCH_FULL = 9,
FAULT_DUTY = 10,
FAULT_STARTUP = 11
} BalanceState;
typedef enum {
@ -91,23 +90,29 @@ static float proportional, integral, derivative;
static float last_proportional, abs_proportional;
static float pid_value;
static float setpoint, setpoint_target, setpoint_target_interpolated;
static float constanttilt_target, constanttilt_interpolated;
static float torquetilt_filtered_current, torquetilt_target, torquetilt_interpolated;
static float turntilt_target, turntilt_interpolated;
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;
static systime_t fault_angle_pitch_timer, fault_angle_roll_timer, fault_switch_timer, fault_switch_half_timer, fault_duty_timer;
static float d_pt1_state, d_pt1_k;
static float d_pt1_lowpass_state, d_pt1_lowpass_k, d_pt1_highpass_state, d_pt1_highpass_k;
void app_balance_configure(balance_config *conf, imu_config *conf2) {
balance_conf = *conf;
imu_conf = *conf2;
// Set calculated values from config
if(balance_conf.kd_pt1_frequency > 0){
if(balance_conf.kd_pt1_lowpass_frequency > 0){
float dT = 1.0 / balance_conf.hertz;
float RC = 1.0 / ( 2.0 * M_PI * balance_conf.kd_pt1_frequency);
d_pt1_k = dT / (RC + dT);
float RC = 1.0 / ( 2.0 * M_PI * balance_conf.kd_pt1_lowpass_frequency);
d_pt1_lowpass_k = dT / (RC + dT);
}
if(balance_conf.kd_pt1_highpass_frequency > 0){
float dT = 1.0 / balance_conf.hertz;
float RC = 1.0 / ( 2.0 * M_PI * balance_conf.kd_pt1_highpass_frequency);
d_pt1_highpass_k = dT / (RC + dT);
}
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
@ -128,11 +133,14 @@ void reset_vars(void){
last_proportional = 0;
yaw_integral = 0;
yaw_last_proportional = 0;
d_pt1_state = 0;
d_pt1_lowpass_state = 0;
d_pt1_highpass_state = 0;
// Set values for startup
setpoint = pitch_angle;
setpoint_target_interpolated = pitch_angle;
setpoint_target = 0;
constanttilt_target = 0;
constanttilt_interpolated = 0;
torquetilt_target = 0;
torquetilt_interpolated = 0;
torquetilt_filtered_current = 0;
@ -271,15 +279,6 @@ void calculate_setpoint_target(void){
}
setpointAdjustmentType = TILTBACK;
state = RUNNING_TILTBACK_LOW_VOLTAGE;
}else if(balance_conf.tiltback_constant != 0 && abs_erpm > balance_conf.tiltback_constant_erpm){
// Nose angle adjustment
if(erpm > 0){
setpoint_target = balance_conf.tiltback_constant;
} else {
setpoint_target = -balance_conf.tiltback_constant;
}
setpointAdjustmentType = TILTBACK;
state = RUNNING_TILTBACK_CONSTANT;
}else{
setpointAdjustmentType = TILTBACK;
setpoint_target = 0;
@ -300,6 +299,26 @@ void calculate_setpoint_interpolated(void){
}
}
void apply_constanttilt(void){
// Nose angle adjustment
if(erpm > balance_conf.tiltback_constant_erpm){
constanttilt_target = balance_conf.tiltback_constant;
} else if(erpm < -balance_conf.tiltback_constant_erpm){
constanttilt_target = -balance_conf.tiltback_constant;
}else{
constanttilt_target = 0;
}
if(fabsf(constanttilt_target - constanttilt_interpolated) < tiltback_step_size){
constanttilt_interpolated = constanttilt_target;
}else if (constanttilt_target - constanttilt_interpolated > 0){
constanttilt_interpolated += tiltback_step_size;
}else{
constanttilt_interpolated -= tiltback_step_size;
}
setpoint += constanttilt_interpolated;
}
void apply_torquetilt(void){
// Filter current (Basic LPF)
torquetilt_filtered_current = ((1-balance_conf.torquetilt_filter) * motor_current) + (balance_conf.torquetilt_filter * torquetilt_filtered_current);
@ -391,15 +410,22 @@ void set_current(float current, float yaw_current){
timeout_reset();
// Set current
if(balance_conf.multi_esc){
// Set the current delay
mc_interface_set_current_off_delay((1000.0 / balance_conf.hertz)/1000.0);
// Set Current
mc_interface_set_current(current + yaw_current);
// 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(msg->id, current - yaw_current);// Assume 2 motors, i don't know how to steer 3 anyways
comm_can_set_current_off_delay(msg->id, current - yaw_current, (1000.0 / balance_conf.hertz)/1000.0);// Assume 2 motors, i don't know how to steer 3 anyways
}
}
} else {
// Set the current delay
mc_interface_set_current_off_delay((1000.0 / balance_conf.hertz)/1000.0);
// Set Current
mc_interface_set_current(current);
}
}
@ -498,7 +524,6 @@ static THD_FUNCTION(balance_thread, arg) {
case (RUNNING_TILTBACK_DUTY):
case (RUNNING_TILTBACK_HIGH_VOLTAGE):
case (RUNNING_TILTBACK_LOW_VOLTAGE):
case (RUNNING_TILTBACK_CONSTANT):
// Check for faults
if(check_faults(false)){
@ -509,6 +534,7 @@ static THD_FUNCTION(balance_thread, arg) {
calculate_setpoint_target();
calculate_setpoint_interpolated();
setpoint = setpoint_target_interpolated;
apply_constanttilt();
apply_torquetilt();
apply_turntilt();
@ -521,9 +547,13 @@ static THD_FUNCTION(balance_thread, arg) {
derivative = proportional - last_proportional;
// Apply D term only filter
if(balance_conf.kd_pt1_frequency > 0){
d_pt1_state = d_pt1_state + d_pt1_k * (derivative - d_pt1_state);
derivative = d_pt1_state;
if(balance_conf.kd_pt1_lowpass_frequency > 0){
d_pt1_lowpass_state = d_pt1_lowpass_state + d_pt1_lowpass_k * (derivative - d_pt1_lowpass_state);
derivative = d_pt1_lowpass_state;
}
if(balance_conf.kd_pt1_highpass_frequency > 0){
d_pt1_highpass_state = d_pt1_highpass_state + d_pt1_highpass_k * (derivative - d_pt1_highpass_state);
derivative = derivative - d_pt1_highpass_state;
}
pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);

9
confgenerator.c
View File

@ -301,7 +301,8 @@ 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_uint16(buffer, conf->app_balance_conf.kd_pt1_frequency, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.kd_pt1_lowpass_frequency, &ind);
buffer_append_uint16(buffer, conf->app_balance_conf.kd_pt1_highpass_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);
@ -655,7 +656,8 @@ 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.kd_pt1_frequency = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.kd_pt1_lowpass_frequency = buffer_get_uint16(buffer, &ind);
conf->app_balance_conf.kd_pt1_highpass_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);
@ -993,7 +995,8 @@ 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.kd_pt1_frequency = APPCONF_BALANCE_KD_PT1_FREQUENCY;
conf->app_balance_conf.kd_pt1_lowpass_frequency = APPCONF_BALANCE_KD_PT1_LOWPASS_FREQUENCY;
conf->app_balance_conf.kd_pt1_highpass_frequency = APPCONF_BALANCE_KD_PT1_HIGHPASS_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;

2
confgenerator.h
View File

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

3
datatypes.h
View File

@ -707,7 +707,8 @@ typedef struct {
float roll_steer_erpm_kp;
float brake_current;
float yaw_current_clamp;
uint16_t kd_pt1_frequency;
uint16_t kd_pt1_lowpass_frequency;
uint16_t kd_pt1_highpass_frequency;
float booster_angle;
float booster_ramp;
float booster_current;