LSM6DS3 fix gyro scale (#21040)

pull/21041/head
Willem Melching 4 years ago committed by GitHub
parent 476ee91845
commit 79a2f06760
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
  1. 4
      selfdrive/sensord/sensors/lsm6ds3_gyro.cc

@ -45,13 +45,13 @@ void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event){
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer)); int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
assert(len == sizeof(buffer)); assert(len == sizeof(buffer));
float scale = 250.0f / (1 << 15); float scale = 8.75 / 1000.0;
float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale); float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale); float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale); float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3); event.setSource(cereal::SensorEventData::SensorSource::LSM6DS3);
event.setVersion(1); event.setVersion(2);
event.setSensor(SENSOR_GYRO_UNCALIBRATED); event.setSensor(SENSOR_GYRO_UNCALIBRATED);
event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED); event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
event.setTimestamp(start_time); event.setTimestamp(start_time);

Loading…
Cancel
Save