spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

comma two: remove lens sag compensation (#23113)

pull/23218/head
Willem Melching 2021-12-14 13:29:59 +01:00 committed by GitHub
parent 8cb83b29a6
commit 36db473bab
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 3 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
selfdrive/camerad/cameras/camera_common.cc
View File

@ -200,7 +200,6 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction);
framed.setTargetGreyFraction(frame_data.target_grey_fraction);
framed.setLensPos(frame_data.lens_pos);
framed.setLensSag(frame_data.lens_sag);
framed.setLensErr(frame_data.lens_err);
framed.setLensTruePos(frame_data.lens_true_pos);
}

1
selfdrive/camerad/cameras/camera_common.h
View File

@ -68,7 +68,6 @@ typedef struct FrameMetadata {
// Focus
unsigned int lens_pos;
float lens_sag;
float lens_err;
float lens_true_pos;
} FrameMetadata;

35
selfdrive/camerad/cameras/camera_qcom.cc
View File

@ -852,21 +852,7 @@ static void parse_autofocus(CameraState *s, uint8_t *d) {
s->focus_err = max_focus*1.0;
}
static std::optional<float> get_accel_z(SubMaster *sm) {
sm->update(0);
if(sm->updated("sensorEvents")) {
for (auto event : (*sm)["sensorEvents"].getSensorEvents()) {
if (event.which() == cereal::SensorEventData::ACCELERATION) {
if (auto v = event.getAcceleration().getV(); v.size() >= 3)
return -v[2];
break;
}
}
}
return std::nullopt;
}
static void do_autofocus(CameraState *s, SubMaster *sm) {
static void do_autofocus(CameraState *s) {
float lens_true_pos = s->lens_true_pos.load();
if (!isnan(s->focus_err)) {
// learn lens_true_pos
@ -874,23 +860,10 @@ static void do_autofocus(CameraState *s, SubMaster *sm) {
lens_true_pos -= s->focus_err*focus_kp;
}
if (auto accel_z = get_accel_z(sm)) {
s->last_sag_acc_z = *accel_z;
}
const float sag = (s->last_sag_acc_z / 9.8) * 128;
// stay off the walls
lens_true_pos = std::clamp(lens_true_pos, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP));
int target = std::clamp(lens_true_pos - sag, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP));
s->lens_true_pos.store(lens_true_pos);
/*char debug[4096];
char *pdebug = debug;
pdebug += sprintf(pdebug, "focus ");
for (int i = 0; i < NUM_FOCUS; i++) pdebug += sprintf(pdebug, "%2x(%4d) ", s->confidence[i], s->focus[i]);
pdebug += sprintf(pdebug, " err: %7.2f offset: %6.2f sag: %6.2f lens_true_pos: %6.2f cur_lens_pos: %4d->%4d", err * focus_kp, offset, sag, s->lens_true_pos, s->cur_lens_pos, target);
LOGD(debug);*/
actuator_move(s, target);
actuator_move(s, lens_true_pos);
}
void camera_autoexposure(CameraState *s, float grey_frac) {
@ -1046,12 +1019,11 @@ static void ops_thread(MultiCameraState *s) {
CameraExpInfo driver_cam_op;
util::set_thread_name("camera_settings");
SubMaster sm({"sensorEvents"});
while(!do_exit) {
road_cam_op = road_cam_exp.load();
if (road_cam_op.op_id != last_road_cam_op_id) {
do_autoexposure(&s->road_cam, road_cam_op.grey_frac);
do_autofocus(&s->road_cam, &sm);
do_autofocus(&s->road_cam);
last_road_cam_op_id = road_cam_op.op_id;
}
@ -1165,7 +1137,6 @@ void cameras_run(MultiCameraState *s) {
.frame_length = (uint32_t)c->frame_length,
.integ_lines = (uint32_t)c->cur_integ_lines,
.lens_pos = c->cur_lens_pos,
.lens_sag = c->last_sag_acc_z,
.lens_err = c->focus_err,
.lens_true_pos = c->lens_true_pos,
.gain = c->cur_gain_frac,

1
selfdrive/camerad/cameras/camera_qcom.h
View File

@ -76,7 +76,6 @@ typedef struct CameraState {
// rear camera only,used for focusing
unique_fd actuator_fd;
std::atomic<float> focus_err;
std::atomic<float> last_sag_acc_z;
std::atomic<float> lens_true_pos;
std::atomic<int> self_recover; // af recovery counter, neg is patience, pos is active
uint16_t cur_step_pos;