diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index 9c0ae8f6a7..0c10b7700d 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/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 // gz compensation - s->sm_rear->update(0); - if (s->sm_rear->updated("sensorEvents")) { - float vals[3] = {0.0}; - bool got_accel = false; - auto sensor_events = (*(s->sm_rear))["sensorEvents"].getSensorEvents(); - for (auto sensor_event : sensor_events) { + if (uint64_t ts = nanos_since_boot(); + (ts - s->rear.last_sag_ts) > 10000000 && s->sm_rear->update(0) > 0 && s->sm_rear->updated("sensorEvents")) { + for (auto sensor_event : (*(s->sm_rear))["sensorEvents"].getSensorEvents()) { if (sensor_event.which() == cereal::SensorEventData::ACCELERATION) { - auto v = sensor_event.getAcceleration().getV(); - if (v.size() < 3) { - continue; + if (auto v = sensor_event.getAcceleration().getV(); v.size >= 3) { + s->rear.last_sag_ts = ts; + s->rear.last_sag_acc_z = -v[2]; } - for (int j = 0; j < 3; j++) { - vals[j] = v[j]; - } - got_accel = true; 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;