From e25d4f5918cc703c3295063efa221c082aaf6522 Mon Sep 17 00:00:00 2001 From: bkleiner Date: Sun, 22 Oct 2023 21:15:46 +0200 Subject: [PATCH] drop pid_precalc, use single function --- src/core/looptime.c | 2 + src/flight/angle_pid.c | 6 +- src/flight/control.c | 2 - src/flight/control.h | 10 +- src/flight/imu.c | 36 +++---- src/flight/pid.c | 226 ++++++++++++++++++----------------------- src/flight/pid.h | 1 - src/flight/sixaxis.c | 4 + 8 files changed, 127 insertions(+), 160 deletions(-) diff --git a/src/core/looptime.c b/src/core/looptime.c index f3ca9e1c6..3e43ab2be 100644 --- a/src/core/looptime.c +++ b/src/core/looptime.c @@ -74,6 +74,8 @@ void looptime_update() { state.looptime_us = CYCLES_TO_US(time_cycles() - last_loop_cycles); state.looptime = state.looptime_us * 1e-6f; + // 0.0032f is there for legacy purposes, should be 0.001f = looptime + state.timefactor = 0.0032f / state.looptime; state.loop_counter++; last_loop_cycles = time_cycles(); diff --git a/src/flight/angle_pid.c b/src/flight/angle_pid.c index d8e452fab..a2e347db2 100644 --- a/src/flight/angle_pid.c +++ b/src/flight/angle_pid.c @@ -16,8 +16,6 @@ static float apidoutput2[ANGLE_PID_SIZE]; static float apidoutput[ANGLE_PID_SIZE]; static float lasterror[ANGLE_PID_SIZE]; -extern float timefactor; - float angle_pid(int x) { const float angle_error_abs = fabsf(state.angleerror[x]); @@ -28,10 +26,10 @@ float angle_pid(int x) { apidoutput2[x] = angle_error_abs * state.angleerror[x] * profile.pid.big_angle.kp; // D term 1 weighted + P term 1 weighted - apidoutput1[x] += (state.angleerror[x] - lasterror[x]) * profile.pid.small_angle.kd * (1 - angle_error_abs) * timefactor; + apidoutput1[x] += (state.angleerror[x] - lasterror[x]) * profile.pid.small_angle.kd * (1 - angle_error_abs) * state.timefactor; // D term 2 weighted + P term 2 weighted - apidoutput2[x] += ((state.angleerror[x] - lasterror[x]) * profile.pid.big_angle.kd * angle_error_abs * timefactor); + apidoutput2[x] += ((state.angleerror[x] - lasterror[x]) * profile.pid.big_angle.kd * angle_error_abs * state.timefactor); // apidoutput sum apidoutput[x] = apidoutput1[x] + apidoutput2[x]; diff --git a/src/flight/control.c b/src/flight/control.c index bcd4e3c0a..c471c8922 100644 --- a/src/flight/control.c +++ b/src/flight/control.c @@ -265,8 +265,6 @@ static void control_flight_mode() { } void control() { - pid_precalc(); - if (rx_aux_on(AUX_TURTLE) && !rx_aux_on(AUX_MOTOR_TEST)) { // turtle active when aux high turtle_mode_start(); } else { diff --git a/src/flight/control.h b/src/flight/control.h index b4105f97e..5227cddd5 100644 --- a/src/flight/control.h +++ b/src/flight/control.h @@ -45,6 +45,7 @@ typedef struct { uint16_t looptime_autodetect; float looptime; // looptime in seconds + float timefactor; // timefactor for pid calc uint32_t looptime_us; // looptime in us uint32_t loop_counter; // number of loops ran float uptime; // running sum of looptimes @@ -85,9 +86,10 @@ typedef struct { vec3_t accel_raw; // raw accel reading with rotation and scaling applied vec3_t accel; // filtered accel readings - float gyro_temp; // gyro temparture reading - vec3_t gyro_raw; // raw gyro reading with rotation and scaling applied - vec3_t gyro; // filtered gyro reading + float gyro_temp; // gyro temparture reading + vec3_t gyro_raw; // raw gyro reading with rotation and scaling applied + vec3_t gyro; // filtered gyro reading + vec3_t gyro_delta_angle; // angle covered in last time interval vec3_t GEstG; // gravity vector vec3_t attitude; @@ -109,6 +111,7 @@ typedef struct { MEMBER(failloop, uint8_t) \ MEMBER(looptime_autodetect, uint16_t) \ MEMBER(looptime, float) \ + MEMBER(timefactor, float) \ MEMBER(looptime_us, uint32_t) \ MEMBER(loop_counter, uint32_t) \ MEMBER(uptime, float) \ @@ -139,6 +142,7 @@ typedef struct { MEMBER(gyro_temp, float) \ MEMBER(gyro_raw, vec3_t) \ MEMBER(gyro, vec3_t) \ + MEMBER(gyro_delta_angle, vec3_t) \ MEMBER(GEstG, vec3_t) \ MEMBER(attitude, vec3_t) \ MEMBER(setpoint, vec3_t) \ diff --git a/src/flight/imu.c b/src/flight/imu.c index b2d92bcb5..01fc3309b 100644 --- a/src/flight/imu.c +++ b/src/flight/imu.c @@ -55,20 +55,14 @@ void imu_init() { #ifdef SILVERWARE_IMU void imu_calc() { - const float gyro_delta_angle[3] = { - state.gyro.axis[0] * state.looptime, - state.gyro.axis[1] * state.looptime, - state.gyro.axis[2] * state.looptime, - }; + state.GEstG.axis[2] = state.GEstG.axis[2] - (state.gyro_delta_angle.axis[0]) * state.GEstG.axis[0]; + state.GEstG.axis[0] = (state.gyro_delta_angle.axis[0]) * state.GEstG.axis[2] + state.GEstG.axis[0]; - state.GEstG.axis[2] = state.GEstG.axis[2] - (gyro_delta_angle[0]) * state.GEstG.axis[0]; - state.GEstG.axis[0] = (gyro_delta_angle[0]) * state.GEstG.axis[2] + state.GEstG.axis[0]; + state.GEstG.axis[1] = state.GEstG.axis[1] + (state.gyro_delta_angle.axis[1]) * state.GEstG.axis[2]; + state.GEstG.axis[2] = -(state.gyro_delta_angle.axis[1]) * state.GEstG.axis[1] + state.GEstG.axis[2]; - state.GEstG.axis[1] = state.GEstG.axis[1] + (gyro_delta_angle[1]) * state.GEstG.axis[2]; - state.GEstG.axis[2] = -(gyro_delta_angle[1]) * state.GEstG.axis[1] + state.GEstG.axis[2]; - - state.GEstG.axis[0] = state.GEstG.axis[0] - (gyro_delta_angle[2]) * state.GEstG.axis[1]; - state.GEstG.axis[1] = (gyro_delta_angle[2]) * state.GEstG.axis[0] + state.GEstG.axis[1]; + state.GEstG.axis[0] = state.GEstG.axis[0] - (state.gyro_delta_angle.axis[2]) * state.GEstG.axis[1]; + state.GEstG.axis[1] = (state.gyro_delta_angle.axis[2]) * state.GEstG.axis[0] + state.GEstG.axis[1]; if (flags.on_ground) { // happyhour bartender - quad is ON GROUND and disarmed // calc acc mag @@ -127,20 +121,14 @@ void imu_calc() { #ifdef QUICKSILVER_IMU void imu_calc() { - const float gyro_delta_angle[3] = { - state.gyro.axis[0] * state.looptime, - state.gyro.axis[1] * state.looptime, - state.gyro.axis[2] * state.looptime, - }; - - state.GEstG.axis[2] = state.GEstG.axis[2] - (gyro_delta_angle[0]) * state.GEstG.axis[0]; - state.GEstG.axis[0] = (gyro_delta_angle[0]) * state.GEstG.axis[2] + state.GEstG.axis[0]; + state.GEstG.axis[2] = state.GEstG.axis[2] - (state.gyro_delta_angle.axis[0]) * state.GEstG.axis[0]; + state.GEstG.axis[0] = (state.gyro_delta_angle.axis[0]) * state.GEstG.axis[2] + state.GEstG.axis[0]; - state.GEstG.axis[1] = state.GEstG.axis[1] + (gyro_delta_angle[1]) * state.GEstG.axis[2]; - state.GEstG.axis[2] = -(gyro_delta_angle[1]) * state.GEstG.axis[1] + state.GEstG.axis[2]; + state.GEstG.axis[1] = state.GEstG.axis[1] + (state.gyro_delta_angle.axis[1]) * state.GEstG.axis[2]; + state.GEstG.axis[2] = -(state.gyro_delta_angle.axis[1]) * state.GEstG.axis[1] + state.GEstG.axis[2]; - state.GEstG.axis[0] = state.GEstG.axis[0] - (gyro_delta_angle[2]) * state.GEstG.axis[1]; - state.GEstG.axis[1] = (gyro_delta_angle[2]) * state.GEstG.axis[0] + state.GEstG.axis[1]; + state.GEstG.axis[0] = state.GEstG.axis[0] - (state.gyro_delta_angle.axis[2]) * state.GEstG.axis[1]; + state.GEstG.axis[1] = (state.gyro_delta_angle.axis[2]) * state.GEstG.axis[0] + state.GEstG.axis[1]; filter_lp_pt1_coeff(&filter, PT1_FILTER_HZ); diff --git a/src/flight/pid.c b/src/flight/pid.c index d19d0486f..f4d59344e 100644 --- a/src/flight/pid.c +++ b/src/flight/pid.c @@ -19,10 +19,6 @@ #define RELAX_FACTOR (RELAX_FACTOR_DEG * DEGTORAD) #define RELAX_FACTOR_YAW (RELAX_FACTOR_YAW_DEG * DEGTORAD) -extern profile_t profile; - -float timefactor; - // "p term setpoint weighting" 0.0 - 1.0 where 1.0 = normal pid static const float setpoint_weigth_brushed[3] = {0.93, 0.93, 0.9}; // BRUSHED FREESTYLE // float setpoint_weigth_brushed[3] = { 0.97 , 0.98 , 0.95}; //BRUSHED RACE @@ -38,9 +34,9 @@ static const float integral_limit_brushless[PID_SIZE] = {0.8, 0.8, 0.6}; static const float pid_scales[PID_SIZE][PID_SIZE] = { // roll, pitch, yaw - {628.0f, 628.0f, 314.0f}, // kp - {50.0f, 50.0f, 50.0f}, // ki - {120.0f, 120.0f, 120.0f}, // kd + {1.0f / 628.0f, 1.0f / 628.0f, 1.f / 314.0f}, // kp + {1.0f / 50.0f, 1.0f / 50.0f, 1.f / 50.0f}, // ki + {1.0f / 120.0f, 1.0f / 120.0f, 1.f / 120.0f}, // kd }; static float lasterror[PID_SIZE] = {0, 0, 0}; @@ -49,15 +45,8 @@ static float lasterror2[PID_SIZE] = {0, 0, 0}; static float lastrate[PID_SIZE] = {0, 0, 0}; static float lastsetpoint[PID_SIZE] = {0, 0, 0}; -static float current_kp[PID_SIZE] = {0, 0, 0}; -static float current_ki[PID_SIZE] = {0, 0, 0}; -static float current_kd[PID_SIZE] = {0, 0, 0}; - static float ierror[PID_SIZE] = {0, 0, 0}; -static float v_compensation = 1.00; -static float tda_compensation = 1.00; - static filter_t filter[FILTER_MAX_SLOTS]; static filter_state_t filter_state[FILTER_MAX_SLOTS][3]; @@ -80,50 +69,6 @@ void pid_init() { } } -// calculate change from ideal loop time -// 0.0032f is there for legacy purposes, should be 0.001f = looptime -// this is called in advance as an optimization because it has division -void pid_precalc() { - timefactor = 0.0032f / state.looptime; - - filter_lp_pt1_coeff(&rx_filter, state.rx_filter_hz); - filter_coeff(profile.filter.dterm[0].type, &filter[0], profile.filter.dterm[0].cutoff_freq); - filter_coeff(profile.filter.dterm[1].type, &filter[1], profile.filter.dterm[1].cutoff_freq); - - if (profile.voltage.pid_voltage_compensation) { - v_compensation = mapf((state.vbat_filtered_decay / (float)state.lipo_cell_count), 2.5f, 3.85f, PID_VC_FACTOR, 1.0f); - v_compensation = constrain(v_compensation, 1.0f, PID_VC_FACTOR); - -#ifdef LEVELMODE_PID_ATTENUATION - if (rx_aux_on(AUX_LEVELMODE)) - v_compensation *= LEVELMODE_PID_ATTENUATION; -#endif - } else { - v_compensation = 1.0f; - } - - if (profile.pid.throttle_dterm_attenuation.tda_active) { - tda_compensation = mapf(state.throttle, profile.pid.throttle_dterm_attenuation.tda_breakpoint, 1.0f, 1.0f, profile.pid.throttle_dterm_attenuation.tda_percent); - tda_compensation = constrain(tda_compensation, profile.pid.throttle_dterm_attenuation.tda_percent, 1.0f); - } else { - tda_compensation = 1.0f; - } - - if (profile.filter.dterm_dynamic_enable) { - float dynamic_throttle = state.throttle + state.throttle * (1 - state.throttle) * DTERM_DYNAMIC_EXPO; - float d_term_dynamic_freq = mapf(dynamic_throttle, 0.0f, 1.0f, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); - d_term_dynamic_freq = constrain(d_term_dynamic_freq, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); - - filter_lp_pt1_coeff(&dynamic_filter, d_term_dynamic_freq); - } - - for (uint8_t i = 0; i < PID_SIZE; i++) { - current_kp[i] = profile_current_pid_rates()->kp.axis[i] / pid_scales[0][i]; - current_ki[i] = profile_current_pid_rates()->ki.axis[i] / pid_scales[1][i]; - current_kd[i] = profile_current_pid_rates()->kd.axis[i] / pid_scales[2][i]; - } -} - // (iwindup = 0 windup is not allowed) (iwindup = 1 windup is allowed) static inline float pid_compute_iterm_windup(uint8_t x, float pid_output) { const float *out_limit = target.brushless ? out_limit_brushless : out_limit_brushed; @@ -186,87 +131,116 @@ static inline bool pid_should_enable_iterm(uint8_t x) { return stick_movement; } -// pid calculation for acro ( rate ) mode -// input: error[x] = setpoint - gyro -// output: state.pidoutput.axis[x] = change required from motors -static inline void pid(uint8_t x) { - // P term - const float *setpoint_weigth = target.brushless ? setpoint_weigth_brushless : setpoint_weigth_brushed; - state.pid_p_term.axis[x] = state.error.axis[x] * (setpoint_weigth[x]) * current_kp[x]; - state.pid_p_term.axis[x] += -(1.0f - setpoint_weigth[x]) * current_kp[x] * state.gyro.axis[x]; - - // Pid Voltage Comp applied to P term only - state.pid_p_term.axis[x] *= v_compensation; - - // I term - static vec3_t pid_output = {.roll = 0, .pitch = 0, .yaw = 0}; - - // SIMPSON_RULE_INTEGRAL - // assuming similar time intervals - const float *integral_limit = target.brushless ? integral_limit_brushless : integral_limit_brushed; - const float iterm_windup = pid_compute_iterm_windup(x, pid_output.axis[x]); - if (!pid_should_enable_iterm(x)) { - // wind down integral while we are still on ground and we do not get any input from the sticks - ierror[x] *= 0.98f; +static inline float pid_voltage_compensation() { + if (!profile.voltage.pid_voltage_compensation) { + return 1.0f; } - ierror[x] = ierror[x] + 0.166666f * (lasterror2[x] + 4 * lasterror[x] + state.error.axis[x]) * current_ki[x] * iterm_windup * state.looptime; - ierror[x] = constrain(ierror[x], -integral_limit[x], integral_limit[x]); - lasterror2[x] = lasterror[x]; - lasterror[x] = state.error.axis[x]; - state.pid_i_term.axis[x] = ierror[x]; - - // D term - const uint8_t stck_boost_profile = rx_aux_on(AUX_STICK_BOOST_PROFILE) ? STICK_PROFILE_ON : STICK_PROFILE_OFF; - - const float stick_accelerator = profile.pid.stick_rates[stck_boost_profile].accelerator.axis[x]; - const float stick_transition = profile.pid.stick_rates[stck_boost_profile].transition.axis[x]; - - float transition_setpoint_weight = 0; - if (stick_accelerator < 1) { - transition_setpoint_weight = (fabsf(state.rx_filtered.axis[x]) * stick_transition) + (1 - stick_transition); - } else { - transition_setpoint_weight = (fabsf(state.rx_filtered.axis[x]) * (stick_transition / stick_accelerator)) + (1 - stick_transition); - } + float res = constrain(mapf((state.vbat_filtered_decay / (float)state.lipo_cell_count), 2.5f, 3.85f, PID_VC_FACTOR, 1.0f), 1.0f, PID_VC_FACTOR); +#ifdef LEVELMODE_PID_ATTENUATION + if (rx_aux_on(AUX_LEVELMODE)) + res *= LEVELMODE_PID_ATTENUATION; +#endif + return res; +} - float setpoint_derivative = (state.setpoint.axis[x] - lastsetpoint[x]) * current_kd[x] * timefactor; - if (state.rx_filter_hz > 0.1f) { - setpoint_derivative = filter_lp_pt1_step(&rx_filter, &rx_filter_state[x], setpoint_derivative); +static inline float pid_tda_compensation() { + if (!profile.pid.throttle_dterm_attenuation.tda_active) { + return 1.0f; } - lastsetpoint[x] = state.setpoint.axis[x]; - - const float gyro_derivative = (state.gyro.axis[x] - lastrate[x]) * current_kd[x] * timefactor * tda_compensation; - lastrate[x] = state.gyro.axis[x]; - - const float dterm = (setpoint_derivative * stick_accelerator * transition_setpoint_weight) - (gyro_derivative); - state.pid_d_term.axis[x] = pid_filter_dterm(x, dterm); - - const float *out_limit = target.brushless ? out_limit_brushless : out_limit_brushed; - state.pidoutput.axis[x] = pid_output.axis[x] = state.pid_p_term.axis[x] + state.pid_i_term.axis[x] + state.pid_d_term.axis[x]; - state.pidoutput.axis[x] = constrain(state.pidoutput.axis[x], -out_limit[x], out_limit[x]); + const float tda_compensation = mapf(state.throttle, profile.pid.throttle_dterm_attenuation.tda_breakpoint, 1.0f, 1.0f, profile.pid.throttle_dterm_attenuation.tda_percent); + return constrain(tda_compensation, profile.pid.throttle_dterm_attenuation.tda_percent, 1.0f); } +// input: error[] = setpoint - gyro +// output: state.pidoutput.axis[] = change required from motors void pid_calc() { // rotates errors, originally by joelucid - const float gyro_delta_angle[3] = { - state.gyro.axis[0] * state.looptime, - state.gyro.axis[1] * state.looptime, - state.gyro.axis[2] * state.looptime, - }; // rotation around x axis: - ierror[1] -= ierror[2] * gyro_delta_angle[0]; - ierror[2] += ierror[1] * gyro_delta_angle[0]; + ierror[1] -= ierror[2] * state.gyro_delta_angle.axis[0]; + ierror[2] += ierror[1] * state.gyro_delta_angle.axis[0]; // rotation around y axis: - ierror[2] -= ierror[0] * gyro_delta_angle[1]; - ierror[0] += ierror[2] * gyro_delta_angle[1]; + ierror[2] -= ierror[0] * state.gyro_delta_angle.axis[1]; + ierror[0] += ierror[2] * state.gyro_delta_angle.axis[1]; // rotation around z axis: - ierror[0] -= ierror[1] * gyro_delta_angle[2]; - ierror[1] += ierror[0] * gyro_delta_angle[2]; + ierror[0] -= ierror[1] * state.gyro_delta_angle.axis[2]; + ierror[1] += ierror[0] * state.gyro_delta_angle.axis[2]; + + filter_lp_pt1_coeff(&rx_filter, state.rx_filter_hz); + filter_coeff(profile.filter.dterm[0].type, &filter[0], profile.filter.dterm[0].cutoff_freq); + filter_coeff(profile.filter.dterm[1].type, &filter[1], profile.filter.dterm[1].cutoff_freq); - pid(0); - pid(1); - pid(2); + static vec3_t pid_output = {.roll = 0, .pitch = 0, .yaw = 0}; + const float v_compensation = pid_voltage_compensation(); + const float tda_compensation = pid_tda_compensation(); + const float *out_limit = target.brushless ? out_limit_brushless : out_limit_brushed; + const float *setpoint_weigth = target.brushless ? setpoint_weigth_brushless : setpoint_weigth_brushed; + const float *integral_limit = target.brushless ? integral_limit_brushless : integral_limit_brushed; + + const uint8_t stck_boost_profile = rx_aux_on(AUX_STICK_BOOST_PROFILE) ? STICK_PROFILE_ON : STICK_PROFILE_OFF; + const float *stick_accelerator = profile.pid.stick_rates[stck_boost_profile].accelerator.axis; + const float *stick_transition = profile.pid.stick_rates[stck_boost_profile].transition.axis; + + if (profile.filter.dterm_dynamic_enable) { + float dynamic_throttle = state.throttle + state.throttle * (1 - state.throttle) * DTERM_DYNAMIC_EXPO; + float d_term_dynamic_freq = mapf(dynamic_throttle, 0.0f, 1.0f, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); + d_term_dynamic_freq = constrain(d_term_dynamic_freq, profile.filter.dterm_dynamic_min, profile.filter.dterm_dynamic_max); + + filter_lp_pt1_coeff(&dynamic_filter, d_term_dynamic_freq); + } + +#pragma GCC unroll 3 + for (uint8_t x = 0; x < PID_SIZE; x++) { + const float current_kp = profile_current_pid_rates()->kp.axis[x] * pid_scales[0][x]; + const float current_ki = profile_current_pid_rates()->ki.axis[x] * pid_scales[1][x]; + const float current_kd = profile_current_pid_rates()->kd.axis[x] * pid_scales[2][x]; + + // P term + state.pid_p_term.axis[x] = state.error.axis[x] * (setpoint_weigth[x]) * current_kp; + state.pid_p_term.axis[x] += -(1.0f - setpoint_weigth[x]) * current_kp * state.gyro.axis[x]; + + // Pid Voltage Comp applied to P term only + state.pid_p_term.axis[x] *= v_compensation; + + // I term + const float iterm_windup = pid_compute_iterm_windup(x, pid_output.axis[x]); + if (!pid_should_enable_iterm(x)) { + // wind down integral while we are still on ground and we do not get any input from the sticks + ierror[x] *= 0.98f; + } + // SIMPSON_RULE_INTEGRAL + // assuming similar time intervals + ierror[x] = ierror[x] + 0.166666f * (lasterror2[x] + 4 * lasterror[x] + state.error.axis[x]) * current_ki * iterm_windup * state.looptime; + ierror[x] = constrain(ierror[x], -integral_limit[x], integral_limit[x]); + lasterror2[x] = lasterror[x]; + lasterror[x] = state.error.axis[x]; + + state.pid_i_term.axis[x] = ierror[x]; + + // D term + float transition_setpoint_weight = 0; + if (stick_accelerator[x] < 1) { + transition_setpoint_weight = (fabsf(state.rx_filtered.axis[x]) * stick_transition[x]) + (1 - stick_transition[x]); + } else { + transition_setpoint_weight = (fabsf(state.rx_filtered.axis[x]) * (stick_transition[x] / stick_accelerator[x])) + (1 - stick_transition[x]); + } + + float setpoint_derivative = (state.setpoint.axis[x] - lastsetpoint[x]) * current_kd * state.timefactor; + if (state.rx_filter_hz > 0.1f) { + setpoint_derivative = filter_lp_pt1_step(&rx_filter, &rx_filter_state[x], setpoint_derivative); + } + lastsetpoint[x] = state.setpoint.axis[x]; + + const float gyro_derivative = (state.gyro.axis[x] - lastrate[x]) * current_kd * state.timefactor * tda_compensation; + lastrate[x] = state.gyro.axis[x]; + + const float dterm = (setpoint_derivative * stick_accelerator[x] * transition_setpoint_weight) - (gyro_derivative); + state.pid_d_term.axis[x] = pid_filter_dterm(x, dterm); + + state.pidoutput.axis[x] = pid_output.axis[x] = state.pid_p_term.axis[x] + state.pid_i_term.axis[x] + state.pid_d_term.axis[x]; + state.pidoutput.axis[x] = constrain(state.pidoutput.axis[x], -out_limit[x], out_limit[x]); + } } diff --git a/src/flight/pid.h b/src/flight/pid.h index ba17373cd..7483514f5 100644 --- a/src/flight/pid.h +++ b/src/flight/pid.h @@ -3,5 +3,4 @@ void rotateErrors(); void pid_init(); -void pid_precalc(); void pid_calc(); diff --git a/src/flight/sixaxis.c b/src/flight/sixaxis.c index 705600830..cd339630b 100644 --- a/src/flight/sixaxis.c +++ b/src/flight/sixaxis.c @@ -134,6 +134,10 @@ void sixaxis_read() { state.gyro.axis[i] = filter_step(profile.filter.gyro[0].type, &filter[0], &filter_state[0][i], state.gyro.axis[i]); state.gyro.axis[i] = filter_step(profile.filter.gyro[1].type, &filter[1], &filter_state[1][i], state.gyro.axis[i]); } + + state.gyro_delta_angle.axis[0] = state.gyro.axis[0] * state.looptime; + state.gyro_delta_angle.axis[1] = state.gyro.axis[1] * state.looptime; + state.gyro_delta_angle.axis[2] = state.gyro.axis[2] * state.looptime; } static bool test_gyro_move(const gyro_data_t *last_data, const gyro_data_t *data) {