diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h index f03a9674..9fa9fe90 100644 --- a/appconf/appconf_default.h +++ b/appconf/appconf_default.h @@ -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 diff --git a/applications/app_balance.c b/applications/app_balance.c index 23aea2fd..171336ea 100644 --- a/applications/app_balance.c +++ b/applications/app_balance.c @@ -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); + } } diff --git a/confgenerator.c b/confgenerator.c index 26fcd3bd..665e831a 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -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; diff --git a/confgenerator.h b/confgenerator.h index 83d77e9f..e76855db 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -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); diff --git a/datatypes.h b/datatypes.h index b6a9ad95..e8f66b9c 100644 --- a/datatypes.h +++ b/datatypes.h @@ -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