A tweak which improves aggressive flight

Firmware/software/electronics/mechanics
Post Reply
Analogy
Beginner
Posts: 19
Joined: Fri Oct 24, 2014 6:55 am

A tweak which improves aggressive flight

Post by Analogy »

I've been hacking my V2 a bit in the name of improving its performance for FPV acrobatics and other aggressive flight. I'm not sure if it would be appropriate for being pulled into the main line of firmware, so I thought I'd share here for anybody interested. Note that I'm more of a script kiddie than a real programmer, so if there's anything WTF about this feel free to let me know.

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);
}
and in pid.c:

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;
}
Post Reply