fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 05:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

inav: subscribe to multi dist instances and check orientation

Browse Source
This commit is contained in:
ChristophTobler
2017-09-25 08:52:21 +02:00
committed by ChristophTobler
parent bfe28c1df3
commit 16825e6460
1 changed files with 23 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+23 -7
View File
@@ -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