mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 22:23:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensord: add helper function verify_chip_id to I2CSensor (#25830)

Browse Source
This commit is contained in:
Dean Lee
2022-12-13 07:00:07 +08:00
committed by GitHub
parent f49520db0f
commit 6acf4e26b8
9 changed files with 35 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
selfdrive/sensord/sensors/bmx055_accel.cc
View File

@@ -9,20 +9,8 @@
BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Accel::init() {
int ret = 0;
uint8_t buffer[1];
ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1);
if(ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
goto fail;
}
if(buffer[0] != BMX055_ACCEL_CHIP_ID) {
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID);
ret = -1;
goto fail;
}
int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID});
if (ret == -1) return -1;
ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
if (ret < 0) {

16
selfdrive/sensord/sensors/bmx055_gyro.cc
View File

@@ -12,20 +12,8 @@
BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Gyro::init() {
int ret = 0;
uint8_t buffer[1];
ret =read_register(BMX055_GYRO_I2C_REG_ID, buffer, 1);
if(ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
goto fail;
}
if(buffer[0] != BMX055_GYRO_CHIP_ID) {
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_GYRO_CHIP_ID);
ret = -1;
goto fail;
}
int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID});
if (ret == -1) return -1;
ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE);
if (ret < 0) {

15
selfdrive/sensord/sensors/bmx055_magn.cc
View File

@@ -66,8 +66,6 @@ static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t
BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Magn::init() {
int ret;
uint8_t buffer[1];
uint8_t trim_x1y1[2] = {0};
uint8_t trim_x2y2[2] = {0};
uint8_t trim_xy1xy2[2] = {0};
@@ -78,25 +76,18 @@ int BMX055_Magn::init() {
uint8_t trim_xyz1[2] = {0};
// suspend -> sleep
ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
if(ret < 0) {
LOGE("Enabling power failed: %d", ret);
goto fail;
}
util::sleep_for(5); // wait until the chip is powered on
// read chip ID
ret = read_register(BMX055_MAGN_I2C_REG_ID, buffer, 1);
if(ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID});
if (ret == -1) {
goto fail;
}
if(buffer[0] != BMX055_MAGN_CHIP_ID) {
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_MAGN_CHIP_ID);
return -1;
}
// Load magnetometer trim
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
if(ret < 0) goto fail;

18
selfdrive/sensord/sensors/bmx055_temp.cc
View File

@@ -9,23 +9,7 @@
BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Temp::init() {
int ret = 0;
uint8_t buffer[1];
ret = read_register(BMX055_ACCEL_I2C_REG_ID, buffer, 1);
if(ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
goto fail;
}
if(buffer[0] != BMX055_ACCEL_CHIP_ID) {
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], BMX055_ACCEL_CHIP_ID);
ret = -1;
goto fail;
}
fail:
return ret;
return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0;
}
bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {

16
selfdrive/sensord/sensors/i2c_sensor.h
View File

@@ -2,12 +2,12 @@
#include <cstdint>
#include <unistd.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/i2c.h"
#include "common/gpio.h"
#include "common/swaglog.h"
#include "selfdrive/sensord/sensors/constants.h"
#include "selfdrive/sensord/sensors/sensor.h"
@@ -33,4 +33,18 @@ public:
virtual int init() = 0;
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
virtual int shutdown() = 0;
int verify_chip_id(uint8_t address, const std::vector<uint8_t> &expected_ids) {
uint8_t chip_id = 0;
int ret = read_register(address, &chip_id, 1);
if (ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
return -1;
}
for (int i = 0; i < expected_ids.size(); ++i) {
if (chip_id == expected_ids[i]) return chip_id;
}
LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]);
return -1;
}
};

17
selfdrive/sensord/sensors/lsm6ds3_accel.cc
View File

@@ -118,8 +118,6 @@ int LSM6DS3_Accel::self_test(int test_type) {
}
int LSM6DS3_Accel::init() {
int ret = 0;
uint8_t buffer[1];
uint8_t value = 0;
bool do_self_test = false;
@@ -128,19 +126,10 @@ int LSM6DS3_Accel::init() {
do_self_test = true;
}
ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1);
if(ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
goto fail;
}
int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID});
if (ret == -1) return -1;
if(buffer[0] != LSM6DS3_ACCEL_CHIP_ID && buffer[0] != LSM6DS3TRC_ACCEL_CHIP_ID) {
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_ACCEL_CHIP_ID);
ret = -1;
goto fail;
}
if (buffer[0] == LSM6DS3TRC_ACCEL_CHIP_ID) {
if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}

17
selfdrive/sensord/sensors/lsm6ds3_gyro.cc
View File

@@ -107,8 +107,6 @@ int LSM6DS3_Gyro::self_test(int test_type) {
}
int LSM6DS3_Gyro::init() {
int ret = 0;
uint8_t buffer[1];
uint8_t value = 0;
bool do_self_test = false;
@@ -117,19 +115,10 @@ int LSM6DS3_Gyro::init() {
do_self_test = true;
}
ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1);
if(ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
goto fail;
}
int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID});
if (ret == -1) return -1;
if(buffer[0] != LSM6DS3_GYRO_CHIP_ID && buffer[0] != LSM6DS3TRC_GYRO_CHIP_ID) {
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_GYRO_CHIP_ID);
ret = -1;
goto fail;
}
if (buffer[0] == LSM6DS3TRC_GYRO_CHIP_ID) {
if (ret == LSM6DS3TRC_GYRO_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}

22
selfdrive/sensord/sensors/lsm6ds3_temp.cc
View File

@@ -8,27 +8,13 @@
LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
int LSM6DS3_Temp::init() {
int ret = 0;
uint8_t buffer[1];
int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID});
if (ret == -1) return -1;
ret = read_register(LSM6DS3_TEMP_I2C_REG_ID, buffer, 1);
if(ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
goto fail;
}
if(buffer[0] != LSM6DS3_TEMP_CHIP_ID && buffer[0] != LSM6DS3TRC_TEMP_CHIP_ID) {
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_TEMP_CHIP_ID);
ret = -1;
goto fail;
}
if (buffer[0] == LSM6DS3TRC_TEMP_CHIP_ID) {
if (ret == LSM6DS3TRC_TEMP_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}
fail:
return ret;
return 0;
}
bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {

16
selfdrive/sensord/sensors/mmc5603nj_magn.cc
View File

@@ -8,20 +8,8 @@
MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
int MMC5603NJ_Magn::init() {
int ret = 0;
uint8_t buffer[1];
ret = read_register(MMC5603NJ_I2C_REG_ID, buffer, 1);
if(ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
goto fail;
}
if(buffer[0] != MMC5603NJ_CHIP_ID) {
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], MMC5603NJ_CHIP_ID);
ret = -1;
goto fail;
}
int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
if (ret == -1) return -1;
// Set 100 Hz
ret = set_register(MMC5603NJ_I2C_REG_ODR, 100);