Scale obstacle distance with vehicle attitude for varying sensor orientations (Collision Prevention)

[mahimayoga]

Solved Problem

Scaling of distance_reading to an obstacle was done under the assumption that the sensor was always mounted facing forward, so distance_reading was only scaled for pitch.

Solution

Orientation of sensor w.r.t vehicle body frame is taken into account, and the relevant vehicle attitude angles (pitch and/or roll) are used to scale the outputted distance.

Changelog Entry

For release notes:

Feature: Obstacle distance in obstacle_distance_fused scaled for pitch/roll, based on the orientation of the sensor in the XY plane of the vehicle. 

Alternatives

We could also ...

Test coverage

• Simulation/hardware testing logs: (Indoor, giving pitch/roll input manually for different sensor orientations @ fixed distance to flat wall)

• Front facing:
  
  

• Left facing (270 deg):

• 45 deg:

Roll scaling not super accurate in the 45deg case, probably because the placement of the sensor wasn't very exact. Can see it works well in the Left facing scenario.

[bresch]

This looks ok, but there is a more efficient way to do this without passing by euler angles. You can compensate for a tilt by taking the last element of

PX4-Autopilot/src/lib/matrix/matrix/Quaternion.hpp

Lines 521 to 541 in 4db55cd

	/**
	* Corresponding body z-axis to an attitude quaternion /
	* last orthogonal unit basis vector
	*
	* == last column of the equivalent rotation matrix
	* but calculated more efficiently than a full conversion
	*/
	Vector3<Type> dcm_z() const
	{
	const Quaternion &q = *this;
	Vector3<Type> R_z;
	const Type a = q(0);
	const Type b = q(1);
	const Type c = q(2);
	const Type d = q(3);
	R_z(0) = 2 * (a * c + b * d);
	R_z(1) = 2 * (c * d - a * b);
	R_z(2) = a * a - b * b - c * c + d * d;
	return R_z;
	}
	};

, which is the length of the projection of the body_z vector on the "down" vector.

[bresch]

Suggested change

// unit vector of sensor orientation

The name of the variable is good enough

[bresch]

Suggested change

// rotate sensor unit vector into vehicle body frame

[bresch]

Suggested change

// Function to rotate a 2D point by pitch and roll

_rotatePointByPitchAndRoll is self explanatory