customized_MEMS.cpp
1.85 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
#include "simple_obd_test.h"
#include "customized_DateAndTime.h"
#include "customized_MEMS.h"
RTC_DATA_ATTR MEMS_dev md_dev;
void init_MEMS_dev(void){
#if USE_SERIAL==1
Serial.print("MEMS:");
#endif
md_dev.mems = new MPU9250;
byte ret = (md_dev.mems)->begin(ENABLE_ORIENTATION);
if (ret) {
#if USE_SERIAL==1
Serial.println("MPU-9250");
#endif
} else {
(md_dev.mems)->end();
delete md_dev.mems;
md_dev.mems = new ICM_20948_I2C;
ret = (md_dev.mems)->begin(ENABLE_ORIENTATION);
if (ret) {
#if USE_SERIAL==1
Serial.println("ICM-20948");
#endif
} else {
#if USE_SERIAL==1
Serial.println("NO");
#endif
while(1);
}
}
esp_sleep_wakeup_cause_t wakeup_reason;
wakeup_reason = esp_sleep_get_wakeup_cause();
if (wakeup_reason!=ESP_SLEEP_WAKEUP_TIMER)
calibrateMEMS();
}
void calibrateMEMS(void){
float temp=0;
while (!((md_dev.mems)->read(md_dev.accBias, md_dev.gyrBias, md_dev.magBias, &temp))){}
#if USE_SERIAL==1
Serial.println("MEMS is calibrated successfully");
#endif
}
int isDataMEMSReady(MEMS_dev* md){
float temp=0;
if (((md_dev.mems)->read(md_dev.acc, md_dev.gyr, md_dev.mag, &temp))){
for (int i=0; i<3; i++){
md_dev.acc[i]-=md_dev.accBias[i];
md_dev.gyr[i]-=md_dev.gyrBias[i];
md_dev.mag[i]-=md_dev.magBias[i];
}
*md=md_dev;
return 1;
}
return 0;
}
int AddMEMSData(char* isoTime,char* p,int* nbr,int GPS_Stat){
int MEMS_Stat=0;
MEMS_dev md;
int i;
*nbr=0;
if (!isDataMEMSReady(&md)){
#if USE_SERIAL==1
Serial.println("Unable to read MEMS !");
#endif
MEMS_Stat=0;
}else{
if (!GPS_Stat){
GetCurrentTime(isoTime);
i=sprintf(p,"%s,,,,,,",isoTime);
*nbr+=i;
p+=i;
}
i=sprintf(p,",%f,%f,%f,%f,%f,%f,%f,%f,%f",\
md.acc[0],md.acc[1],md.acc[2],\
md.gyr[0],md.gyr[1],md.gyr[2],\
md.mag[0],md.mag[1],md.mag[2]\
);
*nbr+=i;
MEMS_Stat=1;
}
return MEMS_Stat;
}