retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

simplify building capnp messages with arrays (#2617)

albatross
Dean Lee 2020-11-30 19:08:52 +08:00 committed by GitHub
parent 42183d913f
commit bd4f6650fa
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 47 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
selfdrive/camerad/cameras/camera_qcom.cc
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
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
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
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
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
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
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
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
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
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
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
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;
}