Multicopter auto - Yaw at waypoint before move #12348

bresch · 2019-06-27T12:41:29Z

Describe problem solved by the proposed pull request
In some cases, the user wants the drone to yaw first and then move instead of doing a simultaneous yaw and linear motion. A few examples are:

A drone with a front facing anti-collision sensor (camera, rangefinder, ...)
A standard vtol plane (low yaw speed and lateral motions are to avoid)
A highspeed multirotor with specific aerodynamic properties

Test data / coverage
SITL tests with MPC_POS_MODE 0 and 1.
On the plot below, you can see that the drone starts to move only when the yaw setpoint reaches the desired heading.

Usage
Set MPC_YAW_MODE = 4

Note that the proposed solution only cares about setpoints and doesn't check for the actual yaw of the vehicle. If the yaw tracking of your vehicle is bad, improve it or reduce MPC_YAWR_AUTO_MAX

fixes #11053

@mrivi Can you try that with avoidance please?

mrivi · 2019-06-27T13:09:45Z

@bresch
This PR

Master

bresch · 2019-06-27T15:26:01Z

@PX4/testflights Can you try this PR with the parameter MPC_YAW_MODE = 0,1,2,3 and 4 please?

0 towards waypoint
1 towards home
2 away from home
3 along trajectory
4 towards waypoint (yaw first)

jkflying

Just some questions.

jkflying · 2019-06-27T15:52:31Z

src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp

+		// Wait for the yaw setpoint to be aligned
+		if (!_position_locked) {
+			_velocity_setpoint.setAll(0.f);
+			_position_setpoint = _position;


If this happens at considerable speed we might have some strange behavior with overshoot and returning back to the point where the trajectory change happened. Maybe just setting the velocity setpoint to 0 would be enough?

There is no smoothing in that flight task, setting velocity to 0 would lead to some drift.

src/lib/FlightTasks/tasks/AutoLine/FlightTaskAutoLine.cpp

src/modules/mc_pos_control/mc_pos_control_params.c

xdwgood · 2019-06-28T05:27:31Z

Will this parameter be automatically selected when automatic avoidance is turned on?
Is it necessary for the customer to manually select and then perform an automatic avoidance flight? 😃

Stifael · 2019-06-28T06:42:43Z

I have a couple of questions:

In some cases, the user wants the drone to yaw first and then move instead of doing simultaneous yaw and linear motion

Unfortunately, the way the navigator is written, you basically always have to yaw first before proceeding the next waypoint IF the vehicle heading is set to point towards the next waypoint. Otherwise, the heading check within mission_block (https://github.com/PX4/Firmware/blob/master/src/modules/navigator/mission_block.cpp#L368) is not true and will introduce a sudden brake. I am actually surprised that this has not yet happened with the new smoothing algorithm (I haven't had time yet to test the smoothing yet).

bresch · 2019-06-28T09:03:12Z

@xdwgood I don't think we should couple those because voidance doesn't necessarily mean that the vehicle has a single front facing sensor.

@Stifael It's only performing that check in navigator if the yaw is defined in a mission item. In out case, the yaw setpoint is generated in the FlightTaskAuto and the Navigator check is not done.

Btw, I added the logic to force yaw before linear acceleration in the standard line tracking algorithm to be consistent but I am not sure is this is really required since it is already limiting the lateral acceleration when the yaw isn't aligned.

dannyfpv · 2019-06-28T21:18:28Z

Tested on Pixhawk4 v5 f-450
Modes Tested:

Position Mode: no issue
Altitude Mode: no issue
Stabilized Mode: no issue
mission mode : no issues

PR log: towards waypoint:
https://review.px4.io/plot_app?log=94089c0d-7833-4f81-98b4-0b32f37260d4

PR log: towards home:
https://review.px4.io/plot_app?log=c04830b5-16ba-4b0a-8c67-d260ec7fbe7c

PR log: away from home:
https://review.px4.io/plot_app?log=d85f24b3-992c-4b10-8788-7339ce28f84f

PR log: along trajectory:
https://review.px4.io/plot_app?log=b9a7ab4a-7a69-4698-a580-06e425fbec21

jorge789 · 2019-06-28T21:24:49Z

Tested on Pixhawk 2 Cube V3:

Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL (Automated): Good.
Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint activate RTL and see landing behaviour.

Notes:

No issues noted, good flight in general.

Log: The MPC_YAW_MODE 0 towards waypoint parameter has changed.
**Note:**The vehicle flippel when landing by the strong air.
https://review.px4.io/plot_app?log=2f682235-2328-4974-a798-6f4f66cae5e3

Log: The MPC_YAW_MODE 1 towards home parameter has changed.
https://review.px4.io/plot_app?log=b6257d4d-523c-4c5d-baed-4ac7b769085b

Log: The MPC_YAW_MODE 2 away from home parameter has changed.
https://review.px4.io/plot_app?log=0f19cc06-e33a-4e6b-8abb-31334d05257a

Log: The MPC_YAW_MODE 3 along trajectory parameter has changed.
https://review.px4.io/plot_app?log=6acc9c06-4137-4d91-bfac-f8db9de87b02

jorge789 · 2019-07-01T20:06:00Z

Tested on PixRacer V4:

Flight Card 2

Modes Tested
Mission Plan Mode (Automated): Good.
RTL (Return To Land): Good.

Procedure
Arm and Take off in mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint see landing behavior.

Note:
At the landing the vehicle began to oscillate and descend at the same time, but the descent was not continuous, had brief breaks.Wind 6.0 meters per second

Log: The MPC_YAW_MODE 0 towards waypoint parameter has changed.
https://review.px4.io/plot_app?log=50ae8918-64d8-4777-ae15-42a4fca30c65

Log: The MPC_YAW_MODE 1 towards home parameter has changed.
https://review.px4.io/plot_app?log=42f20474-bc33-4d8c-9127-3b77d142306e

Log: The MPC_YAW_MODE 2 away from home parameter has changed.
https://review.px4.io/plot_app?log=f7f8bcfa-bec5-44ca-b65a-b52c58445f36

Log: The MPC_YAW_MODE 3 along trajectory parameter has changed.
https://review.px4.io/plot_app?log=0fae0055-1baa-41c2-8843-f47412974d37

Junkim3DR · 2019-07-01T20:42:51Z

Tested on Pixhawk Pro v4:

Modes Tested

Position Mode: Good.
Altitude Mode: Good.
Stabilized Mode: Good.
Mission Plan Mode (Automated): Good.
RTL (Automated): Good.

Procedure
Arm and Take off in position mode, after flying for approximately one minute, switched to altitude then stabilized mode proceed to switch to mission plan mode then make sure that vehicle follows all waypoints as shown in QGC, once completed all waypoint activate RTL and see landing behaviour.

Notes:
No issues noted, good flight in general.

Logs:

MPC_YAW_MODE at 0 towards waypoint.
https://review.px4.io/plot_app?log=b093fe86-ffdd-427a-86b8-4a16c47d7294
MPC_YAW_MODE at 1 towards home.
https://review.px4.io/plot_app?log=0e6c95ea-88c5-4cae-b651-9234d45cc568
MPC_YAW_MODE at 2 away from home.
https://review.px4.io/plot_app?log=f0a4ee05-a569-4470-bdc5-abb79d110961
MPC_YAW_MODE at 3 along trajectory.
https://review.px4.io/plot_app?log=5137497a-5214-41b8-941a-bb9398cefe5e

jkflying · 2019-07-05T10:18:10Z

Test flew with avoidance with MPC_YAW_MODE=4. The mission at the end is with the avoidance enabled:
Flight log: https://logs.px4.io/plot_app?log=09d60813-d0b3-4f3b-83f0-0459eb5c6bb6

This was flown immediately before with MPC_YAW_MODE=0 and MPC_AUTO_MODE=1, same mission:
https://logs.px4.io/plot_app?log=28337c55-137d-4003-a477-d92dd7e4acc5

It seemed to have strange behavior when turning. It would go back to the location where it was when the new setpoint was received (while yawing), instead of coming to a standstill.
This gave it a wobbly appearance, and it had lots of weird 'overshoot'-like behavior any time there was a yaw as can be seen in the picture.

MPC_YAW_MODE=4

MPC_YAW_MODE=0 and MPC_AUTO_MODE=1

bresch · 2019-07-09T11:12:28Z

@jkflying As you can see in the first 2D plot you shared and the one here below, the vehicle overshoots the setpoint, it's not the setpoint that overshoots

For me, the problem is that the velocity/position loops are poorly tuned. The reason why you didn't see it with the other mission mode is that is resets the setpoint to the current position when the vehicle reaches the waypoint. This is not something I would since such a reset produce infinite jerk, acceleration and velocity.

bresch · 2019-07-10T16:51:17Z

@jkflying I rebased on top of master, can you review again please such that we can merge?
Btw, the diff in FlightTaskAutoLineSmoothVel.cpp looks large, but in fact, I just added :

	if (_param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned) {
		// Wait for the yaw setpoint to be aligned
		_velocity_setpoint.setAll(0.f);
        } else {

the rest is just indentation

jkflying

LGTM

bresch · 2019-07-11T08:03:53Z

12 bytes overflow on v2 xD

…mode ensures that the vehicle yaws towards the next waypoint before accelerating. This is required for drones with front vision and aerodynamic multicopters such as standard vtol planes or highspeed multirotors.

bresch added Admin: Enhancement (improvement) 💡 Multirotor 🛸 labels Jun 27, 2019

bresch requested review from MaEtUgR and Stifael June 27, 2019 12:41

jkflying reviewed Jun 27, 2019

View reviewed changes

weekly-digest bot mentioned this pull request Jun 30, 2019

Weekly Digest (23 June, 2019 - 30 June, 2019) #12374

Closed

weekly-digest bot mentioned this pull request Jul 7, 2019

Weekly Digest (30 June, 2019 - 7 July, 2019) #12430

Closed

bresch force-pushed the dev-auto-yaw-before-move branch from f52fea0 to 20a7e88 Compare July 10, 2019 16:47

jkflying previously approved these changes Jul 11, 2019

View reviewed changes

bresch dismissed jkflying’s stale review via a94b10e July 11, 2019 09:03

bresch force-pushed the dev-auto-yaw-before-move branch from 14b18a8 to a94b10e Compare July 11, 2019 09:03

bresch force-pushed the dev-auto-yaw-before-move branch from a94b10e to 019e613 Compare July 11, 2019 11:31

Cosmetic - Use c++ list initialization

56d7c1b

bresch force-pushed the dev-auto-yaw-before-move branch from 019e613 to 56d7c1b Compare July 11, 2019 14:15

bresch merged commit e5128ac into master Jul 11, 2019

bresch deleted the dev-auto-yaw-before-move branch July 11, 2019 15:46

weekly-digest bot mentioned this pull request Jul 14, 2019

Weekly Digest (7 July, 2019 - 14 July, 2019) #12474

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Multicopter auto - Yaw at waypoint before move #12348

Multicopter auto - Yaw at waypoint before move #12348

bresch commented Jun 27, 2019

mrivi commented Jun 27, 2019

bresch commented Jun 27, 2019

jkflying left a comment

jkflying Jun 27, 2019

bresch Jun 28, 2019

xdwgood commented Jun 28, 2019 •

edited

Loading

Stifael commented Jun 28, 2019 •

edited

Loading

bresch commented Jun 28, 2019

dannyfpv commented Jun 28, 2019 •

edited by Tony3dr

Loading

jorge789 commented Jun 28, 2019 •

edited

Loading

jorge789 commented Jul 1, 2019

Junkim3DR commented Jul 1, 2019 •

edited

Loading

jkflying commented Jul 5, 2019

bresch commented Jul 9, 2019

bresch commented Jul 10, 2019

jkflying left a comment

bresch commented Jul 11, 2019

Multicopter auto - Yaw at waypoint before move #12348

Multicopter auto - Yaw at waypoint before move #12348

Conversation

bresch commented Jun 27, 2019

mrivi commented Jun 27, 2019

bresch commented Jun 27, 2019

jkflying left a comment

Choose a reason for hiding this comment

jkflying Jun 27, 2019

Choose a reason for hiding this comment

bresch Jun 28, 2019

Choose a reason for hiding this comment

xdwgood commented Jun 28, 2019 • edited Loading

Stifael commented Jun 28, 2019 • edited Loading

bresch commented Jun 28, 2019

dannyfpv commented Jun 28, 2019 • edited by Tony3dr Loading

jorge789 commented Jun 28, 2019 • edited Loading

Tested on Pixhawk 2 Cube V3:

jorge789 commented Jul 1, 2019

Tested on PixRacer V4:

Junkim3DR commented Jul 1, 2019 • edited Loading

Tested on Pixhawk Pro v4:

jkflying commented Jul 5, 2019

bresch commented Jul 9, 2019

bresch commented Jul 10, 2019

jkflying left a comment

Choose a reason for hiding this comment

bresch commented Jul 11, 2019

xdwgood commented Jun 28, 2019 •

edited

Loading

Stifael commented Jun 28, 2019 •

edited

Loading

dannyfpv commented Jun 28, 2019 •

edited by Tony3dr

Loading

jorge789 commented Jun 28, 2019 •

edited

Loading

Junkim3DR commented Jul 1, 2019 •

edited

Loading