FlightTask - Proper initialization after switching task #12389
Conversation
bresch
added
Admin: Enhancement (improvement) 💡
Multirotor 🛸
Hybrid VTOL 🛩️🚁
why cannot each task check for finiteness and initialize the setpoints, especially since not every task cares about every setpoint? |
|
@Stifael For example, each class that cares about position has it own |
|
VTOL flight with the Convergence in mission: https://logs.px4.io/plot_app?log=6d36fc4a-a195-4666-a667-1f05289a9712 end of back-transition switches smoothly to MC auto mode |
|
The deceleration is not as high as in simulation but we can still see that velocity setpoint initializes with the correct value and slope (i.e.: acceleration). |
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
Outdated
Show resolved
Hide resolved
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
Show resolved
Hide resolved
| @@ -110,6 +143,14 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con | |||
| return math::constrain(val, min, max); | |||
| } | |||
|
|
|||
| void FlightTaskAutoLineSmoothVel::_initEkfResetCounters() | |||
There was a problem hiding this comment.
Why is this method duplicated in FlightTaskManualPositionSmoothVel? Should it not be added to the common ancestor that anyways evaluates _sub_vehicle_local_position?
There was a problem hiding this comment.
The common ancestor of Auto and ManualPosition is - if I remember correctly - FlightTask. I agree that this duplication is far from nice, but I don't see how to do it otherwise...
There was a problem hiding this comment.
In principle the subscription of the vehicle position state is handled here: https://github.com/PX4/Firmware/blob/f613581b701700d2abea1caf132ec17788ce0bed/src/lib/FlightTasks/tasks/FlightTask/FlightTask.cpp#L89 and hence the common ancestor makes sense... I'm also not sure how to handle ekf resets properly. I'd like to have them wrapped because resetting every state in every possible position related task is very cumbersome and error-prone.
bresch
force-pushed
the
dev-flighttask-continuous-switch
branch
from
c748579 to
1455a57
Compare
|
@MaEtUgR Thanks for the review. I went through the comments, made the modifications and rebased on master. Can you review again? |
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
Outdated
Show resolved
Hide resolved
|
Can you also get rid of the merge commit? Thanks |
…ious task to the new task This is done to allow proper initialization of the new FlightTask and give it a chance to continue the setpoints without discontinuity. The function checkSetpoints replaces the setpoints containing NANs with an estimate of the state. The estimate is usually the current estimate of the EKF or zero. The transition FlightTask also provides an estimate of the current acceleration to properly initialize the next FlightTask after back-transition. This avoid having to initialize the accelerations to zero knowing that the actual acceleration is usually far from zero.
…ilored to the specific needs and avoid checking all the setpoints
bresch
force-pushed
the
dev-flighttask-continuous-switch
branch
from
212182a to
727e8ac
Compare
Describe problem solved by the proposed pull request
Currently, a task doesn't know the setpoints that the previous one was outputting. The best initial setpoint is then to set it to the current estimate. The problem with this strategy is that the setpoints are discontinuous and this generates glitches when stitching tasks. This problem is particularly visible when switching out of the transition FlightTask: the switch generally occurs at a high speed and deceleration; without proper initialization, an attitude glitch is quite visible.
The image below shows a back transition of a standard VTOL plane. We can see that initializing the current acceleration setpoint to zero is a quite bad assumption.
Also, between manual position control and auto (hold in this example), the velocity setpoint is discontinuous:
Solution
When switching tasks, the FlightTasks class can get the last setpoints of the previous task and transfer them to the new task through the activate function.
Result on a VTOL back-transition:
During transition, the FlightTask estimates the current acceleration and can transmit it to the manual or auto flight task for proper initialization.
Result when switching from manual position to auto:
The switch is invisible as the auto task initializes with the exact setpoints of the manual position one.
@Stifael the checkSetpoints() function is there to replace the NANs in the setpoints with the estimate. This is called in the activate function; The problem is that if the activate function calls an other activate function, checkSetpoints could be called multiple times and this is inefficient. On the other hand, I don't want to store all those setpoints in the base class because they are only needed during initialization. Do you have a better idea?