Vector asigment with vec.xy() = vec.xy() does not work [Bug]

[sverrevr]

Describe the bug

The following assignment fails:

Vector3f lhs(1,2,3);
Vector3f rhs(4,5,6);
lhs.xy() = rhs.xy();
EXPECT_EQ(lhs, Vector3f(4,5,3));

[ RUN      ] MatrixVector3Test.Vector3
../../src/lib/matrix/test/MatrixVector3Test.cpp:70: Failure
Expected equality of these values:
  lhs
    Which is: 
               1               2               3

  Vector3f(4,5,3)
    Which is: 
               4               5               3

[  FAILED  ] MatrixVector3Test.Vector3 (0 ms)

So lhs is not getting assigned.

This form of assignment is used multiple places in the code for example here:

PX4-Autopilot/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp

Lines 112 to 121 in c63214b

	if (apply_brake && stopped && !Vector2f(_position_setpoint).isAllFinite()) {
	_position_setpoint.xy() = _position.xy();
	} else if (Vector2f(_position_setpoint).isAllFinite() && apply_brake) {
	// Position is locked but check if a reset event has happened.
	// We will shift the setpoints.
	if (_sub_vehicle_local_position.get().xy_reset_counter != _reset_counter) {
	_position_setpoint.xy() = _position.xy();
	_reset_counter = _sub_vehicle_local_position.get().xy_reset_counter;
	}

I noticed this change when the position lock no longer activated when testing 1.14

Note that the following test which is originally a part of MatrixVector3Test.cpp passes:

Vector2f g2(1, 3);
Vector3f g3(7, 11, 17);
g3.xy() = g2;
EXPECT_EQ(g3, Vector3f(1, 3, 17));

To Reproduce

Add the following lines of code to PX4-Autopilot/src/lib/matrix/test/MatrixVector3Test.cpp

Vector3f lhs(1,2,3);
Vector3f rhs(4,5,6);
lhs.xy() = rhs.xy();
EXPECT_EQ(lhs, Vector3f(4,5,3));

Run:
make tests

Expected behavior

lhs should be assigned, the tests should pass.

Screenshot / Media

No response

Flight Log

Not relevant as it can be tested with unit tests

Software Version

Newest main when writing this issue: c63214b
And on 1.14 release

Flight controller

N/A

Vehicle type

None

How are the different components wired up (including port information)

No response

Additional context

No response

[sverrevr]

In 1.13 the position lock worked. This was broken by this commit which changed to using .xy instead of assigning each element separately. https://github.com/PX4/PX4-Autopilot/pull/20440/files#diff-6a93912921c62df6bced857c3dea68c3c275d25faa3f6c78866eafedc4e5612fR113

[bresch]

Thanks for reporting, I'm looking into this issue.

[MaEtUgR]

@sverrevr Thanks for spotting and reporting this in detail! We'll have a look.

[MaEtUgR]

We're making progress thanks to my colleague: #22574
I'm verifying the effects and that things are working as expected now. Then we can backport the fix to 1.14 and release with 1.14.1.
@sverrevr If you want to give this a go yourself that would be helpful.

[sverrevr]

Good job! And thanks for solving this so rapidly :) This solves the observed problem of the position lock not engaging. Position set points are now set when the joystick is released.