Dear engineer, help me please.
I did in the below site to bring up optical flow. And I check about whether PX4 recieve optical flow data by ORB.
-> http://dev.px4.io/optical_flow.html
-> I check opticflow data recieving is ok at orb_copy(ORB_ID(optical_flow), _optical_flow_sub, &optical_flow) / task_main function / EKF2.cpp / PX4.
but ekf app reject opticflow data. because delta_time_good is not good. delta_time_good condition is that dt of optic flow is upper 50 msec .
-> _ekf.setOpticalFlowData(optical_flow.timestamp, &flow);, this function reject optcal flow data.
Reject reason is delta_time ( flow->dt ) is 0.03 ,the function demand
the function is judged to delta_time_good is OK if delta_time ( flow->dt ) is in 50msec undercondition.
So I check delta_time ( flow->dt ), but delta_time ( flow->dt ) is always 0.03 ~ 0.035.
I think that delta_time ( flow->dt ) is time gap between optical flow data get time gyro sensor data get time.
I think I can not control this time gap.
I want to see stable hovering by optical flow.
please help me. thank you.
Estimator_interface.cpp
void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow)
{
if (!_initialised) {
return;
}
// limit data rate to prevent data being lost
if (time_usec - _time_last_optflow > _min_obs_interval_us) {
// check if enough integration time
float delta_time = 1e-6f * (float)flow->dt;
bool delta_time_good = (delta_time >= 0.05f); // this is problem point
// check magnitude is within sensor limits
float flow_rate_magnitude;
bool flow_magnitude_good = false;
if (delta_time_good) {
flow_rate_magnitude = flow->flowdata.norm() / delta_time;
flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max);
}
// check quality metric
bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
//ECL_INFO("%d delta_time %f ",delta_time_good, (double)delta_time );
if (delta_time_good && flow_magnitude_good && flow_quality_good) {
flowSample optflow_sample_new;
// calculate the system time-stamp for the mid point of the integration period
optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;
// copy the quality metric returned by the PX4Flow sensor
optflow_sample_new.quality = flow->quality;
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis.
// copy the optical and gyro measured delta angles
optflow_sample_new.flowRadXY = - flow->flowdata;
optflow_sample_new.gyroXYZ = - flow->gyrodata;
// compensate for body motion to give a LOS rate
optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0);
optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1);
// convert integration interval to seconds
optflow_sample_new.dt = 1e-6f * (float)flow->dt;
_time_last_optflow = time_usec;
// push to buffer
_flow_buffer.push(optflow_sample_new);
ECL_INFO("setOpticalFlowData %f , %f", (double)flow->flowdata(0),(double)flow->flowdata(1));
//PX4_LOG("setRangeData %f ", *data);
}
}
}
// push to buffer
_flow_buffer.push(optflow_sample_new);
}