Roel Smits / Mbed 2 deprecated angle_test

test angle measurement LSM6DS3

Dependencies:   ESC IMUfilter LSM6DS3 mbed

main.cpp@1:0e63617e1965, 2018-01-17 (annotated)

Committer:
rsmits
Date:
Wed Jan 17 20:43:09 2018 +0000
Revision:
1:0e63617e1965
Parent:
0:a606f47629c3
Child:
2:405f8e1a01d3
printing angle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rsmits 0:a606f47629c3 1 #include "mbed.h"
rsmits 0:a606f47629c3 2 #include "esc.h"
rsmits 0:a606f47629c3 3 #include "LSM6DS3.h"
rsmits 0:a606f47629c3 4 #include "IMUfilter.h"
rsmits 0:a606f47629c3 5
rsmits 0:a606f47629c3 6 // refresh time. set to 500 for part 2 and 50 for part 4
rsmits 1:0e63617e1965 7 #define REFRESH_TIME_MS 100
rsmits 1:0e63617e1965 8 //Gravity at Earth's surface in m/s/s
rsmits 1:0e63617e1965 9 #define g0 9.812865328
rsmits 1:0e63617e1965 10 //Convert from degrees to radians.
rsmits 1:0e63617e1965 11 #define toRadians(x) (x * 0.01745329252)
rsmits 0:a606f47629c3 12
rsmits 0:a606f47629c3 13 const int LSM6DS3_ADDRESS = 0xD4;
rsmits 0:a606f47629c3 14 int PERIOD = 20;
rsmits 1:0e63617e1965 15
rsmits 0:a606f47629c3 16 //float MOTOR_PRINT;
rsmits 0:a606f47629c3 17 Serial pc(SERIAL_TX, SERIAL_RX);
rsmits 0:a606f47629c3 18 ESC MOTOR(PA_8, PERIOD);
rsmits 0:a606f47629c3 19 AnalogIn PRESSURE_SENSOR(PA_0);
rsmits 0:a606f47629c3 20 //Ticker UpdateScreen;
rsmits 0:a606f47629c3 21 LSM6DS3 imu(PF_0, PF_1, LSM6DS3_ADDRESS);
rsmits 1:0e63617e1965 22 IMUfilter imuFilter(REFRESH_TIME_MS/1000, 10);
rsmits 0:a606f47629c3 23
rsmits 0:a606f47629c3 24 //Init Serial port and LSM6DS3 chip
rsmits 0:a606f47629c3 25 void setup()
rsmits 0:a606f47629c3 26 {
rsmits 0:a606f47629c3 27 // Use the begin() function to initialize the LSM9DS0 library.
rsmits 0:a606f47629c3 28 // You can either call it with no parameters (the easy way):
rsmits 0:a606f47629c3 29 // SLEEP
rsmits 0:a606f47629c3 30 // uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
rsmits 0:a606f47629c3 31 // imu.G_POWER_DOWN, imu.A_POWER_DOWN);
rsmits 0:a606f47629c3 32 // LOWEST
rsmits 0:a606f47629c3 33 // uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
rsmits 0:a606f47629c3 34 // imu.G_ODR_13_BW_0, imu.A_ODR_13);
rsmits 0:a606f47629c3 35 // HIGHEST
rsmits 0:a606f47629c3 36 // uint16_t status = imu.begin(imu.G_SCALE_2000DPS, imu.A_SCALE_8G,
rsmits 0:a606f47629c3 37 // imu.G_ODR_1660, imu.A_ODR_6660);
rsmits 0:a606f47629c3 38
rsmits 0:a606f47629c3 39 uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
rsmits 0:a606f47629c3 40 imu.G_ODR_1660, imu.A_ODR_6660);
rsmits 0:a606f47629c3 41
rsmits 0:a606f47629c3 42 //Make sure communication is working
rsmits 0:a606f47629c3 43 pc.printf("LSM6DS3 WHO_AM_I's returned: 0x%X\r\n", status);
rsmits 0:a606f47629c3 44 pc.printf("Should be 0x69\r\n");
rsmits 0:a606f47629c3 45 }
rsmits 0:a606f47629c3 46
rsmits 0:a606f47629c3 47 int main()
rsmits 0:a606f47629c3 48 {
rsmits 1:0e63617e1965 49 //MOTOR.setThrottle(0.0);
rsmits 0:a606f47629c3 50 bool started = false;
rsmits 0:a606f47629c3 51
rsmits 0:a606f47629c3 52 while (true){
rsmits 0:a606f47629c3 53
rsmits 0:a606f47629c3 54 if(PRESSURE_SENSOR > 0.9){
rsmits 0:a606f47629c3 55
rsmits 0:a606f47629c3 56 if (not started){
rsmits 0:a606f47629c3 57 setup(); //Setup sensor and Serial
rsmits 0:a606f47629c3 58 pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n");
rsmits 0:a606f47629c3 59 started=true;
rsmits 0:a606f47629c3 60 }
rsmits 0:a606f47629c3 61
rsmits 1:0e63617e1965 62 // imu.readTemp();
rsmits 1:0e63617e1965 63 // pc.printf("Temp:\r\n");
rsmits 1:0e63617e1965 64 // pc.printf("TC: %2f\r\n", imu.temperature_c);
rsmits 1:0e63617e1965 65 // pc.printf("TF: %2f\r\n", imu.temperature_f);
rsmits 1:0e63617e1965 66 //
rsmits 0:a606f47629c3 67 imu.readAccel();
rsmits 1:0e63617e1965 68 // pc.printf("Accel:\r\n");
rsmits 1:0e63617e1965 69 // pc.printf("AX: %2f\r\n", imu.ax);
rsmits 1:0e63617e1965 70 // pc.printf("AY: %2f\r\n", imu.ay);
rsmits 1:0e63617e1965 71 // pc.printf("AZ: %2f\r\n", imu.az);
rsmits 1:0e63617e1965 72 //
rsmits 0:a606f47629c3 73 imu.readGyro();
rsmits 1:0e63617e1965 74 // pc.printf("Gyro:\r\n");
rsmits 1:0e63617e1965 75 // pc.printf("GX: %2f\r\n", imu.gx);
rsmits 1:0e63617e1965 76 // pc.printf("GY: %2f\r\n", imu.gy);
rsmits 1:0e63617e1965 77 // pc.printf("GZ: %2f\r\n\r\n", imu.gz);
rsmits 1:0e63617e1965 78
rsmits 1:0e63617e1965 79 // radians = (degrees*pi)/180
rsmits 1:0e63617e1965 80 double rx = (imu.gx*PI)/180;
rsmits 1:0e63617e1965 81 double ry = (imu.gy*PI)/180;
rsmits 1:0e63617e1965 82 double rz = (imu.gz*PI)/180;
rsmits 1:0e63617e1965 83
rsmits 1:0e63617e1965 84 // m/(s^2) = g0*g
rsmits 1:0e63617e1965 85 double mx = g0*imu.ax;
rsmits 1:0e63617e1965 86 double my = g0*imu.ay;
rsmits 1:0e63617e1965 87 double mz = g0*imu.az;
rsmits 1:0e63617e1965 88
rsmits 1:0e63617e1965 89 imuFilter.updateFilter(rx, ry, rz, mx, my, mz);
rsmits 1:0e63617e1965 90 imuFilter.computeEuler();
rsmits 1:0e63617e1965 91 pc.printf("%f, %f, %f\r\n\r\n", imuFilter.getRoll(), imuFilter.getPitch(), imuFilter.getYaw());
rsmits 0:a606f47629c3 92
rsmits 0:a606f47629c3 93 wait_ms(REFRESH_TIME_MS);
rsmits 0:a606f47629c3 94 }
rsmits 0:a606f47629c3 95 else {
rsmits 0:a606f47629c3 96 started=false;
rsmits 1:0e63617e1965 97 imuFilter.reset();
rsmits 0:a606f47629c3 98 }
rsmits 0:a606f47629c3 99 }
rsmits 0:a606f47629c3 100 }