`#ifndef __AHRSREG_H

define __AHRSREG_H

define SAVE 0x00

define CALSW 0x01

define RSW 0x02

define RRATE 0x03

define BAUD 0x04

define AXOFFSET 0x05

define AYOFFSET 0x06

define AZOFFSET 0x07

define GXOFFSET 0x08

define GYOFFSET 0x09

define GZOFFSET 0x0a

define HXOFFSET 0x0b

define HYOFFSET 0x0c

define HZOFFSET 0x0d

define D0MODE 0x0e

define D1MODE 0x0f

define D2MODE 0x10

define D3MODE 0x11

define D0PWMH 0x12

define D1PWMH 0x13

define D2PWMH 0x14

define D3PWMH 0x15

define D0PWMT 0x16

define D1PWMT 0x17

define D2PWMT 0x18

define D3PWMT 0x19

define IICADDR 0x1a

define LEDOFF 0x1b

define GPSBAUD 0x1c

define YYMM 0x30

define DDHH 0x31

define MMSS 0x32

define MS 0x33

define AX 0x34

define AY 0x35

define AZ 0x36

define GX 0x37

define GY 0x38

define GZ 0x39

define HX 0x3a

define HY 0x3b

define HZ 0x3c

define Roll 0x3d

define Pitch 0x3e

define Yaw 0x3f

define TEMP 0x40

define D0Status 0x41

define D1Status 0x42

define D2Status 0x43

define D3Status 0x44

define PressureL 0x45

define PressureH 0x46

define HeightL 0x47

define HeightH 0x48

define LonL 0x49

define LonH 0x4a

define LatL 0x4b

define LatH 0x4c

define GPSHeight 0x4d

define GPSYAW 0x4e

define GPSVL 0x4f

define GPSVH 0x50

define DIO_MODE_AIN 0

define DIO_MODE_DIN 1

define DIO_MODE_DOH 2

define DIO_MODE_DOL 3

define DIO_MODE_DOPWM 4

define DIO_MODE_GPS 5

struct STime { unsigned char ucYear; unsigned char ucMonth; unsigned char ucDay; unsigned char ucHour; unsigned char ucMinute; unsigned char ucSecond; unsigned short usMiliSecond; }; struct SAcc { short a[3]; short T; }; struct SGyro { short w[3]; short T; }; struct SAngle { short Angle[3]; short T; }; struct SMag { short h[3]; short T; };

struct SDStatus { short sDStatus[4]; };

struct SPress { long lPressure; long lAltitude; };

struct SLonLat { long lLon; long lLat; };

struct SGPSV { short sGPSHeight; short sGPSYaw; long lGPSVelocity; }; extern struct STime stcTime; extern struct SAcc stcAcc; extern struct SGyro stcGyro; extern struct SAngle stcAngle; extern struct SMag stcMag; extern struct SDStatus stcDStatus; extern struct SPress stcPress; extern struct SLonLat stcLonLat; extern struct SGPSV stcGPSV; void CopeSerialData(unsigned char ucData);

endif

`#include

include "JY901.h"

struct STime stcTime={0}; struct SAcc stcAcc={0}; struct SGyro stcGyro={0}; struct SAngle stcAngle={0}; struct SMag stcMag={0}; struct SDStatus stcDStatus={0}; struct SPress stcPress={0}; struct SLonLat stcLonLat={0}; struct SGPSV stcGPSV={0};

void CharToLong(char Dest[],char Source[]) { Dest = Source[3]; (Dest+1) = Source[2]; (Dest+2) = Source[1]; (Dest+3) = Source[0]; } void CopeSerialData(unsigned char ucData) { static unsigned char ucRxBuffer[12]; static unsigned char ucRxCnt = 0;

ucRxBuffer[ucRxCnt++]=ucData;
if (ucRxBuffer[0]!=0x55) //数据头不对，则重新开始寻找0x55数据头
{
    ucRxCnt=0;
    return;                                                                                                                                   
}
if (ucRxCnt<11) {return;}//数据不满11个，则返回
else
{
    switch(ucRxBuffer[1])
    {
        case 0x50: stcTime.ucYear       = ucRxBuffer[2];
                    stcTime.ucMonth     = ucRxBuffer[3];
                    stcTime.ucDay       = ucRxBuffer[4];
                    stcTime.ucHour      = ucRxBuffer[5];
                    stcTime.ucMinute    = ucRxBuffer[6];
                    stcTime.ucSecond    = ucRxBuffer[7];
                    stcTime.usMiliSecond=((unsigned short)ucRxBuffer[9]<<8)|ucRxBuffer[8];
                    break;
        case 0x51:  stcAcc.a[0] = ((unsigned short)ucRxBuffer[3]<<8)|ucRxBuffer[2];
                    stcAcc.a[1] = ((unsigned short)ucRxBuffer[5]<<8)|ucRxBuffer[4];
                    stcAcc.a[2] = ((unsigned short)ucRxBuffer[7]<<8)|ucRxBuffer[6];
                    break;
        case 0x52:  stcGyro.w[0] = ((unsigned short)ucRxBuffer[3]<<8)|ucRxBuffer[2];
                    stcGyro.w[1] = ((unsigned short)ucRxBuffer[5]<<8)|ucRxBuffer[4];
                    stcGyro.w[2] = ((unsigned short)ucRxBuffer[7]<<8)|ucRxBuffer[6];
                    break;
        case 0x53:  stcAngle.Angle[0] = ((unsigned short)ucRxBuffer[3]<<8)|ucRxBuffer[2];
                    stcAngle.Angle[1] = ((unsigned short)ucRxBuffer[5]<<8)|ucRxBuffer[4];
                    stcAngle.Angle[2] = ((unsigned short)ucRxBuffer[7]<<8)|ucRxBuffer[6];
                    stcAngle.T = ((unsigned short)ucRxBuffer[9]<<8)|ucRxBuffer[8];
                    break;
        case 0x54:  stcMag.h[0] = ((unsigned short)ucRxBuffer[3]<<8)|ucRxBuffer[2];
                    stcMag.h[1] = ((unsigned short)ucRxBuffer[5]<<8)|ucRxBuffer[4];
                    stcMag.h[2] = ((unsigned short)ucRxBuffer[7]<<8)|ucRxBuffer[6];
                    stcAngle.T = ((unsigned short)ucRxBuffer[9]<<8)|ucRxBuffer[8];
                    break;
        case 0x55:  stcDStatus.sDStatus[0] = ((unsigned short)ucRxBuffer[3]<<8)|ucRxBuffer[2];
                    stcDStatus.sDStatus[1] = ((unsigned short)ucRxBuffer[5]<<8)|ucRxBuffer[4];
                    stcDStatus.sDStatus[2] = ((unsigned short)ucRxBuffer[7]<<8)|ucRxBuffer[6];
                    stcDStatus.sDStatus[3] = ((unsigned short)ucRxBuffer[9]<<8)|ucRxBuffer[8];
                    break;
        case 0x56:  ucRxBuffer[2] = 0x12;ucRxBuffer[3] = 0x34;ucRxBuffer[4] = 0x56;ucRxBuffer[5] = 0x78;
                    CharToLong((char*)&stcPress.lPressure,(char*)&ucRxBuffer[2]);
                    CharToLong((char*)&stcPress.lAltitude,(char*)&ucRxBuffer[6]);
                    break;
        case 0x57:  CharToLong((char*)&stcLonLat.lLon,(char*)&ucRxBuffer[2]);
                    CharToLong((char*)&stcLonLat.lLat,(char*)&ucRxBuffer[6]);
                    break;
        case 0x58:  stcGPSV.sGPSHeight = ((unsigned short)ucRxBuffer[3]<<8)|ucRxBuffer[2];
                    stcGPSV.sGPSYaw = ((unsigned short)ucRxBuffer[5]<<8)|ucRxBuffer[4];
                    CharToLong((char*)&stcGPSV.lGPSVelocity,(char*)&ucRxBuffer[6]);
                    break;
    }
    ucRxCnt=0;
}

} `

ShellAlbert / 3-axies-robot-arm

111 #39