Orbit: Adding orbit yaw behaviours #13426

dayjaby · 2019-11-11T09:39:45Z

Describe problem solved by this pull request
Implementing the yaw behaviours for orbit flight mode according the current mavlink specification (https://mavlink.io/en/messages/common.html#MAV_CMD_DO_ORBIT) and an additional yaw behaviour as suggested in mavlink/mavlink#1266. This new addition is important if you have a fpv camera installed in the front of the drone and expect the drone to always look forward.

Describe your solution

For yaw behaviour equals 0, we keep looking at the centre, like the code did before in all cases.
For 1, we do a yaw setpoint to the heading that we had in the beginning of the flight task.
For 2, we do no yaw setpoint.
For 3, we have the yaw setpoint looking always forward.

Test data / coverage
Tested via QGC (without parameter 3 being set) and via mavros (https://logs.px4.io/plot_app?log=7725a2a3-5af4-4710-a6a6-dbba12f5dd20):
rosservice call /mavros/cmd/command "{broadcast: false, command: 34, confirmation: 0, param1: 50.0, param2: 5.0, param3: 3.0, param4: 0.0, param5: 47.3977449, param6: 8.5465953, param7: 2.5}"

@TSC21 Thank you for your help with the implementation!

TSC21 · 2019-11-11T09:48:23Z

This new addition is important if you have a fpv camera installed in the front of the drone and expect the drone to always look forward.

Also relevant to combine with obstacle avoidance when one has a forward facing sensor and wants to safely execute an orbit around a certain position.

MaEtUgR

Nice! Thanks for looking into it.
I hope my review comments make sense to you. If you're tired of them I can also put a commit somewhere with my suggestion and you check it.

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp

dayjaby · 2019-11-14T13:42:24Z

@MaEtUgR Following your advice, please have a look at the most recent commit. I restructured the code a little and created an enum.

Btw, does it make sense to do a setpoint for yawspeed and yaw at the same time?

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp

msg/orbit_status.msg

TSC21 · 2019-11-19T12:13:52Z

@dayjaby can you rebase on master? git pull --rebase https://github.com/PX4/Firmware.git master

dayjaby · 2019-11-20T10:34:55Z

@TSC21 Done. Thanks for the command ;)

TSC21 · 2019-11-20T11:03:30Z

@TSC21 Done. Thanks for the command ;)

Neat! Now it appears without any conflicts :)

TSC21 · 2019-11-20T11:04:19Z

@MaEtUgR all good here for you?

TSC21 · 2019-11-20T15:16:08Z

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.hpp

@@ -104,6 +105,10 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
 	const float _velocity_max = 10.f;
 	const float _acceleration_max = 2.f;

+	int _yaw_behaviour =
+		orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER; /**<  yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */
+	float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */


@dayjaby this is actually the heading of the vehicle when the vehicle command DO_ORBIT gets processed, or when we can say when the MAV_CMD_DO_ORBIT is received and processed in the flight controller.

MaEtUgR

Hey @dayjaby looks a lot better with the enumerated behaviors 👍 Sorry for the remaining complaints but I'd like to keep this clean and since I have to maintain it also according to how it was designed. I really appreciate your implementation of the yaw modes and could help you directly with code suggestions. Just let me know.

MaEtUgR · 2019-11-22T08:37:46Z

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp

@@ -224,5 +231,37 @@ void FlightTaskOrbit::generate_circle_setpoints(Vector2f center_to_position)
 	_position_setpoint(0) = _position_setpoint(1) = NAN;

 	// yawspeed feed-forward because we know the necessary angular rate
-	_yawspeed_setpoint = _v / _r;
+	_yawspeed_setpoint = NAN; //_v / _r;


Please don't leave commented out code

MaEtUgR · 2019-11-22T08:40:16Z

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp

+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
+		if (_r > 0) {
+			_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F / 2.f;
+
+		} else {
+			_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) - M_PI_F / 2.f;
+		}
+
+		break;
+
+	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED:


For the front tangential and front to center yaw behavior you want a yawspeed feed forward which was there before and is now commented out otherwise the tracking performance in real world scenarios significantly degrades.

MaEtUgR · 2019-11-22T08:53:25Z

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp


-	// make vehicle front always point towards the center
-	_yaw_setpoint = atan2f(center_to_position(1), center_to_position(0)) + M_PI_F;
+	if (start_to_circle.norm() < 0.5f) {


This is overriding the logic of the motion primitive StraightLine _circle_approach_line. I can only guess that according to your taste the approach takes too much time to slow down at the end. But then it's the fault of the StraightLine implementation which we want to fill with the jerk optimized trajectory generation anyways very soon and we should not interfere with its operation locally here.

I added that not because the slow down took too much time, but because switching from one orbit command to another orbit command made the drone brake down (same orbit center, same radius, but different yaw behaviour). But thinking about it now, this braking down seems reasonable.

MaEtUgR · 2019-11-22T08:54:09Z

src/lib/flight_tasks/tasks/Orbit/FlightTaskOrbit.cpp

-	_yawspeed_setpoint = _v / _r;
+	_yawspeed_setpoint = NAN; //_v / _r;
+
+	switch (_yaw_behaviour) {


I was asking to put this logic in its own function. Now it's not in the main update anymore but just uses another already existing one.

Is generate_circle_setpoints not the proper place? Or do you prefer an additional generate_circle_yaw_setpoints function?

dayjaby · 2019-11-27T05:00:52Z

Rebased and addressed your comments @MaEtUgR . Thanks for your help with this!

stale · 2020-02-27T05:20:31Z

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

TSC21 · 2020-04-04T17:12:21Z

@dayjaby can you do a last rebase on this? @MaEtUgR maybe a last review as well?

For the RC controlled yaw behaviour, we do a yaw setpoint according to the stick expo. The uncontrolled yaw behaviour behaves undefined. Switching between yaw behaviours makes the drone stand still for a moment, which probably can be improved.

dayjaby · 2020-04-05T08:02:56Z

@dayjaby can you do a last rebase on this? @MaEtUgR maybe a last review as well?

Done, no conflicts on rebase.

stale · 2020-07-04T08:37:58Z

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

MaEtUgR · 2020-07-13T09:11:48Z

@dayjaby Hey, thanks a lot for your contribution and patience! I'm so sorry for the slow follow up! I finally went through it in all detail and have some suggestions that I pushed on top here: https://github.com/PX4/Firmware/compare/adding_orbit_yaw_behaviours
Let me know what you think. We can merge your changes first (even before #15292) and I can create a pull request for mine. This is for transparency.

Explanations:

If there is a line break in the Doxygen comment anyways we can put it before the declaration.
FlightTaskManualAltitude which orbit is inheriting from is already generating yaw setpoints from RC. I know this is confusing and the goal is to move this stuff into libraries such that you can just write "I want the yaw generation from RC" instead of silently inheriting.
Your way of calculating start_to_circle is much nicer 👍 I just moved it back into the local scope such that it's only calculated when used. Unrelated I also added some const qualifiers and switched the parameter structs to pass by reference.
As a suggestion, I set the initial heading for every new command that arrives which is probably what Orbit: Adding orbit yaw behaviours #13426 (comment) wanted to address. It's up to discussion. I thought if you ran any other yaw behavior first and then command the HOLD_INITIAL_HEADING it should hold what was there at the moment and not when orbit was first commanded. If HOLD_INITIAL_HEADING was already used then it should stay very close to the same yaw.
I simplified the HOLD_FRONT_TANGENT_TO_CIRCLE yaw setpoint generation using the sign() function and made it depend on the sign of the velocity and not the radius since inside the task for technical reasons the radius is always positive but the velocity changes sign for the opposite rotation direction.

dayjaby · 2020-07-13T13:37:45Z

@MaEtUgR I agree with all your changes! Thanks for taking the time.

MaEtUgR

Thanks for the feedback. Let's get it in 👍

TSC21 requested review from MaEtUgR and Stifael November 11, 2019 09:42

TSC21 added the Admin: Enhancement (improvement) 💡 label Nov 11, 2019

TSC21 previously approved these changes Nov 11, 2019

View reviewed changes

MaEtUgR reviewed Nov 13, 2019

View reviewed changes

dayjaby dismissed TSC21’s stale review via 663dc06 November 14, 2019 13:40

dayjaby force-pushed the adding_orbit_yaw_behaviours branch from acd193d to 663dc06 Compare November 14, 2019 13:40

TSC21 mentioned this pull request Nov 15, 2019

common: MAV_CMD_DO_ORBIT: add new option for heading behaviour (param index #3) mavlink/mavlink#1266

Merged

TSC21 reviewed Nov 15, 2019

View reviewed changes

src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp Outdated Show resolved Hide resolved

TSC21 requested a review from MaEtUgR November 18, 2019 23:00

TSC21 reviewed Nov 18, 2019

View reviewed changes

msg/orbit_status.msg Show resolved Hide resolved

dayjaby force-pushed the adding_orbit_yaw_behaviours branch from 22643b7 to 82a4d96 Compare November 19, 2019 04:00

TSC21 requested changes Nov 19, 2019

View reviewed changes

msg/orbit_status.msg Show resolved Hide resolved

dayjaby force-pushed the adding_orbit_yaw_behaviours branch from 82a4d96 to 6b35fca Compare November 20, 2019 10:33

TSC21 previously approved these changes Nov 20, 2019

View reviewed changes

TSC21 reviewed Nov 20, 2019

View reviewed changes

MaEtUgR requested changes Nov 22, 2019

View reviewed changes

dayjaby dismissed TSC21’s stale review via 18992b5 November 26, 2019 11:50

dayjaby force-pushed the adding_orbit_yaw_behaviours branch 3 times, most recently from 7152c5f to e454e34 Compare November 27, 2019 04:59

stale bot added the stale label Feb 27, 2020

weekly-digest bot mentioned this pull request Mar 1, 2020

Weekly Digest (23 February, 2020 - 1 March, 2020) #14268

Closed

stale bot removed the stale label Apr 4, 2020

TSC21 requested a review from MaEtUgR April 4, 2020 17:13

dayjaby added 3 commits April 5, 2020 10:01

Orbit: add more yaw behaviours

e99197e

Orbit: Adjust yaw setpoint on circle approach

3eb3240

Orbit: Add RC controlled yaw mode

f92f168

For the RC controlled yaw behaviour, we do a yaw setpoint according to the stick expo. The uncontrolled yaw behaviour behaves undefined. Switching between yaw behaviours makes the drone stand still for a moment, which probably can be improved.

dayjaby force-pushed the adding_orbit_yaw_behaviours branch from e454e34 to f92f168 Compare April 5, 2020 08:02

stale bot added the stale label Jul 4, 2020

weekly-digest bot mentioned this pull request Jul 5, 2020

Weekly Digest (28 June, 2020 - 5 July, 2020) #15258

Closed

dayjaby mentioned this pull request Jul 9, 2020

Orbit approach fixes #15292

Merged

stale bot removed the stale label Jul 13, 2020

MaEtUgR approved these changes Jul 13, 2020

View reviewed changes

MaEtUgR merged commit 589aff7 into PX4:master Jul 13, 2020

MaEtUgR added the Multirotor 🛸 label Jul 13, 2020

MaEtUgR mentioned this pull request Jul 13, 2020

Orbit yaw behaviours addition #15319

Merged

weekly-digest bot mentioned this pull request Jul 19, 2020

Weekly Digest (12 July, 2020 - 19 July, 2020) #15376

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Orbit: Adding orbit yaw behaviours #13426

Orbit: Adding orbit yaw behaviours #13426

dayjaby commented Nov 11, 2019

TSC21 commented Nov 11, 2019

MaEtUgR left a comment

dayjaby commented Nov 14, 2019 •

edited

Loading

TSC21 commented Nov 19, 2019

dayjaby commented Nov 20, 2019

TSC21 commented Nov 20, 2019

TSC21 commented Nov 20, 2019

TSC21 Nov 20, 2019

MaEtUgR left a comment

MaEtUgR Nov 22, 2019

MaEtUgR Nov 22, 2019

MaEtUgR Nov 22, 2019

dayjaby Nov 26, 2019

MaEtUgR Nov 22, 2019

dayjaby Nov 26, 2019

dayjaby commented Nov 27, 2019

stale bot commented Feb 27, 2020

TSC21 commented Apr 4, 2020

dayjaby commented Apr 5, 2020

stale bot commented Jul 4, 2020

MaEtUgR commented Jul 13, 2020 •

edited

Loading

dayjaby commented Jul 13, 2020

MaEtUgR left a comment •

edited

Loading

Orbit: Adding orbit yaw behaviours #13426

Orbit: Adding orbit yaw behaviours #13426

Conversation

dayjaby commented Nov 11, 2019

TSC21 commented Nov 11, 2019

MaEtUgR left a comment

Choose a reason for hiding this comment

dayjaby commented Nov 14, 2019 • edited Loading

TSC21 commented Nov 19, 2019

dayjaby commented Nov 20, 2019

TSC21 commented Nov 20, 2019

TSC21 commented Nov 20, 2019

TSC21 Nov 20, 2019

Choose a reason for hiding this comment

MaEtUgR left a comment

Choose a reason for hiding this comment

MaEtUgR Nov 22, 2019

Choose a reason for hiding this comment

MaEtUgR Nov 22, 2019

Choose a reason for hiding this comment

MaEtUgR Nov 22, 2019

Choose a reason for hiding this comment

dayjaby Nov 26, 2019

Choose a reason for hiding this comment

MaEtUgR Nov 22, 2019

Choose a reason for hiding this comment

dayjaby Nov 26, 2019

Choose a reason for hiding this comment

dayjaby commented Nov 27, 2019

stale bot commented Feb 27, 2020

TSC21 commented Apr 4, 2020

dayjaby commented Apr 5, 2020

stale bot commented Jul 4, 2020

MaEtUgR commented Jul 13, 2020 • edited Loading

dayjaby commented Jul 13, 2020

MaEtUgR left a comment • edited Loading

Choose a reason for hiding this comment

dayjaby commented Nov 14, 2019 •

edited

Loading

MaEtUgR commented Jul 13, 2020 •

edited

Loading

MaEtUgR left a comment •

edited

Loading