陀螺仪

Posted 2020-11-24 serser

#include <iostream>

using namespace std;

int main()
{
cout << "Hello World!" << endl;
return 0;
}
/**
* @brief command
* 0:串口输出频率设置
* 1：串口波特率设置
* 2：模块地址设置
* 3：方位角清零
* 4：体加速度清零
* 5：数据类型输出设定
* 6：
*/
int command;
int address=0x00;
int newaddress;
void setFrequency(int set=0x03)
{
unsigned char buf[6];
buf[0]=0x68;
buf[1]=0x05;
buf[2]=adress;
buf[3]=0x0C;
buf[4]=set;
for(int i=1;i<5;i++)
{
buf[5]=(buf[5]+buf[i])&0xFF;
}
ser.write(buf);
}

bool setFrequencyReturn(unsigned char* buf,int size=6)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]==0x05)&&(buf[2]==address)&&(buf[3]==0x8C)&&(buf[5]==checksum))
{
if(buf[4]==0x00)
{
return true;
}
else
{
return false;
}
}
}
void setBandrate(int set=0x05)
{
unsigned char buf[6];
buf[0]=0x68;
buf[1]=0x05;
buf[2]=adress;
buf[3]=0x0B;
buf[4]=set;
for(int i=1;i<5;i++)
{
buf[5]=(buf[5]+buf[i])&0xFF;
}
ser.write(buf);
}
bool setBandrateReturn(unsigned char* buf,int size=6)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]==0x05)&&(buf[2]==address)&&(buf[3]==0x8B)&&(buf[5]==checksum))
{
if(buf[4]==0x00)
{
return true;
}
else
{
return false;
}
}
}

void setAddress(int set)
{
unsigned char buf[6];
buf[0]=0x68;
buf[1]=0x05;
buf[2]=adress;
buf[3]=0x0F;
buf[4]=set;
for(int i=1;i<5;i++)
{
buf[5]=(buf[5]+buf[i])&0xFF;
}
ser.write(buf);
newaddress=set;
}
bool setAddressReturn(unsigned char* buf,int size=6)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]==0x05)&&(buf[2]==address)&&(buf[3]==0x8F)&&(buf[5]==checksum))
{
if(buf[4]==0x00)
{
address=newaddress;
return true;
}
else
{
return false;
}
}
}

void resetAngle()
{
unsigned char buf[5];
buf[0]=0x68;
buf[1]=0x04;
buf[2]=adress;
buf[3]=0x28;
for(int i=1;i<4;i++)
{
buf[4]=(buf[4]+buf[i])&0xFF;
}
ser.write(buf);
}
bool resetAngleReturn(unsigned char* buf,int size=5)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]==0x04)&&(buf[2]==address)&&(buf[3]==0x28)&&(buf[5]==checksum))
{
if(buf[4]==0x00)
{
return true;
}
else
{
return false;
}
}
}
void resetAcc()
{
unsigned char buf[5];
buf[0]=0x68;
buf[1]=0x04;
buf[2]=adress;
buf[3]=0x27;
for(int i=1;i<4;i++)
{
buf[4]=(buf[4]+buf[i])&0xFF;
}
ser.write(buf);
}

bool resetAngleReturn(unsigned char* buf,int size=5)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]==0x04)&&(buf[2]==address)&&(buf[3]==0x27)&&(buf[5]==checksum))
{
if(buf[4]==0x00)
{
return true;
}
else
{
return false;
}
}
}

void setOutputKind(int set=0x70)
{
unsigned char buf[6];
buf[0]=0x68;
buf[1]=0x05;
buf[2]=adress;
buf[3]=0xFD;
buf[4]=set;
for(int i=1;i<4;i++)
{
buf[5]=(buf[5]+buf[i])&0xFF;
}
ser.write(buf);
}

bool setOutputKindReturn(unsigned char* buf,int size=6)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]==0x05)&&(buf[2]==address)&&(buf[3]==0xFD)&&(buf[5]==checksum))
{
if(buf[4]==0x00)
{
return true;
}
else
{
return false;
}
}
}

void parseNineAxis(unsigned char* buf,int size=0x20)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]=0x1F)&&(buf[2]==address)&&(buf[3]==0x84)&&(buf[size-1]==checksum))
{
int sign;
if((buf[4]&0x80)==0x80)
{
sign=-1;
}
else if((buf[4]&0x80)==0x00)
{
sign=1;
}
roll=sign*((buf[4]&0xEF)*100+buf[5]+buf[6]*0.01);

if((buf[7]&0x80)==0x80)
{
sign=-1;
}
else if((buf[7]&0x80)==0x00)
{
sign=1;
}
pitch=sign*((buf[7]&0xEF)*100+buf[8]+buf[9]*0.01);

if((buf[10]&0x80)==0x80)
{
sign=-1;
}
else if((buf[10]&0x80)==0x00)
{
sign=1;
}
yaw=sign*((buf[10]&0xEF)*100+buf[11]+buf[12]*0.01);

if((buf[13]&0x80)==0x80)
{
sign=-1;
}
else if((buf[13]&0x80)==0x00)
{
sign=1;
}
accx=sign*((buf[13]&0xEF)*10+buf[14]*0.1+buf[15]*0.001);

if((buf[16]&0x80)==0x80)
{
sign=-1;
}
else if((buf[16]&0x80)==0x00)
{
sign=1;
}
accy=sign*((buf[16]&0xEF)*10+buf[17]*0.1+buf[18]*0.001);

if((buf[19]&0x80)==0x80)
{
sign=-1;
}
else if((buf[19]&0x80)==0x00)
{
sign=1;
}
accz=sign*((buf[19]&0xEF)*10+buf[20]*0.1+buf[21]*0.001);

if((buf[22]&0x80)==0x80)
{
sign=-1;
}
else if((buf[22]&0x80)==0x00)
{
sign=1;
}
gyrox=sign*((buf[22]&0xEF)*100+buf[23]+buf[24]*0.01);

if((buf[25]&0x80)==0x80)
{
sign=-1;
}
else if((buf[25]&0x80)==0x00)
{
sign=1;
}
gyroy=sign*((buf[25]&0xEF)*100+buf[26]+buf[27]*0.01);

if((buf[28]&0x80)==0x80)
{
sign=-1;
}
else if((buf[28]&0x80)==0x00)
{
sign=1;
}
gyroz=sign*((buf[28]&0xEF)*100+buf[29]+buf[30]*0.01);
}
}

void parsestandard1(unsigned char* buf,int size=0x0E)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]=0x0D)&&(buf[2]==address)&&(buf[3]==0x84)&&(buf[size-1]==checksum))
{
int sign;
if((buf[4]&0x80)==0x80)
{
sign=-1;
}
else if((buf[4]&0x80)==0x00)
{
sign=1;
}
gyroz=sign*((buf[4]&0xEF)*100+buf[5]+buf[6]*0.01);

if((buf[7]&0x80)==0x80)
{
sign=-1;
}
else if((buf[7]&0x80)==0x00)
{
sign=1;
}
accy=sign*((buf[7]&0xEF)*10+buf[8]*0.1+buf[9]*0.001);

if((buf[10]&0x80)==0x80)
{
sign=-1;
}
else if((buf[10]&0x80)==0x00)
{
sign=1;
}
yaw=sign*((buf[10]&0xEF)*100+buf[11]+buf[12]*0.01);

}
}

 

void parsestandard2(unsigned char* buf,int size=0x0E)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]=0x0D)&&(buf[2]==address)&&(buf[3]==0x84)&&(buf[size-1]==checksum))
{
int sign;
if((buf[4]&0x80)==0x80)
{
sign=-1;
}
else if((buf[4]&0x80)==0x00)
{
sign=1;
}
accx=sign*((buf[4]&0xEF)*10+buf[5]*0.1+buf[6]*0.001);

if((buf[7]&0x80)==0x80)
{
sign=-1;
}
else if((buf[7]&0x80)==0x00)
{
sign=1;
}
accy=sign*((buf[7]&0xEF)*10+buf[8]*0.1+buf[9]*0.001);

if((buf[10]&0x80)==0x80)
{
sign=-1;
}
else if((buf[10]&0x80)==0x00)
{
sign=1;
}
yaw=sign*((buf[10]&0xEF)*100+buf[11]+buf[12]*0.01);

}
}

 

 

void parsestandard3(unsigned char* buf,int size=0x11)
{
int checksum=0;
for(int i=1;i<size-1;i++)
{
checksum=(checksum+buf[i])&0xFF;
}
if((buf[0]==0x68)&&(buf[1]=0x10)&&(buf[2]==address)&&(buf[3]==0x84)&&(buf[size-1]==checksum))
{
int sign;
if((buf[4]&0x80)==0x80)
{
sign=-1;
}
else if((buf[4]&0x80)==0x00)
{
sign=1;
}
gyroz=sign*((buf[4]&0xEF)*100+buf[5]+buf[6]*0.01);

if((buf[7]&0x80)==0x80)
{
sign=-1;
}
else if((buf[7]&0x80)==0x00)
{
sign=1;
}
accx=sign*((buf[7]&0xEF)*10+buf[8]*0.1+buf[9]*0.001);

if((buf[10]&0x80)==0x80)
{
sign=-1;
}
else if((buf[10]&0x80)==0x00)
{
sign=1;
}
accy=sign*((buf[10]&0xEF)*10+buf[11]*0.1+buf[12]*0.001);

if((buf[13]&0x80)==0x80)
{
sign=-1;
}
else if((buf[13]&0x80)==0x00)
{
sign=1;
}
yaw=sign*((buf[13]&0xEF)*100+buf[14]+buf[15]*0.01);

}
}