Commit dd01c4d4 authored by James Strawson's avatar James Strawson
Browse files

fix voa center bin

parent cacf7476
Pipeline #370835969 passed with stages
in 1 minute and 7 seconds
0.9.4
* fix VOA center bin
0.9.3
* add auto_level_horizon calibration feature
0.9.2
......
Package: voxl-vision-px4
Version: 0.9.3
Version: 0.9.4
Section: base
Priority: optional
Architecture: armv7a
......
......@@ -50,10 +50,10 @@
#define PIPE_READ_BUF_SIZE (16*320*240)
#define DFS_POINT_PIPE_LOCATION (MODAL_PIPE_DEFAULT_BASE_DIR "dfs_point_cloud/")
#define UPPER_BOUND_M -0.75
#define UPPER_BOUND_M -0.75 // in body frame, z points down
#define LOWER_BOUND_M 0.75
#define MAX_DISTANCE_CM 2000 // [cm] TODO: need to fiddle with min/max
#define MIN_DISTANCE_CM 20 // [cm]
#define MAX_DISTANCE_CM 800 // 800cm (8m), max reliable distance for DFS
#define MIN_DISTANCE_CM 50 // 50cm (0.5m) nearest distance for DFS
#define N_BINS 13 // MUST BE ODD. each bin is 5 degrees wide
// 13 bins covers 65 degrees which is about the
// FOV of the stereo cameras
......@@ -100,8 +100,8 @@ static int __angle_to_bin(double a)
// TODO: currently the camera and point cloud helpers don't provide a way to
// detect a disconnect, only the simple helper which VIO and Apriltag managers
// use. Add disconnect detection to libmodal_pipe and this in the future
static void obstacle_pipe_helper_cb(__attribute__((unused))int ch,
point_cloud_metadata_t meta, void* inpoints,
static void obstacle_pipe_helper_cb(__attribute__((unused))int ch,
point_cloud_metadata_t meta, void* inpoints,
__attribute__((unused)) void* context)
{
mavlink_message_t msg;
......@@ -139,6 +139,9 @@ static void obstacle_pipe_helper_cb(__attribute__((unused))int ch,
// for each point detected from stereo, populate the appropriate bin
for(i=0; i<n_points; i++){
// reject points with a depth below min
if(T_point_wrt_stereo.d[2]<(MIN_DISTANCE_CM/100.0)) continue;
// put xyz data into our own vector
T_point_wrt_stereo.d[0] = points[(3*i)+0];
T_point_wrt_stereo.d[1] = points[(3*i)+1];
......@@ -157,9 +160,9 @@ static void obstacle_pipe_helper_cb(__attribute__((unused))int ch,
double y = T_point_wrt_level.d[1];
uint16_t dist_cm = (uint16_t)(sqrtf(x*x + y*y)*100);
// bound points in min/max
if(dist_cm<MIN_DISTANCE_CM) dist_cm = MIN_DISTANCE_CM;
if(dist_cm>MAX_DISTANCE_CM) dist_cm = MAX_DISTANCE_CM;
// reject noisy points too close or too far
if(dist_cm<MIN_DISTANCE_CM) continue;
if(dist_cm>MAX_DISTANCE_CM) continue;
// x is forward, y is to the right, positive angle to the right
int bin = __angle_to_bin(atan2(y,x));
......@@ -236,12 +239,12 @@ int voa_manager_init(void)
running = 1;
pipe_client_set_connect_cb(DFS_PIPE_CH, _connect_cb, NULL);
pipe_client_set_disconnect_cb(DFS_PIPE_CH, _disconnect_cb, NULL);
pipe_client_set_point_cloud_helper_cb(DFS_PIPE_CH, obstacle_pipe_helper_cb, NULL);
pipe_client_open(DFS_PIPE_CH, "dfs_point_cloud", PIPE_CLIENT_NAME, \
CLIENT_FLAG_EN_POINT_CLOUD_HELPER, \
PIPE_READ_BUF_SIZE);
return 0;
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment