Initial angular velocities

[jamie-54]

Main Changes

Added initialAngularVelocity in main.cpp which is then subtract from the measuredAngularVelocity to give the angularVelocityCorrected which is used for the controlAction

Associated Issues

• Addresses #43

[alphaville]

@jamie-54 This looks good - have you noticed whether this has an effect on the behaviour of the quadcopter?

[alphaville]

@jamie-54 Would the following work?

void initAngularVelocity()
{
  float angularVelocityTemp[3] = {0};
  int numInitAngularVelocity = 10;

  for (int i = 0; i < numInitAngularVelocity; i++)
  {
    ahrs.update();
    ahrs.angularVelocity(angularVelocityTemp);
    initialAngularVelocity[0] += angularVelocityTemp[0];
    initialAngularVelocity[1] += angularVelocityTemp[1];
    initialAngularVelocity[2] += angularVelocityTemp[2];
  }
  initialAngularVelocity[0] /= (float)numInitAngularVelocity;
  initialAngularVelocity[1] /= (float)numInitAngularVelocity;
  initialAngularVelocity[2] /= (float)numInitAngularVelocity;
}

[jamie-54]

@jamie-54 Would the following work?

void initAngularVelocity()
{
  float angularVelocityTemp[3] = {0};
  int numInitAngularVelocity = 10;

  for (int i = 0; i < numInitAngularVelocity; i++)
  {
    ahrs.update();
    ahrs.angularVelocity(angularVelocityTemp);
    initialAngularVelocity[0] += angularVelocityTemp[0];
    initialAngularVelocity[1] += angularVelocityTemp[1];
    initialAngularVelocity[2] += angularVelocityTemp[2];
  }
  initialAngularVelocity[0] /= (float)numInitAngularVelocity;
  initialAngularVelocity[1] /= (float)numInitAngularVelocity;
  initialAngularVelocity[2] /= (float)numInitAngularVelocity;
}

I don't see any reason for this not to work, I'll test it on hardware tomorrow and then update. Then I'll check PR #45