@Eric-Katzfey Sorry, I didn't save the stereo camera images from voxl-logger, so I can only provide the PX4 log file. Currently, I am addressing the exposure issue using the method I mentioned earlier, and it seems to have improved the effectiveness of collision prevention. Additionally, to ensure that the drone can stably stop in front of obstacles, I made the following modifications to the voxl-vision-hub code.
int pie_binner_filter_to_mavlink_msg(pie_binner_t* b, mavlink_message_t* msg)
{
int i,j;
// final distance data for px4
uint16_t distances[72];
uint16_t dist_max_cm = b->dist_table.dist_max * 100.0f;
// now set the bins to max_distance +1 means no obstacle is present.
for(i=0;i<72;i++){
distances[i] = dist_max_cm - 10;
}
if(b->ringbuf.items_in_buf>0){
// scan bins for any that meet the criteria
// for each angle....
for(i=0; i<b->rows; i++){
// scan the distances from inside out, remember we have one extra column
// than we do distance bins
for(j=0; j<(b->cols-EXTRA_BINS_TO_CHECK); j++){
// skip empty bins
if(b->map_points[i][j] <= 0) continue;
// sum up points in this bin, the two on either side
// and the three behind it
int sum = 0;
for(int k=-1; k<=1; k++){
// row (angle) to inspect with wraparound check
int ii = i-k;
if(ii<0) ii+=b->rows;
else if(ii>=b->rows) ii-=b->rows;
// sum points in this bin plus extras
for(int m=0; m<=EXTRA_BINS_TO_CHECK; m++){
sum += b->map_points[ii][j+m];
}
}
// check the sum of points in all the neighbors
if(sum >= b->pie_threshold){
float dist_m = b->map_dist[i][j] / b->map_points[i][j];
// if(dist_m<0.1f) continue; // error check, not needed?
uint16_t dist_cm = dist_m * 100.0f;
if(dist_cm<150) continue;
else if(dist_cm>=150){
distances[i] = dist_cm;
last_dis[i] = distances[i];
zero_count[i] = 0;
//printf("added a distance! i: %d j:%d dist: %5.1fm\n", i, j, (double)dist_m);
break; // move onto next angle
}
// distances[i] = dist_cm;
// break;
}
}
if(zero_count[i]<30){
distances[i] = last_dis[i];
zero_count[i]++;
}
}
}
// TODO investigate if min distance changes px4 behavior or not
#define MIN_DISTANCE_CM 0
// set other params for mavlink message
uint64_t time_usec = 0;
uint8_t increment_int = 0; // use the float field instead
float increment_f = 360.0f / b->tan_table.n_bins;
float angle_offset = 0.0f; // 0 means bin 0 is straight ahead along X
mavlink_msg_obstacle_distance_pack(autopilot_monitor_get_sysid(), VOXL_COMPID,\
msg, time_usec, MAV_DISTANCE_SENSOR_UNKNOWN, distances, increment_int,\
MIN_DISTANCE_CM, dist_max_cm, increment_f, angle_offset, MAV_FRAME_BODY_FRD);
return 0;
}
However, during the drone's flight to the right, it suddenly pauses. The drone's distance sensors are only equipped with front-facing stereo cameras, so they shouldn't detect obstacles on the right side. Why is this happening? Is there a way to solve it?
Here is my PX4 log file
https://review.px4.io/plot_app?log=fb1ae613-4e56-49a2-a60f-bc272edd78c1
