Jason Noel / Mbed 2 deprecated Noel_A9

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@1:be4184d4674a, 2020-11-30 (annotated)

Committer:
jason_noel
Date:
Mon Nov 30 22:00:32 2020 +0000
Revision:
1:be4184d4674a
Parent:
0:6b20025ef151 
Fully Functional;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jason_noel 0:6b20025ef151 1 #include "mbed.h"
jason_noel 0:6b20025ef151 2 #include "LSM9DS1.h"
jason_noel 0:6b20025ef151 3
jason_noel 0:6b20025ef151 4 DigitalOut myled(LED1);
jason_noel 0:6b20025ef151 5 Serial pc(USBTX, USBRX);
jason_noel 0:6b20025ef151 6 LSM9DS1 lol(p28, p27, 0xD6, 0x3C);
jason_noel 0:6b20025ef151 7 Timer t;
jason_noel 0:6b20025ef151 8
jason_noel 0:6b20025ef151 9 float PI = 3.14159265358979323846f;
jason_noel 0:6b20025ef151 10
jason_noel 0:6b20025ef151 11 void mag_correction(float mx, float my, float mz, float mag_c[3])
jason_noel 0:6b20025ef151 12 {
jason_noel 0:6b20025ef151 13 float bias[3]= {-0.0259,0.2015,0.1879};
jason_noel 0:6b20025ef151 14 float scale[3][3]= {{1.0269,0.0512,0.0242}, {0.0512,1.0074,0.0203}, {0.0242,0.0203,1.0419}};
jason_noel 0:6b20025ef151 15 mag_c[0]=(mx-bias[0])*scale[0][0]+(my-bias[1])*scale[1][0]+(mz-bias[2])*scale[2][0];
jason_noel 0:6b20025ef151 16 mag_c[1]=(mx-bias[0])*scale[0][1]+(my-bias[1])*scale[1][1]+(mz-bias[2])*scale[2][1];
jason_noel 0:6b20025ef151 17 mag_c[2]=(mx-bias[0])*scale[0][2]+(my-bias[1])*scale[1][2]+(mz-bias[2])*scale[2][2];
jason_noel 0:6b20025ef151 18 }
jason_noel 0:6b20025ef151 19
jason_noel 0:6b20025ef151 20 int main()
jason_noel 0:6b20025ef151 21 {
jason_noel 0:6b20025ef151 22 float roll, pitch, yaw;
jason_noel 0:6b20025ef151 23 float accel[3], mag[3], gyro[3];
jason_noel 0:6b20025ef151 24
jason_noel 0:6b20025ef151 25 lol.begin();
jason_noel 0:6b20025ef151 26 if (!lol.begin()) {
jason_noel 0:6b20025ef151 27 pc.printf("Failed to communicate with LSM9DS1. \n");
jason_noel 0:6b20025ef151 28 }
jason_noel 0:6b20025ef151 29
jason_noel 0:6b20025ef151 30 lol.calibrate(true);
jason_noel 0:6b20025ef151 31 pc.printf("Gyro bias = %f,%f,%f\r\n", lol.gBias[0], lol.gBias[1], lol.gBias[2]);
jason_noel 0:6b20025ef151 32 pc.printf("Accel bias = %f,%f,%f\r\n", lol.aBias[0], lol.aBias[1], lol.aBias[2]);
jason_noel 0:6b20025ef151 33
jason_noel 0:6b20025ef151 34 wait(5);
jason_noel 0:6b20025ef151 35 t.start();
jason_noel 0:6b20025ef151 36
jason_noel 0:6b20025ef151 37 while(1) {
jason_noel 0:6b20025ef151 38 lol.readMag();
jason_noel 0:6b20025ef151 39 lol.readGyro();
jason_noel 0:6b20025ef151 40 lol.readAccel();
jason_noel 0:6b20025ef151 41
jason_noel 0:6b20025ef151 42 accel[0]=lol.calcAccel(lol.ax);
jason_noel 0:6b20025ef151 43 accel[1]=lol.calcAccel(lol.ay);
jason_noel 0:6b20025ef151 44 accel[2]=-lol.calcAccel(lol.az);
jason_noel 0:6b20025ef151 45 gyro[0]=lol.calcGyro(lol.gx);
jason_noel 0:6b20025ef151 46 gyro[1]=lol.calcGyro(lol.gy);
jason_noel 0:6b20025ef151 47 gyro[2]=-lol.calcGyro(lol.gz);
jason_noel 0:6b20025ef151 48
jason_noel 0:6b20025ef151 49 mag_correction(lol.calcMag(lol.mx), lol.calcMag(lol.my), lol.calcMag(lol.mz), mag);
jason_noel 0:6b20025ef151 50 mag[2]=-mag[2];
jason_noel 0:6b20025ef151 51
jason_noel 0:6b20025ef151 52 roll=atan2(accel[1], accel[2]/abs(accel[2])*(sqrt((accel[0]*accel[0])+(accel[2]*accel[2]))));
jason_noel 0:6b20025ef151 53 pitch=-atan2(-accel[0],(sqrt((accel[1]*accel[1])+(accel[2]*accel[2]))));
jason_noel 0:6b20025ef151 54
jason_noel 0:6b20025ef151 55 float Yh=(mag[1]*cos(roll))-(mag[2]*sin(roll));
jason_noel 0:6b20025ef151 56 float Xh=(mag[0]*cos(pitch))+(mag[1]*sin(roll)*sin(pitch))+(mag[2]*cos(roll)*sin(pitch));
jason_noel 0:6b20025ef151 57
jason_noel 0:6b20025ef151 58 yaw=atan2(Yh, Xh);
jason_noel 0:6b20025ef151 59 pitch *=180.0f/PI;
jason_noel 0:6b20025ef151 60 yaw *=180.0f/PI;
jason_noel 0:6b20025ef151 61 roll *=180.0f/PI;
jason_noel 0:6b20025ef151 62
jason_noel 0:6b20025ef151 63 if(yaw<=0) {
jason_noel 0:6b20025ef151 64 yaw=yaw+360;
jason_noel 0:6b20025ef151 65 }
jason_noel 0:6b20025ef151 66 if(roll<=0) {
jason_noel 0:6b20025ef151 67 roll=roll+360;
jason_noel 0:6b20025ef151 68 }
jason_noel 0:6b20025ef151 69 if(pitch<=0) {
jason_noel 0:6b20025ef151 70 pitch=pitch+360;
jason_noel 0:6b20025ef151 71 }
jason_noel 0:6b20025ef151 72
jason_noel 0:6b20025ef151 73 pc.printf("$IMU,4,4,%f,%3.3f,%3.3f,%3.3f;\r\n",t.read(),roll,pitch,yaw);
jason_noel 0:6b20025ef151 74 }
jason_noel 0:6b20025ef151 75 }