Skip to content

Commit

Permalink
Remove complete balance bump compensation logic
Browse files Browse the repository at this point in the history 
This hack is no longer needed since we now have
Mitch's Z-axis acc lowpass filter

Signed-off-by: Dado Mista <dadomista@gmail.com>
  • Loading branch information
@surfdado
surfdado committed Oct 10, 2022
1 parent 9dc8a90 commit e9d262b
Show file tree
Hide file tree
Showing 2 changed files with 0 additions and 137 deletions.
130 changes: 0 additions & 130 deletions applications/app_balance.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
#include "timeout.h" // To reset the timeout
#include "commands.h"
#include "imu/imu.h"
#include "imu/ahrs.h"
#include "utils.h"
#include "datatypes.h"
#include "comm_can.h"
Expand Down Expand Up @@ -117,18 +116,6 @@ static systime_t brake_timeout;
static float switch_warn_buzz_erpm;
static bool is_dual_switch;

// Feature: bump compensation / IMU correction
extern bool balance_bump_correction;
extern float balance_bump_adjuster;
extern float bump_correction_intensity;
static float pitch_change_aggregate;
static int pitch_steady_count;
static int pitch_change_count;
static int bump_count;
static systime_t correction_sustain;
static float correction_sustain_duration;
static bool balance_bump_beep;

// Debug values
static int debug_render_1, debug_render_2;
static int debug_sample_field, debug_sample_count, debug_sample_index;
Expand Down Expand Up @@ -200,24 +187,6 @@ void app_balance_configure(balance_config *conf, imu_config *conf2) {
if (angular_rate_kp >= 1)
angular_rate_kp = 0;

// Bump compensation
bump_correction_intensity = 1.5;
correction_sustain_duration = 500;

if (balance_conf.yaw_current_clamp == 0.01) {
bump_correction_intensity = 0;
correction_sustain_duration = 0;
}
else {
if (balance_conf.yaw_current_clamp > 5)
bump_correction_intensity = 1.5;
else
bump_correction_intensity = balance_conf.yaw_current_clamp;

correction_sustain_duration = fmaxf(50, balance_conf.roll_steer_erpm_kp);
balance_bump_beep = (balance_conf.roll_steer_kp == 0);
}

// Variable nose angle adjustment / tiltback (setting is per 1000erpm, convert to per erpm)
tiltback_variable = balance_conf.tiltback_variable / 1000;
if (tiltback_variable > 0) {
Expand Down Expand Up @@ -328,10 +297,6 @@ static void reset_vars(void){
last_time = 0;
diff_time = 0;
brake_timeout = 0;

// Bump compensation
bump_count = 0;
balance_bump_correction = false;
}

static float get_setpoint_adjustment_step_size(void){
Expand Down Expand Up @@ -614,101 +579,6 @@ static THD_FUNCTION(balance_thread, arg) {
abs_roll_angle_sin = sinf(DEG2RAD_f(abs_roll_angle));
imu_get_gyro(gyro);

float pitch_change = pitch_angle - last_pitch_angle;
float pitch_change_abs = fabsf(pitch_change);
float correction_sustain_scaling = 1.0;
if (pitch_change_abs > 0.04) {
if (pitch_change_aggregate == 0) {
pitch_change_aggregate = pitch_change;
pitch_change_count = 1;
}
else if (SIGN(pitch_change_aggregate) == SIGN(pitch_change)) {
pitch_change_count++;
pitch_change_aggregate += pitch_change;
}
else {
if (pitch_change_count > 4) {
if (pitch_change_aggregate > 1.0) {
correction_sustain_scaling = 1.3;
}
else if (pitch_change_aggregate < 0.4) {
correction_sustain_scaling = 0.7;
}
// change has reversed sign, it's considered a bump if the change was severe enough
if ((fabsf(pitch_change_aggregate) > 0.2) && (pitch_change_count < 20)) {
if (fabsf(pitch_change_aggregate) > 0.8) {
// harsh bumps:
if (ST2MS(current_time - correction_sustain) < correction_sustain_duration) {
// harsh bumps count double if we've already been correcting
bump_count += 2;
}
else {
// first harsh bump (e.g. bonk) gets discounted
bump_count += 1;
}
}
else {
// moderate bumps:
bump_count++;
}
}
else if ((fabsf(pitch_change_aggregate) > 0.8) && (pitch_change_count < 30)) {
// bigger but slower bump (may not be rider action?)
bump_count += 1;
}
}
pitch_change_aggregate = pitch_change;
pitch_change_count = 1;
}
pitch_steady_count = 0;
}
else {
pitch_steady_count++;
if (pitch_steady_count > 15) {
pitch_change_count = 0;
pitch_change_aggregate = 0;
if (pitch_steady_count > 30) {
bump_count = 0;
pitch_steady_count = 30;
}
else {
if (bump_count > 1) {
bump_count = bump_count >> 1; // Divide by 2
}
}
}
}

// pitch error is in radians, not degrees!
float pitch_error = imu_ref_get_pitch() - imu_get_pitch();
if (bump_count > 1) {
balance_bump_correction = true;
float boost = 1 + 0.1 * fminf(5, bump_count);
balance_bump_adjuster = pitch_error * bump_correction_intensity * boost;
if (balance_bump_beep)
beep_alert(1, false);
}
else {
if (balance_bump_correction && (bump_count > 0)) {
// keep correcting but by half the amount
balance_bump_adjuster = pitch_error * bump_correction_intensity;
correction_sustain = current_time;
}
else {
if (ST2MS(current_time - correction_sustain) < correction_sustain_duration * correction_sustain_scaling) {
balance_bump_adjuster = pitch_error * bump_correction_intensity * 0.8;
}
else {
if (balance_bump_correction) {
if (balance_bump_beep)
beep_off(true);
}
balance_bump_correction = false;
balance_bump_adjuster = 0;
}
}
}

duty_cycle = mc_interface_get_duty_cycle_now();
abs_duty_cycle = fabsf(duty_cycle);
erpm = mc_interface_get_rpm();
Expand Down
7 changes: 0 additions & 7 deletions imu/ahrs.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,6 @@ static float m_kp = 0.3;
static float m_ki = 0.0;
static float m_beta = 0.1;

bool balance_bump_correction = false;
float balance_bump_adjuster = 0.0;
float bump_correction_intensity = 0.0;

// Private functions
static float invSqrt(float x);
static float calculateAccConfidence(float acc_confidence_decay, float accMag, float *accMagP);
Expand Down Expand Up @@ -167,9 +163,6 @@ void ahrs_update_mahony_imu(float *gyroXYZ, float *accelXYZ, float dt, ATTITUDE_
gy += twoKp * halfey;
gz += twoKp * halfez;
}
if (balance_bump_correction) {
gy += balance_bump_adjuster;
}

// Integrate rate of change of quaternion
gx *= (0.5f * dt); // pre-multiply common factors
Expand Down

0 comments on commit e9d262b

Please sign in to comment.