C++:从2补码计算加速度值

dw1jzc5e  于 2023-03-14  发布在  其他
关注(0)|答案(3)|浏览(147)

我正在从Arduino IDE切换到ESP IDF,我想测试一个加速度计(LIS 2DW 12)。最初,我在硬件相关的编程中遇到了一些问题。在此期间,我得到了很好的值,但我需要一个10的因子。它是一个14位的值,存储在两个8位寄存器中。我搜索了一些论坛,并为此工作了几天。但我一直未能找到,为什么我错了10倍。我做错了什么?我很感激任何参考!RegisterLIS2DW12 Datasheet

#include <stdio.h>
#include "esp_log.h"
#include "driver/i2c.h"
#include "Arduino.h"
#include "string.h"

static const char *TAG = "LIS2DW12: ";

#define I2C_MASTER_SCL_IO           22             /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO           21             /*!< GPIO number used for I2C master data  */
#define I2C_MASTER_NUM              1              /*!< I2C master i2c port number, the number of i2c peripheral interfaces available will depend on the chip */
#define I2C_MASTER_FREQ_HZ          400000         /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE   0              /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0              /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS       1000

// Sensor Reg
#define SENSOR_ADDR                 0x19        /*!< Slave address of the MPU9250 sensor */
#define WHO_AM_I_REG_ADDR           0x0F        /*!< Register addresses of the "who am I" register */

#define Temp_L 0x0D
#define Temp_H 0x0E
#define WhoAmI 0x0F
#define CTRL1  0x20
#define CTRL2  0x21
#define CTRL3  0x22
#define CTRL4  0x23
#define CTRL5  0x24
#define CTRL6  0x25

#define X_LSB 0x28
#define X_MSB 0x29
#define Y_LSB 0x2A
#define Y_MSB 0x2B
#define Z_LSB 0x2C
#define Z_MSB 0x2D

uint16_t X_raw, Y_raw, Z_raw;
float X, Y, Z;
int ix,iy,iz;

void Sensor_data_test(){
    
    byte buf[2];
    
    // read register
    register_read(Z_LSB, buf, 2);

    Z_raw = buf[1]<<8 | buf[0];        // buf = MSB <<8 | LSB;

    // print buffer
    Serial.println("Sensor Data:");   
    Serial.print("buf0: ");Serial.print(buf[0]); 
    Serial.println();
    Serial.print("buf1: ");Serial.print(buf[1]); 
    Serial.println();
    Serial.print("Z uint16_t : ");Serial.print(Z_raw); 
    Serial.println();

    // calculate binary and print
    uint16_t Zh = Z_raw;
    String hz;
    while(Zh!=0) 
    {
        hz=(Zh%2==0 ?"0":"1")+hz; 
        Zh/=2; 
    }
    Serial.print("Z bin : ");Serial.println(hz); 

    //calculate 2'er complement
    int k = 0;
    if(Z_raw & (((uint16_t)0x1) << 15))     //check if binary is negativ
    {                                       //if negativ, do
        Z_raw = ~Z_raw;                     //invert bits
        Z_raw = Z_raw + 1;                  //add one bit
        Z_raw = Z_raw >> 2;                 //shift 2 spaces to the rigt (caused by Sensor register)          
        k = Z_raw * -1;                     //save as Integer an reverse sign
    }
    else{                                   //if positiv, do
        Z_raw = Z_raw >> 2;                 //shift to spaces to the rigt (caused by Sensor register)
        k = Z_raw;                          //save as Integer
    }

    if (k){ 
        Serial.print("Z int: ");Serial.print(k); 
        Serial.println();
    }

    //acceleration value ============================================

    Z= (float)k *16/8191;   // factor 10 needed?

    Serial.print("Z accel: ");Serial.print(Z); 
    Serial.println();

}

void Set_Sensor_Config(){
    register_write_byte(CTRL1,0x55);
    register_write_byte(CTRL2,0x04);
    register_write_byte(CTRL3,0x00);
    register_write_byte(CTRL4,0x00);
    register_write_byte(CTRL5,0x00);
    register_write_byte(CTRL6,0x34);
}

void Show_Sensor_Config(){
    uint8_t buf[1];
    ESP_LOGI(TAG, "LIS12DW Config: ");
    register_read(CTRL1, buf, 1);
    ESP_LOGI(TAG, "CTRL1 = %X", buf[0]);
    register_read(CTRL2, buf, 1);
    ESP_LOGI(TAG, "CTRL2 = %X", buf[0]);
    register_read(CTRL3, buf, 1);
    ESP_LOGI(TAG, "CTRL3 = %X", buf[0]);
    register_read(CTRL4, buf, 1);
    ESP_LOGI(TAG, "CTRL4 = %X", buf[0]);
    register_read(CTRL5, buf, 1);
    ESP_LOGI(TAG, "CTRL5 = %X", buf[0]);
    register_read(CTRL6, buf, 1);
    ESP_LOGI(TAG, "CTRL6 = %X", buf[0]);
}

extern "C" void app_main()
{
    Serial.begin(115200);

    ESP_ERROR_CHECK(i2c_master_init());
    ESP_LOGI(TAG, "I2C initialized successfully");

    Set_Sensor_Config();
    Show_Sensor_Config();

    Sensor_data_test();

    ESP_ERROR_CHECK(i2c_driver_delete(I2C_MASTER_NUM));
    ESP_LOGI(TAG, "I2C de-initialized successfully");
    
    
}

输出:

I (336) LIS2DW12: : LIS12DW Config: 
I (336) LIS2DW12: : CTRL1 = 55
I (337) LIS2DW12: : CTRL2 = 4
I (341) LIS2DW12: : CTRL3 = 0
I (344) LIS2DW12: : CTRL4 = 0
I (348) LIS2DW12: : CTRL5 = 0
I (351) LIS2DW12: : CTRL6 = 34
Sensor Data:
buf0: 40
buf1: 248
Z uint16_t : 63528
Z bin : 1111100000101000
Z int: -502
Z accel: -0.98
I (367) LIS2DW12: : I2C de-initialized successfully

我编写了一个函数,读取Z加速度寄存器并将数据转换为值[m/s²],我期望值约为9.81,但得到的值为0.98(重力加速度)。
编辑:我忘了说我把范围设置成了16 G。而且分辨率是13 bit(8191)。

Z= (float)k *16/8191;   // factor 10 needed?
abithluo

abithluo1#

数据手册上说所有测量值都是微g,所以我想知道你的0.98阅读是0.98g还是9.61m/s²。

jdg4fx2g

jdg4fx2g2#

我没有一个完整的答案,因为我没有这个传感器可用,但我会从重构您的代码开始,因为它可能是您提取轴值的方式,导致最终计算失败。OUT_Z_L和OUT_Z_H的寄存器告诉您,它们各自提供8位,产生16位2s恭维,基本上是int16_t。您试图将它们当作uint16_t来使用,实际上它们不是,并手动转换它们,这总是容易出错,肯定会导致一些奇怪的行为。使用uint8_t,char,提取寄存器数据作为缓冲区。但当您合并MSB和LSB时,应使用正确的类型:int16_t。
Here是一个git存储库,其中包含STMicro为该传感器编写的驱动程序代码,它是用C编写的,但很容易转换为C++,或者您也可以根据需要使用它(不过我不喜欢STMicro代码,所以我总是自己写)。他们通过一个名为lis2dw12_acceleration_raw_get的方法提取X、Y和Z寄存器,他们一次得到所有寄存器,但这应该是一个很好的参考,因为他们制造了传感器。它还为您提供了提取寄存器和获取每个寄存器的正确值的方法。以下是我从他们的存储库中引用的代码

int32_t lis2dw12_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val)
{
  uint8_t buff[6];
  int32_t ret;

  ret = lis2dw12_read_reg(ctx, LIS2DW12_OUT_X_L, buff, 6);
  val[0] = (int16_t)buff[1];
  val[0] = (val[0] * 256) + (int16_t)buff[0];
  val[1] = (int16_t)buff[3];
  val[1] = (val[1] * 256) + (int16_t)buff[2];
  val[2] = (int16_t)buff[5];
  val[2] = (val[2] * 256) + (int16_t)buff[4];

  return ret;
}
w8f9ii69

w8f9ii693#

我正准备写它仍然不工作。但是当我试着写我的问题和格式化我的代码时,我发现了我的错误。感谢deviantgeek的帮助。我按照示例代码,终于让它工作了。
你不能/不应该手动处理这个问题。因为即使我还不能理解这些因素。如果有人得到一个不正确的值,那么看看这个函数“lis2dw12_from_fs2_to_mg”。有各种各样的因素取决于,例如,范围(2g,4g,...)我用这个代码作为“模板”:lis2dw12_read_data_single.c
此代码适用于16 g。它只测量Z,但其他方向相同

int32_t accel_z_raw_get(int16_t *val)
{
    uint8_t buff[2];
    int32_t ret;
    if(STATUS & 0x01){     //check if Data ready to read // #define STATUS 0x27

        ret = register_read(Z_LSB, buff, 2);
        *val = (int16_t)buff[1];
        *val = (*val * 256) + (int16_t)buff[0];
      }
      return ret;
}

float_t lis2dw12_from_fs2_lp1_to_mg(int16_t lsb)
{
  return ((float_t)lsb) * 0.488f;
}

static int16_t data_raw_z[1];
static float accel_z_mg[1];

void get_sensor_data(){

    char str[25];

    accel_z_raw_get(data_raw_z);

    accel_z_mg[0] = lis2dw12_from_fs2_lp1_to_mg(data_raw_z[0]);

    sprintf((char *)str, "acceleration Z: % 4.2f\t\r\n", accel_z_mg[0]);

    puts(str);

}

extern "C" void app_main()
{
    Serial.begin(115200);

    ESP_ERROR_CHECK(i2c_master_init());
    ESP_LOGI(TAG, "I2C initialized successfully");

    Set_Sensor_Config();
    
    Show_Sensor_Config();

    get_sensor_data();

    ESP_ERROR_CHECK(i2c_driver_delete(I2C_MASTER_NUM));
    ESP_LOGI(TAG, "I2C de-initialized successfully");
}

输出为:-981.86这是正确的,因为传感器的测量单位是毫克。负号是可以的,因为传感器是颠倒的...

I (336) LIS2DW12-Register: LIS12DW Config: 
I (336) LIS2DW12-Register: CTRL1 = 55
I (338) LIS2DW12-Register: CTRL2 = 4
I (342) LIS2DW12-Register: CTRL3 = 0
I (347) LIS2DW12-Register: CTRL4 = 0
I (351) LIS2DW12-Register: CTRL5 = 0
I (355) LIS2DW12-Register: CTRL6 = 34
I (359) LIS2DW12-Register: STATUS = 1
acceleration Z: -981.86

谢谢你的帮助!这个论坛太棒了!

相关问题