Skip to content

Commit

Permalink
Remove local copy of vehicle attitude
Browse files Browse the repository at this point in the history
  • Loading branch information
mahimayoga committed Dec 20, 2024
1 parent 1c81ce7 commit c652932
Showing 1 changed file with 1 addition and 2 deletions.
3 changes: 1 addition & 2 deletions src/lib/collision_prevention/CollisionPrevention.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -400,12 +400,11 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);

const Quatf q_vehicle_attitude(vehicle_attitude);
const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f)));

const Vector3f forward_vector(1.0f, 0.0f, 0.0f);

const Quatf q_sensor_rotation = q_vehicle_attitude * q_sensor;
const Quatf q_sensor_rotation = vehicle_attitude * q_sensor;

const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);

Expand Down

0 comments on commit c652932

Please sign in to comment.