Skip to content

Commit

Permalink
SITL: remove dead stores from rangefinder_range
Browse files Browse the repository at this point in the history
roll and pitch are unused if !SITL_RANGEFINDER_AS_OBJECT_SENSOR

../../libraries/SITL/SIM_Aircraft.cpp:546:13: warning: Value stored to 'roll' is never read [deadcode.DeadStores]
            roll = 0;
            ^      ~
../../libraries/SITL/SIM_Aircraft.cpp:551:13: warning: Value stored to 'roll' is never read [deadcode.DeadStores]
            roll = 0;
            ^      ~
../../libraries/SITL/SIM_Aircraft.cpp:557:13: warning: Value stored to 'pitch' is never read [deadcode.DeadStores]
            pitch = 0;
            ^       ~
../../libraries/SITL/SIM_Aircraft.cpp:562:13: warning: Value stored to 'pitch' is never read [deadcode.DeadStores]
            pitch = 0;
  • Loading branch information
peterbarker committed Jan 29, 2025
1 parent 9bfba32 commit a01b7e7
Showing 1 changed file with 19 additions and 19 deletions.
38 changes: 19 additions & 19 deletions libraries/SITL/SIM_Aircraft.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -536,6 +536,25 @@ float Aircraft::perpendicular_distance_to_rangefinder_surface() const

float Aircraft::rangefinder_range() const
{
float altitude = perpendicular_distance_to_rangefinder_surface();

// sensor position offset in body frame
const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset;

// n.b. the following code is assuming rotation-pitch-270:
// adjust altitude for position of the sensor on the vehicle if position offset is non-zero
if (!relPosSensorBF.is_zero()) {
// get a rotation matrix following DCM conventions (body to earth)
Matrix3f rotmat;
sitl->state.quaternion.rotation_matrix(rotmat);
// rotate the offset into earth frame
const Vector3f relPosSensorEF = rotmat * relPosSensorBF;
// correct the altitude at the sensor
altitude -= relPosSensorEF.z;
}

const auto orientation = (Rotation)sitl->sonar_rot.get();
#if SITL_RANGEFINDER_AS_OBJECT_SENSOR

float roll = sitl->state.rollDeg;
float pitch = sitl->state.pitchDeg;
Expand Down Expand Up @@ -563,25 +582,6 @@ float Aircraft::rangefinder_range() const
}
}

float altitude = perpendicular_distance_to_rangefinder_surface();

// sensor position offset in body frame
const Vector3f relPosSensorBF = sitl->rngfnd_pos_offset;

// n.b. the following code is assuming rotation-pitch-270:
// adjust altitude for position of the sensor on the vehicle if position offset is non-zero
if (!relPosSensorBF.is_zero()) {
// get a rotation matrix following DCM conventions (body to earth)
Matrix3f rotmat;
sitl->state.quaternion.rotation_matrix(rotmat);
// rotate the offset into earth frame
const Vector3f relPosSensorEF = rotmat * relPosSensorBF;
// correct the altitude at the sensor
altitude -= relPosSensorEF.z;
}

const auto orientation = (Rotation)sitl->sonar_rot.get();
#if SITL_RANGEFINDER_AS_OBJECT_SENSOR
/*
rover and copter using SITL rangefinders as obstacle sensors
*/
Expand Down

0 comments on commit a01b7e7

Please sign in to comment.