From 7db04df582a61398d322f32b0c75bbf2642385e3 Mon Sep 17 00:00:00 2001 From: Mitch Lustig Date: Fri, 3 Jul 2020 20:33:15 -0700 Subject: [PATCH] Add all the variables --- appconf/appconf_default.h | 115 +++++++++++++++++++++---------------- applications/app_balance.c | 39 +++++++------ datatypes.h | 20 +++---- 3 files changed, 96 insertions(+), 78 deletions(-) diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index 0c6c2b85..904b8cf6 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -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 diff --git a/applications/app_balance.c b/applications/app_balance.c index 1a32405d..d1a0c6df 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -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); diff --git a/datatypes.h b/datatypes.h index 75c5d07b..72369ceb 100644 --- a/datatypes.h +++ b/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