ADIS1647X

Hello, I have been using the ADIS 16475 IMU in hopes of collecting flight data. I have the IMU attached to a drone and am trying to get some inertial measurements. I was encouraged to purchase the EVAL-ADIS-FX3 to use with a raspberry pi to take flight data but, unfortunately the supplied GUI has drivers that are not supported by ARM processors. I then decided to follow the example found on the ADIS1647X Wiki page: ADIS1647x with Teensy 3.2 (Arduino) and successfully soldered up my connections to a teensy board. I was able to output the data registers successfully with serial out commands. I do not know based off the example how to control the data rates but, I was able to use the mills() function to output a time stamp of sorts. The example uses Putty as the interface to see and write the serial outputs from the IMU. I was essentially able to successfully recreate the results from the GitHub example. Unfortunately, Putty on a RPI is not the same given the ARM OS. So ultimately I decided to re-wire the IMU to a teensy board with a microSD card so that rather than outputting serially via Putty, I am now outputting the data registers to a micros SD card.

The only problem is it seems like the teensy is competing in the processes being commanded. It outputs at the same rate as the sample provided via Putty except the data registers are not being updated with each output. For example now I get the same register about 20-30 times before an update occurs. Does anyone know how to make it so that the data registers are being processed first then written to the microSD. I would like to be able to output at 1000Hz.

Thank you!

  • 0
    •  Analog Employees 
    on Oct 11, 2021 7:38 PM

    I am sorry that we have not been able to respond to your request, in this forum.  I feel like I have been saying this for quite some time, but we are going through unique staffing challenges, which made it difficult for us to monitor this forum as effectively as we would like.  As we have in the past, we are working diligently on addressing our coverage plan and hope to improve on this, very soon. 

  • 0
    •  Analog Employees 
    on Oct 11, 2021 7:41 PM

    With respect to your request, I am admittedly confused by all of the different tools that you are mentioning.  At the moment, do I understand correctly, in that you are trying to read ADIS16475 data and store it to an SD card, using the Teensy 3.2? 

  • 0
    •  Analog Employees 
    on Oct 11, 2021 7:43 PM

    I am a bit rusty on SD cards, but my first question might be to verify that the SD cards can support sufficient access times, in order to track the rate at which the ADIS16475 is producing data.  Is there a way we can quantify that potential threat? 

  • 0
    •  Analog Employees 
    on Oct 12, 2021 6:16 AM

    You have an interesting approach to working this problem. You're looking essentially at a "baremetal" approach to logging data. The library you linked works based on interrupt service routines which read the data based on the data ready line. In this particular case, the ISR will read data every time it is free. However at 1000Hz, trying to print that data to the serial console/Putty or log to an SD card in that same routine would effectively "soft lock" your processor and cause you to miss interrupts. A write to Putty/SD card is a long process compared to the ISR which should be executing at 1000Hz. Thus a note at the bottom of the library page shows that the serial is only updated every 2 seconds or so.

    What that means in this case is that you have to determine how and when you're going to update data to the SD card. Specifically you need to determine how you're going to handle the fact that you can write to your SD card at a slower rate than you can service the ISR. Ultimately, it comes down to buffering samples. Since the ISR will read data from the registers, you have fresh data, however you have to store that somewhere. If you need every sample, you'll likely have to introduce some buffer scheme (FIFO) to manage data between your IMU and your SD card. This should be appropriately sized to allow your slower SD card writing process to move data from the FIFO while you still read data every 1000Hz. This is not an easy task, but it's a doable one with careful implementation.

    To help make this process a bit easier, you could break this into several smaller tasks: 1. read/write SD card 2. read IMU data 3. buffer IMU/data

    For 1. you should check that you have a suitable library to write/read from SD cards properly. With this in place, check how quickly you can write to the SD card given the libraries. If you're using a scope, you could set a digital pin high at the start of a test write (make the write mimic what you would write if you used IMU data) and then bring the pin low. You could use a scope to measure the width of this pulse by triggering on the rising edge. Alternatively you could estimate it in SW by performing a series of test writes with time stamps. This all should give you a rough idea of how quickly you can write to the SD card under "ideal circumstances" i.e. without interrupts at 1kHz. Take that rate and then divide it by a factor of 2 (or more) at least to give an idea of how much time you will lose by servicing the interrupts and buffering data

    2. Check you can read IMU data. I think you have done this already and seem to get valid data. You would want to consider the output data rate/decimation rate section starting on page 28 of the UG: https://www.analog.com/media/en/technical-documentation/data-sheets/ADIS16475.pdf That should help you set the data rate to 1kHz. Again you could toggle a digital pin and trigger it on a scope to get an idea of if the ISR was operating at the right rate.

    3. with the IMU data rate set and the SD card rate known, you can roughly estimate the size of a buffer you need to implement. It could be a simple ring buffer in flash if it's not too large, but get something that has plenty of space to accommodate slower SD card reads if something goes awry. 

    With all of that in place, I'd recommend incorporating some sort of start/stop button(s) to enable reading/writing to SD cards. This will help prevent data loss. 

    Hope some of that helps!

    -Chas

  • Thank you Mark and crick for your responses. YEs I have been trying to read data from the ADIS16475 data and store it on an SD card using a Teensy 3.6. I was able to read data at acceptable rates using serial out and interpreting them in the ARDUINO serial output window as well as Putty. I checked to make sure I have a Sandisk Extreme microSD card with write speeds up to 60 MB/s. With regards to testing to determine the write speeds of the SD card, I will have to look into that. I wish to sample the IMU at 1kHz and Im writing to the SD card at 1kHz. However it is inconsistent as I mentioned before and outputs the same registers back to back about 4 samples in a row. I think looking into implementing a buffer between the IMU and the SD card is the way to go.

    If there are any things that jump out at you as obvious improvements or other possible remedies please let me know. 

    Thanks for the continued support!

    Here is a copy of the Arduino code:

    ////////////////////////////////////////////////////////////////////////////////////////////////////////
    // November 2017 | Updated July 2021
    // Author: Juan Jose Chong <juan.chong@analog.com>
    // Edited: Sky Seliquini- <sseli001@odu.edu>
    ////////////////////////////////////////////////////////////////////////////////////////////////////////
    // ADIS16475_Teensy_Expanded_Read.ino
    ////////////////////////////////////////////////////////////////////////////////////////////////////////
    //
    // This Arduino project interfaces with an ADIS16475 using SPI and the
    // accompanying C++ libraries, reads IMU data in LSBs, scales the data, and
    // outputs measurements to a serial debug terminal (PuTTY) via the onboard
    // USB serial port. The Full IMU data set is read, not just the first 20 bytes.
    //
    // This project has been tested on a PJRC 32-Bit Teensy 3.6 Development Board,
    // but should be compatible with any other embedded platform with some modification.
    //
    // Permission is hereby granted, free of charge, to any person obtaining
    // a copy of this software and associated documentation files (the
    // "Software"), to deal in the Software without restriction, including
    // without limitation the rights to use, copy, modify, merge, publish,
    // distribute, sublicense, and/or sell copies of the Software, and to
    // permit persons to whom the Software is furnished to do so, subject to
    // the following conditions:
    //
    // The above copyright notice and this permission notice shall be
    // included in all copies or substantial portions of the Software.
    //
    // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    // EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
    // MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    // NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
    // LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
    // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
    // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
    //
    // Pinout for a Teensy 3.6 Development Board
    // RST = D6
    // SCK = D13/SCK
    // CS = D10/CS
    // DOUT(MISO) = D12/MISO
    // DIN(MOSI) = D11/MOSI
    // DR = D2
    //
    ////////////////////////////////////////////////////////////////////////////////////////////////////////

    #include <ADIS16475.h>
    #include <SPI.h>
    #include <SD.h>


    unsigned long myTime;

    // SD CARD
    // On the Ethernet Shield, CS is pin 4. Note that even if it's not
    // used as the CS pin, the hardware CS pin (10 on most Arduino boards,
    // 53 on the Mega) must be left as an output or the SD library
    // functions will not work.

    // change this to match your SD shield or module;
    // Arduino Ethernet shield: pin 4
    // Adafruit SD shields and modules: pin 10
    // Sparkfun SD shield: pin 8
    // Teensy audio board: pin 10
    // Teensy 3.5 & 3.6 & 4.1 on-board: BUILTIN_SDCARD
    // Wiz820+SD board: pin 4
    // Teensy 2.0: pin 0
    // Teensy++ 2.0: pin 20
    const int chipSelect = BUILTIN_SDCARD;


    //ADIS16475
    // Uncomment to enable debug
    //#define DEBUG

    // Initialize Variables
    // Temporary Data Array
    uint16_t *burstData;

    // Checksum variable
    int16_t burstChecksum = 0;

    // Accelerometer
    float AXS, AYS, AZS = 0;

    // Gyro
    float GXS, GYS, GZS = 0;

    // Delta Angle
    float DAXS, DAYS, DAZS = 0;

    // Delta Velocity
    float DVXS, DVYS, DVZS = 0;

    // Gyro Bias Offset Correction
    float GBXS, GBYS, GBZS = 0;

    // Accelerometer Bias Offset Correction
    float ABXS, ABYS, ABZS = 0;

    // Control registers
    int MSC = 0;
    int FLTR = 0;
    int DECR = 0;

    // Temperature
    float TEMPS = 0;

    // Time Stamp
    float TIME = 0;

    // Delay counter variable
    int printCounter = 0;

    // Call ADIS16475 Class
    ADIS16475 IMU(10,2,6); // Chip Select, Data Ready, Reset Pin Assignments

    void setup()
    {
    Serial.begin(115200); // Initialize serial output via USB
    //IMU.configSPI(); // Configure SPI communication //NOTE .configSPI() is wrong
    IMU.select(); // Configure SPI communication
    delay(500); // Give the part time to start up
    IMU.regWrite(MSC_CTRL, 0xC1); // Enable Data Ready, set polarity
    IMU.regWrite(FILT_CTRL, 0x04); // Set digital filter
    IMU.regWrite(DEC_RATE, 0x00), // Disable decimation

    // Read the control registers once to print to screen
    MSC = IMU.regRead(MSC_CTRL);
    FLTR = IMU.regRead(FILT_CTRL);
    DECR = IMU.regRead(DEC_RATE);

    attachInterrupt(2, grabData, RISING); // Attach interrupt to pin 2. Trigger on the rising edge
    //SD Card Initialization
    while (!Serial) {
    ; // wait for serial port to connect.
    }

    Serial.print("Initializing SD card...");

    // see if the card is present and can be initialized:
    if (!SD.begin(chipSelect)) {
    Serial.println("Card failed, or not present");
    while (1) {
    // No SD card, so don't do anything more - stay stuck here
    }
    }
    Serial.println("card initialized.");
    }

    // Function used to read register values when an ISR is triggered using the IMU's DataReady output
    void grabData()
    {
    burstData = {};
    IMU.select(); // Configure SPI before the read. Useful when talking to multiple SPI devices
    burstData = IMU.wordBurst(); // Read data and insert into array
    }

    // Function used to scale all acquired data (scaling functions are included in ADIS16470.cpp)
    void scaleData()
    {
    GXS = IMU.gyroScale(*(burstData + 1)); //Scale X Gyro
    GYS = IMU.gyroScale(*(burstData + 2)); //Scale Y Gyro
    GZS = IMU.gyroScale(*(burstData + 3)); //Scale Z Gyro
    AXS = IMU.accelScale(*(burstData + 4)); //Scale X Accel
    AYS = IMU.accelScale(*(burstData + 5)); //Scale Y Accel
    AZS = IMU.accelScale(*(burstData + 6)); //Scale Z Accel
    TEMPS = IMU.tempScale(*(burstData + 7)); //Scale Temp Sensor
    TIME = (*(burstData + 8)); //Time Stamp
    DAXS = IMU.deltaAngleScale(*(burstData + 11)); // Scale X Delta Angle
    DAYS = IMU.deltaAngleScale(*(burstData + 11)); // Scale Y Delta Angle
    DAZS = IMU.deltaAngleScale(*(burstData + 12)); // Scale Z Delta Angle

    }

    // Main loop. Print data to the serial port. Sensor sampling is performed in the ISR
    void loop()
    {
    printCounter ++;
    if (printCounter >= 500) // Delay for writing data to the serial port
    {
    detachInterrupt(2); //Detach interrupt to avoid overwriting data
    scaleData(); // Scale data acquired from the IMU
    burstChecksum = IMU.checksum(burstData); // Calculate checksum based on data array
    // open the file.
    File dataFile = SD.open("datalog.txt", FILE_WRITE);

    // if the file is available, write to it:
    if (dataFile) {
    /*dataFile.println(" ");
    dataFile.print(AXS,4);
    dataFile.print(",");
    dataFile.print(AYS,4);
    dataFile.print(",");
    dataFile.print(AZS,4);
    dataFile.close();
    */

    // Print Time Stamp data
    dataFile.println(" ");
    //Serial.print(TIME,2);

    // Print scaled gyro data
    //Serial.print(" ");
    dataFile.print(GXS,3);
    dataFile.print(",");
    dataFile.print(GYS,3);
    dataFile.print(",");
    dataFile.print(GZS,3);

    // Print scaled accel data
    dataFile.print(" ,");
    dataFile.print(AXS,4);
    dataFile.print(",");
    dataFile.print(AYS,4);
    dataFile.print(",");
    dataFile.print(AZS,4);

    // Print scaled delta angle data
    //dataFile.print(" ,");
    //dataFile.print(DAXS,6);
    //dataFile.print(",");
    //dataFile.print(DAYS,6);
    //dataFile.print(",");
    //dataFile.print(DAZS,6);

    //Time
    dataFile.print(" ,");
    dataFile.print((*(burstData + 8)));
    /* // print to the serial port too:
    Serial.println(" ");
    Serial.print(AXS,4);
    Serial.print(",");
    Serial.print(AYS,4);
    Serial.print(",");
    Serial.print(AZS,4);
    */
    // print time since board turned on to serve as clock in milliseconds
    myTime = millis();
    dataFile.print(" ,");
    dataFile.print(myTime);
    delay(10); //sample at 100Hz
    dataFile.close();
    }
    else {
    // if the file isn't open, pop up an error:
    Serial.println("error opening datalog.txt");
    }
    //delay(100); // run at a reasonable not-too-fast speed



    #ifdef DEBUG
    detachInterrupt(2); //Detach interrupt to avoid overwriting data
    scaleData(); // Scale data acquired from the IMU
    burstChecksum = IMU.checksum(burstData); // Calculate checksum based on data array

    // Print Time Stamp data
    Serial.println(" ");
    //Serial.print(TIME,2);

    // Print scaled gyro data
    //Serial.print(" ");
    Serial.print(GXS,3);
    Serial.print(",");
    Serial.print(GYS,3);
    Serial.print(",");
    Serial.print(GZS,3);

    // Print scaled accel data
    Serial.print(" ,");
    Serial.print(AXS,4);
    Serial.print(",");
    Serial.print(AYS,4);
    Serial.print(",");
    Serial.print(AZS,4);

    // Print scaled delta angle data
    Serial.print(" ,");
    Serial.print(DAXS,6);
    Serial.print(",");
    Serial.print(DAYS,6);
    Serial.print(",");
    Serial.print(DAZS,6);

    //Time
    Serial.print(" ,");
    Serial.print((*(burstData + 8)));

    #endif
    printCounter = 0;
    attachInterrupt(2, grabData, RISING);
    }
    }

    Here is an example of the output I am getting when writing to the microSD:

    GyroX/GyroY/GyroZ/AccelX/AccelY/AccelZ/data_register_time/arduino time (milli)

    -0.800,-1.000,8.900 ,0.1324,0.0368,-0.5077 ,10502 ,6204
    -0.800,-1.000,8.900 ,0.1324,0.0368,-0.5077 ,10502 ,6221
    -0.800,-1.000,8.900 ,0.1324,0.0368,-0.5077 ,10502 ,6239
    -0.800,-1.000,8.900 ,0.1324,0.0368,-0.5077 ,10502 ,6253
    -0.800,-1.000,8.900 ,0.1324,0.0368,-0.5077 ,10502 ,6268
    -0.800,-1.000,8.900 ,0.1324,0.0368,-0.5077 ,10502 ,6282
    -0.800,-1.000,8.900 ,0.1324,0.0368,-0.5077 ,10502 ,6297
    -0.800,-1.000,8.900 ,0.1324,0.0368,-0.5077 ,10502 ,6311
    0.200,-0.400,-0.400 ,-0.0147,0.0441,-9.8149 ,10759 ,6326
    0.200,-0.300,-0.400 ,-0.0123,0.0490,-9.8198 ,10792 ,6343
    0.000,-0.200,-0.400 ,-0.0172,0.0515,-9.8149 ,10822 ,6358
    0.000,-0.200,-0.400 ,-0.0172,0.0515,-9.8149 ,10822 ,6373
    0.000,-0.200,-0.400 ,-0.0172,0.0515,-9.8149 ,10822 ,6391
    0.300,-0.300,-0.600 ,-0.0147,0.0515,-9.8149 ,10917 ,6405
    0.300,-0.300,-0.600 ,-0.0147,0.0515,-9.8149 ,10917 ,6420
    0.300,-0.300,-0.600 ,-0.0147,0.0515,-9.8149 ,10917 ,6434
    0.300,-0.300,-0.600 ,-0.0147,0.0515,-9.8149 ,10917 ,6449
    0.100,-0.200,-0.400 ,-0.0172,0.0515,-9.8174 ,11034 ,6464
    0.100,-0.200,-0.400 ,-0.0172,0.0515,-9.8174 ,11034 ,6480
    0.100,-0.200,-0.400 ,-0.0172,0.0515,-9.8149 ,11096 ,6495
    0.100,-0.200,-0.400 ,-0.0172,0.0515,-9.8149 ,11096 ,6510
    0.200,-0.300,-0.500 ,-0.0098,0.0515,-9.8174 ,11156 ,6525
    0.200,-0.300,-0.500 ,-0.0098,0.0515,-9.8174 ,11156 ,6543
    0.200,-0.300,-0.500 ,-0.0098,0.0515,-9.8174 ,11156 ,6557
    0.300,-0.300,-0.500 ,-0.0196,0.0490,-9.8149 ,11250 ,6572
    0.300,-0.300,-0.500 ,-0.0196,0.0490,-9.8149 ,11250 ,6586
    0.300,-0.300,-0.500 ,-0.0196,0.0490,-9.8149 ,11250 ,6601
    0.300,-0.300,-0.600 ,-0.0147,0.0589,-9.8149 ,11341 ,6617
    0.300,-0.200,-0.700 ,-0.0172,0.0613,-9.8149 ,11375 ,6634
    0.400,-0.300,-0.500 ,-0.0147,0.0515,-9.8174 ,11405 ,6649
    0.300,-0.300,-0.400 ,-0.0147,0.0466,-9.8149 ,11435 ,6664
    0.300,-0.300,-0.400 ,-0.0147,0.0466,-9.8149 ,11435 ,6679
    0.300,-0.300,-0.400 ,-0.0147,0.0466,-9.8149 ,11435 ,6697
    0.100,-0.300,-0.400 ,-0.0172,0.0564,-9.8174 ,11530 ,6712
    0.100,-0.300,-0.400 ,-0.0172,0.0564,-9.8174 ,11530 ,6726
    0.100,-0.300,-0.400 ,-0.0172,0.0564,-9.8174 ,11530 ,6743
    0.100,-0.300,-0.400 ,-0.0172,0.0564,-9.8174 ,11530 ,6758
    0.200,-0.200,-0.600 ,-0.0172,0.0490,-9.8174 ,11652 ,6773
    0.200,-0.200,-0.600 ,-0.0172,0.0490,-9.8174 ,11652 ,6787
    0.300,-0.400,-0.400 ,-0.0147,0.0441,-9.8272 ,11711 ,6802
    0.300,-0.400,-0.400 ,-0.0147,0.0441,-9.8272 ,11711 ,6817
    0.300,-0.400,-0.400 ,-0.0147,0.0441,-9.8272 ,11711 ,6832
    0.300,-0.400,-0.400 ,-0.0147,0.0441,-9.8272 ,11711 ,6850
    0.300,-0.400,-0.400 ,-0.0147,0.0441,-9.8272 ,11711 ,6864