LSM9DS1
LSM9DS1 copied to clipboard
kriswiner
Question
Hello, i was wondering how could implement all the code (so no serial code either) in python. I have an LSM9DS1TR chip simular to LSM9DS1. But if i convert the phi, theta and pitch to degrees its really off. There is a big difference, is there anyway u could help?
#Data ophalen vanuit de chip.
accel_x = data.data['accel_x_mg']
accel_y = data.data['accel_y_mg']
accel_z = data.data['accel_z_mg']
gyro_x = data.data['gyro_x_dps']
gyro_y = data.data['gyro_y_dps']
gyro_z = data.data['gyro_z_dps']
magn_x = data.data['magn_x_mG']
magn_y = data.data['magn_y_mG']
magn_z = data.data['magn_z_mG']
dt = (millis() - millisOld) / 1000
millisOld = millis()
q = [1.0, 0.0, 0.0, 0.0]
eInt = [0.0, 0.0, 0.0]
hx = float()
hy = float()
bx = float()
bz = float()
vx = float()
vy = float()
vz = float()
wx = float()
wy = float()
wz = float()
ex = float()
ey = float()
ez = float()
pa = float()
pb = float()
pc = float()
q1 = q[0]
q2 = q[1]
q3 = q[2]
q4 = q[3]
q1q1 = q[0]*q[0]
q1q2 = q[0]*q[1]
q1q3 = q[0]*q[2]
q1q4 = q[0]*q[3]
q2q2 = q[0]*q[1]
q2q3 = q[0]*q[2]
q2q4 = q[0]*q[3]
q3q3 = q[0]*q[2]
q3q4 = q[0]*q[3]
q4q4 = q[0]*q[3]
norm = math.sqrt(accel_x*accel_x + accel_y*accel_y + accel_z*accel_z)
norm = 1.0 / norm #norm berekenen.
accel_x *= norm
accel_y *= norm
accel_z *= norm
#Deze 3 waardes bij elkaar in het kwadraat is ongeveer 1.0, met soms een hele kleine afwijking
# print(np.round(accel_x, decimals=4), np.round(accel_y, decimals=4), np.round(accel_z, decimals=4))
norm = math.sqrt(magn_x*magn_x + magn_y*magn_y + magn_z*magn_z)
norm = 1.0 / norm #norm berekenen.
magn_x *= norm
magn_y *= norm
magn_z *= norm
#plus minus 1
hx = 2.0 * magn_x * (0.5 - q3q3 - q4q4) + 2.0 * magn_y * (q2q2 - q1q4) + 2.0 * magn_z * (q2q4 + q1q3)
hY = 2.0 * magn_x * (q2q3 + q1q4) + 2.0 * magn_y * (0.5 - q2q2 - q4q4) + 2.0 * magn_z * (q3q4 - q1q2)
bx = math.sqrt((hx*hx) + (hy*hy))
bz = 2.0 * bx * (q2q4 - q1q3) + 2.0 * magn_y * (q3q4 + q1q2) + 2.0 * magn_z * (0.5 - q2q2 - q3q3)
vx = 2.0 * (q2q4 - q1q3)
vy = 2.0 * (q1q2 + q3q4)
vz = q1q1 - q2q2 - q3q3 + q4q4
wx = 2.0 * bx * (0.5 - q3q3 - q4q4) + 2.0 * bz * (q2q4 - q1q3)
wy = 2.0 * bx * (q2q3 - q1q4) + 2.0 * bz * (q1q2 + q3q4)
wz = 2.0 * bx * (q1q3 + q2q4) + 2.0 * bz * (0.5 - q2q2 - q3q3)
ex = (accel_y * vz - accel_z * vy) + (magn_y * wz - magn_z * wy)
ey = (accel_z * vx - accel_x * vz) + (magn_z * wx - magn_x * wz)
ez = (accel_x * vy - accel_y * vx) + (magn_x * wy - magn_y * wx)
Ki = float()
if(Ki > 0.0):
eInt[0] += ex
eInt[1] += ey
eInt[2] += ez
else:
eInt[0] = 0.0
eInt[1] = 0.0
eInt[2] = 0.0
Kp = 2.0 * 5.0
gyro_x = gyro_x + Kp * ex + Ki * eInt[0]
gyro_y = gyro_y + Kp * ey + Ki * eInt[1]
gyro_z = gyro_z + Kp * ez + Ki * eInt[2]
pa = q2
pb = q3
pc = q4
q1 = q1 + (-q2 * gyro_x - q3 * gyro_y - q4 * gyro_z) * (0.5 * dt)
q2 = pa + (q1 * gyro_x + pb * gyro_z - pc * gyro_y) * (0.5 * dt)
q3 = pb + (q1 * gyro_y - pa * gyro_z + pc * gyro_x) * (0.5 * dt)
q4 = pc + (q1 * gyro_z + pa * gyro_y - pb * gyro_x) * (0.5 * dt)
# Normalise quaternion
norm = math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4)
norm = 1.0 / norm
q[0] = q1 * norm
q[1] = q2 * norm
q[2] = q3 * norm
q[3] = q4 * norm
#Ongeveer 1.0
# print(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3])
yaw = math.atan2(2.0 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3])
pitch = -math.asin(2.0 * (q[1] * q[3] - q[0] * q[2]))
roll = math.atan2(2.0 * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3])
pitch *= 180.0 / np.pi
yaw *= 180.0 / np.pi
# yaw -= 13.22 # Declination at Los Altos, California is ~13 degrees 13 minutes on 2020-07-19
roll *= 180.0 / np.pi
print(pitch, yaw, roll)
Sorry, I never use Python for programming, no idea...
On Tue, Sep 28, 2021 at 2:08 AM Arthur @.***> wrote:
Hello, i was wondering how could implement all the code (so no serial code either) in python. I have an LSM9DS1TR chip simular to LSM9DS1. But if i convert the phi, theta and pitch to degrees its really off. There is a big difference, is there anyway u could help?
#Data ophalen vanuit de chip. accel_x = data.data['accel_x_mg'] accel_y = data.data['accel_y_mg'] accel_z = data.data['accel_z_mg']
gyro_x = data.data['gyro_x_dps'] gyro_y = data.data['gyro_y_dps'] gyro_z = data.data['gyro_z_dps'] magn_x = data.data['magn_x_mG'] magn_y = data.data['magn_y_mG'] magn_z = data.data['magn_z_mG'] dt = (millis() - millisOld) / 1000 millisOld = millis() q = [1.0, 0.0, 0.0, 0.0] eInt = [0.0, 0.0, 0.0] hx = float() hy = float() bx = float() bz = float() vx = float() vy = float() vz = float() wx = float() wy = float() wz = float() ex = float() ey = float() ez = float() pa = float() pb = float() pc = float() q1 = q[0] q2 = q[1] q3 = q[2] q4 = q[3] q1q1 = q[0]*q[0] q1q2 = q[0]*q[1] q1q3 = q[0]*q[2] q1q4 = q[0]*q[3] q2q2 = q[0]*q[1] q2q3 = q[0]*q[2] q2q4 = q[0]*q[3] q3q3 = q[0]*q[2] q3q4 = q[0]*q[3] q4q4 = q[0]*q[3] norm = math.sqrt(accel_x*accel_x + accel_y*accel_y + accel_z*accel_z) norm = 1.0 / norm #norm berekenen. accel_x *= norm accel_y *= norm accel_z *= norm #Deze 3 waardes bij elkaar in het kwadraat is ongeveer 1.0, met soms een hele kleine afwijking # print(np.round(accel_x, decimals=4), np.round(accel_y, decimals=4), np.round(accel_z, decimals=4)) norm = math.sqrt(magn_x*magn_x + magn_y*magn_y + magn_z*magn_z) norm = 1.0 / norm #norm berekenen. magn_x *= norm magn_y *= norm magn_z *= norm #plus minus 1 hx = 2.0 * magn_x * (0.5 - q3q3 - q4q4) + 2.0 * magn_y * (q2q2 - q1q4) + 2.0 * magn_z * (q2q4 + q1q3) hY = 2.0 * magn_x * (q2q3 + q1q4) + 2.0 * magn_y * (0.5 - q2q2 - q4q4) + 2.0 * magn_z * (q3q4 - q1q2) bx = math.sqrt((hx*hx) + (hy*hy)) bz = 2.0 * bx * (q2q4 - q1q3) + 2.0 * magn_y * (q3q4 + q1q2) + 2.0 * magn_z * (0.5 - q2q2 - q3q3) vx = 2.0 * (q2q4 - q1q3) vy = 2.0 * (q1q2 + q3q4) vz = q1q1 - q2q2 - q3q3 + q4q4 wx = 2.0 * bx * (0.5 - q3q3 - q4q4) + 2.0 * bz * (q2q4 - q1q3) wy = 2.0 * bx * (q2q3 - q1q4) + 2.0 * bz * (q1q2 + q3q4) wz = 2.0 * bx * (q1q3 + q2q4) + 2.0 * bz * (0.5 - q2q2 - q3q3) ex = (accel_y * vz - accel_z * vy) + (magn_y * wz - magn_z * wy) ey = (accel_z * vx - accel_x * vz) + (magn_z * wx - magn_x * wz) ez = (accel_x * vy - accel_y * vx) + (magn_x * wy - magn_y * wx) Ki = float() if(Ki > 0.0): eInt[0] += ex eInt[1] += ey eInt[2] += ez else: eInt[0] = 0.0 eInt[1] = 0.0 eInt[2] = 0.0 Kp = 2.0 * 5.0 gyro_x = gyro_x + Kp * ex + Ki * eInt[0] gyro_y = gyro_y + Kp * ey + Ki * eInt[1] gyro_z = gyro_z + Kp * ez + Ki * eInt[2] pa = q2 pb = q3 pc = q4 q1 = q1 + (-q2 * gyro_x - q3 * gyro_y - q4 * gyro_z) * (0.5 * dt) q2 = pa + (q1 * gyro_x + pb * gyro_z - pc * gyro_y) * (0.5 * dt) q3 = pb + (q1 * gyro_y - pa * gyro_z + pc * gyro_x) * (0.5 * dt) q4 = pc + (q1 * gyro_z + pa * gyro_y - pb * gyro_x) * (0.5 * dt) # Normalise quaternion norm = math.sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4) norm = 1.0 / norm q[0] = q1 * norm q[1] = q2 * norm q[2] = q3 * norm q[3] = q4 * norm #Ongeveer 1.0 # print(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]) yaw = math.atan2(2.0 * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]) pitch = -math.asin(2.0 * (q[1] * q[3] - q[0] * q[2])) roll = math.atan2(2.0 * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]) pitch *= 180.0 / np.pi yaw *= 180.0 / np.pi # yaw -= 13.22 # Declination at Los Altos, California is ~13 degrees 13 minutes on 2020-07-19 roll *= 180.0 / np.pi print(pitch, yaw, roll)— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/18, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKUT6EPEFQDYET6PRX3UEGAZTANCNFSM5E444QNA . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.
Do you have a complete tutorial how it works and just one code file? Because i really find it hard to understand it
Maybe take a look at Adafruit or Sparkfun?
On Tue, Sep 28, 2021 at 12:52 PM Arthur @.***> wrote:
Do you have a complete tutorial how it works and just one code file? Because i really find it hard to understand it
— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/18#issuecomment-929575032, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKXKJBPR4ZKYO7FFI3LUEIMJPANCNFSM5E444QNA . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.
That’s a different chip then right? Because im not allowed to use any other chip
Adafruit or Sparkfun are companies that sell LSM6DS1 sensor breakout boards. Try Google, it is your friend here...
On Tue, Sep 28, 2021 at 2:07 PM Arthur @.***> wrote:
That’s a different chip then right? Because im not allowed to use any other chip
— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/18#issuecomment-929625865, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKURPEPZRYETOAXEC6DUEIVBFANCNFSM5E444QNA . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.
Arthur,
This doesn't look right:
q1q1 = q[0]*q[0]
q1q2 = q[0]*q[1]
q1q3 = q[0]*q[2]
q1q4 = q[0]*q[3]
q2q2 = q[0]*q[1]
q2q3 = q[0]*q[2]
q2q4 = q[0]*q[3]
q3q3 = q[0]*q[2]
q3q4 = q[0]*q[3]
q4q4 = q[0]*q[3]
Should probably be (untested):
q1q1 = q[0]*q[0]
q1q2 = q[0]*q[1]
q1q3 = q[0]*q[2]
q1q4 = q[0]*q[3]
q2q2 = q[1]*q[1]
q2q3 = q[1]*q[2]
q2q4 = q[1]*q[3]
q3q3 = q[2]*q[2]
q3q4 = q[2]*q[3]
q4q4 = q[3]*q[3]
Hope this helps to fix it.
thetaM = math.degrees(math.atan2(accel_x, math.sqrt(accel_y ** 2.0 + accel_z ** 2.0))) #x range van 90 -90
phiM = -math.degrees(math.atan2(accel_y, math.sqrt(accel_x ** 2.0 + accel_z ** 2.0))) #y standaard min, range van90 -90
I started euler angles, wich works fine, but the problem is that the range is only from -90 to 90 degrees. And i need a full rotation.
Kris, are u changing the raw values of the accelerometer, gyroscope and magnetometer into the function?
Not sure what you mean here.
The raw register values are converted to signed 16-bit integers, then scaled, then have their offset biases applied to get physically meaningful data. These data are then fed into a fusion algorithm to get yaw, roll, and pitch.
On Wed, Oct 6, 2021 at 11:29 PM Arthur @.***> wrote:
Kris, are u changing the raw values of the accelerometer, gyroscope and magnetometer into the function?
— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/18#issuecomment-937493539, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKWGFUESJ2FMPIUO4ADUFU46NANCNFSM5E444QNA . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.
i have no idea what wrong means here...
On Thu, Oct 14, 2021 at 4:02 AM Arthur @.***> wrote:
Then i have no idea what im doing wrong.
— You are receiving this because you commented.
Reply to this email directly, view it on GitHub https://github.com/kriswiner/LSM9DS1/issues/18#issuecomment-943249073, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABTDLKQNRPV2J6C4TNHGAP3UG22ERANCNFSM5E444QNA . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.
Well im trying the code (https://github.com/kriswiner/LSM9DS1/issues/18#issue-1009486405) But it doesn't work, it gives like -30 and the next data it gives -180 or something, the difference is to big, even though the chip is not moving
I have no idea how to use python for this as mentioned.
But 99% of these kinds of problems are due to inadequate calibration.
If the sensor is laying flat and motion less do you see 0, 0, 1 g for accel to within 10 mg or less and 0, 0, 0 dps for gyro to within 10 mdps or less and mag values consistent with your local mag field (see USGS or equivalent) and if you flip the board upside down does Mz = -Mz, etc.?