littleshrimp 发表于 2017-6-25 13:59

使用lis3dsh加速度传感器计算倾角

使用3轴加速度传感器lis3dsh读取加速度数据，转换成倾角程序
单片机使用MSP430FR5969软I2C



单片机程序
/*******************************************************************************
* Copyright (C), 2017, XiaCheDan Tech. Co., Ltd.
* FileName: main.c
* Author: littleshrimp
* Version : 1.0
* Date:2017-06-25 13:55
* Description:
* Function List: //
* 1. -------
* History:
* <author>   <time>                <version ><desc>
* littleshrimp      2017-06-25 13:55      build this moudle
******************************************************************************/
/*

            MSP430FR5969
            -----------------
         |   P2.0/UCA0TXD|----> PC
         |               |
         |               |
         |   P2.1/UCA0RXD|<---- PC
         |               |
SCL --o--|P3.5             |
         |               |
SDA --o--|P3.6             |

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

#include "msp430.h"
#include <stdio.h>
#include <math.h>
#include <stdint.h>
#include "uart.h"
#include "soft_i2c.h"
#include "timer.h"
/*******************************************************************************
** MACROS
**/

/*******************************************************************************
** CONSTANTS
**/
#define BASE64      0
#define STRING      1
/*******************************************************************************
** TYPEDEFS
**/

/*******************************************************************************
** GLOBAL VARIABLES
**/

/*******************************************************************************
** EXTERNAL VARIABLES
**/

/*******************************************************************************
** EXTERNAL FUNCTIONS
**/

/*******************************************************************************
** LOCAL VARIABLES
**/

uint8_t data;
uint16_t temp_code;
float temperature;
uint8_t crc;
/*******************************************************************************
** PROFILE CALLBACKS
**/

/*******************************************************************************
** LOCAL FUNCTIONS
**/

/*******************************************************************************
** PUBLIC FUNCTIONS
**/

   
   
#define LIS3DSH_SLAVE_ADDR   (0x1E << 1)//7bit
#define LIS3DSH_WHO_I_AM       0x0F
#define LIS3DSH_OUT_T          0x0C
#define LIS3DSH_STATUS         0x27
#define LIS3DSH_CTRL_REG4      0x20
#define LIS3DSH_CTRL_REG3      0x23
#define LIS3DSH_CTRL_REG5      0x24
#define LIS3DSH_CTRL_REG6      0x25
uint8_t buf;
void lis3dsh_init(void)
{
    uint8_t data;
    i2c_init();//初始化I2C
    i2c_read_n_byte(LIS3DSH_SLAVE_ADDR,LIS3DSH_WHO_I_AM,&buf,1);
//    data = 0x17;//0001:3.125Hz 0:continuous update 111:x,y,z enable
    data = 0x97;//1001:1600Hz 0:continuous update 111:x,y,z enable
    i2c_write_n_byte(LIS3DSH_SLAVE_ADDR,LIS3DSH_CTRL_REG4,&data,1);
    i2c_read_n_byte(LIS3DSH_SLAVE_ADDR,LIS3DSH_OUT_T,&buf,1);
    i2c_read_n_byte(LIS3DSH_SLAVE_ADDR,LIS3DSH_STATUS,&buf,7);
}
void dco_init(void)
{
// Configure one FRAM waitstate as required by the device datasheet for MCLK
// operation beyond 8MHz _before_ configuring the clock system.
FRCTL0 = FRCTLPW | NWAITS_1;

// Clock System Setup
CSCTL0_H = CSKEY >> 8;                  // Unlock CS registers
CSCTL1 = DCORSEL | DCOFSEL_4;             // Set DCO to 16MHz
CSCTL2 = SELA__VLOCLK | SELS__DCOCLK | SELM__DCOCLK; // Set SMCLK = MCLK = DCO,
                                          // ACLK = VLOCLK
CSCTL3 = DIVA__1 | DIVS__1 | DIVM__1;   // Set all dividers
CSCTL0_H = 0;                           // Lock CS registers
}
int main( void )
{
uint32_t i = 0;
float x = 0,y = 0,z = 0;
float ax,ay,az;
uint8_t status;
char inBuf;//base64缓存
char outBuf;//base64缓存
WDTCTL = WDTPW + WDTHOLD;//停止看门狗
PM5CTL0 &= ~LOCKLPM5;//这个是一定要加的，不然GPIO不能使用
dco_init();
lis3dsh_init();
uart_init();
while(1)
{
   
    i2c_read_n_byte(LIS3DSH_SLAVE_ADDR,LIS3DSH_STATUS,&buf,7);

#if (BASE64 == 1)    //base 64输出
    base64_encode((const uint8_t *)(&buf),outBuf,7); //做BASE64转换
    uart_tx_string(outBuf);//输出到PC
    uart_tx_string("\r\n");//换行
#elif (STRING == 1)       //字符串输出    status = buf;//status
    ax = (buf + (buf << 8)) * 0.06 + 2;
    ay = (buf + (buf << 8)) * 0.06 + 2;
    az = (buf + (buf << 8)) * 0.06 + 2;
   
    x = atan((float)ax/(float)sqrt(ay*ay+az*az)) * 180.00 / 3.1415926;
    y = atan((float)ay/(float)sqrt(ax*ax+az*az)) * 180.00 / 3.1415926;
    z = atan((float)az/(float)sqrt(ax*ax+ay*ay)) * 180.00 / 3.1415926;
   
    if (z < 0)
    {
      x= 180-x;
      y= 180-y;
    }
   
    sprintf(outBuf,"angle:\tx=%.2f\ty=%.2f\tz=%.2f\r\n",x,y,z);
//    sprintf(outBuf,"%d",i++);
    uart_tx_string(outBuf);//输出到PC
    uart_tx_string("\r\n");//换行
    sleep(650);
#else                      //二进制输出
    outBuf = 0x55;
    outBuf = buf;
    outBuf = buf;
    outBuf = buf;
    outBuf = buf;
    outBuf = buf;
    outBuf = buf;
    uart_tx_bytes(outBuf,7);//输出到PC
#endif
}
}


上位机演示
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Drawing.Drawing2D;
using System.IO.Ports;
using System.Threading;

namespace 倾角演示
{
    public partial class Form1 : Form
    {
      Bitmap bmp = new Bitmap(Image.FromFile("img.png"));
      SerialPort serialPort = new SerialPort();//串口
      public Form1()
      {
            InitializeComponent();
            UpdateSerialPorts();
      }
      void UpdateSerialPorts()
      {
            String[] ports = System.IO.Ports.SerialPort.GetPortNames();//获得全部串口
            if (ports.Length == 0) return;//如果串口数量为0退出
            comboBox1.Items.Clear();//清除原有comboBox中的数据后重新添加
            comboBox1.Items.AddRange(ports);//添加串口
            comboBox1.Text = ports;//设置显示最后一个串口
      }
      /// <summary>
      /// 以逆时针为方向对图像进行旋转
      /// </summary>
      /// <param name="b">位图流</param>
      /// <param name="angle">旋转角度(前台给的)</param>
      /// <returns></returns>
      public Bitmap Rotate(Bitmap b, int angle)
      {
            angle = angle % 360;

            //弧度转换
            double radian = angle * Math.PI / 180.0;
            double cos = Math.Cos(radian);
            double sin = Math.Sin(radian);

            //原图的宽和高
            int w = b.Width;
            int h = b.Height;
            int W = (int)(Math.Max(Math.Abs(w * cos - h * sin), Math.Abs(w * cos + h * sin)));
            int H = (int)(Math.Max(Math.Abs(w * sin - h * cos), Math.Abs(w * sin + h * cos)));

            //目标位图
            Bitmap dsImage = new Bitmap(W, H);
            Graphics g = Graphics.FromImage(dsImage);

            //计算偏移量
            Point Offset = new Point((W - w) / 2, (H - h) / 2);

            //构造图像显示区域：让图像的中心与窗口的中心点一致
            Rectangle rect = new Rectangle(Offset.X, Offset.Y, w, h);
            Point center = new Point(rect.X + rect.Width / 2, rect.Y + rect.Height / 2);
            g.TranslateTransform(center.X, center.Y);
            g.RotateTransform(360 - angle);

            //恢复图像在水平和垂直方向的平移
            g.TranslateTransform(-center.X, -center.Y);
            g.DrawImage(b, rect);

            //重至绘图的所有变换
            g.ResetTransform();

            g.Save();
            g.Dispose();
            return dsImage;
      }
      private void Form1_Load(object sender, EventArgs e)
      {
            pictureBox1.Image = bmp;
      }
      int i = 0;
      private void button1_Click(object sender, EventArgs e)
      {
            serialPort.PortName = comboBox1.Text;//串口名为当前comboBox的本
            serialPort.BaudRate = 115200;//速率
            serialPort.DataBits = 8;//停止位
            serialPort.Parity = System.IO.Ports.Parity.None;
            serialPort.StopBits = System.IO.Ports.StopBits.One;
            serialPort.Handshake = System.IO.Ports.Handshake.None;
//            serialPort.DataReceived += new System.IO.Ports.SerialDataReceivedEventHandler(serialPort_DataReceived);
            serialPort.Open();//打开串口
            timer1.Enabled = true;
      }

      private void comboBox1_DropDown(object sender, EventArgs e)
      {
            UpdateSerialPorts();
      }
      public void serialPort_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
      {
      }
      private void Form1_FormClosing(object sender, FormClosingEventArgs e)
      {
            Application.Exit();//退出
            System.Environment.Exit(0);//退出时关闭所有线程
      }

      private void timer1_Tick(object sender, EventArgs e)
      {
            float x = 0, y = 0, z = 0;
            try
            {
                if (serialPort.BytesToRead < 10) return;
                string str = serialPort.ReadLine();
                string[] s = str.Split('\t');
                if (s.Length >= 4)
                {
                  s = s.Replace("x=", "");
                  s = s.Replace("y=", "");
                  s = s.Replace("z=", "");
                  s = s.Replace("\r", "");
                  s = s.Replace("\n", "");
                  x = float.Parse(s);
                  y = float.Parse(s);
                  z = float.Parse(s);
                  pictureBox1.BeginInvoke((ThreadStart)delegate
                  {
                        pictureBox1.Image = Rotate(bmp, (int)y);
                        pictureBox1.Refresh();
                  });
                }
            }
            catch (Exception ex)
            {
            }
      }
    }
}



dcexpert 发表于 2017-6-25 14:41

又开始玩430了？

电子微创意 发表于 2017-6-25 15:35

{:1_103:}

littleshrimp 发表于 2017-6-25 15:43

dcexpert 发表于 2017-6-25 14:41
又开始玩430了？

430用起来方便

huaiqiao 发表于 2017-6-26 09:46

还记得ST的F072 discovery的板子上有个L3GD20，这个是个三轴的Gyro，我上次弄了一半，还没完全弄好。但是，事情多，后面就先丢一边就没有分享了。。。。

littleshrimp 发表于 2017-6-26 10:04

huaiqiao 发表于 2017-6-26 09:46
还记得ST的F072 discovery的板子上有个L3GD20，这个是个三轴的Gyro，我上次弄了一半，还没完全弄好。但是， ...

我开始是想弄个ST的9轴或者6轴玩的，在淘宝上找了一下模块都挺贵的
样品也不太好申请

ababzx 发表于 2017-7-3 14:09

顶{:1_102:}{:1_102:}{:1_102:}{:1_102:}{:1_102:}{:1_102:}{:1_102:}{:1_102:}

nmg 发表于 2017-7-17 11:04

{:1_102:}迟到的顶帖，这个驱动是你自己写的吗？

nmg 发表于 2017-7-17 11:05

:sexy:顺便推荐一下这个资源

LIS3DSH（三轴加速度计）驱动例程ver1.1
https://bbs.eeworld.com.cn/thread-544224-1-1.html

littleshrimp 发表于 2017-7-17 11:57

nmg 发表于 2017-7-17 11:04
迟到的顶帖，这个驱动是你自己写的吗？

只对传感器做了简单配置

jack4103 发表于 2017-10-19 11:43

顶一下

JimmyKudo 发表于 2018-7-26 16:51

您好，楼主，可以把pc体态展示软件或者源码共享下吗，谢谢，sun.wei162@zte.com.cn

littleshrimp 发表于 2018-7-26 21:37

JimmyKudo 发表于 2018-7-26 16:51
您好，楼主，可以把pc体态展示软件或者源码共享下吗，谢谢，sun.wei162@zte.com.cn


JimmyKudo 发表于 2018-7-27 11:29

感谢蛋总

381503442 发表于 2018-8-15 15:42

不是静止或者匀速运动情况下用加速度计计算角度是不准确的吧，一般不会这样计算吧

littleshrimp 发表于 2018-8-15 16:06

381503442 发表于 2018-8-15 15:42
不是静止或者匀速运动情况下用加速度计计算角度是不准确的吧，一般不会这样计算吧

这要看希望达到什么样的精度

381503442 发表于 2018-8-15 18:47

littleshrimp 发表于 2018-8-15 16:06
这要看希望达到什么样的精度

目前需要做运动手表上抬手亮屏和翻手亮屏的算法，原理上参考传感器自带的用加速度计直接计算角度然后比较旋转角度的方式，但是考虑到运动特别是跑动运动时这种方式计算的角度肯定有误差，不知道版主能否评估下这种误差是否在可接受的范围内，谢谢

littleshrimp 发表于 2018-8-16 10:25

381503442 发表于 2018-8-15 18:47
目前需要做运动手表上抬手亮屏和翻手亮屏的算法，原理上参考传感器自带的用加速度计直接计算角度然后比较 ...

我没有相应的环境没法帮你评估不同运动模式对AWT功能的影响
你可以参考一下这个文档的第72页，尝试一下不同的配置试试
https://www.st.com/resource/en/application_note/dm00402563.pdf

JimmyKudo 发表于 2018-9-4 23:32

littleshrimp 发表于 2018-8-16 10:25
我没有相应的环境没法帮你评估不同运动模式对AWT功能的影响
你可以参考一下这个文档的第72页，尝试一下 ...

你好，我用LIS2DS12三轴位移加速度推算倾角，再有外力作用下，角度和实际偏差比较大（附件拍了视频），请问如果用三轴角加速度，即陀螺仪，推算倾角是不是没有这个问题？
目前需求：一个物体常态是水平的，当这个物体倾斜大于15度（可设置）时，传感器能产生一个中断信号，我大概评估下位移加速度传感器不太好实现（需要超低功耗，mcu休眠，mems中断唤醒方式，需要mems支持配置角度达到阈值产生硬件中断），请教下，ST那款mems比较合适，陀螺仪可以做到吗？功耗有100uA以下的吗？谢谢~

littleshrimp 发表于 2018-9-5 14:59

JimmyKudo 发表于 2018-9-4 23:32
你好，我用LIS2DS12三轴位移加速度推算倾角，再有外力作用下，角度和实际偏差比较大（附件拍了视频），请 ...

角速度传感器（陀螺仪）一搬功耗比较大，检测倾角需要很高的速率，再加上MCU处理，100uA肯定做不到
角速度传感器测倾角还存在长期漂移问题，也不是很适合
加速度传感器是测量倾角的理想选择，使用集成AWT功能的传感器还可以省去MCU的功耗
https://bbs.eeworld.com.cn/forum.php?mod=viewthread&tid=652398&highlight=%B7%AD%CD%F3
在有振动的环境里（例如检测快递车里的快递倾斜角度）会受振动影响
解决办法可以使用大一点的量程（会牺牲分辨率），使用高一点的输出速率（牺牲功耗），再使用MCU对采集到的数据做处理（牺牲功耗）通过算法消除外力的影响，做好了功耗应该可以控制在100uA以内。
页: [1] 2
查看完整版本: 使用lis3dsh加速度传感器计算倾角