dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

CollisionPrevention: make sure that vehicle tilt compensation is 

correct for all sensor orientation
This commit is contained in:
Martina Rivizzigno 2019-06-13 14:50:51 +02:00 committed by Beat Küng
parent 2439dc09ae
commit a9b1946bea
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/lib/CollisionPrevention/CollisionPrevention.cpp
View File

@ -160,9 +160,13 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle)
wrap_bin = bin - distances_array_size;
}
// rotate vehicle attitude into the sensor body frame
matrix::Quatf attitude_sensor_frame = attitude;
attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
// compensate measurement for vehicle tilt and convert to cm
obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin],
(int)(100 * distance_sensor.current_distance * cosf(Eulerf(attitude).theta())));
(int)(100 * distance_sensor.current_distance * cosf(Eulerf(attitude_sensor_frame).theta())));
}
}
}