Git Product home page Git Product logo

orientationsensorfusion-esp's Introduction

Sensor Fusion Library

Introduction

An easy-to-use interface to the NXP Sensor Fusion version 7 algorithms, providing orientation data derived from a 9 degrees-of-Freedom motion sensor.

It is configured to work with the Adafruit breakout board #3643 using the NXP FXOS8700 magnetometer/accelerometer and FXAS21002 gyroscope sensor ICs, but can be modified to work with other sensors having an I2C interface. With additional modification, it can also work with SPI interface sensors.

The library runs on Espressif's ESP32 and ESP8266 processors. Library output is via Get___() methods that return the various orientation parameters (e.g. GetHeadingDegrees(void), GetPitchDegrees(void), GetAccelXGees(void), etc). A choice of units is provided (e.g. Degrees, Radians, quaternions, Gees or m/s^2). The IC temperature is also available.

An example main.cpp (see examples/fusion_text_output.cc) illustrates how to use this library, and outputs orientation data in text format using serial and WiFi interfaces. For a slightly more complex example, see the adjunct library https://github.com/BjarneBitscrambler/SignalK-Orientation which uses this one to generate the orientation data, then packages it up and sends it via WiFi to a Signal K server.

A C++ class (src/sensor_fusion_class.h) provides simple access to the most common sensor fusion functions, but it is also possible to directly interface with the library methods contained in the underlying C files, which are based on those provided by NXP in their version 7.20 release.

Background

Orientation sensing using combined accelerometer + gyroscope + magnetometer sensors has become quite accurate when coupled with the right sensor fusion software. NXP manufactures these types of sensors, and have written an excellent sensor fusion library for their Kinetis 32-bit microcontrollers. Their library, which is released under the BSD-3-Clause license, was ported in 2015 to the Arduino environment by Adafruit as their AHRS (Attitude and Heading Reference System). The AHRS port uses NXP's version 4.2 fusion code.

A newer version of the NXP sensor fusion library (version 7.2) is available, which is what this present project is using. This newer library has several improvements, including the ability to perform magnetic calibration while in use (as opposed to needing a separate software tool).

My motivation for porting NXP's library is to create an orientation sensor for marine use, using off-the-shelf hardware and the Signal K / SensESP project to provide orientation data (e.g. magnetic heading, roll, and pitch) on a vessel. I have targeted this project for the Espressif ESP32/ESP8266 microcontrollers and the NXP sensors.

Sensors

The present software works with the NXP 9DOF (9 Degrees-Of-Freedom) sensor combination consisting of FXOS8700 magnetometer + accelerometer and FXAS21002 gyroscope. These are conveniently available mounted together on the Adafruit #3463 breakout board.

Other sensors could be used. Note that only the I2C interface has been implemented and tested; a SPI interface is possible with additional work and testing.

Processor

The present software is written for the ESP32 and ESP8266 processors. With some rewriting of the I2C and timing routines, the code can be ported to other processors.

Dependencies

The fusion code and associated project files have been written for and tested in the PlatformIO development environment, as an Arduino framework project on an ESP32 board and on an ESP8266 board.

In addition to the standard Arduino environment, this project uses the following libraries which are automatically installed in the Arduino framework:

  • Wire (I2C) library (used for communicating with the sensor ICs)
  • EEPROM library (used to store calibration values in non-volatile memory)
  • WiFi libraries (only needed by the example main.cpp if WiFi output is enabled)

Where To Find...

  • Documentation for the fusion code is html-based; open the project's docs/html/index.html file in your favourite browser. Documentation is auto-generated from comments in the code itself, using Doxygen.
  • Test plans, data, results, etc are on this project's Github Wiki
  • NXP's version 7 sensor fusion for ESP32 processors is under the Code tab of this Github repository. It is fully functional with NXP's Windows-based Sensor Fusion Toolbox software application.
  • Orientation data output in Signal K format using the SensESP project is on the SignalK-Orientation project page. This project provides an example that uses this Orientation library.

Contributions

Use the Issues and Pull Request tabs on this project's Github repository if you have suggestions or wish to contribute to this project.

How-To Use

To use this library follow these steps (some untested - let me know of any changes you needed to make to get things to work on your setup):

  • setup the PlatformIO development environment
  • create a new PlatformIO project, selecting the Board: Espressif ESP-WROVER-KIT and Framework: Arduino (other ESP32 boards should work without changes; as does the d1_mini board with Espressif's ESP8266 CPU)

Now follow either of these two methods to bring in the library files:

Method 1 (gets you a local clone that you can edit or base pull requests on)

  • create a local clone of this repository on your computer
  • from your local repository, copy the contents of /src into your new project's /src folder. Ensure you get all the files as well as the subfolder /src/sensor_fusion.
  • from your local repository, copy /examples/fusion_text_output.cc into your new PlatformIO project's /src folder. You may want to rename it main.cc or main.cpp to remind yourself that it contains the setup() and loop() functions.

Method 2 (doesn't require manual cloning into a local repository)

  • copy this project's /examples/fusion_text_output.cc into your new PlatformIO project's /src folder. You may want to rename it main.cc or main.cpp to remind yourself that it contains the setup() and loop() functions.
  • copy this project's platformio.iniinto your new project's root directory (or use it to modify the relevant sections in your own project's platformio.ini). Locate the section lib_deps = and add the line https://github.com/BjarneBitscrambler/OrientationSensorFusion-ESP.git.

Method 2 results in the library code being imported into PlatformIO's .pio folder.

Then:

  • edit your new project's platformio.ini file for your specific board and environment. Use this project's platformio.ini file as an example.
  • edit the #defines at the top of your fusion_text_output.cc (the sample main.cc) to reflect your particular hardware. The I2C pins connecting your processor to your sensor ICs will likely be different, and you may also need to change the I2C addresses that the ICs are configured for.
  • compile and download to your processor

Initial Text Output

To confirm that the software is communicating with your sensors, observe the serial port output of the ESP processor. Use a USB connection to a PC running a terminal program at 115200 baud, 8 bits, No parity, 1 stop bit. Several progress messages should appear as the fusion software configures the sensors. Once the algorithm is running, lines of text containing a timestamp and orientation data should scroll by. See the main.cc program's loop() for details.

However, the best way to visualize operation of the orientation algorithm is by using the NXP Sensor Fusion Toolbox.

NXP Sensor Fusion Toolbox

The out-of-the-box software is configured to send data packets containing the sensor fusion results at a rate of 40 Hz over the processor's Serial UART interface (connected to the USB port on my WROVER development kit). These packets are formatted for NXP's Sensor Fusion Toolbox Windows application (available for download from NXP at no cost) which will display the data and can even be used to send commands back to the processor running the fusion algorithms. See the User's Guide under the Help tab of the Toolbox for details.

The Toolbox when working shows a graphic of a PCB rotating on the screen in synchronization with motion of your own board. If there is no motion at all, then check that the data packets are arriving on the expected COM: port of your computer. A terminal program (like HyperTerminal or PuTTY) can help display traffic on a COM: port, but note that the data packets are in binary format (not ASCII text) so you won't be able to interpret them visually. If the Toolbox shows motion but it is jerky or reversed from the actual board motion, then likely one or more of your board's axes are not oriented according to how the fusion software expects. Different sensor board manufacturers will have placed the sensor ICs in orientations particular to their own needs. The file hal_axis_remap.c is used to invert or swap axes to conform to what the fusion algorithm expects. For more details, see that file, and also NXP's Application Note AN5017 (Coordinate Systems).

WiFi Data Streaming

Because testing an orientation sensor with a USB cable tethering it to your development computer is a pain, the software also supports streaming the data over WiFi. In the main setup() code, a WiFi AP (Access Point) is started, which means the ESP processor will broadcast its SSID and you should be able to connect to it with your development system using the password you provide in the main.cc file. Once a WiFi connection is established, you can open a TCP connection to port 23 of the ESP and the orientation data will then stream over your TCP connection. A few hints:

  • view the ESP's serial output (e.g. using that USB connection) to find out what IP address the ESP has assigned itself
  • on a linux system, an easy way to make a TCP connection is the command telnet 192.168.4.1, where you replace the example IP address with the one the ESP has indicated in its serial output.
  • Once you have noted the IP address, it shouldn't change between reboots of the ESP.
  • piping the data from the TCP connection to a serial port on a Windows computer (to make it available for the Sensor Toolbox) isn't trivial unfortunately. I tried two applications: H.W. Virtual Serial Port and TCP-COM. Both are free for time-limited trial copies. The first one exhibited intermittent data dropouts, which confused the Sensor Toolbox quite badly. TCP-COM was better-behaved and has worked well for several hours. There may be better alternatives - if you know of one, let me know and I'll list it here.

I did earlier try having the ESP connect as a client to our WiFi router, rather than acting as an AP itself. Unfortunately, this caused delays in delivery of the streamed traffic that would intermittently freeze the Sensor Toolbox. Performing a ping from the development computer to the ESP showed trip times sometimes exceeded 1000 ms when going through the router. So, using the ESP as an AP is better for timely data delivery on my hardware.

Using WiFi (even when ESP is acting as AP) does introduce noticeable lag in the Toolbox graphic response, compared to a USB connection. It looks like about a 200 ms lag on my system.

Additional Debugging

You can use the GPIO output that toggles each time through the data collection and sending loop to confirm whether your ESP is collecting and transmitting data regularly. Using the default software, the output should toggle every 25 ms (i.e. a 20 Hz square wave). See fusion_text_output.cc for details.

Customizing and Modifying

The file/sensor_fusion/build.h contains defines for various functionality, such as whether the software outputs its data via hardware serial UART or WiFi TCP connections, or both (default). Edit this file as desired, but note that not all combinations of features may be valid or been tested.

Changes to adapt to other hardware are confined to a few files, as the majority of the fusion code is generic C-code that is pretty platform-independent. Files that would be expected to change when using different hardware are the hal_*.* files, board.h, and build.h. As well, new sensor IC driver files may be needed, patterned on the existing driver_fxos8700.* and driver_fxas21002.* files. Finally, calibration_storage.* may need changing depending on how non-volatile memory functions on the different hardware.

If you want to change how the fusion algorithm operates, have a look at control*.*, build.h, and status.*. Quite a lot of parameters are selected via pre-processor #define statements; check the comments for suggestions on how to achieve your goals.

Author

Bjarne Hansen

License

Copyright (c) 2020, Bjarne Hansen All rights reserved.

SPDX-License-Identifier: BSD-3-Clause

orientationsensorfusion-esp's People

Contributors

bjarnebitscrambler avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar

orientationsensorfusion-esp's Issues

Different Flash footprint size when compiling

When compiling the library using example main.cc with the source under /src folder the flash size is 701650 bytes, but when compiling as a library in the .pio folder the code size is 701898 bytes.

SensESP integration

It's desirable to have the orientation code function as a library compatible with the SensESP project.

Work is in-progress. ETA about end-December.

Add method to correct for Magnetic Deviation

@DanielG86 suggested adding a means for correcting the compass readings for Deviation. After the built-in magnetic calibration routine runs, there will likely be some residual error. If this error is repeatable then we should be able to correct for it, similarly to using a deviation table with a physical compass.

Depending on the type of error, the correction might be one or more of:

  • a constant offset
  • an offset that varies sinusoidally around the complete arc

Implement method for causing Calibration to be saved

Magnetic calibration is loaded on boot-up from NVM (EEPROM) if available. Though calibration is checked and generated during regular operation, it is not automatically saved to NVM unless commanded. Currently the only way that command is received is from the NXP Sensor Toolkit. Therefore the sensor has to generate a new magnetic calibration each time it is powered on, which takes a bit of physical manipulation and time.

It is desirable to implement a method in the interface class to cause an existing magnetic calibration to be saved to NVM.

Improve selection of serial input/output stream

Currently the fusion code can output results and accept commands via the Serial object and via TCP socket. These are selected using some inelegant #ifdef - #endif brackets around multiple code sections in multiple places.

It's preferred to implement something that at the lower level would accept a stream pointer of arbitrary type and use it for input/output. The main.cc code could then instantiate whatever type of stream was needed.

Use SysTick feature of Toolbox to measure filter performance

The fusion library has the ability to pass a SysTick parameter to the Toolbox for display, as a measure of how long an iteration of the filter algorithm took to execute. It would be nice to know how much CPU time is used in running the fusion algorithm.

The SysTick is available on ARM processors, and so is currently disabled in the code, but can probably be emulated on the ESP processors.

Reduce SV_xxxx structs to SV_COMMON

The fusion code has 7 different algorithms (with various combinations of accel, gyro, and magnetometer sensors, plus complexity). Each algorithm deposits its results in its own struct, named on the pattern SV_xxxx (e.g. SV_6DOF_GB_BASIC).

All these structs are part of the sensor fusion global sfg object. We could decrease memory used by creating a union of these structs and access them through a SV_COMMON struct. This might also reduce the number of conditional compilation #ifdef - #endif code blocks.

Arduino Library format is desirable

The orientation code needs a wrapper to make it usable as a general Arduino library (note that compatibility with non-Espressif processors is a separate issue).

It will also need additional examples, a manifest, some supporting files, a release, etc.

Scaling of Accelerometer readings appears incorrect

Initial testing with the Sensor Toolbox indicates that the scaling of the accelerometer outputs (all axes) is incorrect. For example, there is not a 2 g difference in readings between an axis that is pointing down and when it points up.

Have to check whether this is related to the initialization of the accelerometer, or in the scaling and corrections that happen during calibration, or during translation into the packet that is sent to the Toolbox.

Renaming this Repository

I'm about to post the sensor-fusion code that I've ported to the ESP32 platform. Before I do, I'm renaming this repository.
@tedenda and @DanielG86 - if you have bookmarks to this repository you may have to update them...

Confirm that the Sensor Axes are oriented correctly

Testing with the Sensor Toolbox indicates that the axes of the sensors might not be oriented correctly. It appears that the X and Y axes of both the magnetometer and the accelerometer are swapped. The file hal_axis_remap.c needs to be edited to correct this. The expected orientation for nautical use (and apparently aeronautical) is called NED, where X,Y,Z are in a right-hand coordinate system pointing North, East, and Down respectively.

Allow fusion to continue despite non-communicating sensor

For compatibility with Signal-K/SensESP project, it's best to allow program execution to continue, despite lack of comms with a sensor. SensESP may have multiple sensors attached, and failure of one should not bring down the whole system.

Will need a method for passing back the status of the sensors, and for retrying comms. Something like GetSensorStatus()...

Implement method to report magnetic interference

NXP's Sensor Fusion Toolbox displays a bargraph showing the reliability of the current magnetic calibration. It is desirable to add a method that reports the same/similar information to the library interface.

Some preliminary research shows that a combination of comparing the current magnetic field magnitude to the expected value, and the current magnetic field dip to the expected value, may be of use.

Document the internals of the sensor fusion code that are useful to modify

The sensor fusion code consists of a large number of source files. It isn't obvious which ones to modify for a given purpose, so documenting the structure of the fusion code and how it relates to particular files would be useful.

The comments in each source file are reasonably clear, but it's tedious to open up each file to have a look. Document the overall structure perhaps in the WiKi, or a Readme.txt file

Move documentation out of project

The Doxygen-generated documentation is placed in the project's /html folder (access it via the index.html file in that folder). However, this has 2 bad side-effects:

  • the size of the project itself has grown substantially, if one downloads a tarball or clones it
  • Github assesses the content of the project incorrectly: it now says > 80% of the project is HTML.

Ideally, we want a link to the documentation pointing outside of the project code itself. A solution might involve GitHub Pages...

Implementing Library For Other Sensors

Hi @BjarneBitscrambler ,
First of all thanks for sharing such an awesome library and complete description in examples and header files, it's really helpful.
But with all these, implementing the library for another sensor is still confusing. I checked the whole library more than once and It looks like using this library for another sensors is a little bit impossible.
It's gonna be great if you help making a better and more understandable simple code :
Lets consider that I have my raw sensor data as ax,ay,az,gx,gy,gz,mx,my,mz variables which is in standard units and frequency for library.
1.Which header files have to be included in main script?
2.How should I feed these variables to sensor fusion?
3.How should I calibrate the whole IMU ACC,GYRO,MAG?(cause I couldn't find any calibration function for GYRO)
4.How should I get the Quaternion data as output?

also making a simple script like above looks like to be so challenging cause the main fusion header file is using build and board files and I couldn't find any possible way to pass the raw sensor data to filter.
anyways I hope there is an easier way.
Kind Regards,
Mason.

Include optional .h files in sample main.cpp

Depending on which sensor reports are enabled in main.cpp, additional header files might be needed. One example is the temperature report, which relies on a Linear transform to scale/correct the values.

Review the sample code, and add appropriate comments for needed header files.

Wireless data streaming is desirable

Currently the code streams data via serial UART to the outside world. This is inconvenient for testing: the USB cable objects to being twisted into a pretzel while manipulating the sensor. Adding wireless streaming, e.g. via TCP socket over WiFi, would be handy.

Support

Hello Bjarne,
Great project and just wanted to let you know that Iโ€™m willing to support where possible.
Currently just prepared a test version on a breadboard, with some issues, as reported in the PR (I think #166)

General code cleanup

Filenames should be standardized.
Consider removing SPI files and just leaving stubs that could be used later if desired.
Consider consolidating the user-edited files into one.
Read up on what's required to make an Arduino library.
Decide whether to convert to C++ or leave as (mostly) C.
Define an interface to outside world - one that will be compatible with SensESP.

Improve Magnetometer SNR by Sampling Twice

Looking at the I2C transactions with a scope, it is apparent that there is sufficient time to perform additional reads of the magnetometer during each fusion cycle. Currently one one read is performed, and because the magnetometer portion of the FXOS8700 does not have a FIFO, that read only obtains a single data point. It should be feasible to perform a magnetometer read before and after the block read of the accelerometer, and possibly also after the gyro read. Thus there might be 2 or 3 data points per fusion cycle, which would improve the signal to noise ratio if they are averaged.

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    ๐Ÿ–– Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. ๐Ÿ“Š๐Ÿ“ˆ๐ŸŽ‰

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google โค๏ธ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.