Problem with NavX inversion

[kvanrhein]

I have two robots, both with navX, one is upside down, and works great with yagsl out of the box. The other is right side up and I could not get it to work with invert true or false, so I looked in the code and found that the inversion seemed to be having no effect on getZ() from gyro, so I downloaded the library and changed the code in navXSwerve.java to do the inversion in getRotation3d instead of getRawRotation3d and that seems to work.

The right-side-up navX I set invert to true and now it works with this code change:

public Rotation3d getRotation3d()
{
if (inverted) {
return getRawRotation3d().rotateBy(offset.unaryMinus()).unaryMinus();
}
else {
return getRawRotation3d().rotateBy(offset.unaryMinus());
}
}

Don't know why, maybe I am imagining things...

[thenetworkgrinch]

That makes no sense. Could you send a screencap of it happening and shorten your fix then submit a PR?

[kvanrhein]

I agree, it doesn't make sense, which is why I am asking about it.

I don't know how to "submit a pr" but here is a link to a video I made (sorry, the audio is horrible):
screencapture
and a link to the modified NavXSwerve.java:
NavXSwerve.java

[kvanrhein]

Looking deeper found this:
original raw rotation3d reported by navX: w 0.0102 x -0.0569 y 0.9983 z -0.0057
result of unaryMinus of that angle: w 0.0102 x 0.0569 y -0.9983 z 0.0057

Now, using getZ() function:
final var cycz = 1.0 - 2.0 * (y * y + z * z);
final var cysz = 2.0 * (w * z + x * y);
final var cy_sq = cycz * cycz + cysz * cysz;
if (cy_sq > 1e-20) {
return Math.atan2(cysz, cycz);
} else {
return Math.atan2(2.0 * w * z, w * w - z * z);
}

for both the original and inverted values, this formula yields -3.0276 which is why the yaw is not changing after inversion

[kvanrhein]

that works.

[kvanrhein]

private Rotation3d negate(Rotation3d rot) {
    return new Rotation3d(-rot.getX(), -rot.getY(), -rot.getZ());
  }

  /**
   * Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
   *
   * @return {@link Rotation3d} from the IMU.
   */
  @Override
  public Rotation3d getRawRotation3d()
  {
    return inverted ? negate(imu.getRotation3d()) : imu.getRotation3d();
  }

[kvanrhein]

to @thenetworkgrinch I have unusual request, but I am old and use to old ways, yet newly acquainted with github universe, so was wondering if there is a chance we could get on a phone call or google meet or something I have a number of open ended questions about yagsl, yams, and this issue that I would like to discuss, realizing you may be super busy (kvanrhein@cps.edu), if you have some time.

[Technologyman00]

@kvanrhein If you haven't already I would recommend joining the discord. https://discord.gg/5pYxSDnymr

[thenetworkgrinch]

https://discord.gg/yagsl also works. I normally dont email or accept emails (although we have one somewhere that the students manage)