angle_pid: tidy
@@ -16,19 +16,22 @@ 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]);
+
// P term 1 weighted
- apidoutput1[x] = (1 - fabsf(state.angleerror[x])) * state.angleerror[x] * profile.pid.small_angle.kp;
+ apidoutput1[x] = (1 - angle_error_abs) * state.angleerror[x] * profile.pid.small_angle.kp;
// P term 2 weighted
- apidoutput2[x] = fabsf(state.angleerror[x]) * state.angleerror[x] * profile.pid.big_angle.kp;
+ apidoutput2[x] = angle_error_abs * state.angleerror[x] * profile.pid.big_angle.kp;
- extern float timefactor;
// D term 1 weighted + P term 1 weighted
- apidoutput1[x] += (state.angleerror[x] - lasterror[x]) * profile.pid.small_angle.kd * (1 - fabsf(state.angleerror[x])) * timefactor;
+ apidoutput1[x] += (state.angleerror[x] - lasterror[x]) * profile.pid.small_angle.kd * (1 - angle_error_abs) * timefactor;
// D term 2 weighted + P term 2 weighted
- apidoutput2[x] += ((state.angleerror[x] - lasterror[x]) * profile.pid.big_angle.kd * fabsf(state.angleerror[x]) * timefactor);
+ apidoutput2[x] += ((state.angleerror[x] - lasterror[x]) * profile.pid.big_angle.kd * angle_error_abs * timefactor);
// apidoutput sum
apidoutput[x] = apidoutput1[x] + apidoutput2[x];
