Shop OBEX P1 Docs P2 Docs Learn Events
LSM9DS1 and balancing math - Page 4 — Parallax Forums

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: 508
    edited 2019-04-26 01:55
    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: 74
    edited 2019-04-26 02:45
    and you were able to get a stable, repeatable good result from the gyro with that code?...drift wasnt running away excessively?
  • ShawnaShawna Posts: 508
    edited 2019-04-26 03:06
    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: 74
    edited 2019-04-28 01:31
    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: 74
    edited 2019-04-28 04:55
    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: 74
    edited 2019-04-28 20:03
    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: 74
    edited 2019-04-29 01:23
    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: 74
    edited 2019-04-29 00:52
    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: 74
    edited 2019-04-29 01:12
    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: 508
    edited 2019-04-29 15:42
    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: 74
    edited 2019-04-29 23:09
    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: 74
    edited 2019-04-29 23:10
    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: 508
    edited 2019-04-29 23:14
    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: 74
    edited 2019-05-01 22:37
    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: 74
    edited 2019-05-07 02:09
    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

  • Trying to fuse the accel & gyro output is turning out to be a nightmare. It's like trying to learn the alphabet but it takes 4 months to figure out the letter E.
    I have found implemented kalman filters for balancing bots, but they are hardcoded for specific chips.
    I have gone through kalman tutorials, which seemed promising, but it demonstrates too many theoretical situations and has confused me to an extreme level of frustration.

    Is there no possibility of fusing gyro & accel data without having a completely functional knowledge of a jacobian matrix and linear algebra based on covariance noise and bias with an adjusted gain from an estimated state updated from a prediction formula based on multiple observed states.............
    My point is, it seems absurd that there is no usable formula here based on commonly used variables that would be applicable to pretty much all gyro+accel sensors.
    could someone PLEASE help given these variables

    dt = Loop/Sensor Read Time
    ZgyroX = Gyro x-axis value in degrees (already converted from dt)
    ZgyroY = Gyro y-axis value in degrees (already converted from dt)
    ZgyroZ = Gyro z-axis value in degrees (already converted from dt)
    ZaccelX = Accel x-axis value in degrees (already converted from dt & atan2)
    ZaccelY = Accel y-axis value in degrees (already converted from dt & atan2)
    ZaccelZ = Accel z-axis value in degrees (already converted from dt & atan2)
    VaccelNoise = Taken from average of multiple accel readings
    VgyroNoise = Taken from average of multiple gyro readings


    Or maybe someone who could explain how to choose and apply the formulas... im at my wits end right now.
    Reading with no feedback about this is as helpful as learning to drive a car from a Chiltons manual
  • I feel your pain. I have gone done this road some time ago and have learned a lot about the process.

    I have no less than 3 different balancing robots using varies methods to balance them by.

    Still I have not put together a unit on my own. I am so burnt out on this I have not gone back to it yet.

    You are trying to do this in SPIN which I think is impossible do to with the lack of speed with this code.

    At least I am using C code and I am able to see the code that these other robots used and can see how the math was used.

    One of my problems was I wanted to build the code using integer math only.

    My current thought process is to use an EM7180 sensor hub that reads the three IMU's and does all the math for you. All you need to do is read the end results and balance the robot. So far I have not gotten around to mounting the unit on a robot.

    I have build the library to read this sensor and it looks promising.

    here is a link to the guy that put the unit together and he also explains the math and the two different units that are available.
    EM7180 Sensor hub

    Mike

  • Hi Jwolf and iseries,

    Instead of using an extended Kalman filter, there is an easier way to estimate angles by fusing gyro and accel measurements - the complementary filter. Copied from my first post in this thread:

    For balancing applications, the complementary filter works fine. It can be implemented in code by:
    accelAngle = atan2(ax,az); 
    theta = 0.98*(theta+gy*Ts)+.02*accelAngle; 
    
    where accelAngle is the angle computed from the accelerometer, ax is the acceleration value in g's in the x direction, az is the acceleration value in g's in the z direction, theta is the pitch angle you are estimating, gy is the gyroscope output about the y axis, and Ts is the loop sample time. You may have to map your sensor outputs according to a north, east, down coordinate system where north is in the x direction and points out of the front of your vehicle, east is in the y direction and points out the right wing of your vehicle, and down is in the z direction and points out of the bottom of your vehicle towards earth. This way, if you are estimating theta (pitch), the gy measurement is about the the y axis. Once you are able to estimate theta, you can easily implement an estimation of phi (roll).

    This is in C and I understand you are using spin, so I'm not sure what the differences will be. Please try this and let me know what you get. I have used this code on my balancing bot and it works well.

    David
  • Yes, I have seen this code used before. However I have not been able to make it work in practice.

    I believe the problem I have is getting the loop timing to work out so that I can generate the correct angle. Also being able to come up with a PID loop that properly controls the wheels. How do you get from an angler measurement to wheel force in the correct direction.

    I always end up with the unit dancing from one extreme to the other. Balances just fine and then ciaos. Some of it caused by the motors and the other by the accelerometer.

    Mike
  • Mike,

    Can you post your code? I can take a look. I'll also post mine as well and we can compare.

    Thanks,
    David
  • ShawnaShawna Posts: 508
    edited 2019-05-26 13:46
    @Jwolf
    The complementary filter should work fine for a balancing bot. Have you tried implementing a comp filter? If so what type of problems did you have with it? From what I have read, the results obtained from a comp filter vs a kalman filter are usually pretty similar, however the kalman filter is harder to understand and requires more resources from the micro. A long time ago I was going to try and use a kalman filter for a quadcopter, but members here on the forum quickly persuaded me to use a comp filter. I don't understand the Kalman filter at all.




    iseries wrote: »
    Also being able to come up with a PID loop that properly controls the wheels.

    I'm at this step currently..........actually I'm at the step of building the bot itself. Then I can try messing around with the PID loop for controlling the wheels. I'm going to try and use stepper motors for the balancing bot. At first I was going to try and use the high speed 360 degree servos, but I didn't like the amount of backlash in them.



  • I am using NEMA 17 stepper motors controlled by A4988 steppers.

    I built a test roadsteer to see how well the motor library works on the ground before trying to use them to balance something.

    Roadsteer Project

    So far the library works well but have some issues with wheel lock if I try to accelerate the motors to quickly. Need to determine how fast I can change the step rate given a speed. Also may need to calibrate the A4988 stepper current.

    Mike
Sign In or Register to comment.