-
Notifications
You must be signed in to change notification settings - Fork 13.7k
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
Dev traj fixes #11399
Conversation
CI failure is a trivial style issue. http://ci.px4.io:8080/blue/organizations/jenkins/PX4%2FFirmware/detail/dev-traj-fixes/4/pipeline |
_jerk_min.set(0.f); | ||
} | ||
|
||
if (_jerk_min.get() > FLT_EPSILON) { |
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
Show resolved
Hide resolved
There was a problem hiding this 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) { |
There was a problem hiding this comment.
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.
Can you rebase on master (to consume an unrelated bug fix) and we'll ask @PX4/testflights to give this a try? |
…velocity target of the trajectory generator
…s. Handle saturations in the flight tasks only
…ller for now as it is needed for the current smooth takeoff logic. Will be reverted after smooth takeoff refactor
…t initial takeoff speed to 0 and remove useless else
122b2c6
to
40b8ae1
Compare
Tested on pixhawk4 v5 good flight Pixhawk4 mini v5 Pixhawk v4 pro Pixracer v4 All good flights, takeoff -waypoint- Transition, was successful. |
@dannyfpv Thanks for the non-regression tests!
And also a mission using the parameters above and:
Thanks! |
tested on pixhawk4 v5 mission mode pixhawk4 mini v5 mission mode Good flight performance, no issues found. |
test on pixhawk racer mission mode teste on pixhawk pro. mission mode Good flight performance, no issues found. |
Okay, looks good to go! Side note: @jorge789 your vehicle with the Pixhawk Pro has some vibration issues :/ |
@bresch Thanks for pointing that out, we just got some new props for that setup. We will look into it. |
There was a problem hiding this 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()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@@ -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; |
There was a problem hiding this comment.
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; |
There was a problem hiding this comment.
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; |
There was a problem hiding this comment.
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; |
There was a problem hiding this comment.
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()); |
There was a problem hiding this comment.
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?
Summary of the changes:
FlightTaskManualSmoothVel:
PositionControl:
VelocitySmoothing:
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:
Now:
FYI: @MaEtUgR , @dagar