simplify read sensorEvents

pull/2765/head
deanlee 5 years ago
parent 176acbe803
commit 03183203f3
  1. 24
      selfdrive/camerad/cameras/camera_qcom.cc

@ -2060,29 +2060,17 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
// cache rgb roi and write to cl // cache rgb roi and write to cl
// gz compensation // gz compensation
s->sm_rear->update(0); if (uint64_t ts = nanos_since_boot();
if (s->sm_rear->updated("sensorEvents")) { (ts - s->rear.last_sag_ts) > 10000000 && s->sm_rear->update(0) > 0 && s->sm_rear->updated("sensorEvents")) {
float vals[3] = {0.0}; for (auto sensor_event : (*(s->sm_rear))["sensorEvents"].getSensorEvents()) {
bool got_accel = false;
auto sensor_events = (*(s->sm_rear))["sensorEvents"].getSensorEvents();
for (auto sensor_event : sensor_events) {
if (sensor_event.which() == cereal::SensorEventData::ACCELERATION) { if (sensor_event.which() == cereal::SensorEventData::ACCELERATION) {
auto v = sensor_event.getAcceleration().getV(); if (auto v = sensor_event.getAcceleration().getV(); v.size >= 3) {
if (v.size() < 3) { s->rear.last_sag_ts = ts;
continue; s->rear.last_sag_acc_z = -v[2];
}
for (int j = 0; j < 3; j++) {
vals[j] = v[j];
} }
got_accel = true;
break; break;
} }
} }
uint64_t ts = nanos_since_boot();
if (got_accel && ts - s->rear.last_sag_ts > 10000000) { // 10 ms
s->rear.last_sag_ts = ts;
s->rear.last_sag_acc_z = -vals[2];
}
} }
const uint8_t *rgb_addr = (uint8_t*)b->cur_rgb_buf->addr; const uint8_t *rgb_addr = (uint8_t*)b->cur_rgb_buf->addr;

Loading…
Cancel
Save