Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dev traj fixes #11399

Merged
merged 11 commits into from
Feb 13, 2019
Merged

Dev traj fixes #11399

merged 11 commits into from
Feb 13, 2019

Conversation

bresch
Copy link
Member

@bresch bresch commented Feb 6, 2019

Summary of the changes:

FlightTaskManualSmoothVel:

  • Add Z position lock. Until now, this flight task was controlling the Z velocity but relied on AltitudeSmooth FlightTask to perform altitude hold.
    trajectory_generator_altitude_lock

PositionControl:

  • Saturate the velocity setpoints using constant parameters. The constraints are used inside the FlightTask and not re-saturated to avoid non-linearities.

VelocitySmoothing:

  • Add trajectory logging
  • Global improvements, remove the recomputeMaxJerk logic that doesn't really help and only adjust the jerk when the current dt is longer than the previous one and that this was the last jerk pulse needed to reach the desired acceleration. This avoids acceleration oscillation due to jitter. See below.
    Before this PR:
    traj_generator_before
    Now:
    trajectory_generator_after

FYI: @MaEtUgR , @dagar

@dagar
Copy link
Member

dagar commented Feb 6, 2019

_jerk_min.set(0.f);
}

if (_jerk_min.get() > FLT_EPSILON) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So _jerk_min is not used anymore?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Correct. Several people reported that the logic felt weird so I decided to remove it until I find a better approach.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok, would be nice if we had a good way of dealing with this.

bkueng
bkueng previously approved these changes Feb 8, 2019
Copy link
Member

@bkueng bkueng left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good from my side.

_jerk_min.set(0.f);
}

if (_jerk_min.get() > FLT_EPSILON) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ok, would be nice if we had a good way of dealing with this.

@dagar
Copy link
Member

dagar commented Feb 8, 2019

Can you rebase on master (to consume an unrelated bug fix) and we'll ask @PX4/testflights to give this a try?

@dannyfpv
Copy link

dannyfpv commented Feb 11, 2019

@bresch
Copy link
Member Author

bresch commented Feb 12, 2019

@dannyfpv Thanks for the non-regression tests!
Could you also test in position mode with the following parameters:

  • MPC_POS_MODE = 3 (smooth velocity)
  • MPC_ACC_DOWN_MAX = 2
  • MPC_ACC_UP_MAX = 4
  • MPC_ACC_HOR_MAX = 5

And also a mission using the parameters above and:

  • MPC_YAW_MODE = 3 (along trajectory)
  • MPC_XY_TRAJ_P = MPC_Z_TRAJ_P = 1

Thanks!

@dannyfpv
Copy link

dannyfpv commented Feb 12, 2019

tested on pixhawk4 v5
test in position mode(smooth velocity)
https://review.px4.io/plot_app?log=e5e9296f-ca4f-4018-aeb9-74d78d01f5e8

mission mode
https://review.px4.io/plot_app?log=c6af13a2-849e-4087-8b28-0f403fac95dc

pixhawk4 mini v5
test in position mode(smooth velocity)
https://review.px4.io/plot_app?log=41d7f899-d4bd-44c8-adfa-a8fc0fdfa118

mission mode
https://review.px4.io/plot_app?log=3795d075-750a-4547-91db-f0ae6dd0b558

Good flight performance, no issues found.

@jorge789
Copy link

jorge789 commented Feb 13, 2019

test on pixhawk racer
test in position mode(smooth velocity).
https://review.px4.io/plot_app?log=ed96e3aa-6f34-4e40-9ba4-24d7d34785fb

mission mode
https://review.px4.io/plot_app?log=25cdec90-03ba-4100-8f30-856a1b317921

teste on pixhawk pro.
test in position mode(smooth velocity).
https://review.px4.io/plot_app?log=f1a43f60-c5fb-4258-9e80-c71f9dbd6a92

mission mode
https://review.px4.io/plot_app?log=83b5cb70-26dc-4430-9b52-c1aa8713b783

Good flight performance, no issues found.

@bresch
Copy link
Member Author

bresch commented Feb 13, 2019

Thanks @dannyfpv & @jorge789 !

I'll check the logs ang give the final result before merging.

@bresch
Copy link
Member Author

bresch commented Feb 13, 2019

Okay, looks good to go!

Side note: @jorge789 your vehicle with the Pixhawk Pro has some vibration issues :/

@Tony3dr
Copy link

Tony3dr commented Feb 13, 2019

@bresch Thanks for pointing that out, we just got some new props for that setup. We will look into it.

@bresch bresch merged commit 197ddd3 into master Feb 13, 2019
@bresch bresch deleted the dev-traj-fixes branch February 13, 2019 19:57
@MaEtUgR MaEtUgR mentioned this pull request May 10, 2019
Copy link
Member

@MaEtUgR MaEtUgR left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@bresch Please check in the future if you didn't break any existing functionality and let me review the core position control changes such that I can hopefully find potential problems before they go in.

Fixing the smooth takeoff problems is now tracked here: #12000 I'll try to find an acceptable solution and let you test as well if it works for all the cases you require.

@@ -221,7 +221,7 @@ void PositionControl::_positionController()
// Constrain horizontal velocity by prioritizing the velocity component along the
// the desired position setpoint over the feed-forward term.
const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
Vector2f(_vel_sp - vel_sp_position), _constraints.speed_xy);
Vector2f(_vel_sp - vel_sp_position), MPC_XY_VEL_MAX.get());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

With this the controller ignores any horizontal velocity constraint set by a flight task and hence broke FlightTaskAuto and all missions cruise or customized speed handling and was the root cause for this whole churn:
#11764
#11772
#11811
#11836.

@@ -1045,18 +1046,15 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl
0.2f;

// takeoff was initiated through velocity setpoint
_smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < math::min(-_tko_speed.get(), -0.6f);
_smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This makes the drone starting to take off already with the smallest possible upwards setpoint and can hence lead to accidental early takeoffs with not enough thrust to really leave the ground and scraping the ground with hover thrust or even tip overs.


if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) {
// There is a position setpoint above current position or velocity setpoint larger than
// takeoff speed. Enable smooth takeoff.
_in_smooth_takeoff = true;
_takeoff_speed = -0.5f;
_takeoff_speed = 0.f;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This breaks the smooth takeoff by not starting below hover thrust anymore and as a result the drone will always directly jump into hover.


} else {
// No ramp, directly go to desired takeoff speed
_takeoff_speed = desired_tko_speed;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not skip the entire smooth takeoff logic when there's anyway no ramp?

_in_smooth_takeoff = _takeoff_speed < -vz_sp;
} else {
// Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector
_in_smooth_takeoff = (_takeoff_speed < -vz_sp) || _vehicle_land_detected.landed;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It doesn't make sense to stay in smooth takeoff when the desired velocity setpoint after the initial step was already reached. It doesn't matter if the vehicle is still on the ground or not. This will result in a downwards step of the velocity as soon as the ramp is over because the takeoff was detected by the land detector.

I'm assuming you inserted this because you lowered the threshold to start the takeoff to a value so low (_smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; from above) that the vehicle stays on the ground and correctly still reports landed.

_takeoff_speed = desired_tko_speed;
}

_takeoff_speed = math::min(_takeoff_speed, _tko_speed.get());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This prevents the smooth velocity ramp to ramp up correctly. As soon as the desired takeoff setpoint is above the takeoff speed parameter it will get limited and not reach the goal anymore until the desired velocity setpoint comes down again.
The result will be: The ramp will still be faster for high initial desired setpoint but then suddenly gets cut off to the takeoff speed parameter value until you stop vertical movement and start again to go up. Was that intended?

@MaEtUgR MaEtUgR mentioned this pull request May 11, 2019
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

7 participants