Adding a tilt-compensated compass example

[dewalex]

Would it be possible to add to the examples a compass that uses accelerometer values to compensate for tilt?

[jposada202020]

@dewalex PR would be great if you coudl submit one. :)

[dewalex]

@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!

[jposada202020]

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

[dewalex]

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)

[ladyada]

sounds fun, please submit it as an example PR!

[jposada202020]

@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 :)

[dewalex]

@jposada202020 Thanks! I am a total newbie. That was my first pull request. Please let me know if I did anything wrong!

[jposada202020]

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
adafruit/Adafruit_CircuitPython_Display_Text#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 :)

[jposada202020]

@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

[dewalex]

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

[jposada202020]

@dewalex lucky you!, my pleasure, thank you for working in the example! :)

[dewalex]

@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.

[jposada202020]

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

[dewalex]

@jposada202020 Ok, I found that .rst file and updated that, made the other changes, and then submitted the PR on this side.