Use velocity_setpoint for feedforward instead of constraint #14155

jkflying · 2020-02-13T15:46:54Z

Describe problem solved by this pull request
We want to be able to track a moving setpoint from the avoidance interface. However, right now velocity setpoints are used as constraints, which prevents their being used for the feedforward term to be able to specify the position setpoint as a moving target

Describe your solution
Use the _velocity_setpoint as a feedforward (which the name would imply), instead of using it as a constraint.

~~Use the _constraints to constrain the takeoff velocity, which was the only usage of the velocity setpoint term for contraining velocities from position setpoints before.~~ _constraints is used in mc_pos_control which introduces lots of discontinuities.

Ported the the only previous usage of the velocity constraint (takeoff speed) in the FlightTaskAutoLineSmoothVel. When the _constraints are removed from mc_pos_control this small code block can be moved to FlightTaskAutoMapper instead, and the constraints passed through via a constraints struct instead.

Test data / coverage
Test flown a bunch in SITL. Everything seems to behave exactly as it did before.

@PX4/testflights could you do some flights here? Specifically, make sure auto-takeoffs for missions seem the same as they were before.

bresch

Nice!

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

jorge789 · 2020-02-13T20:46:44Z

Tested on PixRacer V4

Flight Card 2

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

Procedure

Armed form QGC.
Took-off on mission plan mode
. Note that commands were sent form QGC (Armed and takeoff)
Once all waypoint completed vehicle switched to RTL and landed as expected.
Good flight in general.
Note:
No issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=9600f0ee-16e0-43ad-9f7d-6c7bfce7e4c8

Log:
Comparison to MASTER
https://review.px4.io/plot_app?log=0b21f0bf-1bdc-44e0-b544-db9a0d1fbdfd

Tested on CUAV nano V5:

Flight Card 2

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

Procedure

Armed form QGC.
Took-off on mission plan mode
. Note that commands were sent form QGC (Armed and takeoff)
Once all waypoint completed vehicle switched to RTL and landed as expected.
Good flight in general.
Note:
No issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=129af300-e3f7-4470-bf09-d3198eea822b

Log:
Comparison to MASTER
https://review.px4.io/plot_app?log=1625281b-196f-401a-b448-a13ba3c301a2

dannyfpv · 2020-02-13T21:05:04Z

Tested on pixhawk4 v5 f-450
Flight Card 2

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

Procedure

Armed form QGC.
Took-off on mission plan mode
. Note that commands were sent form QGC (Armed and takeoff)
Once all waypoint completed vehicle switched to RTL and landed as expected.
Good flight in general.
Note:
No issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=f13b055f-2be9-48f3-8a6d-84194dab8f88
Log:
Comparison to MASTER
https://review.px4.io/plot_app?log=fc323a4d-2610-4442-91bb-d8ca546e7728

jkflying · 2020-02-14T12:24:36Z

Thanks @jorge789 @dannyfpv , looks like there's an issue.

It seems using the existing contraints causes problems because it is later used in mc_pos_control, and when it step-changes later on, this causes a step in power straight from the controller. Unfortunately I think this will need a different approach.

jkflying · 2020-02-14T15:14:47Z

src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

+		// emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint.
+		if (_type == WaypointType::takeoff &&  _dist_to_ground < _param_mpc_land_alt1.get()) {
+			z_vel_constraint = _param_mpc_tko_speed.get();
+			z_accel_constraint = math::min(z_accel_constraint, _param_mpc_tko_speed.get() / _param_mpc_tko_ramp_t.get());


FYI @bresch This fixes the tracking while the controller is doing the velocity ramp with constraints.

Before:
https://review.px4.io/plot_app?log=0b49ba68-dd07-4bbb-9f91-1ac0a8aee33f

After:
https://review.px4.io/plot_app?log=a703e396-eddd-420d-8815-4f4f85b09422

jkflying · 2020-02-14T15:35:15Z

@PX4/testflights Could you give this another test? Same as before, run a mission that includes a takeoff and a landing.

dannyfpv · 2020-02-17T21:56:53Z

Tested on pixhawk4 v5 f-450
Flight Card 2

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

Procedure

Armed form QGC.
Took-off on mission plan mode
. Note that commands were sent form QGC (Armed and takeoff)
Once all waypoint completed vehicle switched to RTL and landed as expected.
Good flight in general.
Note:
No issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=08ee4eb2-9c3d-40d5-b7bf-938063ee3ec9
Log:
Comparison to MASTER
https://review.px4.io/plot_app?log=77228d20-2972-4c25-9913-ff875ecbdd14

jorge789 · 2020-02-17T22:43:46Z

Tested on PixRacer V4

Flight Card 2

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

Procedure

Armed form QGC.
Took-off on mission plan mode
. Note that commands were sent form QGC (Armed and takeoff)
Once all waypoint completed vehicle switched to RTL and landed as expected.
Good flight in general.
Note:
No issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=45702b81-f9f6-477e-bd59-bfca3a6fedfe

Log:
Comparison to MASTER
https://review.px4.io/plot_app?log=64bb06f1-7c78-4aa7-90be-7a564f1c550c

Tested on CUAV nano V5:

Flight Card 2

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

Procedure

Armed form QGC.
Took-off on mission plan mode
. Note that commands were sent form QGC (Armed and takeoff)
Once all waypoint completed vehicle switched to RTL and landed as expected.
Good flight in general.
Note:
No issues were noted, good flight in general.

Logs:
https://review.px4.io/plot_app?log=5bff3a96-9d2b-406a-b203-7c79f514ad5a

Log:
Comparison to MASTER
https://review.px4.io/plot_app?log=08519c19-bd02-4e2f-8551-cbd58f777b72

jkflying · 2020-02-18T09:26:20Z

Thanks @jorge789 @dannyfpv , looking good now.

This PR also fixes the velocity overshoot on takeoff, since that was a problem before:

Master:

This PR:

@bresch this is ready for review. Not 100% happy with where I had to put the logic, but I don't see a better way of doing it considering that the constraints introduce tracking problems.

bresch

~~Just change alt1 to alt2 and this is ok. Having to check the WP type in the flight task isn't great, but you can't have a better solution without more changes.~~ Sorry, I was confused because we have a ramp between alt1 and alt2 in position mode. We can update the logic later if needed.

src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

bresch

Thank you!

Unfortunately the constraints cannot be implemented using the _constraints because the controller uses that directly. This causes discontinuities in the velocity and/or accel at various points of the flight. In particular this was used in Takeoff. Instead this was done by changing target accel/velocity in the jerk-optimal velocity planner for Z.

MaEtUgR · 2020-03-05T08:40:14Z

This is a very nice improvement, we should move the takeoff and landing logic to be used within the flight task. That would leave no unexpected/external setpoint adjustment between task setpoint and position control.

jkflying requested a review from bresch February 13, 2020 15:46

jkflying changed the title ~~Change trajectory interface to use contraints for constraints and vel…~~ Use velocity_setpoint for feedforward instead of constraint Feb 13, 2020

bresch previously approved these changes Feb 13, 2020

View reviewed changes

src/lib/flight_tasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp Outdated Show resolved Hide resolved

jkflying dismissed bresch’s stale review via 6f9ad0f February 14, 2020 08:58

jkflying force-pushed the pr-constraint-vel-ff branch 2 times, most recently from 6f9ad0f to 4218771 Compare February 14, 2020 11:57

jkflying force-pushed the pr-constraint-vel-ff branch from 4218771 to c649d33 Compare February 14, 2020 14:47

jkflying commented Feb 14, 2020

View reviewed changes

weekly-digest bot mentioned this pull request Feb 16, 2020

Weekly Digest (9 February, 2020 - 16 February, 2020) #14167

Closed

bresch requested changes Feb 18, 2020

View reviewed changes

src/lib/flight_tasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp Show resolved Hide resolved

jkflying force-pushed the pr-constraint-vel-ff branch 2 times, most recently from 2e707d4 to 7ec42ed Compare February 18, 2020 10:16

jkflying requested a review from bresch February 18, 2020 10:19

jkflying force-pushed the pr-constraint-vel-ff branch from 7ec42ed to a5fb513 Compare February 18, 2020 12:29

bresch previously approved these changes Feb 18, 2020

View reviewed changes

jkflying dismissed bresch’s stale review via abb4873 February 18, 2020 12:55

jkflying force-pushed the pr-constraint-vel-ff branch from a5fb513 to abb4873 Compare February 18, 2020 12:55

jkflying merged commit e7aa899 into master Feb 18, 2020

jkflying deleted the pr-constraint-vel-ff branch February 18, 2020 13:00

weekly-digest bot mentioned this pull request Feb 23, 2020

Weekly Digest (16 February, 2020 - 23 February, 2020) #14213

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Use velocity_setpoint for feedforward instead of constraint #14155

Use velocity_setpoint for feedforward instead of constraint #14155

jkflying commented Feb 13, 2020 •

edited

Loading

bresch left a comment

jorge789 commented Feb 13, 2020

dannyfpv commented Feb 13, 2020 •

edited by Tony3dr

Loading

jkflying commented Feb 14, 2020

jkflying Feb 14, 2020 •

edited

Loading

jkflying commented Feb 14, 2020

dannyfpv commented Feb 17, 2020

jorge789 commented Feb 17, 2020

jkflying commented Feb 18, 2020 •

edited

Loading

bresch left a comment •

edited

Loading

bresch left a comment

MaEtUgR commented Mar 5, 2020

Use velocity_setpoint for feedforward instead of constraint #14155

Use velocity_setpoint for feedforward instead of constraint #14155

Conversation

jkflying commented Feb 13, 2020 • edited Loading

bresch left a comment

Choose a reason for hiding this comment

jorge789 commented Feb 13, 2020

Tested on PixRacer V4

Tested on CUAV nano V5:

dannyfpv commented Feb 13, 2020 • edited by Tony3dr Loading

jkflying commented Feb 14, 2020

jkflying Feb 14, 2020 • edited Loading

Choose a reason for hiding this comment

jkflying commented Feb 14, 2020

dannyfpv commented Feb 17, 2020

jorge789 commented Feb 17, 2020

Tested on PixRacer V4

Tested on CUAV nano V5:

jkflying commented Feb 18, 2020 • edited Loading

bresch left a comment • edited Loading

Choose a reason for hiding this comment

bresch left a comment

Choose a reason for hiding this comment

MaEtUgR commented Mar 5, 2020

jkflying commented Feb 13, 2020 •

edited

Loading

dannyfpv commented Feb 13, 2020 •

edited by Tony3dr

Loading

jkflying Feb 14, 2020 •

edited

Loading

jkflying commented Feb 18, 2020 •

edited

Loading

bresch left a comment •

edited

Loading