Alek Boving / Mbed 2 deprecated BovingAssign9

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed LSM9DS1

main.cpp@0:042921e9375d, 2020-11-28 (annotated)

Committer:
alekboving
Date:
Sat Nov 28 16:42:41 2020 +0000
Revision:
0:042921e9375d
Child:
1:9331c419c9b9
fixed errors, added yaw

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alekboving 0:042921e9375d 1 #include "LSM9DS1.h"
alekboving 0:042921e9375d 2
alekboving 0:042921e9375d 3 DigitalOut myled(LED1);
alekboving 0:042921e9375d 4 Serial pc(USBTX,USBRX);
alekboving 0:042921e9375d 5 LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
alekboving 0:042921e9375d 6 Timer t;
alekboving 0:042921e9375d 7
alekboving 0:042921e9375d 8 float PI=3.1415926535897;
alekboving 0:042921e9375d 9
alekboving 0:042921e9375d 10 void mag_correction(float mx, float my, float mz, float mag_c[3])
alekboving 0:042921e9375d 11 {
alekboving 0:042921e9375d 12 float bias[3] = {-0.1102,0.3965,-0.0683};
alekboving 0:042921e9375d 13 float scale[3][3] = {{1.0558, 0.0640, -0.0220},
alekboving 0:042921e9375d 14 {0.0640, 1.0657, 0.0645},
alekboving 0:042921e9375d 15 {-0.0220, 0.0645, 1.0650}
alekboving 0:042921e9375d 16 };
alekboving 0:042921e9375d 17
alekboving 0:042921e9375d 18 mag_c[0] = (mx-bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
alekboving 0:042921e9375d 19 mag_c[1] = (mx-bias[0]) *scale[0][1] + (my - bias[1]) *scale[1][1] + (mz - bias[2]) *scale[2][1];
alekboving 0:042921e9375d 20 mag_c[2] = (mx-bias[0]) *scale[0][2] + (my - bias[1]) *scale[1][2] + (mz - bias[2]) *scale[2][2];
alekboving 0:042921e9375d 21 }
alekboving 0:042921e9375d 22 int main()
alekboving 0:042921e9375d 23 {
alekboving 0:042921e9375d 24 float roll, pitch, yaw;
alekboving 0:042921e9375d 25 float accel[3], mag[3], gyro[3];
alekboving 0:042921e9375d 26
alekboving 0:042921e9375d 27 lol.begin();
alekboving 0:042921e9375d 28 if (!lol.begin()) {
alekboving 0:042921e9375d 29 pc.printf("failed to communicate with LSM9DS1.\n");
alekboving 0:042921e9375d 30 }
alekboving 0:042921e9375d 31 lol.calibrate(true);
alekboving 0:042921e9375d 32 pc.printf("Gyro bias = %f,%f,%f\r\n", lol.gBias[0], lol.gBias[1], lol.gBias[2]);
alekboving 0:042921e9375d 33 pc.printf("Accel bias = %f,%f,%f\r\n", lol.aBias[0], lol.aBias[1], lol.aBias[2]);
alekboving 0:042921e9375d 34 wait(1);
alekboving 0:042921e9375d 35 t.start();
alekboving 0:042921e9375d 36
alekboving 0:042921e9375d 37 while(1) {
alekboving 0:042921e9375d 38 lol.readMag();
alekboving 0:042921e9375d 39 lol.readGyro();
alekboving 0:042921e9375d 40 lol.readAccel();
alekboving 0:042921e9375d 41 accel[0]= lol.calcAccel(lol.ax);
alekboving 0:042921e9375d 42 accel[1]= lol.calcAccel(lol.ay);
alekboving 0:042921e9375d 43 accel[2]= - lol.calcAccel(lol.az); //reverse z
alekboving 0:042921e9375d 44 gyro[0]= lol.calcGyro(lol.gx);
alekboving 0:042921e9375d 45 gyro[1]= lol.calcGyro(lol.gy);
alekboving 0:042921e9375d 46 gyro[2]= - lol.calcGyro(lol.gz); //reverse z
alekboving 0:042921e9375d 47 mag_correction(lol.calcMag(lol.mx), lol.calcMag(lol.my), lol.calcMag(lol.mz), mag);
alekboving 0:042921e9375d 48 mag[2] = - mag[2]; //reverse z
alekboving 0:042921e9375d 49 //reg geometry conversion
alekboving 0:042921e9375d 50 roll = atan2 (accel[1], accel[2]/abs(accel[2])*(sqrt ((accel[0]*accel[0]) +(accel[2] * accel[2]))));
alekboving 0:042921e9375d 51 //negative is added after testing pose in real situation
alekboving 0:042921e9375d 52 pitch = - atan2 (-accel[0], (sqrt((accel[1]*accel[1]+(accel[2]*accel[2])))));
alekboving 0:042921e9375d 53 float Yh = (mag[1] * cos(roll)) - (mag[2] *sin(roll));
alekboving 0:042921e9375d 54 float Xh = (mag[0] * cos(pitch))+(mag[1] * sin(roll)*sin(pitch)) + (mag[2]*cos(roll) * sin(pitch));
alekboving 0:042921e9375d 55 yaw = atan2(Yh,Xh);
alekboving 0:042921e9375d 56 pitch *= 180.0f / PI;
alekboving 0:042921e9375d 57 yaw *= 180.0f / PI;
alekboving 0:042921e9375d 58 roll *= 180.0f /PI;
alekboving 0:042921e9375d 59 if(yaw<=0) {
alekboving 0:042921e9375d 60 yaw = yaw+360;
alekboving 0:042921e9375d 61 }
alekboving 0:042921e9375d 62 if(roll<=0) {
alekboving 0:042921e9375d 63 roll = roll+360;
alekboving 0:042921e9375d 64 }
alekboving 0:042921e9375d 65 if(pitch<=0) {
alekboving 0:042921e9375d 66 pitch = pitch + 360;
alekboving 0:042921e9375d 67 }
alekboving 0:042921e9375d 68
alekboving 0:042921e9375d 69 pc.printf("$imu,4,4,%f,%3.3f, %3.3f, %3.3f;\r\n",t.read(),roll,pitch,yaw);
alekboving 0:042921e9375d 70 }
alekboving 0:042921e9375d 71 }