Roel Smits / Mbed 2 deprecated angle_test

test angle measurement LSM6DS3

Dependencies:   ESC IMUfilter LSM6DS3 mbed

main.cpp@2:405f8e1a01d3, 2018-02-03 (annotated)

Committer:
rsmits
Date:
Sat Feb 03 15:21:25 2018 +0000
Revision:
2:405f8e1a01d3
Parent:
1:0e63617e1965
Child:
3:78cf56b8eb21
test angle working

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 2:405f8e1a01d3 9 #define g0 9.812865328f
rsmits 2:405f8e1a01d3 10 //Convert from degrees to radians. radians = (degrees*pi)/180
rsmits 2:405f8e1a01d3 11 #define toRadians(x) (x * 0.01745329252f)
rsmits 2:405f8e1a01d3 12 //Convert from radians to degrees.
rsmits 2:405f8e1a01d3 13 #define toDegrees(x) (x * 57.2957795)
rsmits 0:a606f47629c3 14
rsmits 0:a606f47629c3 15 const int LSM6DS3_ADDRESS = 0xD4;
rsmits 2:405f8e1a01d3 16 float DEGREES[3];
rsmits 2:405f8e1a01d3 17 float ACCELERATIONS[3];
rsmits 2:405f8e1a01d3 18 float ANGLES[3];
rsmits 2:405f8e1a01d3 19 //const int LSM6DS3_ADDRESS = 0x69;
rsmits 1:0e63617e1965 20
rsmits 0:a606f47629c3 21 Serial pc(SERIAL_TX, SERIAL_RX);
rsmits 0:a606f47629c3 22 AnalogIn PRESSURE_SENSOR(PA_0);
rsmits 2:405f8e1a01d3 23 LSM6DS3 imu(PB_9, PB_8, LSM6DS3_ADDRESS);
rsmits 2:405f8e1a01d3 24 IMUfilter imuFilter(0.1, 10);
rsmits 2:405f8e1a01d3 25 Ticker ScreenTicker;
rsmits 0:a606f47629c3 26
rsmits 0:a606f47629c3 27 //Init Serial port and LSM6DS3 chip
rsmits 0:a606f47629c3 28 void setup()
rsmits 0:a606f47629c3 29 {
rsmits 0:a606f47629c3 30 // Use the begin() function to initialize the LSM9DS0 library.
rsmits 0:a606f47629c3 31 // You can either call it with no parameters (the easy way):
rsmits 0:a606f47629c3 32 // SLEEP
rsmits 0:a606f47629c3 33 // uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
rsmits 0:a606f47629c3 34 // imu.G_POWER_DOWN, imu.A_POWER_DOWN);
rsmits 0:a606f47629c3 35 // LOWEST
rsmits 0:a606f47629c3 36 // uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
rsmits 0:a606f47629c3 37 // imu.G_ODR_13_BW_0, imu.A_ODR_13);
rsmits 0:a606f47629c3 38 // HIGHEST
rsmits 0:a606f47629c3 39 // uint16_t status = imu.begin(imu.G_SCALE_2000DPS, imu.A_SCALE_8G,
rsmits 0:a606f47629c3 40 // imu.G_ODR_1660, imu.A_ODR_6660);
rsmits 0:a606f47629c3 41
rsmits 0:a606f47629c3 42 uint16_t status = imu.begin(imu.G_SCALE_245DPS, imu.A_SCALE_2G,
rsmits 0:a606f47629c3 43 imu.G_ODR_1660, imu.A_ODR_6660);
rsmits 0:a606f47629c3 44
rsmits 0:a606f47629c3 45 //Make sure communication is working
rsmits 0:a606f47629c3 46 pc.printf("LSM6DS3 WHO_AM_I's returned: 0x%X\r\n", status);
rsmits 0:a606f47629c3 47 pc.printf("Should be 0x69\r\n");
rsmits 0:a606f47629c3 48 }
rsmits 2:405f8e1a01d3 49
rsmits 2:405f8e1a01d3 50 void screenUpdate(){
rsmits 2:405f8e1a01d3 51 pc.printf("Accel (m/s^2):\r\n");
rsmits 2:405f8e1a01d3 52 pc.printf("AX: %2f\r\n", ACCELERATIONS[0]);
rsmits 2:405f8e1a01d3 53 pc.printf("AY: %2f\r\n", ACCELERATIONS[1]);
rsmits 2:405f8e1a01d3 54 pc.printf("AZ: %2f\r\n", ACCELERATIONS[2]);
rsmits 2:405f8e1a01d3 55
rsmits 2:405f8e1a01d3 56 pc.printf("Gyro (degrees/s):\r\n");
rsmits 2:405f8e1a01d3 57 pc.printf("GX: %f\r\n", DEGREES[0]);
rsmits 2:405f8e1a01d3 58 pc.printf("GY: %f\r\n", DEGREES[1]);
rsmits 2:405f8e1a01d3 59 pc.printf("GZ: %2f\r\n", DEGREES[2]);
rsmits 2:405f8e1a01d3 60
rsmits 2:405f8e1a01d3 61 pc.printf("Angle (degrees):\r\n");
rsmits 2:405f8e1a01d3 62 pc.printf("X: %f\r\n", ANGLES[0]);
rsmits 2:405f8e1a01d3 63 pc.printf("Y: %f\r\n", ANGLES[1]);
rsmits 2:405f8e1a01d3 64 pc.printf("Z: %f\r\n\r\n", ANGLES[2]);
rsmits 2:405f8e1a01d3 65 }
rsmits 0:a606f47629c3 66
rsmits 0:a606f47629c3 67 int main()
rsmits 0:a606f47629c3 68 {
rsmits 0:a606f47629c3 69 bool started = false;
rsmits 0:a606f47629c3 70 while (true){
rsmits 0:a606f47629c3 71
rsmits 2:405f8e1a01d3 72 if(PRESSURE_SENSOR > 0.9f){
rsmits 0:a606f47629c3 73
rsmits 0:a606f47629c3 74 if (not started){
rsmits 0:a606f47629c3 75 setup(); //Setup sensor and Serial
rsmits 0:a606f47629c3 76 pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n");
rsmits 0:a606f47629c3 77 started=true;
rsmits 2:405f8e1a01d3 78 ScreenTicker.attach(&screenUpdate, 1);
rsmits 0:a606f47629c3 79 }
rsmits 2:405f8e1a01d3 80
rsmits 2:405f8e1a01d3 81 wait_ms(REFRESH_TIME_MS);
rsmits 0:a606f47629c3 82 imu.readAccel();
rsmits 0:a606f47629c3 83 imu.readGyro();
rsmits 2:405f8e1a01d3 84
rsmits 1:0e63617e1965 85
rsmits 2:405f8e1a01d3 86 DEGREES[0] = imu.gx;
rsmits 2:405f8e1a01d3 87 DEGREES[1] = imu.gy;
rsmits 2:405f8e1a01d3 88 DEGREES[2] = imu.gz;
rsmits 1:0e63617e1965 89
rsmits 1:0e63617e1965 90 // m/(s^2) = g0*g
rsmits 2:405f8e1a01d3 91 ACCELERATIONS[0] = g0*imu.ax;
rsmits 2:405f8e1a01d3 92 ACCELERATIONS[1] = g0*imu.ay;
rsmits 2:405f8e1a01d3 93 ACCELERATIONS[2] = g0*imu.az;
rsmits 2:405f8e1a01d3 94
rsmits 2:405f8e1a01d3 95 imuFilter.updateFilter(toRadians(imu.gx), toRadians(imu.gy), toRadians(imu.gz),
rsmits 2:405f8e1a01d3 96 g0*imu.ax, g0*imu.ay, g0*imu.az);
rsmits 2:405f8e1a01d3 97 imuFilter.computeEuler();
rsmits 1:0e63617e1965 98
rsmits 2:405f8e1a01d3 99 ANGLES[0] = toDegrees(imuFilter.getRoll());
rsmits 2:405f8e1a01d3 100 ANGLES[1] = toDegrees(imuFilter.getPitch());
rsmits 2:405f8e1a01d3 101 ANGLES[2] = toDegrees(imuFilter.getYaw());
rsmits 0:a606f47629c3 102 }
rsmits 0:a606f47629c3 103 else {
rsmits 0:a606f47629c3 104 started=false;
rsmits 1:0e63617e1965 105 imuFilter.reset();
rsmits 2:405f8e1a01d3 106 ScreenTicker.detach();
rsmits 0:a606f47629c3 107 }
rsmits 0:a606f47629c3 108 }
rsmits 0:a606f47629c3 109 }