Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-26 09:26:25 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

CollisionPrevention: compute the attitude_sensor_frame outside for loop

Browse Source
This commit is contained in:
Martina Rivizzigno
2019-07-08 17:28:43 +02:00
committed by Beat Küng
parent 560c9f972a
commit ac67d5603f
1 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/CollisionPrevention/CollisionPrevention.cpp
+7 -6
View File
@@ -156,6 +156,11 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
lower_bound = 0;
}
// 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));
float attitude_sensor_frame_pitch = cosf(Eulerf(attitude_sensor_frame).theta());
for (int bin = lower_bound; bin <= upper_bound; ++bin) {
int wrap_bin = bin;
@@ -169,13 +174,9 @@ 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_sensor_frame).theta())));
(int)(100 * distance_sensor.current_distance * attitude_sensor_frame_pitch));
}
}
}
@@ -187,7 +188,7 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
const Vector2f &curr_pos, const Vector2f &curr_vel)
{
obstacle_distance_s obstacle = {};
obstacle_distance_s obstacle{};
_updateOffboardObstacleDistance(obstacle);
_updateDistanceSensor(obstacle);