From 16825e6460e5839ee5267ba1e825c8e3cf3f1b2f Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 22 Sep 2017 11:19:56 +0200 Subject: [PATCH] inav: subscribe to multi dist instances and check orientation --- .../position_estimator_inav_main.cpp | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 8234111eec..87a195e8ce 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -379,8 +379,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); int vision_position_sub = orb_subscribe(ORB_ID(vehicle_vision_position)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); - int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + // because we can have several distance sensor instances with different orientations + int distance_sensor_subs[ORB_MULTI_MAX_INSTANCES]; + + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + distance_sensor_subs[i] = orb_subscribe_multi(ORB_ID(distance_sensor), i); + } /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); @@ -535,13 +540,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } - /* lidar alt estimation */ - orb_check(distance_sensor_sub, &updated); + /* lidar alt estimation + * update lidar separately, needed by terrain estimator */ + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - /* update lidar separately, needed by terrain estimator */ - if (updated) { - orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); - lidar.current_distance += params.lidar_calibration_offset; + orb_check(distance_sensor_subs[i], &updated); + + if (updated) { + + orb_copy(ORB_ID(distance_sensor), distance_sensor_subs[i], &lidar); + + if (lidar.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) { + updated = false; + + } else { + lidar.current_distance += params.lidar_calibration_offset; + break; // only the first valid distance sensor instance is used + } + } } if (updated) { //check if altitude estimation for lidar is enabled and new sensor data