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:

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: -88 byte (-0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +50  [ = ]       0    .debug_abbrev
  -0.2%      -6  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%     -16  [ = ]       0    .debug_aranges
  -2.0%      -8  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +4  [ = ]       0    .debug_frame
+0.0%    +531  [ = ]       0    .debug_info
  +0.6%    +535  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
-0.0%    -285  [ = ]       0    .debug_line
  -1.9%    -254  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.5%      -6  [ = ]       0    task/task_cancelpt.c
-0.0%    -713  [ = ]       0    .debug_loc
  +2.4%    +677  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -0.4% -1.36Ki  [ = ]       0    [section .debug_loc]
-0.0%    -425  [ = ]       0    .debug_ranges
  -5.2%    -320  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.2%     -96  [ = ]       0    [section .debug_ranges]
  -1.5%      -1  [ = ]       0    task/task_cancelpt.c
+0.0%     +42  [ = ]       0    .debug_str
  +1.0%     +42  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
+0.5%      +1  [ = ]       0    .shstrtab
-0.0%     -53  [ = ]       0    .strtab
  -3.4%     -53  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +32  [ = ]       0    [section .strtab]
-0.0%     -48  [ = ]       0    .symtab
  +1.0%     +16  [ = ]       0    ../../src/drivers/adc/board_adc/ADC.cpp
  -5.3%     -64  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
+0.5%     +88  [ = ]       0    [Unmapped]
-0.0%     -88  -0.0%     -88    .text
  +0.0%      +5  +0.0%      +5    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  -1.7%     -96  -1.7%     -96    ../../src/lib/collision_prevention/CollisionPrevention.cpp
-0.0%    -912  -0.0%     -88    TOTAL

px4_fmu-v6x [Total VM Diff: -96 byte (-0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +50  [ = ]       0    .debug_abbrev
  -0.2%      -6  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%     -16  [ = ]       0    .debug_aranges
  -2.0%      -8  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%      +4  [ = ]       0    .debug_frame
+0.0%    +531  [ = ]       0    .debug_info
  +0.6%    +535  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
-0.0%    -285  [ = ]       0    .debug_line
  -1.9%    -254  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.5%      -6  [ = ]       0    task/task_cancelpt.c
-0.0%    -698  [ = ]       0    .debug_loc
  +2.5%    +692  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -0.4% -1.36Ki  [ = ]       0    [section .debug_loc]
-0.0%    -424  [ = ]       0    .debug_ranges
  -5.2%    -320  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  -0.2%     -96  [ = ]       0    [section .debug_ranges]
+0.0%     +42  [ = ]       0    .debug_str
  +1.0%     +42  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
+0.5%      +1  [ = ]       0    .shstrtab
-0.0%     -53  [ = ]       0    .strtab
  -3.4%     -53  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.0%     +32  [ = ]       0    [section .strtab]
-0.0%     -48  [ = ]       0    .symtab
  +1.0%     +16  [ = ]       0    ../../src/drivers/adc/board_adc/ADC.cpp
  -5.3%     -64  [ = ]       0    ../../src/lib/collision_prevention/CollisionPrevention.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
+0.1%     +96  [ = ]       0    [Unmapped]
-0.0%     -96  -0.0%     -96    .text
  -1.7%     -96  -1.7%     -96    ../../src/lib/collision_prevention/CollisionPrevention.cpp
-0.0%    -896  -0.0%     -96    TOTAL

Updated: 2024-12-20T13:33:24

[mahimayoga]

https://review.px4.io/plot_app?log=777e5193-7f40-40f5-a52d-d4f6fb519eba

Log for flight test today with 45 degree mounted sensor, everything seems to work fine, no weird behaviour. @bresch is this ok to merge into main?

[bresch]

error: local copy 'q_vehicle_attitude' of the variable 'vehicle_attitude' is never modified; consider avoiding the copy [performance-unnecessary-copy-initialization,-warnings-as-errors]
                const Quatf q_vehicle_attitude(vehicle_attitude);

vehicle_attitude is already a quaternion