Handle velocity frames correctly for rover offboard velocity control

[Jaeyoung-Lim]

Describe problem solved by this pull request
While support for offboard velocity setpoints were added in #13225, the velocity frames were not considered properly.

Describe your solution
For multirotors, this is handled by the Offboard flight task but for rover since flight task is not running the conversion needs to happen in the rover position control module.

Test data / coverage
The following logs show the same velocity setpoint v = [1.0, 1.0, 0.0] streamed to the vehicle with the following frame IDs:

• VELOCITY_FRAME_LOCAL_NED = 1 : Log
• VELOCITY_FRAME_BODY_NED = 8 : Log

As expected the local frame velocity leads to a straight line trajectory, while a body frame velocity leads to a circular trajectory of the vehicle.

Additional context
This was a bug reported by @jbeyerstedt

[dagar]

@Jaeyoung-Lim it would be great if we could start adding simple CI testing that goes along with the various levels of OFFBOARD we want to support on different vehicle types. Otherwise they tend to to quietly break over time and slip through a release cycle. FYI @Pedro-Roque

Simple tests here: https://github.com/PX4/Firmware/tree/master/integrationtests/python_src/px4_it/mavros

Running on Jenkins SITL Tests here: http://ci.px4.io:8080/blue/organizations/jenkins/PX4_misc%2FFirmware-SITL_tests/detail/PR-13363/2/pipeline/54

[Jaeyoung-Lim]

@dagar That makes a lot of sense. I think that is what happened before with fw offboard mode. I will have a look.

[julianoes]

Nice!

[junwoo091400]

One small input / question though.
In terms of this local / body frame determining logic, the "FlightTaskOffboard" code checks if the velocity_frame is either LOCAL or BODY_NED, and if it's neither of the two, it returns 'false'(See the code below).
So I was just wondering, regarding this commit, by @Jaeyoung-Lim, whether the rover should also have this conditional, or if the flightTaskOffboard code should be simplified(like this PR code, only one if-statement)? Having 2 different logic regarding similar situation seems kind of odd. Unless there actually isn't a 3rd-frame, some discrepancy might arise. Am I missing something?

if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_LOCAL_NED) {
			// in local frame: don't require any transformation
			_velocity_setpoint(0) = _sub_triplet_setpoint.get().current.vx;
			_velocity_setpoint(1) = _sub_triplet_setpoint.get().current.vy;

		} else if (_sub_triplet_setpoint.get().current.velocity_frame == position_setpoint_s::VELOCITY_FRAME_BODY_NED) {
			// in body frame: need to transorm first
			// Note, this transformation is wrong because body-xy is not neccessarily on the same plane as locale-xy
			_velocity_setpoint(0) = cosf(_yaw) * _sub_triplet_setpoint.get().current.vx - sinf(
							_yaw) * _sub_triplet_setpoint.get().current.vy;
			_velocity_setpoint(1) = sinf(_yaw) * _sub_triplet_setpoint.get().current.vx + cosf(
							_yaw) * _sub_triplet_setpoint.get().current.vy;

		} else {
			// no valid frame
			return false;
		}

[Jaeyoung-Lim]

@junwoo091400 I am aware of that part of the code, and I was thinking of fixing it for sometime. The FlightTask Offboard conversion is not correct, since it is assuming the body xy plane matches the local xy plane.

I believe this was never surfaced since in multirotors having velocity reference in the bodyframe doesn't make much sense. But nevertheless, we should fix it. Would be awesome if you can make a PR for it!

[junwoo091400]

@Jaeyoung-Lim Created #13373!

[Pedro-Roque]

Thanks @dagar , I'll check on this and will also talk with @Jaeyoung-Lim to see which tests to create.