Merge AutoMapper2 and AutoMapper

[Stifael]

Currently there are two code paths for two different Auto implementation. Although Flighttask was designed to make it simple to add new tasks without effecting other tasks, we still should try to keep similar functionalities together.
For instance, AutoMapper and AutoMapper2 share similar or exactly the same functionalities, but all the methods are duplicated. This means that any change to Automapper has to be done twice. However, I am quite confident that contributors will forget about it at some point and only make changes to one of the files.
An other problem with having two code paths is to find bugs. As the issue here illustrates, there is currently even a different behavior between MPC_AUTO_MODE 0 and MPC_AUTO_MODE 1 when the vehicle arms in hold-mode. There is no reason why the two different modes cannot share the exact same code path for arming.

I attached a diff-patch-file between AutoMapper and AutoMapper2. As you can see from the patch-file, the diff is mainly due to renaming and some changes in AutoMapper2 would also improve AutoMapper.

Automapper_diff.zip

[MaEtUgR]

How about a link to the diff: https://www.diffchecker.com/ZpJnGs2N

[bresch]

@Stifael I totally agree. In order to combine those similar files, I have a few question and remarks:

1. I removed the "reset when switching to line_following"
   https://github.com/PX4/Firmware/blob/29f034c53e1f187d99edfc07908fe94defe7d0ac/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp#L57-L63
   because this introduces a glitch when switching from takeoff to line-following and switching from line-following to landing for example. Do you really need that in the standard line-following algorithm? Can we maybe move it into the line-following flight-task?

2. Since the constraints are used to saturate the velocity controllers, I don't like to use them to constrain the flight task.
   For example:
   https://github.com/PX4/Firmware/blob/29f034c53e1f187d99edfc07908fe94defe7d0ac/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp#L123
   I think that giving the correct velocity value is enough.
   Furthermore, in order to set a velocity constraint in addition to a position setpoint to the flight task (without changing the constraint of the controller), I decided to use the corresponding velocity_setpoint.
   For example
   https://github.com/PX4/Firmware/blob/29f034c53e1f187d99edfc07908fe94defe7d0ac/src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp#L138-L140
   gives the target as 3D point and limits the Z up speed to speed_tko
   Can we do the same logic in the default line-following?

3. I'm not using MPC_LAND_ALT2 to generate the acceleration/deceleration in takeoff and landing since this is handled by the constraints of the trajectory generator itself.
   https://github.com/PX4/Firmware/blob/29f034c53e1f187d99edfc07908fe94defe7d0ac/src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp#L134-L135
   Removing that from the AutoMapper will remove a desired behavior when using the default line-following algorithm. Do we simply set ALT1 = ALT2 by default (in addition to moving the constraint into velocity_setpoint)?

[MaEtUgR]

3. ... Do we simply set ALT1 = ALT2 by default

That ALT1, ALT2 is my construct. We could simply have one altitude (rename ALT1 to ALT) and for my gradual implementation take a fixed offset above that to start slowing down and your implementation does it's own smooth implementation. I could make a pr for this. What do you think?

[bresch]

1. ... Do we simply set ALT1 = ALT2 by default

That ALT1, ALT2 is my construct. We could simply have one altitude (rename ALT1 to ALT) and for my gradual implementation take a fixed offset above that to start slowing down and your implementation does it's own smooth implementation. I could make a pr for this. What do you think?

Fine for me!

[bresch]

Also, in #11169 I change the actual behavior of the velocity waypoint:
https://github.com/PX4/Firmware/pull/11169/files#diff-820cd5bfd49c97786dc540f5f3974b3b
Such that one can specify the desired velocity and altitude without an XY position. I believe this was never used in the code. Are you okay with that change as well? @Stifael @MaEtUgR

[MaEtUgR]

@bresch

1. I think @Stifael can best answer
2. If you are setting no position setpoint there's no point in setting a constraint for the velocity setpoint because you can set and also limit it directly before it goes out of the task. The constraints setup is for the case where you set a position setpoint which the controller should execute but you don't want to go there as fast as possible. Takeoff is a good example: you want to reach the predetermined takeoff point but with a limited takeoff speed. In that case you should not replicate a position controller within your task but rather set the position setpoint and the constraint. Does that makes sense?
3. ok, I'll do that then.

I change the actual behavior of the velocity waypoint

Sorry I don't understand what changed there generally with a task you can specify the desired velocity and altitude without an XY position and this is e.g. used in the FlightTaskAltitude. Is it now different in the mapper for auto modes?

[MaEtUgR]

@bresch

1. I think @Stifael can answer best.
2. If you don't set any position setpoint it's not useful to constrain the velocity setpoint because you can set it to a different value yourself. But if you set a position setpoint bot don't want to fly there as quick as possible that's what the constraint is for. A takeoff is a good example: you set the position setpoint to the point you want to reach but want to fly there with the takeoff speed instead of jumping up. This functionality is there Does that makes sense?
3. ok, then I'll do it.

I change the actual behavior of the velocity waypoint

Sorry I don't undesrstand what changed. For the output setpoint of a task you can specify the desired velocity and altitude without an XY position like it's done in FlightTaskAltitude. Are you talking specifically about the mapper task here?

[Stifael]

@bresh

Can we maybe move it into the line-following flight-task?

Yes, that is possible and even better.

a.) constraints during landing
Sure, you can remove it. As @MaEtUgR already mentioned, if there is only a velocity-setpoint present (no position-setpoint), then there is no need to set the constraints. However, as long as the constraint is the same as the velocity-setpoint, then it should not hurt either.

b.) takeoff
I think both solutions are valid. More important is that Autosmooth and Autoline have the exact same takeoff. If you prefer to use the velocity-setpoint, that is also fine.

Sure, let's have the exact same takeoff, and if necessary I can make further adjustments in AutoLine.

[Stifael]

Such that one can specify the desired velocity and altitude without an XY position. I believe this was never used in the code. Are you okay with that change as well?

sure, you can mix the xy- and z-setpoints as much as you want and combine them to get feedforward. However, there is no support for position/velocity-setpoint + thrust setpoint. This is mentioned here.
In addition, even though there is an acceleration-setpoint in flighttask, this setpoint is currently not handled.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[bresch]

Thanks stale bot, but don't close that!

[MaEtUgR]

I did this in 7e78eed Saved us a lot of flash space.