Altimeter using a Accelerometer and Barometer
Shawna
Posts: 508
I have been working on my quad again, and I am making pretty good progress. I have hit a dead end with the tilt compensated magnetometer, so I have decided to have a try at an altimeter. I have been messing with the altimeter for about a week now and have hit a dead end here also. I was wondering if any of you guys have done this? I have code that I have been trying, but it is a mess with all of my trial and error, I am going to give it another try and clean it up a little. Once I get the code reworked I will post it here whether it is a success or failure.
I am messing with the acc portion of the code now, and I am getting nowhere. I have the acc running and the values filtered but I am having problems getting a meaningful velocity.
This is the site I am using for equations for calculating velocity.
http://www.aerospaceweb.org/question/dynamics/q0203.shtml
I know I do not have much info here yet, but I thought I would get a feeler out there and see what you guys think about this project.
Thanks
Shawn
I am messing with the acc portion of the code now, and I am getting nowhere. I have the acc running and the values filtered but I am having problems getting a meaningful velocity.
This is the site I am using for equations for calculating velocity.
http://www.aerospaceweb.org/question/dynamics/q0203.shtml
I know I do not have much info here yet, but I thought I would get a feeler out there and see what you guys think about this project.
Thanks
Shawn
Comments
CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 dTsqr = 0.0001 dT = 0.01 ACCLSB = 0.004 AccelerrationIncSec = 386.2206 Half = 0.5 Zero = 0.0 Two = 2.0 '------------------------------------------------------------------------------ VAR Long aX,aY,aZ,aXRaw,aYRaw,aZRaw Long LoopTime,Status,Cog,Stack[30],DbgVar[50] Long Velocity,PrevVelocity,AccelerationZ,PrevAccelerationZ,Altitude Long Displaying,DisplayEnb Long Counter '------------------------------------------------------------------------------ OBJ IMU : "9DOF" 'By Jason Dorie. 1 Cog. PST : "Parallax Serial Terminal" 'By Jeff Martin, Andy Lindsay, Chip Gracey. 1 Cog F32 : "F32" 'By Jonathan "lonesock" Dummer. 1 Cog FS : "FloatString" '------------------------------------------------------------------------------ PUB Main waitcnt(clkfreq * 2 + cnt) 'Pause for 2 Seconds. Initialize_Cogs Initialize_Variables Populate_Filter Main_Loop '------------------------------------------------------------------------------ PUB Initialize_Cogs F32.start IMU.Start(4,5) PST.Start(115_200) Status := Cog := cognew(Debugger_Display, @Stack[0]) '------------------------------------------------------------------------------ PUB Initialize_Variables Displaying := 0 'Used for displaying variables in PST. DisplayEnb := 1 'Used for displaying variables in PST. Velocity := 0 'Zeros Velocity Variable. Velocity := F32.FFloat(Velocity) 'Converts Velocity Integer into a FloatingPoint Variable. Altitude := 0 'Zeros Altitude Variable. Altitude := F32.FFloat(Altitude) 'Converts Altitude Integer into a FloatingPoint Variable. aX := 0 aY := 0 aZ := 0 aZraw := 0 '------------------------------------------------------------------------------ PUB Populate_Filter Counter := 0 LoopTime := cnt Repeat while Counter < 600 Get_IMU 'Takes 165.6uS Filter_IMU 'Takes 373uS Counter += 1 waitcnt(constant(80_000_000 / 100) + LoopTime) LoopTime += constant(80_000_000 / 100) '------------------------------------------------------------------------------ PUB Main_Loop LoopTime := cnt Repeat Get_IMU 'Takes 165.6uS Filter_IMU 'Takes 373uS Calc_ACC_Velocity Display_Var_Loader waitcnt(constant(80_000_000 / 100) + LoopTime) LoopTime += constant(80_000_000 / 100) '------------------------------------------------------------------------------ PUB Get_IMU 'Fetches Raw ACC Data. aXraw := IMU.GetAX - 20 'Gets ACC X axis raw data and adds an offset. aYraw := IMU.GetAY + 3 'Gets ACC Y axis raw data and adds an offset. aZraw := IMU.GetAZ + 22 'Gets ACC Z axis raw data and adds an offset. '------------------------------------------------------------------------------ PUB Filter_IMU 'Filters Accelerometer Data. aX := ((aX * 500) + (aXRaw * 500))/1000 aY := ((aY * 500) + (aYRaw * 500))/1000 aZ := ((aZ * 500) + (aZRaw * 500))/1000 '------------------------------------------------------------------------------ PUB Calc_ACC_Velocity 'Sets a Deadband around 250 which is 0 g's If (aZ > 246) and (aZ < 254) AccelerationZ := 250 Velocity := F32.FMul(Velocity,Zero) 'Sets Velocity to Zero PrevVelocity := F32.FMul(PrevVelocity,Zero) 'Sets Previous Velocity to Zero. Else AccelerationZ := aZ AccelerationZ := AccelerationZ - 250 'Subtracts Gravity from the Accelerometer's Filtered Reading. AccelerationZ := F32.FMul(F32.FFloat(AccelerationZ),ACCLSB) 'Converts the Accelerometers Value, to G's. G's = accZaxis * .004mG AccelerationZ := F32.FMul(AccelerationZ,AccelerrationIncSec) 'Takes G's * 386.2206 which is Inches/sec at 1 G. AccelerationZ := F32.FMul(AccelerationZ,dT) 'Multiplies the Acceleration by the calculation interval(Loop Time) Velocity := F32.FDiv(F32.FAdd(AccelerationZ,PrevAccelerationZ),Two) PrevAccelerationZ := AccelerationZ Altitude := F32.FAdd(F32.FDiv(F32.FAdd(Velocity,PrevVelocity),Two),Altitude) PrevVelocity := Velocity '------------------------------------------------------------------------------ PUB Display_Var_Loader If Displaying == 0 DisplayEnb := 0 DbgVar[00] := aX DbgVar[01] := aY DbgVar[02] := aZ DbgVar[03] := aXRaw DbgVar[04] := aYRaw DbgVar[05] := aZRaw DbgVar[06] := Velocity DbgVar[07] := Altitude DisplayEnb := 1 '------------------------------------------------------------------------------ PUB Debugger_Display | x, idx Repeat If DisplayEnb == 1 Displaying := 1 PST.Clear PST.Str(String("aX = ")) PST.Dec(00) PST.NewLine PST.Str(String("aY = ")) PST.Dec(01) PST.NewLine PST.Str(String("aZ = ")) PST.Dec(DbgVar[02]) PST.NewLine PST.Str(String("aZraw = ")) PST.Dec(DbgVar[05]) PST.NewLine PST.Str(String("Vel = ")) PST.Str(fs.FloatToString(DbgVar[06])) PST.NewLine PST.Str(String("Alt = ")) PST.Str(fs.FloatToString(DbgVar[7])) PST.NewLine PST.Str(String("AltAz = ")) PST.Str(fs.FloatToString(DbgVar[11])) PST.NewLine Displaying := 0 waitcnt(constant(80_000_000 / 5) + LoopTime) '------------------------------------------------------------------------------2. in PUB Calc_ACC_Velocity when ACC = 0 g's you should not reset the accumulated velocity to 0 - it just stays at it is.
do you need to multiply with 0 to set a variable to 0 ?? - I haven't used F32 before ...
3. and I recommend metric units, since there are no strange factors to take care of in calculations.
if you need it for display, you can do the scaling in the end.
Your right floating point is not really needed. I tend to use floating point a lot when I am trying to get math equations working, and then I switch them over to integers.
I new for sure taking the float X 0 it would reset my floating point variable to 0.
I set velocity to zero when acc = 0 because at zero the chip is not moving, but the formula is still accumulating movement, because the velocity is not 0. This maybe one of the points I am missing.
I need to revisit some definitions, this doesnt seem like it should be real complicated.
x(next)=x(initial)+v(now)*t+(1/2)*A(now)*t^2
http://forums.parallax.com/showthread.php/67104-using-an-accelerometer-to-measure-distance
This is the paper I am studying now.
http://cache.freescale.com/files/sensors/doc/app_note/AN3397.pdf?fsrch=1&sr=2
What do you guys think, am I on the right track?
This is just the main loop below.
'Main Loop LoopTime := cnt Repeat Get_IMU 'Sets a Deadband around 0 which is 0 g's If (aY > -3) and (aY < 3) AccelerationY1 := 0 Else AccelerationY1 := aY 'First Y integration: 'VelocityY[1]= velocityY[0]+ accelerationY[0]+ ((accelerationY[1] -accelerationY[0])>>1) VelocityY1 := VelocityY0 + AccelerationY0 + ((AccelerationY1 - AccelerationY0) / 2) 'Second Y integration: 'PositionY[1]= positionY[0] + velocityY[0] + ((velocityY[1] - velocityY[0])>>1) PositionY1 := PositionY0 + VelocityY0 + ((VelocityY1 - VelocityY0) / 2) AccelerationY0 := AccelerationY1 PositionY0 := PositionY1 VelocityY0 := VelocityY1 'Count the number of accelerations in a row that equal zero. If (AccelerationY1 == 0) CountY += 1 Else CountY := 0 'If the number exceeds 5 then assume velocity is zero. If CountY => 1 VelocityY1 := 0 VelocityY0 := 0 Display_Var_Loader waitcnt(constant(80_000_000 / 100) + LoopTime) LoopTime += constant(80_000_000 / 100) '------------------------------------------------------------------------------Here is the whole program if any of your are even more interested.
CON _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 VAR Long aX,aY,aZ,aXRaw,aYRaw,aZRaw Long LoopTime,Status,Cog,Stack[30],DbgVar[50] Long Displaying,DisplayEnb Long Counter Long AccelerationY0,AccelerationY1,VelocityY0,VelocityY1,Temp,PositionY0,PositionY1,CountY Long DbAccelerationY0,DbAccelerationY1,DbVelocityY0,DbVelocityY1,DbTemp,DbPositionY0,DbPositionY1,DbCountY '------------------------------------------------------------------------------ OBJ IMU : "9DOF" 'By Jason Dorie. 1 Cog. PST : "Parallax Serial Terminal" 'By Jeff Martin, Andy Lindsay, Chip Gracey. 1 Cog PUB Main waitcnt(clkfreq * 2 + cnt) 'Pause for 2 Seconds Initialize_Cogs 'Populates the Complimentary Filter. Counter := 0 LoopTime := cnt Repeat while Counter < 600 Get_IMU Counter += 1 waitcnt(constant(80_000_000 / 100) + LoopTime) LoopTime += constant(80_000_000 / 100) 'Main Loop LoopTime := cnt Repeat Get_IMU 'Sets a Deadband around 0 which is 0 g's If (aY > -3) and (aY < 3) AccelerationY1 := 0 Else AccelerationY1 := aY 'First Y integration: 'VelocityY[1]= velocityY[0]+ accelerationY[0]+ ((accelerationY[1] -accelerationY[0])>>1) VelocityY1 := VelocityY0 + AccelerationY0 + ((AccelerationY1 - AccelerationY0) / 2) 'Second Y integration: 'PositionY[1]= positionY[0] + velocityY[0] + ((velocityY[1] - velocityY[0])>>1) PositionY1 := PositionY0 + VelocityY0 + ((VelocityY1 - VelocityY0) / 2) AccelerationY0 := AccelerationY1 PositionY0 := PositionY1 VelocityY0 := VelocityY1 'Count the number of accelerations in a row that equal zero. If (AccelerationY1 == 0) CountY += 1 Else CountY := 0 'If the number exceeds 5 then assume velocity is zero. If CountY => 1 VelocityY1 := 0 VelocityY0 := 0 Display_Var_Loader waitcnt(constant(80_000_000 / 100) + LoopTime) LoopTime += constant(80_000_000 / 100) '------------------------------------------------------------------------------ PUB Initialize_Cogs IMU.Start(4,5) PST.Start(115_200) Status := Cog := cognew(Debugger_Display, @Stack[0]) '------------------------------------------------------------------------------ PUB Get_IMU 'Fetches Raw ACC Data. aXraw := IMU.GetAX - 20 'Gets ACC X axis raw data and adds an offset. aYraw := IMU.GetAY + 3 'Gets ACC Y axis raw data and adds an offset. aZraw := IMU.GetAZ + 22 'Gets ACC Z axis raw data and adds an offset. 'Filters Accelerometer Data. aX := ((aX * 500) + (aXRaw * 500))/1000 aY := ((aY * 200) + (aYRaw * 800))/1000 aZ := ((aZ * 500) + (aZRaw * 500))/1000 '------------------------------------------------------------------------------ PUB Display_Var_Loader If Displaying == 0 DisplayEnb := 0 DbgVar[00] := aX DbgVar[01] := aY DbgVar[02] := aZ DbAccelerationY0 := AccelerationY0 DbAccelerationY1 := AccelerationY1 DbVelocityY0 := VelocityY0 DbVelocityY1 := VelocityY1 DbTemp := Temp DbPositionY0 := PositionY0 DbPositionY1 := PositionY1 DbCountY := CountY DisplayEnb := 1 '------------------------------------------------------------------------------ PUB Debugger_Display | x, idx Repeat If DisplayEnb == 1 Displaying := 1 PST.Clear PST.Str(String("aX = ")) PST.Dec(DbgVar[00]) PST.NewLine PST.Str(String("aY = ")) PST.Dec(DbgVar[01]) PST.NewLine PST.Str(String("aZ = ")) PST.Dec(DbgVar[02]) PST.NewLine PST.Str(String("Acl0 = ")) PST.Dec(DbAccelerationY0) PST.NewLine PST.Str(String("Acl1 = ")) PST.Dec(DbAccelerationY1) PST.NewLine PST.Str(String("V0 = ")) PST.Dec(DbVelocityY0) PST.NewLine PST.Str(String("V1 = ")) PST.Dec(DbVelocityY1) PST.NewLine PST.Str(String("P0 = ")) PST.Dec(DbPositionY0) PST.NewLine PST.Str(String("P1 = ")) PST.Dec(DbPositionY1) PST.NewLine PST.Str(String("ZCnt = ")) PST.Dec(DbCountY) PST.NewLine Displaying := 0 waitcnt(constant(80_000_000 / 5) + LoopTime) '------------------------------------------------------------------------------Give it a look, if you would.
Thanks
Shawn
If you start at zero velocity and accelerate then you add up the acceleration values to get the velocity, if you then move at constant speed the velocity value remains the same as nothing is added to it (acceleration is zero) then if you decelerate negative values of acceleration are added to velocity eventually bringing it to zero.
Similar story with displacement.
Cheers,
Graham