mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 00:03:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

LSM6DS3 (#2268)

Browse Source 
* add initial lsm6ds3 setup

* accel works

* ignore lsm6ds3 in locationd

* add gyro

* add temp

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: e51da619c15dea3ac6cc77f4f9a387923e2a3ea6
This commit is contained in:
Willem Melching
2020-10-05 16:16:25 +02:00
committed by GitHub
parent 67b02775da
commit 03b2a6a137
10 changed files with 281 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.py
+7
View File
@@ -2,6 +2,7 @@
import numpy as np
import sympy as sp
import cereal.messaging as messaging
from cereal import log
import common.transformations.coordinates as coord
from common.transformations.orientation import ecef_euler_from_ned, \
euler_from_quat, \
@@ -19,6 +20,8 @@ from selfdrive.swaglog import cloudlog
from sympy.utilities.lambdify import lambdify
from rednose.helpers.sympy_helpers import euler_rotate
SensorSource = log.SensorEventData.SensorSource
VISION_DECIMATION = 2
SENSOR_DECIMATION = 10
@@ -235,6 +238,10 @@ class Localizer():
def handle_sensors(self, current_time, log):
# TODO does not yet account for double sensor readings in the log
for sensor_reading in log:
# TODO: handle messages from two IMUs at the same time
if sensor_reading.source == SensorSource.lsm6ds3:
continue
# Gyro Uncalibrated
if sensor_reading.sensor == 5 and sensor_reading.type == 16:
self.gyro_counter += 1
selfdrive/sensord/SConscript
+3
View File
@@ -13,5 +13,8 @@ else:
'sensors/bmx055_gyro.cc',
'sensors/bmx055_magn.cc',
'sensors/bmx055_temp.cc',
'sensors/lsm6ds3_accel.cc',
'sensors/lsm6ds3_gyro.cc',
'sensors/lsm6ds3_temp.cc',
]
env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj'])
selfdrive/sensord/sensors/bmx055_magn.cc
+5 -3
View File
@@ -6,22 +6,24 @@
#include "bmx055_magn.hpp"
int16_t parse_xy(uint8_t lsb, uint8_t msb){
static int16_t parse_xy(uint8_t lsb, uint8_t msb){
// 13 bit
uint16_t combined = (uint16_t(msb) << 5) | uint16_t(lsb >> 3);
return int16_t(combined << 3) / (1 << 3);
}
int16_t parse_z(uint8_t lsb, uint8_t msb){
static int16_t parse_z(uint8_t lsb, uint8_t msb){
// 15 bit
uint16_t combined = (uint16_t(msb) << 7) | uint16_t(lsb >> 1);
return int16_t(combined << 1) / (1 << 1);
}
uint16_t parse_rhall(uint8_t lsb, uint8_t msb){
/*
static uint16_t parse_rhall(uint8_t lsb, uint8_t msb){
// 14 bit
return (uint16_t(msb) << 6) | uint16_t(lsb >> 2);
}
*/
BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
selfdrive/sensord/sensors/lsm6ds3_accel.cc
+62
View File
@@ -0,0 +1,62 @@
#include <cassert>
#include "common/swaglog.h"
#include "common/timing.h"
#include "lsm6ds3_accel.hpp"
LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus) : I2CSensor(bus) {}
int LSM6DS3_Accel::init(){
int ret = 0;
uint8_t buffer[1];
ret = read_register(LSM6DS3_ACCEL_I2C_REG_ID, buffer, 1);
if(ret < 0){
LOGE("Reading chip ID failed: %d", ret);
goto fail;
}
if(buffer[0] != LSM6DS3_ACCEL_CHIP_ID){
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_ACCEL_CHIP_ID);
ret = -1;
goto fail;
}
// TODO: set scale and bandwith. Default is +- 2G, 50 Hz
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ);
if (ret < 0){
goto fail;
}
fail:
return ret;
}
void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event){
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 9.81 * 2.0f / (1 << 15);
float x = read_16_bit(buffer[0], buffer[1]) * scale;
float y = read_16_bit(buffer[2], buffer[3]) * scale;
float z = read_16_bit(buffer[4], buffer[5]) * scale;
event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3);
event.setVersion(1);
event.setSensor(SENSOR_ACCELEROMETER);
event.setType(SENSOR_TYPE_ACCELEROMETER);
event.setTimestamp(start_time);
float xyz[] = {y, -x, z};
kj::ArrayPtr<const float> vs(&xyz[0], 3);
auto svec = event.initAcceleration();
svec.setV(vs);
svec.setStatus(true);
}
selfdrive/sensord/sensors/lsm6ds3_accel.hpp
+24
View File
@@ -0,0 +1,24 @@
#pragma once
#include "sensors/i2c_sensor.hpp"
// Address of the chip on the bus
#define LSM6DS3_ACCEL_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_ACCEL_I2C_REG_ID 0x0F
#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10
#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28
// Constants
#define LSM6DS3_ACCEL_CHIP_ID 0x69
#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4)
class LSM6DS3_Accel : public I2CSensor {
uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;}
public:
LSM6DS3_Accel(I2CBus *bus);
int init();
void get_event(cereal::SensorEventData::Builder &event);
};
selfdrive/sensord/sensors/lsm6ds3_gyro.cc
+65
View File
@@ -0,0 +1,65 @@
#include <cassert>
#include <cmath>
#include "common/swaglog.h"
#include "common/timing.h"
#include "lsm6ds3_gyro.hpp"
#define DEG2RAD(x) ((x) * M_PI / 180.0)
LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus) : I2CSensor(bus) {}
int LSM6DS3_Gyro::init(){
int ret = 0;
uint8_t buffer[1];
ret = read_register(LSM6DS3_GYRO_I2C_REG_ID, buffer, 1);
if(ret < 0){
LOGE("Reading chip ID failed: %d", ret);
goto fail;
}
if(buffer[0] != LSM6DS3_GYRO_CHIP_ID){
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_GYRO_CHIP_ID);
ret = -1;
goto fail;
}
// TODO: set scale. Default is +- 250 deg/s
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ);
if (ret < 0){
goto fail;
}
fail:
return ret;
}
void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event){
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 250.0f / (1 << 15);
float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3);
event.setVersion(1);
event.setSensor(SENSOR_GYRO_UNCALIBRATED);
event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
event.setTimestamp(start_time);
float xyz[] = {y, -x, z};
kj::ArrayPtr<const float> vs(&xyz[0], 3);
auto svec = event.initAcceleration();
svec.setV(vs);
svec.setStatus(true);
}
selfdrive/sensord/sensors/lsm6ds3_gyro.hpp
+24
View File
@@ -0,0 +1,24 @@
#pragma once
#include "sensors/i2c_sensor.hpp"
// Address of the chip on the bus
#define LSM6DS3_GYRO_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_GYRO_I2C_REG_ID 0x0F
#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11
#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22
// Constants
#define LSM6DS3_GYRO_CHIP_ID 0x69
#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4)
class LSM6DS3_Gyro : public I2CSensor {
uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;}
public:
LSM6DS3_Gyro(I2CBus *bus);
int init();
void get_event(cereal::SensorEventData::Builder &event);
};
selfdrive/sensord/sensors/lsm6ds3_temp.cc
+45
View File
@@ -0,0 +1,45 @@
#include <cassert>
#include "common/swaglog.h"
#include "common/timing.h"
#include "lsm6ds3_temp.hpp"
LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
int LSM6DS3_Temp::init(){
int ret = 0;
uint8_t buffer[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){
LOGE("Chip ID wrong. Got: %d, Expected %d", buffer[0], LSM6DS3_TEMP_CHIP_ID);
ret = -1;
goto fail;
}
fail:
return ret;
}
void LSM6DS3_Temp::get_event(cereal::SensorEventData::Builder &event){
uint64_t start_time = nanos_since_boot();
uint8_t buffer[2];
int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / 16.0f;
event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3);
event.setVersion(1);
event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
event.setTimestamp(start_time);
event.setTemperature(temp);
}
selfdrive/sensord/sensors/lsm6ds3_temp.hpp
+22
View File
@@ -0,0 +1,22 @@
#pragma once
#include "sensors/i2c_sensor.hpp"
// Address of the chip on the bus
#define LSM6DS3_TEMP_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_TEMP_I2C_REG_ID 0x0F
#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20
// Constants
#define LSM6DS3_TEMP_CHIP_ID 0x69
class LSM6DS3_Temp : public I2CSensor {
uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;}
public:
LSM6DS3_Temp(I2CBus *bus);
int init();
void get_event(cereal::SensorEventData::Builder &event);
};
selfdrive/sensord/sensors_qcom2.cc
+24 -8
View File
@@ -11,10 +11,16 @@
#include "sensors/sensor.hpp"
#include "sensors/constants.hpp"
#include "sensors/bmx055_accel.hpp"
#include "sensors/bmx055_gyro.hpp"
#include "sensors/bmx055_magn.hpp"
#include "sensors/bmx055_temp.hpp"
#include "sensors/lsm6ds3_accel.hpp"
#include "sensors/lsm6ds3_gyro.hpp"
#include "sensors/lsm6ds3_temp.hpp"
#include "sensors/light_sensor.hpp"
volatile sig_atomic_t do_exit = 0;
@@ -36,18 +42,28 @@ int sensor_loop() {
return -1;
}
BMX055_Accel accel(i2c_bus_imu);
BMX055_Gyro gyro(i2c_bus_imu);
BMX055_Magn magn(i2c_bus_imu);
BMX055_Temp temp(i2c_bus_imu);
BMX055_Accel bmx055_accel(i2c_bus_imu);
BMX055_Gyro bmx055_gyro(i2c_bus_imu);
BMX055_Magn bmx055_magn(i2c_bus_imu);
BMX055_Temp bmx055_temp(i2c_bus_imu);
LSM6DS3_Accel lsm6ds3_accel(i2c_bus_imu);
LSM6DS3_Gyro lsm6ds3_gyro(i2c_bus_imu);
LSM6DS3_Temp lsm6ds3_temp(i2c_bus_imu);
LightSensor light("/sys/class/i2c-adapter/i2c-2/2-0038/iio:device1/in_intensity_both_raw");
// Sensor init
std::vector<Sensor *> sensors;
sensors.push_back(&accel);
sensors.push_back(&gyro);
sensors.push_back(&magn);
sensors.push_back(&temp);
sensors.push_back(&bmx055_accel);
sensors.push_back(&bmx055_gyro);
sensors.push_back(&bmx055_magn);
sensors.push_back(&bmx055_temp);
sensors.push_back(&lsm6ds3_accel);
sensors.push_back(&lsm6ds3_gyro);
sensors.push_back(&lsm6ds3_temp);
sensors.push_back(&light);