kauailabs / navxmxp Goto Github PK

Sample output reported by a user when NavX and REV were running in simulation together:

I just found an issue with the print statements before the simulator runs. A navx print got mixed up with one for the spark max and this happened:

na[vCX-ASenN SPARsK MAoXr] Connected. nUanvaX-blSee ntso roetrri esvtaer tSupP AiRKni MtAiXa lfiziatrimware version for CAN ID: 1. Pleasoen veanrdify the dev iscetIDa frtiueld matcp chaleisb rattihoen ccoonfmigpulrede CAtNe. InD ofav Xt-hSeen scoontrr oollnboerard ,st aand rtup tchat althe coinbtrroller is connectedation comp to the CAN Bus. l[eCtANe .S navX-Sensor Yaw angle aPutAoR-resetK tMoA X0]. 0W due to sPItarLtiubp coalri bEration. xternal

It would be useful if NavX used a logger that could be hooked instead of printf(). There's one in wpiutil and/or ntcore that would fit the bill.

In the usage reporting spreadsheet from 2019, the NavX is not reported.

Forgot to add - first_address to it?

https://github.com/kauailabs/navxmxp/blob/master/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO.java#L120

As a result, it's possible that if the user invokes AHRS.reset() while receiving data, that the underlying calls to the WPI Library I2C/SPI Read/Write will be re-entered. There has been a report from a team that when issuing() reset calls w/Java and I2C, that sometimes hangs are seen.

Submitted by a LabVIEW user:

After installing the newest LabVIEW libraries for the navX, I could not see the navX components in the Third Party area of the WPI - Sensors.
I found out that the installer puts those files in the default folder on the C: drive (C:\Program Files (x86)\National Instruments\LabVIEW 2016\vi.lib\Rock Robotics\WPI\ThirdParty )

But my LabVIEW installation was done onto the D: drive.

The workaround for me was to copy the files from the above folder to D:\Program Files (x86)\National Instruments\LabVIEW 2016\vi.lib\Rock Robotics\WPI\ThirdParty

The installer should be corrected to use the LabVIEW installation directory.

If zeroing the yaw, the fused heading is no longer accurate. This appears to be related to the change in this commit.

They return integrator.getVelocity*() instead of integrator.getDisplacementX().

Make the tool palette menu items work properly.

There's an issue when using the navx library with labview on the FRC RoboRIO, it blocks every PWM and DIO on the robot preventing normal code from sending outputs to motors or read the DIO properly and the navX doesn't even work. It happens only on a robot project, the examples are working fine.
The issue is going on for the past two weeks still without a solution that works!

for more info:
https://www.chiefdelphi.com/forums/showthread.php?t=154205

The AHRS class already implements most of the Gyro interface's methods, so it would make sense to also implement Gyro alongside PIDSource and Sendable. This would allow us to be able to easily swap the NavX in and out with other gyros that implement the Gyro interface.

I think there is a parenthesis error in the calculation of the displacement in the mpucontroller.cpp firmware code (line 995).
FILE:
https://github.com/kauailabs/navxmxp/blob/master/stm32/MPU9250/mpucontroller.cpp

This is the line (995):
pdata->displacement[i] += (((pdata->velocity[i] + (curr_velocity_m_s - pdata->velocity[i]))/2.0) * curr_sample_period);
Over one small dt time period, one should calculate Xfinal = Xinitial + ((Vfinal+Vintial)/2)*dt for trapezoidal integration. In the code, this is expanded as Xfinal = Xinitial + (Vinitial + (Vfinal-Vinitial)/2)*dt. (Note, the comment in line 994 is missing the "Xinitial" portion of the equation also, but this is not relevant to the actual code.)
If you follow the parentheses in the line 995 expression, it appears that what is being calculated is:
Xfinal = Xinitial + (((Vinitial + (Vfinal-Vinitial))/2)*dt // NOTE, this is wrong
The "/2" is being applied to the first initial velocity term as well, so the Vinitial ends up being subtracted out and one gets just (Vfinal/2)*dt being added to Xinitial. I suggest the change needed is:
NEW PROPOSE LINE 995:
pdata->displacement[i] += (pdata->velocity[i] + (curr_velocity_m_s - pdata->velocity[i])/2.0) * curr_sample_period);

I know there has been discussion about the accuracy (or lack thereof) of the displacement from this sensor board. This bug may be contributing to this.

The below stack trace was provided to the navX-MXP support forum by RB.

It appears to show a 0-length result being returned (without any other sign of error) from the Core Device Interface Module. This is an unexpected behavior, but it points to an issue that the code should add some bounds checking in the decodeNavxProcessedData method to handle I2C read data that is less than the expected amount.

**** Log Data ****

02-05 12:29:18.079 17567 17680 E AndroidRuntime: FATAL EXCEPTION: r/w loop: Modern Robotics USB Core Device Interface Module [AL00VBY8]
02-05 12:29:18.079 17567 17680 E AndroidRuntime: Process: com.qualcomm.ftcrobotcontroller, PID: 17567
02-05 12:29:18.079 17567 17680 E AndroidRuntime: java.lang.ArrayIndexOutOfBoundsException: length=0; index=2
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.kauailabs.navx.AHRSProtocol.decodeBinaryUint32(AHRSProtocol.java:685)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.kauailabs.navx.ftc.AHRS$navXIOThread.decodeNavxProcessedData(AHRS.java:1306)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.kauailabs.navx.ftc.AHRS$navXIOThread.ioComplete(AHRS.java:1052)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.kauailabs.navx.ftc.AHRS$DimI2cDeviceReader.portDone(AHRS.java:1688)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.kauailabs.navx.ftc.AHRS$DimI2cDeviceReader.access$600(AHRS.java:1513)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.kauailabs.navx.ftc.AHRS$DimI2cDeviceReader$1.portIsReady(AHRS.java:1546)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.qualcomm.robotcore.hardware.I2cDeviceImpl.portIsReady(I2cDeviceImpl.java:249)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.qualcomm.hardware.modernrobotics.ModernRoboticsUsbDeviceInterfaceModule.readComplete(ModernRoboticsUsbDeviceInterfaceModule.java:932)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.qualcomm.hardware.modernrobotics.comm.ReadWriteRunnableStandard$1.run(ReadWriteRunnableStandard.java:369)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.qualcomm.robotcore.util.ThreadPool.logThreadLifeCycle(ThreadPool.java:513)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at com.qualcomm.hardware.modernrobotics.comm.ReadWriteRunnableStandard.run(ReadWriteRunnableStandard.java:304)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at java.util.concurrent.ThreadPoolExecutor.runWorker(ThreadPoolExecutor.java:1113)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at java.util.concurrent.ThreadPoolExecutor$Worker.run(ThreadPoolExecutor.java:588)
02-05 12:29:18.079 17567 17680 E AndroidRuntime: at java.lang.Thread.run(Thread.java:818)

The existing AHRS.IsDisconnected() method returns the value from the is_disconnected variable in the member IOThread. When this IOThread is stopped (e.g., via AHRS.close()), the IOThread's is_disconnected variable is not being set FALSE, and thus AHRS.IsDisconnected() can return TRUE in this case.

The resolution is almost certainly to condition the is_disconnected variable to FALSE whenever the IOThread is stopped.

This issue came up during a debug session w/a mentor from FTC Team 8373.

Detail: Update online documentation to include recommendation to add navX-Micro and the I2C port # used into the approprite entry in the Core Device Interface Module configuration.

This might have helped avoid the conflict that Team 8373 encountered.

Seconds? ms? ns? years?

While viewing my DS logs from driver practice, I noticed the following message. Is this an issue? Is there any solution?

java.lang.RuntimeException: Code: -1073807253. HAL - VISA: Framing Error at com.kauailabs.navx.frc.AHRS$IOThread.run(AHRS.java:1242) at edu.wpi.first.wpilibj.SerialPort.read(SerialPort.java:256)

I'm seeing a reproducible crash that looks to be happening in libnavx_frc_cpp.a.

Here is the test program: https://github.com/strykeforce/roborio/blob/master/src/navx_test.cc. To reproduce:

1. We routinely stop NI auto-run runtime when running test robot code from the command-line: /etc/init.d/nilvrt stop
2. Run test program on roboRIO in gdb
3. Enable in driver station
4. Disable in driver station
5. SIGSEGV

And here is the stack trace:

Program received signal SIGSEGV, Segmentation fault.
[Switching to LWP 5748]
0x00025678 in OffsetTracker::UpdateHistory (this=0x0, curr_value=-3.28999996) at ..\src/OffsetTracker.cpp:21
21      ..\src/OffsetTracker.cpp: No such file or directory.
(gdb) bt
#0  0x00025678 in OffsetTracker::UpdateHistory (this=0x0, curr_value=-3.28999996) at ..\src/OffsetTracker.cpp:21
#1  0x00022600 in AHRSInternal::SetAHRSData (this=0x8d778, ahrs_update=..., sensor_timestamp=787208)
    at ..\src/AHRS.cpp:150
#2  0x00026600 in RegisterIO::GetCurrentData (this=0x8f370) at ..\src/RegisterIO.cpp:187
#3  0x00025d90 in RegisterIO::Run (this=0x8f370) at ..\src/RegisterIO.cpp:86
#4  0x00021964 in AHRS::ThreadFunc (io_provider=0x8f370) at ..\src/AHRS.cpp:1305
#5  0x00024bf8 in std::_Bind_simple<int (*(IIOProvider*))(IIOProvider*)>::_M_invoke<0u>(std::_Index_tuple<0u>) (
    this=0x8f248) at c:\frc\arm-frc-linux-gnueabi\include\c++\4.9.3/functional:1700
#6  0x00024a70 in std::_Bind_simple<int (*(IIOProvider*))(IIOProvider*)>::operator()() (this=0x8f248)
    at c:\frc\arm-frc-linux-gnueabi\include\c++\4.9.3/functional:1688
#7  0x000249d4 in std::thread::_Impl<std::_Bind_simple<int (*(IIOProvider*))(IIOProvider*)> >::_M_run() (
    this=0x8f23c) at c:\frc\arm-frc-linux-gnueabi\include\c++\4.9.3/thread:115
#8  0xb6e5a748 in ?? () from /usr/lib/libstdc++.so.6
#9  0xb6e8ee50 in ?? () from /lib/libpthread.so.0
Backtrace stopped: previous frame identical to this frame (corrupt stack?)

This pollutes the global namespace of any source that includes the header, and could trick unwitting programmers into not writing frc:: only to be dumbfounded once removing #include "AHRS.h" breaks all their symbols. Not having using declarations in headers is a well-documented best practice: https://stackoverflow.com/a/5849668

The current download includes a setup.exe executable that extracts and installs the navX-MXP software and libraries, but this works only on Windows. Users that are on MacOS or Linux do not have a way of accessing the library files for Java and C++.

Please provide a second downloadable ZIP file that includes only the library files and examples for Java and C++ (really, everything already in the java and cpp directories created in the installation by the current installer) but can be used by anyone running Windows, MacOS, or Linux.

The Gyro/Raw Data Update packet is too small for the data.
IMUProtocol.h - Line 78

#define GYRO_UPDATE_TEMP_VALUE_INDEX                38
#define GYRO_UPDATE_CHECKSUM_INDEX                  42

The size of GYRO_UPDATE_TEMP_VALUE_INDEX is 4 when it should be 7 as it uses encodeProtocolFloat to encode/decode the data

Could kauailabs please put the Java Roborio library (navx_frc.jar) on a Maven repo so that users can add the navx library as a dependency? Thanks.

2020's wpilib removes SampleRobot, which means that the NavX examples don't compile. In addition, the examples are still eclipse projects, and not VS Code / gradle projects.

My first question is why is there a 1ms delay here? May I remove/shorten this delay or at least update the comment? 1ms is a very long time to be blocking in SPI world.

Errors occurred during the build.
Errors running builder 'Integrated External Tool Builder' on project 'navx_frc'.
The archive: C:/Users/Scott/GitHub/navxmxp/java/navx/jar/navx.jar which is referenced by the classpath, does not exist.
The archive: C:/Users/Scott/GitHub/navxmxp/java/navx/jar/navx.jar which is referenced by the classpath, does not exist.

[It's not clear yet whether this work will be done by Jaci, or by the Kauai Labs buildmaster; this issue is created to track the dependency.]

From email from Jaci Brunning on 6/7/2018:

2. There are a few changes that will have to be made to the build.gradle. The most important change is to add your Raspberry Pi as a deploy target in the build.gradle, which follows the same sort of structure as the RoboRIO, but with a new IP address and such. There's a few internal things in GradleRIO we'll have to change as well (the deploy process to still do FRC-specific behaviour on a non-RoboRIO target). The VMX dependencies will also have to be added, but other than that it should all be pretty much the same. I'll get back to you on how exactly to go about this, but I'll need the maven repo to be alive and well before this can be done.

I note that the C++ AHRS implementation doesn't use any kind of synchronization in SetYawPitchRoll, SetAHRSPosData, et al.

In the java implementation, each one of the set* functions have a block like..

synchronized (this) {
    ... set things
}

But none of the get* methods are synchronized, so the only things that are blocking each other are the update methods. Was this intentional?

One of the things we discussed the other day was the new serial packet that includes the AHRS data, the Positioning Data, as well as the device timestamp. Since you will be wanting likely the device timestamp, you'll want to configure the serial stream to receive this new packet type.

We need to add this to the LabVIEW library.

wpilib no longer installs to frcXXXX (where XXXX is the year). It now installs to wpilib/XXXX.

The Reset function (at least in Java and C++) only resets the gyro Z axis. Is there any reason it does not reset all three axes? Our RoboRio is mounted sideways this year, meaning we are using a different axis as our "yaw", but there is no easy way to reset it.

We need to add videos that explain the use of the examples provided int he navX WPI library.

First priority is Drive Straight and Rotate to an angle.

Others to follow after that.

This is a similar issue to one that affected NetworkTables in prior years. When calling any of those APIs, they eventually call into the RegisterIO_* functions... which try to obtain the same lock that is being used by the I/O read thread. The I/O read thread may potentially block for at least 0.001s, if not longer depending on the update rate -- a read could block for up to 16ms at a 60hz update rate. This seems quite bad.

Reference: https://github.com/kauailabs/navxmxp/blob/master/roborio/java/navx_frc/src/com/kauailabs/navx/frc/RegisterIO_SPI.java#L74

I would propose either having a separate write thread, or perhaps create some queue that commands could be pushed onto.

Provide a way for users to disable/enable the logging of things like SPI CRC Errors when disconnected. Alternatively, only output SPI CRC Errors until the point when a "no longer disconnected" condition is detected, and only begin again logging of these errors when a "connected" condition is again detected.

Are there plans to support simulation? It should be pretty straightforward to add a SimDevice.

For example:

• https://github.com/wpilibsuite/allwpilib/blob/master/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp#L22
• https://github.com/wpilibsuite/allwpilib/blob/master/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp#L64

From Java, if I use the Microsoft Lifecam and use the default resolution (160 x 120) and navX-Micro on USB, communication seems to work OK. It also seems to work OK if the resolution is 320x240.

However, if the camera resolution is 640x480, the navX-Micro gets a communication error like this:

java.lang.NullPointerException
at com.kauailabs.navx.frc.SerialIO.run(SerialIO.java:174)
at com.kauailabs.navx.frc.AHRS$IOThread.run(AHRS.java:1253)
at java.lang.Thread.run(Thread.java:745)
java.lang.NullPointerException
at com.kauailabs.navx.frc.SerialIO.run(SerialIO.java:191)
at com.kauailabs.navx.frc.AHRS$IOThread.run(AHRS.java:1253)
at java.lang.Thread.run(Thread.java:745)

Via code inspection, while the navX SerialIO module is resilient to communication exceptions that occur after it has already connected, it appears that if communication cannot be established during initialization (line 174, line 191 as shown above), the navX SerialIO module will not reattempt communication after that point.

Reported by mentor Richard Thomas on FRC team 4287. They were running the example program DataMonitor and see an error on the NetConsole after running it for about 35 minutes. There is a system exception bad_alloc. If all statements except the construction of the navX object are removed, the error is still seen.

This has been reproduced w/the 2017 FRC Roborio Library for C++.

Currently, myRobot is only constructed with 2 motors in the samples. However, mecanumDrive_Catesian requires a 4 motor drive, and throws a null pointer exception when attempted to be used with a 2 motor drive. Since all exceptions are caught, this doesn't throw, however it doesn't work properly either. It will only set the front left motor before throwing and not continuing.

Your setup program is exported as a .exe, which is not natively executable on *nix operating systems. However, when I ran the app with WINE, the program tried to use my JVM, which obviously means that the setup program uses compiled java code. Why was this program packaged as a .exe instead of a runable .jar?
Additionally, I see nowhere a location for downloading the .jar files themselves, which seems counter-intuitive, since, from your setup tutorial, it appears that all your setup program does is put your libraries in <HomeDirectory>\nav-mxp\ or ~/nav-mxp/. Why not give this freedom to the user by simply supplying the libraries. If you're expecting the users to edit their own build.properties, I'd imagine you can trust them to put files into folders.
Also keeping IDE specific files in your repository is terrible practice, even if you assume most people will be using Eclipse.

The INVENSENSE_LICENSE_README.txt instructions in the root directory do not match the INVENSENSE_LICENSE_README.txt in the stm32 directory. These instructions also do not account for the fact that INVENSENSE is no longer freely allowing downloads of the source code.

In addition to this, the firmware customization documentation does not seem to be up to date. Specifically, the Sourcery G++ Lite compiler is no longer being updated and available by Mentor Graphics.

Providing updated documentation on what the developers of the navX-PI-Dev branch are using to build the firmware for the STM32 would be greatly appreciated.

Thanks for open sourcing!

We are programming in Labview and we have some problems using navx this year.
When we connect it in USB, we get delayed angle data.
When we connect it via I2C, it works until we get error -1 in I2C write and read actions.
We made sure that both our navx and labview libraries have been updated to the most recent versions.
Any help will be greatly appreciated.

Recommended by a team in Houston. They had some complaints about the support.

NavX support is now included in RobotPy as of 2016.1.1, in the robotpy-wpilib-utilities package. Documentation can be found on our readthedocs website.

I haven't ported all of the samples over yet, BUT the one I did port over (the rotate to angle sample) works in simulation too, so you can see the robot rotating when you hit the button and such. Very cool demo.

Would allow multiple I2C-connected NavX-sensors on a robot.

Recommended by team mentor at Detroit FRC Champs.

We need new pictures for the website for use with the new library.

My intent is to wrap the C++ class with cython so we can use it with RobotPy. RobotPy has it's own version of WPILib, so using the C++ WPILib would most likely cause conflicts.

At the moment, I think the best approach would be to turn the existing code into AHRSBase, and remove the WPILib.h include and the inheritance from SensorBase et al. The RegisterIO interface might be a good boundary here, I haven't looked too carefully at it.

Then, a separate class would be called AHRS, which would have the WPILib specific pieces in it. This would retain backwards compatibility, and be good separation of concerns anyways.

Under the java examples the collision detection and auto balance programs, the auto balance is just an exact copy of the collision detection example.

After upgrading to 2017.3.1, I2C communication with NAVX micro failed. USB communication still functioning. Roll back to 2017.2.1 resolved issue. Reference issue #490 in WPILib repo... wpilibsuite/allwpilib#490

Contrary to the javadoc, it seems AHRS.getRate() does not return degrees per second. Experimentally, when the robot was spinning at breakneck speeds (>1 rotation per second) with a well-behaved, calibrated NavX (yaw looked correct), the getRate function was returning between 7 and 8, when I expected greater than 360.

Digging into the code, it looks like AHRS.getRate() calls ContinuousAngleTracker.getRate(), which doesn't account for the sample rate of the incoming data:

It seems that somewhere you'd need to take into account the timestamps of the data coming in, or use the set update rate to determine the rotation rate in terms of degrees per second.

I think my sensor was set to 60Hz by default (I didn't change this), so my 7-8 observation would indicate 7*60 = 420DPS to 8*60 = 480 DPS...plausible, I think.

I'm tinkering with fixing this and might submit a pull request if I get the gumption to do so tomorrow.

After the AHRS class successfully communicates w/the navX-Micro, it sets connected = true. And it sets it false if it ever gets a response from the CDIM that has invalid data (this behavior occurs if navX-Micro gets reset or power-cycled). However, once connected, if there's a I2C read timeout (which occurs in case of the conflict you've found), the connected flag is not being set false. So that's an outright bug that needs fixing. It should be set to false, which will trigger the navx_ftc IO thread to re-initialize communication with the navX-Micro.