Robotique FIP / IMU_FIP_nucleo

class IMU nucleo

Dependents:   Coupe-Robotique-FIP-Main

Fork of IMU_FIP by Robotique FIP

Imu.cpp@3:89e327e1217f, 2015-04-10 (annotated)

Committer:
quentin9696
Date:
Fri Apr 10 22:11:12 2015 +0000
Revision:
3:89e327e1217f
Parent:
2:3088586fe88c 
fin de la classe imu.; ; TODO : -magn?to en degr?;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
quentin9696 0:528e23a13fb7 1 #include "mbed.h"
quentin9696 0:528e23a13fb7 2 #include "Imu.h"
quentin9696 3:89e327e1217f 3 #include "hts221.h"
quentin9696 3:89e327e1217f 4 #include "lis3mdl.h"
quentin9696 3:89e327e1217f 5 #include "lps25h.h"
quentin9696 3:89e327e1217f 6 #include "lsm6ds0.h"
quentin9696 3:89e327e1217f 7 #include "struct_IMU.h"
quentin9696 0:528e23a13fb7 8
quentin9696 0:528e23a13fb7 9 Imu::Imu() {
quentin9696 0:528e23a13fb7 10
quentin9696 3:89e327e1217f 11 rawData.is_Raw = false;
quentin9696 3:89e327e1217f 12 rawData.TEMPERATURE_Value_C = 0.0f;
quentin9696 3:89e327e1217f 13 rawData.HUMIDITY_Value = 0.0f;
quentin9696 3:89e327e1217f 14 rawData.PRESSURE_Value = 0.0f;
quentin9696 3:89e327e1217f 15 rawData.MAG_Value = new AxesRaw_TypeDef();
quentin9696 3:89e327e1217f 16 rawData.ACC_Value = new AxesRaw_TypeDef();
quentin9696 3:89e327e1217f 17 rawData.GYR_Value = new AxesRaw_TypeDef();
quentin9696 3:89e327e1217f 18
quentin9696 3:89e327e1217f 19 offsetData.is_Raw = false;
quentin9696 3:89e327e1217f 20 offsetData.TEMPERATURE_Value_C = 0.0f;
quentin9696 3:89e327e1217f 21 offsetData.HUMIDITY_Value = 0.0f;
quentin9696 3:89e327e1217f 22 offsetData.PRESSURE_Value = 0.0f;
quentin9696 3:89e327e1217f 23 offsetData.MAG_Value = new AxesRaw_TypeDef();
quentin9696 3:89e327e1217f 24 offsetData.ACC_Value = new AxesRaw_TypeDef();
quentin9696 3:89e327e1217f 25 offsetData.GYR_Value = new AxesRaw_TypeDef();
quentin9696 3:89e327e1217f 26 }
quentin9696 3:89e327e1217f 27
quentin9696 3:89e327e1217f 28 void Imu::refresh(int i) {
quentin9696 3:89e327e1217f 29
quentin9696 3:89e327e1217f 30 if(i == 1)
quentin9696 3:89e327e1217f 31 {
quentin9696 3:89e327e1217f 32 //Offset
quentin9696 3:89e327e1217f 33 refresh(0);
quentin9696 3:89e327e1217f 34 rawData.is_Raw = false;
quentin9696 3:89e327e1217f 35
quentin9696 3:89e327e1217f 36 rawData.TEMPERATURE_Value_C -= offsetData.TEMPERATURE_Value_C;
quentin9696 3:89e327e1217f 37 rawData.HUMIDITY_Value -= offsetData.HUMIDITY_Value;
quentin9696 3:89e327e1217f 38 rawData.PRESSURE_Value -= offsetData.PRESSURE_Value;
quentin9696 3:89e327e1217f 39
quentin9696 3:89e327e1217f 40 rawData.MAG_Value->AXIS_X -= offsetData.MAG_Value->AXIS_X;
quentin9696 3:89e327e1217f 41 rawData.MAG_Value->AXIS_Y -= offsetData.MAG_Value->AXIS_Y;
quentin9696 3:89e327e1217f 42 rawData.MAG_Value->AXIS_Z -= offsetData.MAG_Value->AXIS_Z;
quentin9696 3:89e327e1217f 43
quentin9696 3:89e327e1217f 44 rawData.ACC_Value->AXIS_X -= offsetData.ACC_Value->AXIS_X;
quentin9696 3:89e327e1217f 45 rawData.ACC_Value->AXIS_Y -= offsetData.ACC_Value->AXIS_Y;
quentin9696 3:89e327e1217f 46 rawData.ACC_Value->AXIS_Z -= offsetData.ACC_Value->AXIS_Z;
quentin9696 3:89e327e1217f 47
quentin9696 3:89e327e1217f 48 rawData.GYR_Value->AXIS_X -= offsetData.GYR_Value->AXIS_X;
quentin9696 3:89e327e1217f 49 rawData.GYR_Value->AXIS_Y -= offsetData.GYR_Value->AXIS_Y;
quentin9696 3:89e327e1217f 50 rawData.GYR_Value->AXIS_Z -= offsetData.GYR_Value->AXIS_Z;
quentin9696 3:89e327e1217f 51 }
quentin9696 3:89e327e1217f 52 else
quentin9696 3:89e327e1217f 53 {
quentin9696 3:89e327e1217f 54 // Raw
quentin9696 3:89e327e1217f 55 rawData.is_Raw = true;
quentin9696 3:89e327e1217f 56 setRawHUM();
quentin9696 3:89e327e1217f 57 setRawTEMP();
quentin9696 3:89e327e1217f 58 setRawPRES();
quentin9696 3:89e327e1217f 59 setRawMAG();
quentin9696 3:89e327e1217f 60 setRawACC();
quentin9696 3:89e327e1217f 61 setRawGYR();
quentin9696 3:89e327e1217f 62 }
quentin9696 3:89e327e1217f 63
quentin9696 0:528e23a13fb7 64
quentin9696 0:528e23a13fb7 65 }
quentin9696 0:528e23a13fb7 66
quentin9696 3:89e327e1217f 67 Data_IMU * Imu::getData() {
quentin9696 3:89e327e1217f 68 return &rawData;
quentin9696 3:89e327e1217f 69 }
quentin9696 3:89e327e1217f 70
quentin9696 3:89e327e1217f 71 void Imu::setRawHUM() {
quentin9696 3:89e327e1217f 72 hts221.GetHumidity((float *)&rawData.HUMIDITY_Value);
quentin9696 3:89e327e1217f 73 }
quentin9696 3:89e327e1217f 74
quentin9696 3:89e327e1217f 75 void Imu::setRawTEMP() {
quentin9696 3:89e327e1217f 76 hts221.GetTemperature((float *) &rawData.TEMPERATURE_Value_C);
quentin9696 0:528e23a13fb7 77 }
quentin9696 0:528e23a13fb7 78
quentin9696 3:89e327e1217f 79 void Imu::setRawPRES() {
quentin9696 3:89e327e1217f 80 lps25h.GetPressure((float *)&rawData.PRESSURE_Value);
quentin9696 0:528e23a13fb7 81 }
quentin9696 0:528e23a13fb7 82
quentin9696 3:89e327e1217f 83 void Imu::setRawMAG() {
quentin9696 3:89e327e1217f 84 lis3mdl.GetAxes(rawData.MAG_Value);
quentin9696 3:89e327e1217f 85 }
quentin9696 3:89e327e1217f 86
quentin9696 3:89e327e1217f 87 void Imu::setRawACC() {
quentin9696 3:89e327e1217f 88 lsm6ds0.Acc_GetAxes(rawData.ACC_Value);
quentin9696 3:89e327e1217f 89 }
quentin9696 3:89e327e1217f 90
quentin9696 3:89e327e1217f 91 void Imu::setRawGYR() {
quentin9696 3:89e327e1217f 92 lsm6ds0.Gyro_GetAxes(rawData.GYR_Value);
quentin9696 0:528e23a13fb7 93 }
quentin9696 0:528e23a13fb7 94
quentin9696 3:89e327e1217f 95 /* get and set offset */
quentin9696 3:89e327e1217f 96
quentin9696 3:89e327e1217f 97 Data_IMU * Imu::getOffset() {
quentin9696 3:89e327e1217f 98 return &offsetData;
quentin9696 0:528e23a13fb7 99 }
quentin9696 0:528e23a13fb7 100
quentin9696 3:89e327e1217f 101 void Imu::setOffset() {
quentin9696 3:89e327e1217f 102
quentin9696 3:89e327e1217f 103 refresh(0);
quentin9696 3:89e327e1217f 104
quentin9696 3:89e327e1217f 105 offsetData.is_Raw = rawData.is_Raw;
quentin9696 3:89e327e1217f 106
quentin9696 3:89e327e1217f 107 offsetData.TEMPERATURE_Value_C = rawData.TEMPERATURE_Value_C;
quentin9696 3:89e327e1217f 108 offsetData.HUMIDITY_Value = rawData.HUMIDITY_Value;
quentin9696 3:89e327e1217f 109 offsetData.PRESSURE_Value = rawData.PRESSURE_Value;
quentin9696 3:89e327e1217f 110
quentin9696 3:89e327e1217f 111 offsetData.MAG_Value->AXIS_X = rawData.MAG_Value->AXIS_X;
quentin9696 3:89e327e1217f 112 offsetData.MAG_Value->AXIS_Y = rawData.MAG_Value->AXIS_Y;
quentin9696 3:89e327e1217f 113 offsetData.MAG_Value->AXIS_Z = rawData.MAG_Value->AXIS_Z;
quentin9696 3:89e327e1217f 114
quentin9696 3:89e327e1217f 115 offsetData.ACC_Value->AXIS_X = rawData.ACC_Value->AXIS_X;
quentin9696 3:89e327e1217f 116 offsetData.ACC_Value->AXIS_Y = rawData.ACC_Value->AXIS_Y;
quentin9696 3:89e327e1217f 117 offsetData.ACC_Value->AXIS_Z = rawData.ACC_Value->AXIS_Z;
quentin9696 3:89e327e1217f 118
quentin9696 3:89e327e1217f 119 offsetData.GYR_Value->AXIS_X = rawData.GYR_Value->AXIS_X;
quentin9696 3:89e327e1217f 120 offsetData.GYR_Value->AXIS_Y = rawData.GYR_Value->AXIS_Y;
quentin9696 3:89e327e1217f 121 offsetData.GYR_Value->AXIS_Z = rawData.GYR_Value->AXIS_Z;
quentin9696 0:528e23a13fb7 122 }