In stabilizer.c:
Code: Select all
_Bool motor_saturated = false; // Used to prevent I windup
static void distributePower(const uint16_t thrust, const int16_t roll,
const int16_t pitch, const int16_t yaw)
{
float motors_max;
float motors_min;
float motors_spread;
motor_saturated = false;
int16_t r = roll >> 1;
int16_t p = pitch >> 1;
motorPowerM1 = thrust - r + p + yaw;
motorPowerM2 = thrust - r - p - yaw;
motorPowerM3 = thrust + r - p + yaw;
motorPowerM4 = thrust + r + p - yaw;
motors_max = max(motorPowerM1,max(motorPowerM2,max(motorPowerM3,motorPowerM4)));
motors_min = min(motorPowerM1,min(motorPowerM2,min(motorPowerM3,motorPowerM4)));
motors_spread = motors_max - motors_min;
//"Air Mode" - Maintain the total commanded differences in thrust to provide better control at low and high throttle.
if (motors_spread > UINT16_MAX) {
motor_saturated = true;
motorPowerM1 -= motors_min;
motorPowerM2 -= motors_min;
motorPowerM3 -= motors_min;
motorPowerM4 -= motors_min;
motors_max -= motors_min;
motors_spread = (motors_max-motors_min)/UINT16_MAX;
motorPowerM1 /= motors_spread;
motorPowerM2 /= motors_spread;
motorPowerM3 /= motors_spread;
motorPowerM4 /= motors_spread;
} else if (motors_min < 0) {
motor_saturated = true;
motorPowerM1 -= motors_min;
motorPowerM2 -= motors_min;
motorPowerM3 -= motors_min;
motorPowerM4 -= motors_min;
} else if (motors_max > UINT16_MAX) {
motor_saturated = true;
motors_max -= UINT16_MAX;
motorPowerM1 -= motors_max;
motorPowerM2 -= motors_max;
motorPowerM3 -= motors_max;
motorPowerM4 -= motors_max;
}
//In case of floating point rounding weirdness
motorPowerM1 = limitThrust(motorPowerM1);
motorPowerM2 = limitThrust(motorPowerM2);
motorPowerM3 = limitThrust(motorPowerM3);
motorPowerM4 = limitThrust(motorPowerM4);
motorsSetRatio(MOTOR_M1, motorPowerM1);
motorsSetRatio(MOTOR_M2, motorPowerM2);
motorsSetRatio(MOTOR_M3, motorPowerM3);
motorsSetRatio(MOTOR_M4, motorPowerM4);
}
Code: Select all
extern _Bool motor_saturated; // Used to prevent I windup
float pidUpdate(PidObject* pid, const float measured, const bool updateError)
{
float output;
if (updateError)
{
pid->error = pid->desired - measured;
}
float previ = pid->integ;
pid->integ += pid->error * pid->dt;
// Do not allow integral to increase while motors are saturated, to prevent windup
if (motor_saturated && (fabsf(pid->integ) > fabsf(previ))) {
pid->integ = previ;
}
pid->deriv = (pid->error - pid->prevError) / pid->dt;
pid->outP = pid->kp * pid->error;
pid->outI = pid->ki * pid->integ;
pid->outD = pid->kd * pid->deriv;
output = pid->outP + pid->outI + pid->outD;
pid->prevError = pid->error;
return output;
}