mirror of https://github.com/rusefi/bldc.git
Add all the variables
This commit is contained in:
Mitch Lustig
parent
10f4febf5b
commit
7db04df582
|
|
@ -272,106 +272,121 @@
|
|||
|
||||
// Balance app
|
||||
#ifndef APPCONF_BALANCE_KP
|
||||
#define APPCONF_BALANCE_KP 0.0
|
||||
#define APPCONF_BALANCE_KP 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_KI
|
||||
#define APPCONF_BALANCE_KI 0.0
|
||||
#define APPCONF_BALANCE_KI 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_KD
|
||||
#define APPCONF_BALANCE_KD 0.0
|
||||
#define APPCONF_BALANCE_KD 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_HERTZ
|
||||
#define APPCONF_BALANCE_HERTZ 1000
|
||||
#define APPCONF_BALANCE_HERTZ 1000
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_PITCH_FAULT
|
||||
#define APPCONF_BALANCE_PITCH_FAULT 20
|
||||
#ifndef APPCONF_BALANCE_FAULT_PITCH
|
||||
#define APPCONF_BALANCE_FAULT_PITCH 20
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_ROLL_FAULT
|
||||
#define APPCONF_BALANCE_ROLL_FAULT 45
|
||||
#ifndef APPCONF_BALANCE_FAULT_ROLL
|
||||
#define APPCONF_BALANCE_FAULT_ROLL 45
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_ADC1
|
||||
#define APPCONF_BALANCE_ADC1 0.0
|
||||
#ifndef APPCONF_BALANCE_FAULT_DUTY
|
||||
#define APPCONF_BALANCE_FAULT_DUTY 0.9
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_ADC2
|
||||
#define APPCONF_BALANCE_ADC2 0.0
|
||||
#ifndef APPCONF_BALANCE_FAULT_ADC1
|
||||
#define APPCONF_BALANCE_FAULT_ADC1 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_OVERSPEED_DUTY
|
||||
#define APPCONF_BALANCE_OVERSPEED_DUTY 0.9
|
||||
#ifndef APPCONF_BALANCE_FAULT_ADC2
|
||||
#define APPCONF_BALANCE_FAULT_ADC2 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_TILTBACK_DUTY
|
||||
#define APPCONF_BALANCE_TILTBACK_DUTY 0.75
|
||||
#ifndef APPCONF_BALANCE_FAULT_DELAY_PITCH
|
||||
#define APPCONF_BALANCE_FAULT_DELAY_PITCH 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_FAULT_DELAY_ROLL
|
||||
#define APPCONF_BALANCE_FAULT_DELAY_ROLL 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_FAULT_DELAY_DUTY
|
||||
#define APPCONF_BALANCE_FAULT_DELAY_DUTY 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_FAULT_DELAY_SWITCH_HALF
|
||||
#define APPCONF_BALANCE_FAULT_DELAY_SWITCH_HALF 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_FAULT_DELAY_SWITCH_FULL
|
||||
#define APPCONF_BALANCE_FAULT_DELAY_SWITCH_FULL 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_FAULT_ADC_HALF_ERPM
|
||||
#define APPCONF_BALANCE_FAULT_ADC_HALF_ERPM 1000
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_TILTBACK_ANGLE
|
||||
#define APPCONF_BALANCE_TILTBACK_ANGLE 15.0
|
||||
#define APPCONF_BALANCE_TILTBACK_ANGLE 15.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_TILTBACK_SPEED
|
||||
#define APPCONF_BALANCE_TILTBACK_SPEED 5.0
|
||||
#define APPCONF_BALANCE_TILTBACK_SPEED 5.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_TILTBACK_DUTY
|
||||
#define APPCONF_BALANCE_TILTBACK_DUTY 0.75
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_TILTBACK_HIGH_V
|
||||
#define APPCONF_BALANCE_TILTBACK_HIGH_V 100.0
|
||||
#define APPCONF_BALANCE_TILTBACK_HIGH_V 200.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_TILTBACK_LOW_V
|
||||
#define APPCONF_BALANCE_TILTBACK_LOW_V 0.0
|
||||
#define APPCONF_BALANCE_TILTBACK_LOW_V 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_TILTBACK_CONSTANT
|
||||
#define APPCONF_BALANCE_TILTBACK_CONSTANT 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_TILTBACK_CONSTANT_ERPM
|
||||
#define APPCONF_BALANCE_TILTBACK_CONSTANT_ERPM 500
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_STARTUP_PITCH_TOLERANCE
|
||||
#define APPCONF_BALANCE_STARTUP_PITCH_TOLERANCE 20.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE
|
||||
#define APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE 8.0
|
||||
#define APPCONF_BALANCE_STARTUP_ROLL_TOLERANCE 8.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_STARTUP_SPEED
|
||||
#define APPCONF_BALANCE_STARTUP_SPEED 30.0
|
||||
#define APPCONF_BALANCE_STARTUP_SPEED 30.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_DEADZONE
|
||||
#define APPCONF_BALANCE_DEADZONE 0.0
|
||||
#define APPCONF_BALANCE_DEADZONE 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_CURRENT_BOOST
|
||||
#define APPCONF_BALANCE_CURRENT_BOOST 0.0
|
||||
#define APPCONF_BALANCE_CURRENT_BOOST 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_MULTI_ESC
|
||||
#define APPCONF_BALANCE_MULTI_ESC false
|
||||
#define APPCONF_BALANCE_MULTI_ESC false
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_YAW_KP
|
||||
#define APPCONF_BALANCE_YAW_KP 0.0
|
||||
#define APPCONF_BALANCE_YAW_KP 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_YAW_KI
|
||||
#define APPCONF_BALANCE_YAW_KI 0.0
|
||||
#define APPCONF_BALANCE_YAW_KI 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_YAW_KD
|
||||
#define APPCONF_BALANCE_YAW_KD 0.0
|
||||
#define APPCONF_BALANCE_YAW_KD 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_ROLL_STEER_KP
|
||||
#define APPCONF_BALANCE_ROLL_STEER_KP 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_BRAKE_CURRENT
|
||||
#define APPCONF_BALANCE_BRAKE_CURRENT 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_OVERSPEED_DELAY
|
||||
#define APPCONF_BALANCE_OVERSPEED_DELAY 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_FAULT_DELAY
|
||||
#define APPCONF_BALANCE_FAULT_DELAY 0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_TILTBACK_CONSTANT
|
||||
#define APPCONF_BALANCE_TILTBACK_CONSTANT 0.0
|
||||
#define APPCONF_BALANCE_ROLL_STEER_KP 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_ROLL_STEER_ERPM_KP
|
||||
#define APPCONF_BALANCE_ROLL_STEER_ERPM_KP 0.0
|
||||
#define APPCONF_BALANCE_ROLL_STEER_ERPM_KP 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_BRAKE_CURRENT
|
||||
#define APPCONF_BALANCE_BRAKE_CURRENT 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_YAW_CURRENT_CLAMP
|
||||
#define APPCONF_BALANCE_YAW_CURRENT_CLAMP 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_ADC_HALF_FAULT_ERPM
|
||||
#define APPCONF_BALANCE_ADC_HALF_FAULT_ERPM 1000
|
||||
#define APPCONF_BALANCE_YAW_CURRENT_CLAMP 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_SETPOINT_PITCH_FILTER
|
||||
#define APPCONF_BALANCE_SETPOINT_PITCH_FILTER 0.0
|
||||
#define APPCONF_BALANCE_SETPOINT_PITCH_FILTER 0.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_SETPOINT_TARGET_FILTER
|
||||
#define APPCONF_BALANCE_SETPOINT_TARGET_FILTER 1.0
|
||||
#define APPCONF_BALANCE_SETPOINT_TARGET_FILTER 1.0
|
||||
#endif
|
||||
#ifndef APPCONF_BALANCE_SETPOINT_CLAMP
|
||||
#define APPCONF_BALANCE_SETPOINT_CLAMP 80.0
|
||||
#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
|
||||
|
||||
// IMU
|
||||
|
|
|
|||
|
|
@ -100,10 +100,11 @@ void app_balance_configure(balance_config *conf, imu_config *conf2) {
|
|||
balance_conf = *conf;
|
||||
imu_conf = *conf2;
|
||||
// Set calculated values from config
|
||||
float f_cut = balance_conf.kd_pt1_hertz;
|
||||
float dT = 1.0 / balance_conf.hertz;
|
||||
float RC = 1 / ( 2 * M_PI * f_cut);
|
||||
d_pt1_k = dT / (RC + dT);
|
||||
if(balance_conf.kd_pt1_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);
|
||||
}
|
||||
startup_step_size = balance_conf.startup_speed / balance_conf.hertz;
|
||||
tiltback_step_size = balance_conf.tiltback_speed / balance_conf.hertz;
|
||||
}
|
||||
|
|
@ -189,7 +190,7 @@ bool check_faults(bool ignoreTimers){
|
|||
}
|
||||
|
||||
// Switch partially open and stopped
|
||||
if(switch_state == HALF && abs_erpm < balance_conf.adc_half_fault_erpm){
|
||||
if(switch_state == HALF && abs_erpm < balance_conf.fault_adc_half_erpm){
|
||||
if(ST2MS(current_time - fault_switch_half_timer) > balance_conf.fault_delay_switch_half || ignoreTimers){
|
||||
state = FAULT_SWITCH;
|
||||
return true;
|
||||
|
|
@ -368,24 +369,24 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
#endif
|
||||
|
||||
// Calculate switch state from ADC values
|
||||
if(balance_conf.adc1 == 0 && balance_conf.adc2 == 0){ // No Switch
|
||||
if(balance_conf.fault_adc1 == 0 && balance_conf.fault_adc2 == 0){ // No Switch
|
||||
switch_state = ON;
|
||||
}else if(balance_conf.adc2 == 0){ // Single switch on ADC1
|
||||
if(adc1 > balance_conf.adc1){
|
||||
}else if(balance_conf.fault_adc2 == 0){ // Single switch on ADC1
|
||||
if(adc1 > balance_conf.fault_adc1){
|
||||
switch_state = ON;
|
||||
} else {
|
||||
switch_state = OFF;
|
||||
}
|
||||
}else if(balance_conf.adc1 == 0){ // Single switch on ADC2
|
||||
if(adc2 > balance_conf.adc2){
|
||||
}else if(balance_conf.fault_adc1 == 0){ // Single switch on ADC2
|
||||
if(adc2 > balance_conf.fault_adc2){
|
||||
switch_state = ON;
|
||||
} else {
|
||||
switch_state = OFF;
|
||||
}
|
||||
}else{ // Double switch
|
||||
if(adc1 > balance_conf.adc1 && adc2 > balance_conf.adc2){
|
||||
if(adc1 > balance_conf.fault_adc1 && adc2 > balance_conf.fault_adc2){
|
||||
switch_state = ON;
|
||||
}else if(adc1 > balance_conf.adc1 || adc2 > balance_conf.adc2){
|
||||
}else if(adc1 > balance_conf.fault_adc1 || adc2 > balance_conf.fault_adc2){
|
||||
switch_state = HALF;
|
||||
}else{
|
||||
switch_state = OFF;
|
||||
|
|
@ -430,10 +431,10 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
|
||||
// Clamp setpoint
|
||||
if(setpointAdjustmentType != CENTERING){
|
||||
if(setpoint - setpoint_target_interpolated > balance_conf.setpoint_clamp){
|
||||
setpoint = setpoint_target_interpolated + balance_conf.setpoint_clamp;
|
||||
}else if (setpoint - setpoint_target_interpolated < -balance_conf.setpoint_clamp){
|
||||
setpoint = setpoint_target_interpolated - balance_conf.setpoint_clamp;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -446,8 +447,10 @@ static THD_FUNCTION(balance_thread, arg) {
|
|||
derivative = proportional - last_proportional;
|
||||
|
||||
// Apply D term only filter
|
||||
d_pt1_state = d_pt1_state + d_pt1_k * (derivative - d_pt1_state);
|
||||
derivative = d_pt1_state;
|
||||
if(balance_conf.kd_pt1_frequency > 0){
|
||||
d_pt1_state = d_pt1_state + d_pt1_k * (derivative - d_pt1_state);
|
||||
derivative = d_pt1_state;
|
||||
}
|
||||
|
||||
pid_value = (balance_conf.kp * proportional) + (balance_conf.ki * integral) + (balance_conf.kd * derivative);
|
||||
|
||||
|
|
|
|||
20
datatypes.h
20
datatypes.h
|
|
@ -550,17 +550,17 @@ typedef struct {
|
|||
float fault_pitch;
|
||||
float fault_roll;
|
||||
float fault_duty;
|
||||
uint16_t fault_delay_duty;
|
||||
uint16_t fault_delay_switch_full;
|
||||
uint16_t fault_delay_switch_half;
|
||||
float fault_adc1;
|
||||
float fault_adc2;
|
||||
uint16_t fault_delay_pitch;
|
||||
uint16_t fault_delay_roll;
|
||||
float adc1;
|
||||
float adc2;
|
||||
uint16_t adc_half_fault_erpm;
|
||||
float tiltback_duty;
|
||||
uint16_t fault_delay_duty;
|
||||
uint16_t fault_delay_switch_half;
|
||||
uint16_t fault_delay_switch_full;
|
||||
uint16_t fault_adc_half_erpm;
|
||||
float tiltback_angle;
|
||||
float tiltback_speed;
|
||||
float tiltback_duty;
|
||||
float tiltback_high_voltage;
|
||||
float tiltback_low_voltage;
|
||||
float tiltback_constant;
|
||||
|
|
@ -575,13 +575,13 @@ typedef struct {
|
|||
float yaw_ki;
|
||||
float yaw_kd;
|
||||
float roll_steer_kp;
|
||||
float brake_current;
|
||||
float roll_steer_erpm_kp;
|
||||
float brake_current;
|
||||
float yaw_current_clamp;
|
||||
float setpoint_pitch_filter;
|
||||
float setpoint_target_filter;
|
||||
float setpoint_clamp;
|
||||
float kd_pt1_hertz;
|
||||
float setpoint_filter_clamp;
|
||||
uint16_t kd_pt1_frequency;
|
||||
} balance_config;
|
||||
|
||||
// CAN status modes
|
||||
|
|
|
|||
Loading…
Reference in New Issue