LSM9DS1 and balancing math



  • ShawnaShawna Posts: 437
    edited 2019-04-22 - 22:18:22
    This is one way to figure the gyro angle for the LSM9DS1. Maybe not the best way, but it could be a place to start.
    while(1){ //Repeat forever loop.
    loop_start = CNT; //Preloads system counter value to keep track of loop time.

    gyroAngX = gyroAngX + ((gX * 8.75 * .01)/ 1000);

    waitcnt(loop_start + ((CLKFREQ / 100) - 144)); //Keeps track of while loop guaranteeing dt. 100hz

    gyroAngX = the current angle of rotation

    gX = most recent value read from the gyro.

    8.75 = millidegrees of rotation per second when LSM9DS1 is set for 245 degrees per second. Edited 4/22/18

    .01 = dT of the loop 100Hz or 1/100 = .01

    1000 = converts from millidegrees to degrees.

    So this is not all the code, but it is the core of the math for determining rotation with one axis of the gyro. I have 1 cog dedicated to reading the LSM9DS1, 1 cog for math and 1 cog for my main loop. To start out I would recommend doing it all in 1 cog untill you get the angles reading correctly. Slow the loop down to 50Hz or whatever it takes to get a constant loop time in 1 cog.
  • JwolfJwolf Posts: 71
    edited 2019-04-22 - 22:14:51
    ok can you guys help me work this one... I'm just not getting it, everything I have tried has failed and at the moment my 'cup us full'.
    LSM9DS1 Datasheet:
    Angular rate FS = ±500 dps
    Typical = 17.50
    Unit = mdps/LSB

    From the LSM9DS1 obj, I can get output in either RAW or DPS.
    The DPS output in that obj is calculated like this: ((GyroRaw * (DecimalPrecision * 10)) / (32768 / GyroSensitivity)) //DecimalPrecision=3 , GyroSensitivity=500

    In my program, I can find the amount of time passed per loop with this:
    Time1 := cnt 'Record end of cycle
          'Determine if timers are pos or neg and perform math (cnt value ranges from -2,147,483,648 to 2,147,483,647)
          if Time1 => 0 and Time2 => 0        'If start time and end time pos
             Time3 := Time1 - Time2
          elseif Time1 > 0 and Time2 < 0    'If start time pos and end time neg
             Time1 -= 2147483647
             Time2 := ((-2147483648 - Time2) * -1)
             Time3 := Time1 + Time2
          elseif Time1 =< 0 and Time2 =< 0    'If start and end time neg
             Time3 := ((Time2 - Time1) * -1)
        Time2 := cnt 'Record start of cycle
        Time4 := ff.FDiv(Time3, 80000000)  'Find how much time has passed             
        long[TimerG] := Time4     'Send to PST Display

    Typical results for Time4 = 0.001185
    But so far I cannot get a usable roll or pitch by dividing or multiplying Time4 with either the RAW or DPS gyro output
  • ShawnaShawna Posts: 437
    edited 2019-04-22 - 22:57:34
    Is your display code running in a separate cog? If it is not, it maybe screwing up your timing. Zip up your code and post it.

  • yes the display and balance routines each run in their own cogs
  • Here it is, main file = Balancing Control...
    At the bottom of 'PUB Balance'

  • JwolfJwolf Posts: 71
    edited 2019-04-23 - 05:43:25
    The accelerometer output range is +180deg to -180deg... I'm trying to get the gyro to match on its own so I can start working on the complimentary filter
  • JwolfJwolf Posts: 71
    edited 2019-04-23 - 18:17:19
    Here is the LSM9DS1 Datasheet
    Sensor rates are on pg.12
  • ShawnaShawna Posts: 437
    edited 2019-04-24 - 02:38:48
    I was able to look briefly at your code.

    I always try to keep things as simple as possible. To start, I would eliminate everything except what is needed to calculate and display the gyro angle for one axis . Once you have a working piece of code, it can be built upon.

    Its been years since I have written anything in spin, but I can read it ok. I am not sure if the imu obj you are using, calculates dt when it is figuring dps, so I would just try reading the raw data out and performing the math on it.

    I would fix the frequency or speed of the repeat loop you are using for your imu call and calculations. This may not be the best way to do it, but I think it simplifies the math. It will also help with PID loops when you get to them. If the PID loop frequency is constant, dT does not need to be figured into the PID equation.

    Are your raw gyro values close to zero while the imu is sitting still? I would display the raw gyro values in the terminal until you get the angles working.

    I would try something like this to start with for your Balance function. I didn't compile the code, but this is the gist of what I would try first.
    PUB Balance(ARoll, APitch, GRoll, Gpitch, TimerG) | loop_time 
    imu.Start (SCL_PIN, SDIO_PIN, CS_AG_PIN, CS_M_PIN, INT_AG_PIN, INT_M_PIN)  ' Launch LS9 Driver into new cog (launched through imu OBJ)
    waitcnt((clkfreq / 1) + cnt)       'Pause 1 Second allow IMU to startup
    imu.SetAccelScale(AccelSens)   ' Set Accel Scale sensitivity
    imu.SetGyroScale(GyroSens)     ' Set Gyro Scale sensitivity                          
      loop_time := cnt     
          'Gyro Calculations
          imu.readGyro (@gx, @gy, @gz)       
          gx -= gxa 'Subtract from calibration offset avgs
          'GRTrack = GRTrack + ((gx * 17.5 * .02) / 1000)   Equation needed.
          'ff.fmul(ff.ffloat(gx), 17.5)                     Organizing the equation.
          'ff.fmul( ff.fmul(ff.ffloat(gx), 17.5),0.02)      Organizing the equation.
          'ff.FDiv(ff.fmul( ff.fmul(ff.ffloat(gx), 17.5),0.02),1000.0)                Organizing the equation.
          GRTrack := ff.FAdd(GRTrack,ff.FDiv(ff.fmul( ff.fmul(ff.ffloat(gx), 17.5),0.02),1000.0))
          long[GRoll] := GRTrack
          waitcnt(loop_time+ (CLKFREQ / 50))                'Maintain desired loop time. 50 would be 20mS or 50Hz, 100 would be 10mS or 100Hz
          loop_time := cnt
    Code edited on 4-23-19

  • JwolfJwolf Posts: 71
    edited 2019-04-23 - 20:07:57
    ok this is very very close... (change to my code before i saw your post)
    long[GRoll] := GRTrack := ff.fadd(GRTrack,ff.fdiv((ff.fmul(ff.ffloat(gx), 0.001)), 17.5))
  • JwolfJwolf Posts: 71
    edited 2019-04-23 - 20:23:17
    with quick copy/paste.... results at rest are:
    I dont believe raw output is in milli g's, but could be wrong
    And just to clarify, i made sure this is using raw output, not calculated
  • JwolfJwolf Posts: 71
    edited 2019-04-23 - 20:15:18
    by modifying your code from
    GRTrack := ff.FAdd(GRTrack,ff.FDiv(ff.fmul( ff.fmul(ff.ffloat(gx), 17.5),0.02),1000))
    GRTrack := ff.fadd(GRTrack, ff.fmul(ff.fdiv(ff.ffloat(gx), 17.5),0.02))

    I get an output that is about 3x what it should be... so instead of 90deg i get 270deg... and it drifts by about 0.1dps
  • Its going to drift, that is just one of the properties of the gyro. And that is also why the accelerometer data is fused with the gyro data. I will try and fire up my board with some spin and see if I can get an example going. Probably have to wait until this weekend though, too much stuff going on.

  • JwolfJwolf Posts: 71
    edited 2019-04-23 - 20:42:33
    thanks i appreciate it... this has been way way more difficult than expected
    I dont understand why this isnt something that has not already been solved... anyone wanting to implement an imu with their propeller would have to go through the same, ya?
    seems to me this is work on something that should already be an OBEX
  • Its been done before, I have done it before, but I lost the code. The last time I did it, it was with a different imu chip. I think if you search hard enough you can find some info, but its hard to sift through.
  • I have a library that I use and it works well. Actually I have several IMU libraries that I use and they all work well.

    The problem is that they are all done in C and not SPIN. Porting code from other platforms are always in C code.

  • @iseries
    Yeah, I was pretty sure from posts in other threads you probably have drivers written. My goal is to use C but I also wanted to do one in spin to compare read speeds of the LSM9DS1. So I will try to help as much as I can.

    I know there is other forum members that have worked on this also. It can be hard sometimes to adapt other peoples code to your own too.

    Jason Dorie did a ton of work on this type of project, not really sure what ever happened to him.
  • Jwolf wrote: »
    by modifying your code from
    GRTrack := ff.FAdd(GRTrack,ff.FDiv(ff.fmul( ff.fmul(ff.ffloat(gx), 17.5),0.02),1000))
    GRTrack := ff.fadd(GRTrack, ff.fmul(ff.fdiv(ff.ffloat(gx), 17.5),0.02))

    I get an output that is about 3x what it should be... so instead of 90deg i get 270deg... and it drifts by about 0.1dps

    I think the problem may have been the 1000, since it's a floating point number, it needed to be 1000.0. The decimal place is important.

    Well...... like I said my Spin skills stink! Anyways, I was able to put together a demo for displaying the angle from 1 axis of the LSM9DS gyro. It is all done in 1 cog running at 50Hz. I am rusty and could not get the values to display from a second cog. For a demo I think it runs really well. I can't believe how fast the imu obj reads the chip. It screams.........
      _clkmode = xtal1 + pll16x
      _xinfreq = 5_000_000
     'LSM9DS1 Sensitivity Settings
      GyroSens    = 500   '500deg/sec max rotation limit
     'LSM9DS1 Pin Connections
      SCL_PIN     = 1
      SDIO_PIN    = 2
      CS_AG_PIN   = 3
      CS_M_PIN    = 4
      INT_AG_PIN  = 5
      INT_M_PIN   = 6
      ser       : "com.serial.terminal"       'PST
      imu       : "sensor.imu.tri.lsm9ds1"    'LSM9DS1 Gyro Driver
      fp        : "floatstring"               'Displays floating-point numbers in PST
      ff        : "fme"                       'Floating-point math
      long PitchAng, loop_time, gx, gy, gz, samples, gxa, gya, gza
    PUB Main
      ser.Start (115_200)        ' Start Serial Terminal
      imu.Start (SCL_PIN, SDIO_PIN, CS_AG_PIN, CS_M_PIN, INT_AG_PIN, INT_M_PIN)  ' Launch LS9 Driver into new cog (launched through imu OBJ)
      imu.SetGyroScale(GyroSens)     ' Set Gyro Scale sensitivity
      samples := 5000    'How many data samples to create avg from for offset         
      gxa:=gya:=gza:= 0  'Reset values
      'Accumulate Gyro data to create offset avg
      repeat samples
        imu.readGyro (@gx, @gy, @gz)
        gxa += gx
        gya += gy
        gza += gz
      gxa := gxa / samples  
      gya := gya / samples  
      gza := gza / samples
      loop_time := cnt
        imu.readGyro (@gx, @gy, @gz)
        gx := gx - gxa
        gy := gy - gya
        gz := gz - gza
         PitchAng := ff.FAdd(PitchAng,ff.fDiv(ff.fmul(ff.fmul(ff.ffloat(gx),17.5),0.02),1000.0))   '0.02 is the dT of the loop, must be changed if waitcnt is changed.
        ser.str (fp.floattostring(PitchAng))
        waitcnt(loop_time+ (CLKFREQ / 50))                'Maintain desired loop time. 50 would be 20mS or 50Hz, 100 would be 10mS or 100Hz
        loop_time := cnt

    The gyro offset sample total can be changed, I set it at 5000 samples which might be a little ridiculous, it takes about 5 or 6 seconds to finish.

    Attached is the zip file with all the files.

  • Sorry guy, its not even close
    Results all over the place with no repeatable results even with slow movement
  • JwolfJwolf Posts: 71
    edited 2019-04-25 - 17:08:38
    just to see what happened... I upped the accel from 8g to 16g, and the gyro from 500dps to 2000dps
    Accel output conversion is just fine at both sensitivity settings.
    so to adjust gyro conversion for that change, it should be as simple as changing 17.5 to 70.0 as illustrated in the datasheet.
    but the results are not panning out... theoretically i should get the exact same results as it was with 500dps / 17.5... but instead it throws way off
    when I divided the 2000dps output by 17.5, i get the same results as 500dps.
    I polled the registers to make sure sensitivity was correct for each setting

    I'm just not seeing any logical path to follow and now I'm not sure what approach to take next :(
  • JwolfJwolf Posts: 71
    edited 2019-04-25 - 16:55:48
    btw I also have on hand an MPU6050 and MPU9150... if you have working code I can implement to avoid this rediculousness
    is there a way to use C code as an obj?
  • ok so after sitn and thinking for a bit... I think that 17.5dps is being used wrong in the calc.
    seems to me raw output, from 0 to max, would have to be divided by 180 to find the range per degree... then the current value be divided by the RangePerDeg value, divided by 17.5, divided by time passed
    whadya think?
  • JwolfJwolf Posts: 71
    edited 2019-04-25 - 21:19:48
    now... what is the max output range lol
    is it as simple as scale setting * dps? 500 x 17.5 = 8750
    the imu scales it by 32768/500
  • So were you able to get the example I posted to work, or are you having problems merging it into your own code?
  • JwolfJwolf Posts: 71
    edited 2019-04-25 - 21:20:59
    ok that didnt work.

    now im thinking... the raw gyro output is from the last read to the current read, correct? *could someone verify this plz*
    however if the gyro runs samples at its own frequency, that is probably what im missing in the calc

    assuming raw gyro output is measured between read requests... any reading that comes out, should i figure out what that value would be if it were constant for 1sec, then divide by 17.5, then divide by actual time passed, then divide that by (32768/180)?

  • JwolfJwolf Posts: 71
    edited 2019-04-25 - 21:48:11
    the example you posted did not work.
    at 90deg rotation, the output shows roughly 4.3
    when i return it to rest (should be 0deg)... the output shows 6.9
  • This is why I don't like this sensor. At level 0 the unit will measure +/- 245 degree per second or 8.75 millidegrees per second. That would be .00875 degrees per second. So 245/.00875 = 28000 would be full scale value not 32767.

    So each degree is about 114 units.

  • ShawnaShawna Posts: 437
    edited 2019-04-25 - 22:41:21
    Hmmm, I am not sure what to tell you, it worked just fine on my activity bot with the parallax LSM9DS1 breakout board.

    I will have to think about it.

    This is the proper equation for figuring pitch when the gyro is set for 500dps and the read loop is 50hz.

    Pitch = Pitch + ((gx * 17.5 * .02) / 1000)

    17.5 is the multiplier given by the chip manufacture for 500 degrees per second of rotation. When multiplied by the raw value from the gyro it tells you how fast the gyro was rotating when the sample was read. If the gyro was read only once a second and the rotation was constant, in theory this would be the only multiplier needed in the equation. Which would be in millidegrees per second.
    And this would be your equation

    MilliDegreesRotated = ((gx * 17.5) 'output is in mdps

    Since rotation is never at a constant speed, we need to read the gyro faster than once a second. The faster we read the gyro, the more accurate our calculated angle will be, because we really don't know how fast it is rotating between readings. Its an assumption that the gyro is rotating at the same rate between readings, but it probably isn't. So now that we are reading the gyro faster than once a second we need to take this into account.

    1 sample or reading a second and the equation would look like this.

    MilliDegreesRotated = ((gx * 17.5 * 1) 'output is still in mdps

    50 samples a second or 50Hz and the equation would look like this.

    MilliDegreesRotated = ((gx * 17.5 * .02) 'output is still in mdps

    Dividing by 1000 changes the value from mdps to dps. If you wanted your pitch to be in mdps you could leave the divide by 1000 off.
    If you want it in degrees, you end up with this equation.

    DegreesRotated = ((gx * 17.5 * .02) / 1000) 'output is now in dps.

    Someone correct me if I am wrong but this is how I look at it.


  • JwolfJwolf Posts: 71
    edited 2019-04-26 - 00:28:07
    Im burnt out for today, spent several frustrating hours on this with no progress.

    if you'd like i can post a screenshot... running your code with no changes, never has a value above 10, and it always has a different result for any given angle
    are you using a provided obex to poll data from the LSM9 imu? or are you doing that directly with your own code?
    as you may have already noticed, i'm working with someone else's code provided by obex. unfortunately I cannot verify if the problem happening here, may be an error with the gyro handling.

    so the gyro output is more thought of as 'pressure'?
    measuring at any given point does not show a change over time, but rather the amount of force it is experiencing at that moment?
    is this correct?
  • JwolfJwolf Posts: 71
    edited 2019-04-26 - 00:59:17
    This is why I starting using 32768 to experiment with.... directly from the LSM9DS1 obex
    PUB setGyroScale(gScl) | ctrl1RegValue
    ' Sets the full-scale range of the Gyroscope.
      if ((gScl <> 245) and (gScl <> 500) and (gScl <> 2000))
        gScl := 500
      __settings_gyro_scale := gScl
      __gRes := 32768/gScl
      ' Read current value of CTRL_REG1_G:, ctrl1RegValue
      ReadAGReg (CTRL_REG1_G, @ctrl1RegValue, 1)
      ' Mask out scale bits (3 & 4):
      ctrl1RegValue &= $E7
        500 :
          ctrl1RegValue |= ($1 << 3)
        2000 :
          ctrl1RegValue |= ($3 << 3)
        OTHER :
      WriteAGReg8 (CTRL_REG1_G, ctrl1RegValue)
  • I was using the objects you provided in your zipped project.
Sign In or Register to comment.