LSM9DS1 and balancing math

124»

Comments

  • iseries wrote: »
    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.

    Mike

    I noticed that also, so is the data sheet wrong?



  • ShawnaShawna Posts: 437
    edited 2019-04-26 - 01:55:45
    if ((gScl <> 245) and (gScl <> 500) and (gScl <> 2000))
        gScl := 500
      __settings_gyro_scale := gScl
      __gRes := 32768/gScl
    

    That looks wrong to me, the code basically says gRes = 32768 / 500 = 65.536dps

    I actually think it should be gRes = 500/32768 = .0152dps or 15.2mdps.
    Which is close to what the data sheet says.

    So this kind of plays into what Mike is saying, the data sheet says at 500dps it should be .0175dps or 17.5mdps
    Which is right 17.5mdps or 15.2mdps? I assume the data sheet is right.
  • JwolfJwolf Posts: 71
    edited 2019-04-26 - 02:45:46
    and you were able to get a stable, repeatable good result from the gyro with that code?...drift wasnt running away excessively?
  • ShawnaShawna Posts: 437
    edited 2019-04-26 - 03:06:09
    Jwolf wrote: »
    and you were able to get a stable, repeatable good result from the gyro with that code?...drift wasnt running away excessively?
    Yes, it works good!

    I'm not sure what is wrong!

    Download this file and try it again, don't change anything, the only thing I changed from the last project is file names, nothing else.

    edit: I changed the loop frequency also.

  • The key here is to use Table 3 values. When they say full scale that doesn't mean that the value is +/- 32767 but that the IMU will reach it's max stated value and then clipping will occur.

    You have to use the typical value column to determine the meaning of the numbers. I always converted the number to degree per second and divided instead of multiplying. Now that I see what you did, keeping in millidegrees makes sense.

    Mike

  • The equation can be simplified, I just think its easier when you display all the variables and explain what they do. Not sure if I did a good job, but I tried.

    DegreesRotated = ((gx * 17.5 * .02) / 1000)

    To simplify.......
    First, convert mdps to dps.

    17.5mdps / 1000 = .0175dps

    So now you have an equation like this.

    DegreesRotated = gx * .0175 * .02

    .0175 is degrees per second of rotation, but we don't want degrees per second, we want degrees per dT. We know that the loop samples at 50Hz.

    1/50 = .02mS = dT

    .0175 * .02 = .00035 = DegreesRotationPerDT

    So now the equation is simplified.

    DegreesRotated = gx * .00035 which is the same as DegreesRotated = ((gx * 17.5 * .02) / 1000)

    Now to figure the Pitch Angle.

    PitchAng = PitchAng + (gx * .00035)

    This is the same equation Joop came up with in his video.







  • JwolfJwolf Posts: 71
    edited 2019-04-28 - 01:31:50
    its reacting much better... however at +/-90deg roll or pitch, its off by roughly +/-45deg
    So if I roll/pitch quick or slow, and both the accel and visual result is 90deg.... the gyro shows about 130deg
  • JwolfJwolf Posts: 71
    edited 2019-04-28 - 04:55:16
    Since I'm looking for a result on a circle, maybe the result im getting is linear and need to convert using Pi?

    I tried this and it's producing a very close result... almost no drift (less than 0.1dps)... if i rotate to 90deg according to accel output, the gyro is +10deg... but when i return to 0deg, the gyro matches
    Time4 = LoopTime
    GPTrack := ff.fadd(GPTrack, ff.fmul(ff.ffloat(gx), (ff.fmul(ff.fmul(0.0175, pi), Time4))))
    

  • Were you able to get my example to work? If so, what did you have to do to get it working, since it wasn't working for you before?

    Jwolf wrote: »
    Since I'm looking for a result on a circle, maybe the result im getting is linear and need to convert using Pi?

    Pi should not be needed when using just the gyro! The units are already in degrees.

    When calculating the angle with the ACC you have to use PI because the ATAN2 function outputs in radians. In order to fuse the gyro and acc data together with a complementary filter, the units of the 2 need to be the same. I suppose you could convert the gyro data to radians instead, but degrees are easier to visualize, so most people convert the acc to degrees.

    Degrees = radians * (180/PI)

    1rad * (180/PI) = 57.296 degrees


    Jwolf wrote: »
    I tried this and it's producing a very close result... almost no drift (less than 0.1dps)... if i rotate to 90deg according to accel output, the gyro is +10deg... but when i return to 0deg, the gyro matches
    Time4 = LoopTime
    GPTrack := ff.fadd(GPTrack, ff.fmul(ff.ffloat(gx), (ff.fmul(ff.fmul(0.0175, pi), Time4))))
    

    If you were able to get my example to work and you have moved back to your own code, I would suspect that the problem is with how you are timing the loop.

    To time a loop you would want to do something similar to this.
    PUB public_method_name
    
    StartCNT = cnt                  'Preload system counter into StartCNT. 
    repeat
    
      'read gyro
      'calculate angle
      'do whatever else you need to do
    
      EndCNT =cnt                   'Load system counter into EndCNT.
    
      TotalCount := EndCNT - StartCNT          'TotalCount would equal how many system clock cycles have passed.
    
      StartCNT = cnt                 'Load system counter int StartCNT so loop can start over.
    
    

    I am not the best at timing loops, maybe Mike or someone else can chime in and give a better example.

    We would have to see what your repeat loop code looks like.


    If you were unable to get my example to work, I would investigate that first. That example was about as basic as it gets. If its not working I suspect you have a hardware or object problem.



  • JwolfJwolf Posts: 71
    edited 2019-04-28 - 20:03:19
    your code drifts by about .2dps, and at 90deg, its off by roughly 35-40deg.... so instead of showing 90deg, it shows around 125deg.
    and to clarify, thats using your timed waitcnt loop at .02 with the formula "DegreesRotated = gx * .00035 which is the same as DegreesRotated = ((gx * 17.5 * .02) / 1000)"

  • JwolfJwolf Posts: 71
    edited 2019-04-29 - 01:23:45
    for better timing... wouldnt you want this:
    PUB public_method_name
    
    StartCNT = cnt                  'Preload system counter into StartCNT. 
    repeat
      'read gyro
      'calculate angle
      'do whatever else you need to do
    
      EndCNT =cnt                   'Load system counter into EndCNT.
      TotalCount := EndCNT - StartCNT          'TotalCount would equal how many system clock cycles have passed.
      StartCNT = cnt                 'Load system counter int StartCNT so loop can start over.
    
    To be this...
    PUB GetOffsets
    StartCNT=cnt
    'get offsets
    
    PUB public_method_name
    repeat
      TotalCount := cnt - StartCNT          'TotalCount would equal how many system clock cycles have passed.
      StartCNT = CNT
    
      'read gyro
      'calculate angle
      'do whatever else you need to do       
    
    This would avoid losing the time between loops while calculating 'TotalCount'
  • JwolfJwolf Posts: 71
    edited 2019-04-29 - 00:52:26
    or... should i be marking the time directly before/after making the GyroData call?
    That way I can determine the exact time between readings? as oppossed to the time between making a reading and performing calculations (which some may take longer than others).

    For example in the loop timed to 0.02s... maybe the GyroData call should be immediately after the waitcnt.... otherwise maybe the calculations finished, it makes a new gyro call, but then has to wait until 0.02 has fully passed
    I believe that in order to get an accurate reading based off of CNT... there should only be one "Variable=CNT" per loop
  • JwolfJwolf Posts: 71
    edited 2019-04-29 - 01:12:06
    ok that didnt fix it, still off by +35deg when rotated 90deg

    are you setting your prop speed at 80mhz as well?
    _clkmode = xtal1 + pll16x
    _xinfreq = 5_000_000
  • ShawnaShawna Posts: 437
    edited 2019-04-29 - 15:42:04
    Jwolf wrote: »
    ok that didnt fix it, still off by +35deg when rotated 90deg

    I got out the protractor and added the accelerometer pitch angle to the demo code to see how accurately the gyro reading is being calculated. The results are pretty subjective, but when the accelerometer reads 45 degrees the gyro angle is about 43 degrees. When the accelerometer is at 90 degrees the gyro reads about 84 degrees. Trying to hold the protractor and the activity bot and read the screen and the protractor is comical at best, but it gives a close estimate. The accelerometer probably needs to be calibrated, I have read about this but I don't know how to do it. I believe the data needs to be normalize to see how close the reading is to 1g when stationary.

    I do not understand why your readings are that far off. +35deg is pretty bad.
    " wrote:
    are you setting your prop speed at 80mhz as well?
    _clkmode = xtal1 + pll16x
    _xinfreq = 5_000_000

    Yes these are my settings, and my activity bot has a 5MHz crystal installed.

    I was wondering about this also and thought maybe you didn't really have a 5MHz crystal installed on your board, but if that was the case, I don't think your terminal screen would be printing properly. It would be easy enough to check though.

    I am really at a loss, I have know idea why the example I posted is not working for you. You really got me questioning the code, however, it is what I am trying with my balancing bot at the moment. I am pretty confident in the equation, I don't really see any other way to calculate it. If anyone else would be willing to download the zip file and load it into their prop, I would be curious and grateful to hear your results.

    Jwolf, I am not sure why you object too using a fixed frequency loop for your gyro read and calculations, which is fine, I just don't know how to help you with the other. Understandably it might be in part due to the fact that you couldn't get the demo example to work. It should have worked and I think you will continue to have problems until you figure out why it didn't work. Both loops work on timing, one loop is fixed and the other is not. The loop that is fixed should be easier to make work in my opinion.

    One final thought, I think using a complementary filter for a balancing bot should work just fine, however this type of filter has limitations. If I recall correctly, I read somewhere that the complementary filters results become less and less useful when the tilt angle approaches the 65 degree range. With that in mind a person needs to remember that the balancing bot is not going to have to keep track of 360 degrees of rotation around any axis. At 90 degrees it should be closer than plus or minus 35 degrees, but it doesn't have to be perfect, but it does need to be repeatable, which is where the ACC comes in.

    If you want 360 degrees of orientation tracking, you are going to have to switch to Euler Angles or Quaternions. I believe Quaternions are not subject to gyro lock, but Euler angles are.
  • at 45deg, confirmed by both protractor and accelerometer (they match near perfect)... the gyro value is 68.9deg.
    If I then set the imu down, let it rest for just a brief moment, then try again at 45deg... the gyro value is now 82.3deg
    and its different every single time
  • JwolfJwolf Posts: 71
    edited 2019-04-29 - 23:09:02
    omg i think found the problem lol
    was using gyro calculated not raw gyro... lost when transferred the formula to my code
    sry guys
  • JwolfJwolf Posts: 71
    edited 2019-04-29 - 23:10:09
    ok seems to be quite stable and within 1deg of accel readings now
    drift at roughly .05deg/sec

    alright now on to the complimentary filter lol
    from what i remember its generally just taking 90% of the accel output and 10% of the gyro... ill get into it later this evening

    I am unfamiliar with euler or quaternions... quaternions is what typical 4-engine drones use?
    I need to find a primer unless you can help explain :)
  • I am glad too hear you got it working!
  • ShawnaShawna Posts: 437
    edited 2019-04-29 - 23:14:16
    Jwolf wrote: »
    alright now on to the complimentary filter lol
    from what i remember its generally just taking 90% of the accel output and 10% of the gyro... ill get into it later this evening

    Its the other way around, 98% of the gyro and 2% of the accel.

    The complementary filter is basically a high pass filter for the gyro and a low pass filter for the accel.

    Best of luck!
  • JwolfJwolf Posts: 71
    edited 2019-05-01 - 22:37:46
    Any idea what in general, helicopters use? complimentary, quaternions... something else?
  • Most copters use Quaternions if they want to stay level on there own or can fly by themselves.

    Racing copters use only a gyro since they are only concerned about rotation and don't try to level themselves. A pilot just wants to tell it what angle to fly at and stay there.

    By unwrapping the Quaternion they can determine Pitch, Roll, and Yaw of the object that is then used to level or project an angle needed to fly the object in a direction. This is where Euler angles come in.

    Mike
  • JwolfJwolf Posts: 71
    edited 2019-05-07 - 02:09:41
    Here are the completed routines for those who follow.
    Displays Gyro + Accell output on PST.
    Demonstrates: Preset loop time (50hz)... and Dynamic loop timing which automatically adjusts to changes in code, runs as fast as possible.
  • Hi guys,

    I just discovered this thread, searching for some clues to convert the accelerometer data to degrees (which I found in this thread - the arctangent routines mentioned in FME were what sealed the deal, thanks!) for a project I'm working on.
    When I originally wrote the driver, it was just a direct port of the C++ SimpleIDE library/test program - pretty much my first real foray into spin, of any significance. I've since tried to clean up the code some more (the testing branch on github has my latest changes) and rewrite as many methods as possible in a more HLL way, instead of the many discrete 'WriteReg'-style statements. I wouldn't suggest relying on it for important projects, as I don't consider the API stable, and some of it isn't thoroughly tested. I'd appreciate reports of any issues you have, for instance, the fix for the Gyro scale @Shawna mentioned.

    Cheers,
    Jesse

    --
    WIP Spin drivers for various devices: LSM9DS1 IMU (SPI) | Newhaven 4x20 OLED (I2C) | MLX90621 (I2C) | SHT3x (I2C) | SSD1306 OLED (I2C; P1-SPIN, P2-SPIN2) | TCS3x7x (I2C) | MAX31856 (SPI) | BMP280 (I2C) | TMC2130 (SPI) | nRF24L01+ (SPI) | MLX90614 (I2C) | MAX9744 (I2C) | DS28CM00 (I2C) | TSL2591 (I2C) | CC1101 (SPI) | SX1231 (SPI)
Sign In or Register to comment.