-
Notifications
You must be signed in to change notification settings - Fork 13.2k
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
FlightTask Introduction Replace Legacy Multicopter Position Controller #9731
Conversation
Given #9270 (comment) is it worth pushing the core FlightTasks interface improvements to master in a separate PR? I'm wondering if that would help unblock obstacle avoidance migration. |
@dagar, why would obstacle avoidance be blocked by Flighttask? |
I cherry-picked the yaw-speed correction from the orbit pr because it's important for the functionality here and the orbit needs to be rebased onto this anyways. |
|
||
/* check if stick direction and current velocity are within 60angle */ | ||
const bool is_aligned = (stick_xy_norm * stick_xy_prev_norm) > 0.5f; | ||
if (PX4_ISFINITE(_states.velocity(2))) { |
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.
move this stuff outside. this requires to check for landdetector and if in air, then slowly descend, if landed, then just go idle
The range sensor will be fused into the Local Z estimate for low altitude flight if Range Aid is enabled, however this doesn't explicitly affect the controller.
This is to stay within limits of additional sensors (e.g optical flow or visual odometry), but whether it should be active for altitude-control only is up for discussion.
Generally yes. If the rangefinder sensor is out of range, then these flags will go to false. |
I have some planned changes which add functionality to improve height control when using optical flow over uneven surfaces. I will create them on top of this PR as another branch so this PR will be incorporated into my testing. |
// A change in velocity is demanded. Set velocity to the derivative of position | ||
// because it has less bias but blend it in across the landing speed range | ||
float weighting = fminf(fabsf(vel_sp_z) / _land_speed.get(), 1.0f); | ||
_states.velocity(2) = _local_pos.z_deriv * weighting + _local_pos.vz * (1.0f - weighting); |
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.
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.
fixed
} | ||
} | ||
|
||
void | ||
MulticopterPositionControl::reset_alt_sp() | ||
MulticopterPositionControl::check_vehicle_states(const float &vel_sp_z) |
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.
return ((PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) && !(_pos_sp_triplet.current.cruising_speed < 0.0f)) ? | ||
_pos_sp_triplet.current.cruising_speed : _vel_cruise_xy.get()); | ||
if (PX4_ISFINITE(_local_pos.yaw)) { | ||
_states.yaw = _local_pos.yaw; |
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.
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.
INFINITY and NAN should both return 0 if checked with PX4_ISFINITE.
/* we do manual deceleration */ | ||
intention = deceleration; | ||
// limit altitude only if local position is valid | ||
if (PX4_ISFINITE(_states.position(2))) {limit_altitude(setpoint);} |
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.
if (_jerk_hor_max.get() > _jerk_hor_min.get()) { | ||
_manual_jerk_limit_xy = (_jerk_hor_max.get() - _jerk_hor_min.get()) / _velocity_hor_manual.get() * | ||
sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)) + _jerk_hor_min.get(); | ||
mavlink_log_info(&_mavlink_log_pub, "[mpc] stopped"); |
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.
6cd0169
to
1206d9b
Compare
1206d9b
to
300e6e2
Compare
I rebased again in the hope CI will be happier. |
Here is again the summary of the PR: Here is again the summary of the different files: What needs to be tested: |
For reference here's the current code coverage in master from the CI system executing a simple mission (https://logs.px4.io/plot_app?log=8cda399e-815f-4d48-b614-e989251f7536). Here's this PR. |
@Stifael @MaEtUgR @thomasgubler @RomanBapst @bkueng can we document all the current/expected multicopter position controller capabilities here? https://docs.google.com/document/d/1q1wkqufilFYRzskx9OJcmOHpEQOAgs8fEKEHX2V4ooo/edit?usp=sharing Then let's use it to structure exhaustive manual (flight cards) and automated (ROS or Dronecode SDK) testing to look for regressions and edge cases. |
@dagar sounds good. |
That's fine, we really just need to have a thorough understanding of the state before and after. Functionality doesn't necessarily need to be replicated 100%. For example if follow me doesn't actually work in master we might be better off leaving it temporarily disabled in FlightTasks until someone wants to get it working properly. |
…d distance sensor
…velocity estimate
It scales the yawspeed setpoint arbitrarily by default with 0.5. This makes no sense because when you give a setpoint of 1rad/s then you expect the setpoint to get executed. If you want manual yawspeed response to be less agressive on the stick use the scaling parameter for the stick MPC_MAN_Y_MAX.
…ng() Without this failsafe will be overwritten
invert direction to point upwards and increase to 70% of throttle range between min and hover
32084b7
to
7917a1f
Compare
@PX4/testflights do you mind to test this branch once more, just to get some more flight hours, which should bring us closer to a version that we can finally merge. |
@dagar AV-X target failed: |
Flew on a Pixhack (V5) with new commits stabilized, altitud and position mode. Flew on a Pixmini (V3) with new commits Flew on a Pixhawk pro (V4) with new commits |
It's a follow up to #9253 and #9368.
I rebased all changes to any FlightTasks and the PositionControl logic and left out all the changes to the
mc_pos_control_main.cpp
file because of nasty rebase conflicts. Then in the last commit "mc_pos_control: replace legacy functionality" I switched the mc_pos_control file to the new refactored version that does not contain any unwanted, replaced legacy code anymore.Note: The original changes are mostly by @Stifael as correctly represented in the metadata.
We know what's missing right now is:
See any of these sources:
- History in June
- The file
src/modules/mc_pos_control/mc_pos_control_main.cpp
in this diff: Legacy-Auto-Manual-replacement...master
- The only prs that did changes: Optical Flow Use Improvements - supersedes #9509 #9613 and replace geo _wrap_pi with matrix::wrap_pi #9630
closes #9368