Closed dewalex closed 2 years ago
@dewalex PR would be great if you coudl submit one. :)
@jposada202020 I haven't figured out how to make a tilt-compensated compass yet, unfortunately. If I work it out, I'll totally do that, but until then, if anyone else has code to post, that would be great!
No worries, :) we will see if folks have something to share, maybe if you post in the discord channel folks have some code? just an idea. Thanks again
Ok, I got something going that seems to be working for me. Would definitely want someone with more experience to check that it's all good.
# # Equations from st manual, "Computing tilt measurement and tilt-compensated eCompass" by Andrea Vitali
# https://www.st.com/resource/en/design_tip/dm00269987-computing-tilt-measurement-and-tiltcompensated-ecompass-stmicroelectronics.pdf
# for Phi
# Roll: phi = atan2(Gy, Gz) # for stability ,can be substituted with Gz+Gx*alpha (with alpha=0.01-0.05)
# for Theta
# Gz2 = Gy * Sin( Phi ) + Gz * Cos( Phi )
# Pitch: Theta = Atan( -Gx / Gz2)
# for Psi or "Yaw" (The Tilt Compensated Heading); Bx,By,Bz are magnetometer readings
# By2 = Bz * Sin( Phi ) – By * Cos( Phi )
# Bz2 = By * Sin( Phi ) + Bz * Cos( Phi )
# Bx3 = Bx * Cos( Theta ) + Bz2 * Sin( Theta )
# Yaw: Psi = Atan2( By2 , Bx3)
##############
import board
import busio
from digitalio import DigitalInOut
import re
import time
from math import atan, atan2, degrees, pi, cos, sin
import adafruit_lsm303_accel
import adafruit_lis2mdl
import adafruit_lsm303dlh_mag
# Mag and Accel offsets from calibration. You will need to calibrate for your own offsets
offmag = (-10.8, 4.875, -54.225)
offaccel = (-0.994392, -0.305969, 0.382458)
i2c = busio.I2C(board.SCL, board.SDA)
sensoraccel = adafruit_lsm303_accel.LSM303_Accel(i2c)
sensormag = adafruit_lis2mdl.LIS2MDL(i2c)
def vector_2_degrees(x, y):
angle = degrees(atan2(y, x))
if angle < 0:
angle += 360
return angle
while True:
Xm,Ym,Zm = sensormag.magnetic
Xm = Xm - offmag[0]
Ym = Ym - offmag[1]
Zm = Zm - offmag[2]
ax, ay, az = sensoraccel.acceleration
ax = ax - offaccel[0]
ay = ay - offaccel[1]
az = az - offaccel[2]
# tilt compensation equations
stableZ = az + (ax*0.01)
phi = atan2(ay, stableZ)
Gz2 = ay * sin(phi) + az * cos(phi)
theta = atan(-ax/ Gz2)
By2 = Zm * sin(phi) - Ym * cos(phi)
Bz2 = Ym * sin(phi) + Zm * cos(phi)
Bx3 = Xm * cos(theta) + Bz2 * sin(theta)
Psi = vector_2_degrees(Bx3, By2)
print("Yaw Heading: ", "{:.0f}".format(Psi))
# negative 5 degrees declination in FL ## find yours @ magnetic-declination.com
withdecl = Psi - 5
if withdecl < 0:
withdecl += 360
print("Yaw with Declination: ", "{:.0f}".format(withdecl))
time.sleep(.5)
sounds fun, please submit it as an example PR!
@dewalex let me know if you are familiar contributing to CircuitPython, if not I could help you here, or in the Adafruit Discord Channel. Thanks for the Example :)
@jposada202020 Thanks! I am a total newbie. That was my first pull request. Please let me know if I did anything wrong!
Ok so I am going to give you some links that help you, for sure at the beginning it will look like a lot of work, but all the tools are there to make it easier, and PR after PR you will see that it will be more familiar, I can show you my first PR and all the mistakes I made, lol... So: In CircuitPython we use Pylint and black to verify the python code. you can install it in your machine/pc/mac I have tested this also in Raspberry Pi so I know it work there also The guide is here. https://learn.adafruit.com/improve-your-code-with-pylint If you do at least pylint and black before submiting the PR, 90% of the work is done. the other 10% are corner cases that I could help you solve. no worries As promised this was my first PR in CP https://github.com/adafruit/Adafruit_CircuitPython_Display_Text/pull/112
Let me know if you need anything, or let a message in the #help-with-circuitpython channel in discord, I read that pretty often
Thanks again for your contribution :)
@dewalex Let me know if you are stuck in some of the changes, I put my comments in your repo PR. If you need anything please let me know. Thanks
Thanks @jposada202020 ! I am out of town for a week and a half with just my phone, so will work on this when I’m back. I’m sure I’ll have a couple questions. Thanks again for the help with this
@dewalex lucky you!, my pleasure, thank you for working in the example! :)
@jposada202020 I made changes and submitted them for review. What I don't understand, however, is how to do the things in your initial comment: changing the file name and adding the example to the .rst. I couldn't find where the examples.rst was located.
If you could have a look, I'd really appreciate it. I know there is still work to do on it after resolving those two things too.
Yes I saw you commits in your repo, could you make the PR here, right now is on your fork. you could take a look here for instructions https://learn.adafruit.com/contribute-to-circuitpython-with-git-and-github
@jposada202020 Ok, I found that .rst file and updated that, made the other changes, and then submitted the PR on this side.
Thanks I have just approved the CI to run, If it passes I think we are good to go
Closing this for now. The PR will either move to a library that the example uses. Or get modified to use this library, in which case this can re-open at that time.
Would it be possible to add to the examples a compass that uses accelerometer values to compensate for tilt?