73 lines
2.6 KiB
C
73 lines
2.6 KiB
C
//
|
|
// Created by wakashaw on 22-6-19.
|
|
//
|
|
#include "rtklib.h"
|
|
static imud_t imud={0};
|
|
|
|
extern int decode_typeIMU(rtcm_t *rtcm){
|
|
double tow;
|
|
int i=52,sensor_type,data_type,week;
|
|
double time;
|
|
if(i+80<=rtcm->len*8){
|
|
sensor_type = getbitu(rtcm->buff,i,8); i+=8;
|
|
data_type = getbits(rtcm->buff,48,4);
|
|
/* convert right-forward-up to forward-right-down 0-1-2 to 1-0-2*/
|
|
/* acc */
|
|
if(sensor_type==30){
|
|
if(data_type==3){
|
|
imud.time= getbitu(rtcm->buff,i,32); i+=32;
|
|
imud.accl[1]= getbits(rtcm->buff,i,16)*0.000061 * 9.8; i+=16;
|
|
imud.accl[0]= getbits(rtcm->buff,i,16)*0.000061 * 9.8; i+=16;
|
|
imud.accl[2]= getbits(rtcm->buff,i,16)*0.000061 * 9.8; i+=56;
|
|
/* up to down */
|
|
imud.accl[2]*=-1.0;
|
|
|
|
imud.gyro[1] = getbits(rtcm->buff,i,16)*0.00437 * D2R; i+=16;
|
|
imud.gyro[0]= getbits(rtcm->buff,i,16)*0.00437 * D2R; i+=16;
|
|
imud.gyro[2]= getbits(rtcm->buff,i,16)*0.00437 * D2R;
|
|
/* up to down */
|
|
imud.gyro[2]*=-1.0;
|
|
}
|
|
else{
|
|
time= getbitu(rtcm->buff,i,32); i+=32;
|
|
imud.accl[1]= getbits(rtcm->buff,i,16)*0.000061 * 9.8; i+=16;
|
|
imud.accl[0]= getbits(rtcm->buff,i,16)*0.000061 * 9.8; i+=16;
|
|
imud.accl[2]= getbits(rtcm->buff,i,16)*0.000061 * 9.8;
|
|
/* up to down */
|
|
imud.accl[2]*=-1.0;
|
|
if(time != imud.time){
|
|
imud.time = time;
|
|
for(int j=0;j<3;j++) imud.gyro[j]=0;
|
|
}
|
|
}
|
|
}
|
|
/* gyro */
|
|
else if(sensor_type==31){
|
|
time = getbitu(rtcm->buff,i,32); i+=32;
|
|
imud.gyro[1] = getbits(rtcm->buff,i,16)*0.00437 * D2R; i+=16;
|
|
imud.gyro[0]= getbits(rtcm->buff,i,16)*0.00437 * D2R; i+=16;
|
|
imud.gyro[2]= getbits(rtcm->buff,i,16)*0.00437 * D2R;
|
|
/* up to down */
|
|
imud.gyro[2]*=-1.0;
|
|
|
|
if(time != imud.time){
|
|
imud.time = time;
|
|
for(int j=0;j<3;j++) imud.accl[j]=0;
|
|
}
|
|
}
|
|
else{
|
|
// trace(2,"Not IMU data\n");
|
|
return -1;
|
|
}
|
|
}
|
|
for(int j=0;j<3;j++){
|
|
if(imud.accl[j]==0||imud.gyro[j]==0){
|
|
// trace(2,"lack of acc/gyro data\n");
|
|
return -1;
|
|
}
|
|
}
|
|
rtcm->imud=imud;
|
|
tow=time2gpst(utc2gpst(timeget()),&week);
|
|
rtcm->imud.syn_time=gpst2time(week,tow);
|
|
return 11;
|
|
} |