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.

[mahimayoga]

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.

Thanks! I am having a look now, but it's not intuitive to me how I could use this. I don't know if I am misinterpreting it but: Say I have a forward facing sensor, and I compute the quaternion of the rotated sensor (from the vehicle attitude) w.r.t the initial vehicle frame (FRD). Then taking the last element of quaternion.dcm_z() as the scaling factor would also account for tilt in the roll axis right? But this shouldn't affect the distance to an obstacle.

[mahimayoga]

Projection now found without passing Euler angles. Results for sensor placed with 45 degree orientation: