kriswiner
commented
2 years ago 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.
arthurvanl
marknn3
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?