Android角速度传感器 角度传感器模块

文章目录

• 前言
• JY61简介

• 概述
• 通信协议

• 驱动文件

• JY61Driver.h
• JY61Recver.c
• JY61Cmder.c
• 依赖

• 使用示例
• 更新历史

前言

干活中用到了JY61姿态角度传感器，写了相应的驱动程序，放出来。

驱动被设计为拥有指令器和接收机两个部分，完全被动方式运行，与具体的平台解耦，只支持串口通讯。

JY61简介

概述

JY61姿态角度传感器 六轴模块

• 采用高精度的陀螺加速度计MPU6050，通过处理器读取MPU6050的测量数据然后通过串口输出，免去了用户自己去开发MPU6050复杂的IIC协议，同时精心的PCB布局和工艺保证了MPU6050收到外接的干扰最小，测量的精度最高。
• 模块内部自带电压稳定电路，可以兼容3.3V/5V的嵌入式系统，连接方便。
• 采用先进的数字滤波技术，能有效降低测量噪声，提高测量精度。
• 模块保留了MPU6050的IIC接口，以满足用户访问底层测量数据（加速度、角速度）的需求。
• 模块内部集成了姿态解算器，配合动态卡尔曼滤波算法，能够在动态环境下准确输出模块的当前姿态，姿态测量精度0.05度，稳定性极高。

通信协议

（1）串口通信参数

• 电平：TTL
• 波特率：9600/115200 bps
• 停止位：1
• 校验位0

（2）数据包
模块发送至上位机每帧数据分为3个数据包，分别为加速度包、角速度包和角度包，3个数据包顺序输出。波特率115200时每隔10ms输出一帧数据。

加速度包：

角速度包：

角度包：

（3）控制指令

以上都是从数据手册上复制黏贴下来的，很复杂吧。

但是没事！驱动程序已经都帮你搞定了！

驱动文件

JY61Driver.h

/*
*******************************************************************************************
*
*                                    JY61 DRIVER MODULE
*                                       JY61驱动模块
*
* File : JY61Driver.h
* By   : Lin Shijun()
* Date : 2020/05/03
* version: V1.0
* History: 2020/05/03 V1.0 the prototype
* Note   : The JY61 driver is divided into two part, i.e. cmder and recver. The cmder is for
*          sending the JY61 command. The recver is for resolving data from the JY61.
********************************************************************************************
*/

#ifndef JY61DRIVER_H
#define JY61DRIVER_H

/*
******************************************************************************************
*                                    INCLUDE
******************************************************************************************
*/

#include <stdint.h>

/*
******************************************************************************************
*                                    DEBUG CONFIGURATION
******************************************************************************************
*/

// to enable debug messages in this module
// #define JY61_DEBUG

/*
******************************************************************************************
*                                       TYPE DEFINE
******************************************************************************************
*/

typedef enum {
  JY61DT_Acc,  // Acceleration
  JY61DT_AgV,  // Angular Velocity
  JY61DT_Ang,  // Angle
} JY61DataType;

typedef struct JY61STRUCT_DATA{
  JY61DataType t;
  // m/s^2  when t = Acc
  // °/s   when t = AgV
  // °     when t = Ang
  float x;
  float y;
  float z;
  float temp;  // ℃
} JY61Data;

typedef void (* JY61FUNC_DATA)(const JY61Data * data);

/*
************************************************************************************
*                                    INTERFACES
************************************************************************************
*/

void JY61Cmder_Init(void (* outChannel)(uint8_t *buf, uint16_t len));
void JY61Cmder_Destroy(void);
void JY61Cmder_InitializeAngle(void);
void JY61Cmder_CalibrateAccelerometer(void);
void JY61Cmder_SleepOrWakeup(void);
void JY61Cmder_SerialOutput(void);
void JY61Cmder_IICOutput(void);
void JY61Cmder_BaudRate115200(void);
void JY61Cmder_BaudRate9600(void);
void JY61Cmder_HorizontalInstall(void);
void JY61Cmder_VerticalInstall(void);


void JY61Recver_Init(JY61FUNC_DATA onRecvData);
// Call it to modify the call-back function.
void JY61Recver_RegisterCallback(JY61FUNC_DATA onRecvData);
void JY61Recver_CleanupBuffer(void);
// Feed the receiver every byte received so that receiver can notify user
// the resolved data for each frame.
void JY61Recver_Feed(uint8_t nextByte);
void JY61Recver_Destroy(void);

#endif  // of JY61DRIVER_H

JY61Recver.c

/*
*******************************************************************************************
*
*                                   JY61 RECEIVER MODULE
*                                   JY61驱动模块 - 接收机
*
* File : JY61Recver.c
* By   : Lin Shijun()
* Date : 2020/05/03
* version: V1.0
* History: 2020/05/03 V1.0 the prototype
* Note   : 
********************************************************************************************
*/

/*
*********************************************************************************************
*                                       INCLUDES
*********************************************************************************************
*/

#include <string.h>
#include <ctype.h>
#include "JY61Driver.h"
#include "RxMac.h"

/*
*********************************************************************************************
*                                        CONSTANT
*********************************************************************************************
*/

#define JY61DF_LEN 11  // length of JY61 data frame

/*
*********************************************************************************************
*                                      TYPE DEFINITION
*********************************************************************************************
*/

typedef struct JY61FRAME_STRUCT{
  uint8_t headByte;  // 0x55
  uint8_t cmdByte;
  uint8_t xL;
  uint8_t xH;
  uint8_t yL;
  uint8_t yH;
  uint8_t zL;
  uint8_t zH;
  uint8_t TL;
  uint8_t TH;
  uint8_t Sum;
} JY61Frame;

typedef struct JY61RS_STRUCT{
  int16_t x;
  int16_t y;
  int16_t z;
  int16_t temp;
} JY61ResolvedInt;

/*
*********************************************************************************************
*                                       LOCAL FUNCTION
*********************************************************************************************
*/

#define ARRAYSIZE(arr)  (sizeof(arr)/ sizeof(arr[0]))
static void _onFlushed(RxMac sender,RxMacPtr buf,uint16_t len,RxState state,
                           RxFlag HorU,RxFlag Ender);

/*
*********************************************************************************************
*                                       LOCAL VARIABLE
*********************************************************************************************
*/

static const char * _str_JY61Recver = "JY61Recver";

static JY61FUNC_DATA _onRecvData;
static const uint8_t _flag_headerAcc[] = {(uint8_t)0x55, (uint8_t)0x51};
static const uint8_t _flag_headerAgV[] = {(uint8_t)0x55, (uint8_t)0x52};
static const uint8_t _flag_headerAng[] = {(uint8_t)0x55, (uint8_t)0x53};
static const RXFLAG_STRUCT _flags[] = {
  {_flag_headerAcc, sizeof(_flag_headerAcc), RXFLAG_OPTION_HEADER},
  {_flag_headerAgV, sizeof(_flag_headerAgV), RXFLAG_OPTION_HEADER},
  {_flag_headerAng, sizeof(_flag_headerAng), RXFLAG_OPTION_HEADER}
};
static RxMac _mac;
static uint8_t _macBuf[JY61DF_LEN];

/*
*********************************************************************************************
*                                 INTERFACE IMPLEMENTATION
*********************************************************************************************
*/

void JY61Recver_Init(JY61FUNC_DATA onRecvData){
  _onRecvData = onRecvData;
  _mac = RxMac_Create(_flags, ARRAYSIZE(_flags), _macBuf, sizeof(_macBuf),
                       NULL, NULL, _onFlushed);
  if(_mac == NULL)
    for(;;)
      ;
}

void JY61Recver_Destroy(){
  _onRecvData = NULL;
  RxMac_Destroy(_mac);
}

void JY61Recver_RegisterCallback(JY61FUNC_DATA onRecvData){
  _onRecvData = onRecvData;
}

void JY61Recver_CleanupBuffer(void){
  RxMac_ResetState(_mac);
}

void JY61Recver_Feed(uint8_t nextByte){
  RxMac_FeedData(_mac, nextByte);
}

/*
*********************************************************************************************
*                               LOCAL FUNCTION IMPLEMENTATION
*********************************************************************************************
*/

static int _frameSumRight(const uint8_t * frame){
  uint8_t sum = 0;
  int i;
  for(i = 0; i < 10; i++)
    sum += frame[i];
  return sum == frame[10];
}

static void _frameResolve(JY61ResolvedInt *rst, const JY61Frame *frame){
  rst->x = (((int16_t)frame->xH) << 8) | frame->xL;
  rst->y = (((int16_t)frame->yH) << 8) | frame->yL;
  rst->z = (((int16_t)frame->zH) << 8) | frame->zL;
  rst->temp = (((int16_t)frame->TH) << 8) | frame->TL;
}

static void _onFlushed(RxMac sender,RxMacPtr buf,uint16_t len,RxState state,
                       RxFlag HorU,RxFlag Ender){
  JY61Data data;
  JY61ResolvedInt rData;
  if(_onRecvData == NULL || !state.headerFound)
    return;
  if(!_frameSumRight((const uint8_t *)buf))
    return;

  _frameResolve(&rData, (const JY61Frame *)buf);

  data.x = rData.x / 32768.0f;
  data.y = rData.y / 32768.0f;
  data.z = rData.z / 32768.0f;
  switch (buf[1]){
  case 0x51:
    data.t = JY61DT_Acc;
    data.x *= 16;
    data.y *= 16;
    data.z *= 16;
    break;
  case 0x52:
    data.t = JY61DT_AgV;
    data.x *= 2000;
    data.y *= 2000;
    data.z *= 2000;
    break;
  case 0x53:
    data.t = JY61DT_Ang;
    data.x *= 180;
    data.y *= 180;
    data.z *= 180;
    break;
  default:
    // will never hit here
    return;
  }
  data.temp = (float)rData.temp / 340.0f + 36.53f;
  _onRecvData(&data);
}

JY61Cmder.c

/*
*******************************************************************************************
*
*                                    JY61 COMMANDER MODULE
*                                    JY61驱动模块 - 指令器
*
* File : JY61Cmder.c
* By   : Lin Shijun()
* Date : 2020/05/03
* version: V1.0
* History: 2020/05/03 V1.0 the prototype
* Note   : 
********************************************************************************************
*/

/*
*********************************************************************************************
*                                       INCLUDES
*********************************************************************************************
*/

#include <stdio.h>
#include "JY61Driver.h"

/*
*********************************************************************************************
*                                       LOCAL VARIABLE
*********************************************************************************************
*/
static void (* _out)(uint8_t *buf, uint16_t len) = NULL;

/*
*********************************************************************************************
*                                       CONSTANT
*********************************************************************************************
*/

#define JY61CMDBYTE_INITANGLE      0x52
#define JY61CMDBYTE_CALIACCMETER   0x67
#define JY61CMDBYTE_SLEEPORWAKE    0x60
#define JY61CMDBYTE_OUTPUTSERIAL   0x61
#define JY61CMDBYTE_OUTPUTIIC      0x62
#define JY61CMDBYTE_BAUDRATE115200 0x63
#define JY61CMDBYTE_BAUDRATE9600   0x64
#define JY61CMDBYTE_HORINSTALL     0x65
#define JY61CMDBYTE_VERINSTALL     0x66

/*
*********************************************************************************************
*                                 INTERFACE IMPLEMENTATION
*********************************************************************************************
*/

void JY61Cmder_Init(void (* outChannel)(uint8_t *buf, uint16_t len)){
  _out = outChannel;
}

void JY61Cmder_Destroy(){
  _out = NULL;
}

static void _sendCmd(uint8_t cmdByte){
  static uint8_t frame[3] = {0xFF, 0xAA};
  if(_out == NULL)
    return;
  frame[2] = cmdByte;
  _out(frame, sizeof(frame));
}

void JY61Cmder_InitializeAngle(){
  _sendCmd(JY61CMDBYTE_INITANGLE);
}

void JY61Cmder_CalibrateAccelerometer(void){
  _sendCmd(JY61CMDBYTE_CALIACCMETER);
}

void JY61Cmder_SleepOrWakeup(void){
  _sendCmd(JY61CMDBYTE_SLEEPORWAKE);
}

void JY61Cmder_SerialOutput(void){
  _sendCmd(JY61CMDBYTE_OUTPUTSERIAL);
}

void JY61Cmder_IICOutput(void){
  _sendCmd(JY61CMDBYTE_OUTPUTIIC);
}

void JY61Cmder_BaudRate115200(void){
  _sendCmd(JY61CMDBYTE_BAUDRATE115200);
}

void JY61Cmder_BaudRate9600(void){
  _sendCmd(JY61CMDBYTE_BAUDRATE9600);
}

void JY61Cmder_HorizontalInstall(void){
  _sendCmd(JY61CMDBYTE_HORINSTALL);
}

void JY61Cmder_VerticalInstall(void){
  _sendCmd(JY61CMDBYTE_VERINSTALL);
}

依赖

接收器的实现依赖于我写的通用接收机：
通用接收状态机模块

使用示例

使用此模块基本分几步：

1. 初始化模块
2. 如需要发送指令则实现并注册信道给指令器；注册回调函数给接收器，这样接收器在发现完整的数据帧时就会通过回调函数进行通知。
3. 不断接收数据，并喂(Feed)给接收器；如需要发指令，随时调用指令器的接口。

以下代码示例了在上电后发送校准指令，然后进入自动模式，之后不断读取数据并进行解析(假设使用标准输入输出上的串口和JY61通信)：

#include "JY61Driver.h"
#include <stdio.h>
#include <stdint.h>
……
// 收到数据的回调函数
static void onDataResolved(const JY61Data * data);
// 发送信道
static void sendChannel(uint8_t *buf, uint16_t len);

void main(){
  uint8_t b;
  ……
  JY61Recver_Init(onDataResolved);
  JY61Cmder_Init(sendChannel);
  
  // 可以发送各种命令，比如
  // JY61Cmder_HorizontalInstall();
  
  for(;;){
    // 不断得到下一个字节并喂给接收机
    // 接收机会通过回调函数实时传递解析出来的值
    b = (uint8_t)getchar();
    JY61Recver_Feed(b);
  }
}

static void onDataResolved(const JY61Data * data){
  // 这里会得到实时解析到的各个包的数据
  switch(data->t){
    case JY61DT_Acc:
      // 加速度包的处理
    break;
    case JY61DT_AgV:
      // 角速度包的处理
    break;
    case JY61DT_Ang:
      // 角度包的处理
    break;
    default:
      // 不应该会到这
    break;
  }
}

static void sendChannel(uint8_t *buf, uint16_t len){
  // 往标准输出输出这个字节
  while(len-- > 0)
    putchar((char)*buf++);
}