sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

simplify building capnp messages with arrays (#2617)

Browse Source 
old-commit-hash: bd4f6650fa
commatwo_master
Dean Lee 5 years ago committed by GitHub
parent 57138a6e0f
commit df9e0b2cb4
12 changed files with 47 additions and 88 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      selfdrive/camerad/cameras/camera_qcom.cc
  2. 2
      selfdrive/camerad/cameras/camera_qcom2.cc
  3. 2
      selfdrive/camerad/cameras/camera_webcam.cc
  4. 9
      selfdrive/locationd/ublox_msg.cc
  5. 12
      selfdrive/modeld/models/dmonitoring.cc
  6. 73
      selfdrive/modeld/models/driving.cc
  7. 4
      selfdrive/sensord/sensors/bmx055_accel.cc
  8. 4
      selfdrive/sensord/sensors/bmx055_gyro.cc
  9. 4
      selfdrive/sensord/sensors/bmx055_magn.cc
  10. 4
      selfdrive/sensord/sensors/lsm6ds3_accel.cc
  11. 4
      selfdrive/sensord/sensors/lsm6ds3_gyro.cc
  12. 9
      selfdrive/sensord/sensors_qcom.cc

8
selfdrive/camerad/cameras/camera_qcom.cc
Unescape View File

@ -2141,11 +2141,11 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
if (env_send_rear) {
fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride);
}
framed.setFocusVal(kj::ArrayPtr<const int16_t>(&s->rear.focus[0], NUM_FOCUS));
framed.setFocusConf(kj::ArrayPtr<const uint8_t>(&s->rear.confidence[0], NUM_FOCUS));
framed.setSharpnessScore(kj::ArrayPtr<const uint16_t>(&s->lapres[0], ARRAYSIZE(s->lapres)));
framed.setFocusVal(s->rear.focus);
framed.setFocusConf(s->rear.confidence);
framed.setSharpnessScore(s->lapres);
framed.setRecoverState(self_recover);
framed.setTransform(kj::ArrayPtr<const float>(&b->yuv_transform.v[0], 9));
framed.setTransform(b->yuv_transform.v);
s->pm->send("frame", msg);
}

2
selfdrive/camerad/cameras/camera_qcom2.cc
Unescape View File

@ -1114,7 +1114,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
fill_frame_image(framed, (uint8_t*)b->cur_rgb_buf->addr, b->rgb_width, b->rgb_height, b->rgb_stride);
}
if (c == &s->rear) {
framed.setTransform(kj::ArrayPtr<const float>(&b->yuv_transform.v[0], 9));
framed.setTransform(b->yuv_transform.v);
}
s->pm->send(c == &s->rear ? "frame" : "wideFrame", msg);

2
selfdrive/camerad/cameras/camera_webcam.cc
Unescape View File

@ -266,7 +266,7 @@ void camera_process_rear(MultiCameraState *s, CameraState *c, int cnt) {
auto framed = msg.initEvent().initFrame();
fill_frame_data(framed, b->cur_frame_data, cnt);
framed.setImage(kj::arrayPtr((const uint8_t *)b->yuv_ion[b->cur_yuv_idx].addr, b->yuv_buf_size));
framed.setTransform(kj::ArrayPtr<const float>(&b->yuv_transform.v[0], 9));
framed.setTransform(b->yuv_transform.v);
s->pm->send("frame", msg);
}

9
selfdrive/locationd/ublox_msg.cc
Unescape View File

@ -213,8 +213,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_solution() {
std::time_t utc_tt = timegm(&timeinfo);
gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06);
float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f };
kj::ArrayPtr<const float> ap(&f[0], sizeof(f) / sizeof(f[0]));
gpsLoc.setVNED(ap);
gpsLoc.setVNED(f);
gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03);
gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03);
gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05);
@ -318,10 +317,8 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() {
eph.setTgd(ephem_data.Tgd);
eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid);
if(ephem_data.ionoCoeffsValid) {
kj::ArrayPtr<const double> apa(&ephem_data.ionoAlpha[0], sizeof(ephem_data.ionoAlpha) / sizeof(ephem_data.ionoAlpha[0]));
eph.setIonoAlpha(apa);
kj::ArrayPtr<const double> apb(&ephem_data.ionoBeta[0], sizeof(ephem_data.ionoBeta) / sizeof(ephem_data.ionoBeta[0]));
eph.setIonoBeta(apb);
eph.setIonoAlpha(ephem_data.ionoAlpha);
eph.setIonoBeta(ephem_data.ionoBeta);
} else {
eph.setIonoAlpha(kj::ArrayPtr<const double>());
eph.setIonoBeta(kj::ArrayPtr<const double>());

12
selfdrive/modeld/models/dmonitoring.cc
Unescape View File

@ -188,14 +188,10 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu
framed.setFrameId(frame_id);
framed.setModelExecutionTime(execution_time);
kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
kj::ArrayPtr<const float> face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta));
kj::ArrayPtr<const float> face_position(&res.face_position[0], ARRAYSIZE(res.face_position));
kj::ArrayPtr<const float> face_position_std(&res.face_position_meta[0], ARRAYSIZE(res.face_position_meta));
framed.setFaceOrientation(face_orientation);
framed.setFaceOrientationStd(face_orientation_std);
framed.setFacePosition(face_position);
framed.setFacePositionStd(face_position_std);
framed.setFaceOrientation(res.face_orientation);
framed.setFaceOrientationStd(res.face_orientation_meta);
framed.setFacePosition(res.face_position);
framed.setFacePositionStd(res.face_position_meta);
framed.setFaceProb(res.face_prob);
framed.setLeftEyeProb(res.left_eye_prob);
framed.setRightEyeProb(res.right_eye_prob);

73
selfdrive/modeld/models/driving.cc
Unescape View File

@ -171,8 +171,7 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, fl
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
path.setPoly(poly);
path.setPoly(poly_arr);
path.setProb(1.0);
path.setStd(std);
path.setValidLen(valid_len);
@ -192,8 +191,7 @@ void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * dat
std = stds_arr[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
path.setPoly(poly);
path.setPoly(poly_arr);
path.setProb(prob);
path.setStd(std);
path.setValidLen(valid_len);
@ -208,10 +206,8 @@ void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * d
xyva_arr[i] = data[LEAD_MHP_VALS + i];
xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
}
kj::ArrayPtr<const float> xyva(xyva_arr, LEAD_MHP_VALS);
kj::ArrayPtr<const float> xyva_stds(xyva_stds_arr, LEAD_MHP_VALS);
lead.setXyva(xyva);
lead.setXyvaStd(xyva_stds);
lead.setXyva(xyva_arr);
lead.setXyvaStd(xyva_stds_arr);
}
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) {
@ -235,39 +231,35 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat
softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
&desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
}
kj::ArrayPtr<const float> desire_state(desire_state_softmax, DESIRE_LEN);
meta.setDesireState(desire_state);
meta.setDesireState(desire_state_softmax);
meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1]));
meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2]));
meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3]));
kj::ArrayPtr<const float> desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE);
meta.setDesirePrediction(desire_pred);
meta.setDesirePrediction(desire_pred_softmax);
}
void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) {
float desire_state_softmax[DESIRE_LEN];
float desire_pred_softmax[4*DESIRE_LEN];
float desire_state_softmax[DESIRE_LEN] = {};
float desire_pred_softmax[4*DESIRE_LEN] = {};
softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
for (int i=0; i<4; i++) {
softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
&desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
}
kj::ArrayPtr<const float> desire_state(desire_state_softmax, DESIRE_LEN);
meta.setDesireState(desire_state);
meta.setDesireState(desire_state_softmax);
meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1]));
meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2]));
meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3]));
kj::ArrayPtr<const float> desire_pred(desire_pred_softmax, DESIRE_PRED_SIZE);
meta.setDesirePrediction(desire_pred);
meta.setDesirePrediction(desire_pred_softmax);
}
void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
int columns, int column_offset, float * plan_t_arr) {
float x_arr[TRAJECTORY_SIZE];
float y_arr[TRAJECTORY_SIZE];
float z_arr[TRAJECTORY_SIZE];
float x_arr[TRAJECTORY_SIZE] = {};
float y_arr[TRAJECTORY_SIZE] = {};
float z_arr[TRAJECTORY_SIZE] = {};
//float x_std_arr[TRAJECTORY_SIZE];
//float y_std_arr[TRAJECTORY_SIZE];
//float z_std_arr[TRAJECTORY_SIZE];
@ -288,20 +280,16 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
z_arr[i] = data[i*columns + 2 + column_offset];
//z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset];
}
kj::ArrayPtr<const float> x(x_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> y(y_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> z(z_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> x_std(x_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> y_std(y_std_arr, TRAJECTORY_SIZE);
//kj::ArrayPtr<const float> z_std(z_std_arr, TRAJECTORY_SIZE);
kj::ArrayPtr<const float> t(t_arr, TRAJECTORY_SIZE);
xyzt.setX(x);
xyzt.setY(y);
xyzt.setZ(z);
xyzt.setX(x_arr);
xyzt.setY(y_arr);
xyzt.setZ(z_arr);
//xyzt.setXStd(x_std);
//xyzt.setYStd(y_std);
//xyzt.setZStd(z_std);
xyzt.setT(t);
xyzt.setT(t_arr);
}
@ -334,14 +322,10 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15];
}
auto position = framed.initPosition();
fill_xyzt(position, best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
auto velocity = framed.initVelocity();
fill_xyzt(velocity, best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
auto orientation = framed.initOrientation();
fill_xyzt(orientation, best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
auto orientation_rate = framed.initOrientationRate();
fill_xyzt(orientation_rate, best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr);
fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr);
fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr);
fill_xyzt(framed.initOrientation(), best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr);
fill_xyzt(framed.initOrientationRate(), best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr);
// lane lines
auto lane_lines = framed.initLaneLines(4);
@ -352,8 +336,7 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i]);
lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
}
kj::ArrayPtr<const float> lane_line_probs(lane_line_probs_arr, 4);
framed.setLaneLineProbs(lane_line_probs);
framed.setLaneLineProbs(lane_line_probs_arr);
framed.setLaneLineStds(lane_line_stds_arr);
// road edges
@ -483,14 +466,10 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
MessageBuilder msg;
auto posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry();
kj::ArrayPtr<const float> trans_vs(&trans_arr[0], 3);
posenetd.setTrans(trans_vs);
kj::ArrayPtr<const float> rot_vs(&rot_arr[0], 3);
posenetd.setRot(rot_vs);
kj::ArrayPtr<const float> trans_std_vs(&trans_std_arr[0], 3);
posenetd.setTransStd(trans_std_vs);
kj::ArrayPtr<const float> rot_std_vs(&rot_std_arr[0], 3);
posenetd.setRotStd(rot_std_vs);
posenetd.setTrans(trans_arr);
posenetd.setRot(rot_arr);
posenetd.setTransStd(trans_std_arr);
posenetd.setRotStd(rot_std_arr);
posenetd.setTimestampEof(timestamp_eof);
posenetd.setFrameId(vipc_frame_id);

4
selfdrive/sensord/sensors/bmx055_accel.cc
Unescape View File

@ -62,10 +62,8 @@ void BMX055_Accel::get_event(cereal::SensorEventData::Builder &event){
event.setTimestamp(start_time);
float xyz[] = {x, y, z};
kj::ArrayPtr<const float> vs(&xyz[0], 3);
auto svec = event.initAcceleration();
svec.setV(vs);
svec.setV(xyz);
svec.setStatus(true);
}

4
selfdrive/sensord/sensors/bmx055_gyro.cc
Unescape View File

@ -72,10 +72,8 @@ void BMX055_Gyro::get_event(cereal::SensorEventData::Builder &event){
event.setTimestamp(start_time);
float xyz[] = {x, y, z};
kj::ArrayPtr<const float> vs(&xyz[0], 3);
auto svec = event.initGyroUncalibrated();
svec.setV(vs);
svec.setV(xyz);
svec.setStatus(true);
}

4
selfdrive/sensord/sensors/bmx055_magn.cc
Unescape View File

@ -96,10 +96,8 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event){
event.setTimestamp(start_time);
float xyz[] = {x, y, z};
kj::ArrayPtr<const float> vs(&xyz[0], 3);
auto svec = event.initMagneticUncalibrated();
svec.setV(vs);
svec.setV(xyz);
svec.setStatus(true);
}

4
selfdrive/sensord/sensors/lsm6ds3_accel.cc
Unescape View File

@ -53,10 +53,8 @@ void LSM6DS3_Accel::get_event(cereal::SensorEventData::Builder &event){
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.setV(xyz);
svec.setStatus(true);
}

4
selfdrive/sensord/sensors/lsm6ds3_gyro.cc
Unescape View File

@ -56,10 +56,8 @@ void LSM6DS3_Gyro::get_event(cereal::SensorEventData::Builder &event){
event.setTimestamp(start_time);
float xyz[] = {y, -x, z};
kj::ArrayPtr<const float> vs(&xyz[0], 3);
auto svec = event.initGyroUncalibrated();
svec.setV(vs);
svec.setV(xyz);
svec.setStatus(true);
}

9
selfdrive/sensord/sensors_qcom.cc
Unescape View File

@ -150,8 +150,7 @@ void sensor_loop() {
switch (data.type) {
case SENSOR_TYPE_ACCELEROMETER: {
auto svec = log_event.initAcceleration();
kj::ArrayPtr<const float> vs(&data.acceleration.v[0], 3);
svec.setV(vs);
svec.setV(data.acceleration.v);
svec.setStatus(data.acceleration.status);
break;
}
@ -164,8 +163,7 @@ void sensor_loop() {
}
case SENSOR_TYPE_MAGNETIC_FIELD: {
auto svec = log_event.initMagnetic();
kj::ArrayPtr<const float> vs(&data.magnetic.v[0], 3);
svec.setV(vs);
svec.setV(data.magnetic.v);
svec.setStatus(data.magnetic.status);
break;
}
@ -178,8 +176,7 @@ void sensor_loop() {
}
case SENSOR_TYPE_GYROSCOPE: {
auto svec = log_event.initGyro();
kj::ArrayPtr<const float> vs(&data.gyro.v[0], 3);
svec.setV(vs);
svec.setV(data.gyro.v);
svec.setStatus(data.gyro.status);
break;
}

Write Preview
Loading…
Cancel
Save