Thursday, July 7, 2016

UART IMU sensor for EV3

Some time ago I made a Segway-like robot using Lego NXT set. I based it on this mathematical model. The controller used the control loop with 8 ms period, so it required the gyroscope sensor that could read samples faster than 8 ms. I had two gyroscope sensors: Hitechnic Gyro Sensor and Mindsensors AbsoluteIMU.
Hitechnic sensor is an analog one and it can produce samples very fast, but it has significant drift and that depends on the battery voltage (see this post). AbsoluteIMU is a 3-axis sensor that produce more accurate measurements, but it is an I2C sensor that works quite slow on NXT and EV3 platforms. Reading sample time is about 25 ms.
Fortunately, there is a way to speed up I2C bus on NXT platform. LeJOS for NXT can turn I2C sensor to high-speed mode where reading sample time is about 2 ms.
But when I ported my code to EV3 platform, I found that high speed I2C support work incorrectly on EV3. So I could use the sensor in low-speed mode only. It was about a year ago.
I wanted an IMU that combines gyroscope and accelerometer and would be able to read samples faster that 8 ms.
EV3 supports new UART sensors that can support very high communication speed (up to 460 kbit/sec). I have found an article that can be a good starting point to create own UART sensors for EV3.
But I wanted to have a complete device that can be mounted to Lego robot, so I needed to fit it into the an existing Lego sensor case. I used Lego Light Sensor as a case for my IMU sensor.
During working on the sensors I tested three IMU chips: LSM330DLC, LSM9DS0 and LSM6DS3.
The first one is a cheap 6DOF sensor, the second one is a 9DOF sensor (I want to tests 9DOF sensor fusion algorithm on it), and the last one is a low-noise 6DOF sensor.

Sensors use UART speed 115200 bps to be compatible with EV3 Soft-UART ports 3 and 4. Ports 1 and 2 allow to use speed up to 460800 bps. But due to a bug in EV3 firmware it actually uses frequency 485294 bps, so it is necessary to use this value to be able run UART in hi-speed mode (in sensor descriptor we should specify speed 460800, but set uart speed to 485294).  I have prepared a class uart_speed that calculates correct UART speed, compatible with EV3.


Sensor hardware design

 


LSM330DLC sensor schematic
The sensor consist of sensor chip, MCU that reads data samples from the sensor chip and sends them to host using EV3 UART protocol, and power converter that converts 5V power supply voltage from EV3 brick to 3.3V that is required for sensor chip and MCU.
I used STM8S103F3U MCU that has very small package that allows me to fit the sensor circuit into required PCB size. PCB has two-layers design.

Top PCB view

Bottom PCB view

The first prototype I have made at home, the next ones I have ordered on a PCB fab.
Hardware design for all IMUs can be found in Git repository.
I used STM8L Discovery board to debug the sensor software. ST-LINK can be used too.

Photos of the assembled sensors.

LSM330DLC based IMU

LSM9DS0 based IMU
LSM6DS3 based IMU

Sensor software design

Sensor software consist of two components: sensor software (firmware) and EV3 driver software. Sensor software is written using C++, EV3 driver is written using Java and can be used from LeJOS EV3 programs. Currently, I don't have enough information to write EV3-G blocks for my IMUs.

Sensor software is written using Real-Time OS called scmRTOS. It is written on C++. I used IAR C++ for STM8 version 2.10.5. There is a new version 2.20 but it has problems with code inlining, so I don't recommend it.

Sensor software supports sending data samples to EV3 host, changing sensor sensitivity (all used sensor chips have multiple full-scale ranges), writing transformation matrix to MCU EEPROM (implemented for LSM6DS3 and LSM9DS0). IMUs can be used in different modes: gyroscope, accelerometer, gyroscope+accelerometer. LSM9DS0 has additional mode - magnetometer. But, I don't have the calibration tool for magnetometer yet.

The software is designed using C++ templates. The application class can be assembled from modules. The main class is Ev3UartSensor that implements EV3 UART protocol. It has following template parameters:

  • type - sensor type constant to register sensor class in EV3 block.
  • Uart - UART protocol implementation. Currently I use blocking UART implementation with internal circular input buffer. It is the main reason, why RTOS has been used.
  • Config - UART config. The sensor starts using the fixed UART speed 2400 bps, than is changed to the necessary communication speed after getting response from the host. The UART config contains necessary information to set up communication parameters.
  • Device - IMU implementation. This implementation uses 'Curiously recurring template pattern' to allow sensor's core sending samples to the EV3 host
This class is responsible to send device information to EV3, to receive commands from EV3 host and to send sensor samples to EV3. 
IMU implementation, that is passed to Device parameter, is responsible to init sensor chip and to read samples. IMU implementation is template class IMU that has following parameters:
  • ImuCore - device-specific template that contains device communication protocol.
  • Commands - class that implements commands that EV3 host can send to the sensor.
  • event_queue_size - size of internal ring buffer that keeps received commands from EV3 host.
  • EepromWriter - class that provides ability to write data to MCU EEPROM.
  • Derived - EV3 sensor implementation. Actually, it is Ev3UartSensor.

For each sensor chip I have prepared appropriate implementation of ImuCore and Commands classes. These implementations are stored in the sensor specific IMU project folder.
ImuCore template has following template parameters (LSM6DS3 version):
  • ImuTransport - communication protocol for the sensor chip. Currently it is blocking SPI implementation. The implementation SpiTransport is sensor-specific. It has a parameter that allows to customize address selection format (LSM6DS3 has different address selection algorithm).
  • SampleProvider - class that reads samples from the sensor and performs necessary data transformations. I have prepared two implementation: SimpleProvider, that provides raw samples without any transformation, and TransformProvider that corrects samples using transformation matrix stored in the MCU EEPROM. The transformation matrix is calculated during sensor's calibration.
  • Derived - derived class that implements sending samples to EV3 host mechanism. Actually, it is Ev3UartSensor.
Diagram of assembled IMU class
LSM330DLC and LSM9DS0 sensors are implemented as composition of two sensors (gyroscope and accelerometer), and have difefrent parameters for accelerometer and gyroscope transports.

Example of sensor object:

In this example we create an instance of IMU sensor based on LSM6DS3 with communication speed 115200 bps. F_MASER - MCU frequency. It supports sensor calibration and saving calibration matrix into EEPROM. Queue size is 32 for all types. Usage of the same queue size for all classes reduces the generated code size.
//---------------------------------------------------------------------------
//
//      UART configuration
//

typedef UartConfig<F_MASTER, 115200> uart_config;
typedef Uart<Uart1, 32> uart_type;

//---------------------------------------------------------------------------
//
//      IMU class that integrates all devices
//

template <typename Derived>
struct imu_core_type : ev3::lsm6ds3::ImuCore
<
     ImuTransport, 
     sensors::TransformProvider<eeprom_type, eeprom>::Provider, 
     Derived
> {};
template <typename Derived>
struct imu_type : ev3::imu::IMU
<
     imu_core_type, 
     ev3::lsm6ds3::Commands, 
     32, 
     ev3::EepromWriter<TranformationMatrix>, 
     Derived
> {};


//---------------------------------------------------------------------------
//
//      EV3 UART sensor that provides data to EV3 host
//

typedef ev3::Ev3UartSensor<97, uart_type, uart_config, imu_type> sensor_type;
// sensor instance
sensor_type sensor;

Implementation details

STM8 MCU has reserved area for stack. When PUSH command crosses the bottom stack border, the stack pointer sets to the top address of the stack area. This causes problems in multi-stack RTOS like scmRTOS. More details can found here. So I place additional stack (process objects in scmRTOS terms) within the stack area using following commands:

CommandHandler commandHandler @ "HW_STACK";
SensorHandler sensorHandler  @ "HW_STACK";

HW_STACK area is described in lnkstm8s103f3.icf files that are located in the IMU project folders.

This causes build problems on the latest version of scmRTOM in debug mode. The problem appears after adding some diagnostic code to scmRTOS core. I prepared a custom version of scmRTOS that doesn't have this code. 

LSM9DS0 has a dedicated pin to signal that magnetometer sample is ready to read. But I miss this opportunity and this signal is sent using accelerometer data ready line. As a result, the sensor software implements data ready driven behavior in magnetometer-only mode. In combined mode, the magnetometer sample is read when the accelerometer sample is ready. I have prepared the second version of HW design of LSM9DS0 sensor, but have not implemented it yet.

Initialization of MCU pins is performed by PortConfigurer template. It has a template parameter, that contains list of pin descriptors. I used C++ template meta-programming to work with list of types that describe MCU pins.
//---------------------------------------------------------------------------
//
//      Hardware initialization section
//
//---------------------------------------------------------------------------
//
//      GPIO pins configuration
//

//Unused I/O pins are configured as input with internal pull-up resistor.
typedef input_traits<PullUp, InterruptsDisabled> unused_trait;
typedef Pin<Port_B, 4, unused_trait> PinB4;
typedef Pin<Port_B, 5, unused_trait> PinB5;
typedef Pin<Port_D, 3, unused_trait> PinD3;
typedef Pin<Port_D, 4, unused_trait> PinD4;
typedef Pin<Port_C, 3, unused_trait> PinC3;


//UART pins
typedef Pin<Port_D, 5, output_traits<PushPull, Speed_10Mhz> > PinUartTX;
typedef Pin<Port_D, 6, input_traits<Floating, InterruptsDisabled> > PinUartRX;

//Chip select pins
typedef Pin<Port_A, 3, output_traits<PushPull, Speed_10Mhz, High>, Low> PinImuSel;

//Data ready interrupt pins
typedef Pin<Port_C, 4, input_traits<Floating, RisingEdge> > PinAccelDataReady;
typedef Pin<Port_D, 2, input_traits<Floating, RisingEdge> > PinGyroDataReady;

typedef PortConfigurer<mpl::make_type_list<
    PinImuSel,
    PinAccelDataReady, PinGyroDataReady,
    PinUartTX, PinUartRX,
    PinB4, PinB5, PinD3, PinD4, PinC3>::type> configurer;
Port initialization code: 
//Initial port configuration
configurer::configure();
This code used template metaprograming and code inlining to generate small code to initialize STM8 ports. 





EV3 software



IMU sensor driver is inherited from UARTSensor class from LeJOS library. Also, it can implement interfaces to select sensor sensitivity and to save transformation matrix to EEPROM.








package lejos.hardware.sensor.imu;

/**
 * @author Max Morozov
 */
public interface ScaleSelector {
    /**
     * Changes the gyroscope full-scale range
     *
     * @param scaleNo scale range index
     * @return true if the current scale range has been successfully changed
     */
    boolean setGyroscopeScale(int scaleNo);

    /**
     * Changes the accelerometer full-scale range
     *
     * @param scaleNo scale range index
     * @return true if the current scale range has been successfully changed
     */
    boolean setAccelerometerScale(int scaleNo);

    /**
     * Changes the magnetometer full-scale range
     *
     * @param scaleNo scale range index
     * @return true if the current scale range has been successfully changed
     */
    boolean setMagnetometerScale(int scaleNo);
}








package lejos.hardware.sensor.imu;

/**
 * @author Max Morozov
 */
public interface ImuEepromWriter {
    /**
     * Update accelerometer EEPROM
     *
     * @param scaleNo scale index
     * @param data EEPROM data (4x3 matrix of 16-bit integers)
     * @return true if the EEPROM data has been successfully written
     */
    boolean writeAccelerometerEeprom(int scaleNo, short[] data);

    /**
     * Update gyroscope EEPROM
     *
     * @param scaleNo scale index
     * @param data EEPROM data (4x3 matrix of 16-bit integers)
     * @return true if the EEPROM data has been successfully written
     */
    boolean writeGyroscopeEeprom(int scaleNo, short[] data);

    /**
     * Update magnetometer EEPROM
     *
     * @param scaleNo scale index
     * @param data EEPROM data (4x3 matrix of 16-bit integers)
     * @return true if the EEPROM data has been successfully written
     */
    boolean writeMagnetomtereEeprom(int scaleNo, short[] data);
}







IMU sensor driver for LSM6DS3 sensor:






package lejos.hardware.sensor.imu;

import lejos.hardware.port.Port;
import lejos.hardware.sensor.SensorMode;
import lejos.hardware.sensor.UARTSensor;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;

public class ImuLsm6ds3 extends UARTSensor implements ImuEepromWriter, ScaleSelector {
    private static final long SWITCHDELAY = 200;
    private static final long SCALE_SWITCH_DELAY = 10;

    //Return device to the state right after power on
    public static final byte DEVICE_RESET  = 0x11;

    //Accelerometer sensitivity
    public static final byte ACC_SCALE_2G  = 0x20;
    public static final byte ACC_SCALE_4G  = 0x21;
    public static final byte ACC_SCALE_8G  = 0x22;
    public static final byte ACC_SCALE_16G = 0x23;

    //Gyroscope sensitivity
    public static final byte GYRO_SCALE_245DPS  = 0x30;
    public static final byte GYRO_SCALE_500DPS  = 0x31;
    public static final byte GYRO_SCALE_1000DPS = 0x32;
    public static final byte GYRO_SCALE_2000DPS = 0x33;
    public static final byte GYRO_SCALE_125DPS  = 0x34;

    //Write calibration matrix into EEPROM
    public static final byte CALIBRATE_ACC_2G  = 0x40;
    public static final byte CALIBRATE_ACC_4G  = 0x41;
    public static final byte CALIBRATE_ACC_8G  = 0x42;
    public static final byte CALIBRATE_ACC_16G = 0x43;

    public static final byte CALIBRATE_GYRO_245DPS  = 0x50;
    public static final byte CALIBRATE_GYRO_500DPS  = 0x51;
    public static final byte CALIBRATE_GYRO_1000DPS = 0x52;
    public static final byte CALIBRATE_GYRO_2000DPS = 0x53;
    public static final byte CALIBRATE_GYRO_125DPS  = 0x54;

    private static final int ACCEL_SCALE = Short.MAX_VALUE + 1;

    private static final float[] gyroScale = {
         8.75e-3f, 
         17.5e-3f,
           35e-3f,
           70e-3f,
        4.375e-3f};//in degree per second / digit
    private static final float[] accelScale = {
         2f / ACCEL_SCALE,
         4f / ACCEL_SCALE,
         8f / ACCEL_SCALE ,
        16f / ACCEL_SCALE}; //in g / digit
    private boolean rawMode;

    public ImuLsm6ds3(Port port) {
        this(port, false);
    }

    public ImuLsm6ds3(Port port, boolean rawMode) {
        super(port);
        this.rawMode = rawMode;
        setModes(new SensorMode[]{
             new CombinedMode(), 
             new AccelerationMode(), 
             new GyroMode()});
    }

    public void reset() {
        byte[] buffer = new byte[] {DEVICE_RESET};
        port.write(buffer, 0, buffer.length);
    }

    @Override
    public boolean setGyroscopeScale(int scaleNo) {
        if (scaleNo >= 0 && scaleNo < 5) {
            ImuSensorMode mode = (ImuSensorMode) getMode(getCurrentMode());
            mode.setGyroScale(getGyroScale(scaleNo));
            return setScale(GYRO_SCALE_245DPS + scaleNo);
        }
        return false;
    }

    @Override
    public boolean setAccelerometerScale(int scaleNo) {
        if (scaleNo >=0 && scaleNo < 4) {
            ImuSensorMode mode = (ImuSensorMode)getMode(getCurrentMode());
            mode.setAccelScale(getAccelScale(scaleNo));
            return setScale(ACC_SCALE_2G + scaleNo);
        }
        return false;
    }

    @Override
    public boolean setMagnetometerScale(int scaleNo) {
        return false;
    }

    @Override
    public boolean writeAccelerometerEeprom(int scaleNo, short[] data) {
        return writeEeprom(CALIBRATE_ACC_2G + scaleNo, data);
    }

    @Override
    public boolean writeGyroscopeEeprom(int scaleNo, short[] data) {
        return writeEeprom(CALIBRATE_GYRO_245DPS + scaleNo, data);
    }

    /**
     * Update magnetometer EEPROM
     *
     * @param scaleNo scale index
     * @param data    EEPROM data (4x3 matrix of 16-bit integers)
     * @return true if the EEPROM data has been successfully written
     */
    @Override
    public boolean writeMagnetomtereEeprom(int scaleNo, short[] data) {
        return false;
    }

    private boolean writeEeprom(int writeCommand, short[] data) {
        ByteBuffer command = ByteBuffer.allocate(data.length * 2 + 1);
        command.order(ByteOrder.LITTLE_ENDIAN);

        command.put((byte) writeCommand);
        for (short aData : data) {
            command.putShort(aData);
        }
        byte[] array = command.array();
        boolean success = port.write(array, 0, array.length) == array.length;
        if (success) {
            //Wait for writing the data to EEPROM
            Delay.msDelay(array.length * 3);
        }
        return success;
    }

    private boolean setScale(int scaleCommand) {
        byte[] buffer = new byte[] {(byte)scaleCommand};
        boolean success = port.write(buffer, 0, buffer.length) == buffer.length;
        if (success) {
            Delay.msDelay(SCALE_SWITCH_DELAY);
        }
        return success;
    }

    private float getAccelScale(int scaleNo) {
        if (rawMode)
            return 1;
        else
            return accelScale[scaleNo];
    }

    private float getGyroScale(int scaleNo) {
        if (rawMode)
            return 1;
        else
            return gyroScale[scaleNo];
    }

    public SampleProvider getCombinedMode() {
        return getMode(0);
    }

    public SampleProvider getAccelerationMode() {
        return getMode(1);
    }

    public SampleProvider getGyroMode() {
        return getMode(2);
    }


    private class CombinedMode extends BaseSensorMode {
        @Override
        public int sampleSize() {
            return 6;
        }

        @Override
        public String getName() {
            return "ALL";
        }

        @Override
        public int getMode() {
            return 0;
        }

        @Override
        public void setGyroScale(float scale) {
            for (int i = 3; i < sampleSize(); ++i) {
                this.scale[i] = scale;
            }
        }

        @Override
        public void setAccelScale(float scale) {
            for (int i = 0; i < 3; ++i) {
                this.scale[i] = scale;
            }
        }
    }

    private class AccelerationMode extends BaseSensorMode {
        @Override
        public int sampleSize() {
            return 3;
        }

        @Override
        public String getName() {
            return "Acceleration";
        }

        @Override
        public int getMode() {
            return 1;
        }

        @Override
        public void setAccelScale(float scale) {
            for (int i = 0; i < sampleSize(); ++i) {
                this.scale[i] = scale;
            }
        }
    }

    private class GyroMode extends BaseSensorMode {
        @Override
        public int sampleSize() {
            return 3;
        }

        @Override
        public String getName() {
            return "Rate";
        }

        @Override
        public int getMode() {
            return 2;
        }

        @Override
        public void setGyroScale(float scale) {
            for (int i = 0; i < sampleSize(); ++i) {
                this.scale[i] = scale;
            }
        }
    }

    abstract class BaseSensorMode implements ImuSensorMode {
        protected float[] scale;
        private short[] buffer;

        public BaseSensorMode() {
            this.scale = new float[sampleSize()];
            this.buffer = new short[sampleSize()];
            setAccelScale(getAccelScale(0));
            setGyroScale(getGyroScale(0));
        }

        /**
         * Fetches a sample from a sensor or filter.
         *
         * @param sample The array to store the sample in.
         * @param offset
         */
        @Override
        public void fetchSample(float[] sample, int offset) {
            switchMode(getMode(), SWITCHDELAY);
            port.getShorts(buffer, 0, sampleSize());
            for(int i=0;i<sampleSize();++i) {
                sample[offset + i] = buffer[i] * scale[i];
            }
        }

        public void setGyroScale(float scale) { }.
        public void setAccelScale(float scale) { }
    }

    interface ImuSensorMode extends SensorMode {
        int getMode();

        void setGyroScale(float scale);
        void setAccelScale(float scale);
    }


}







Calibration software



To calibrate accelerometer and gyroscope sensors I used algorithm described in this application note.



I have written a program to measure sensor raw data at 6 stationary positions and  then calculate transformation matrix. For gyroscope sensor calibration I used a vinyl disk player turntable to provide fixed and stable angular velocity. The player has minimal rotation speed about 200 dps, so I couldn't calibrate LSM6DS3 rotation scale 125 dps. But I found that matrix for this scale can be computed from the matrix for scale 250 dps with scaled offset values because other elements of the transformation matrix are the same for different scales (gyroscope only).



The calibration software is run on EV3 block. It measures a number of raw samples at 6 positions, then used least square method to calculate transformation matrix. After that it writes the transformation matrix to the sensor.



Calibration software can be found in Git repository. 







The calibration software for the magnetometer is not completed yet.






The source code





Git repository https://github.com/maxmorozov/ev3imu contains the hardware design in DipTrace format, source code for sensor firmware, driver for LeJOS EV3 and calibration software.




















Posted by



at







































No comments:















Post a Comment


















        

      









Subscribe to:
Post Comments (Atom)