Separate out FlightTasks from position control module

[MaEtUgR]

Describe problem solved by this pull request
The FlightTasks library was designed to separate the core multicopter position control from the position, velocity, acceleration setpoint generation to fulfill various tasks. The separation hepled a lot in terms of modularity and debuggabilty and the main data flow is through generated message structs like vehicle_local_position_setpoint (, vehicle constraints, landing_gear) but the two pieces are not completely separate. The FlightTasks library is technically still inside the position control module. Also there are certain dependencies e.g. for landing takeoff limiting and similar cases that still lead to unexpected problems every now and then.

Describe your solution
I strated seprating out the FlightTasks library to live in a separate module that I called flight_mode_manager with the plan of the Navigator Architecture planning in mind that we aim towards having flight mode classes like flight tasks for all setpoint generation in the future. It's unfinished and I'm trying to separate certain dependencies in a good way to be able to cut a clear line between the modules.

The main pain points so far are:

• Position state preprocessing - The position control module should use the state as published by the estimator and not generate its own mixed state that is not available to other modules or in the log.
  EDIT: This has to be solved in a idfferent pr but I made sure it's no blocker anymore here.
• Emergency failsafe - I think the position control module should be able to handle the situation where setpoints are gone for a certain time or forever and the setpoint generation would stop publishing when things go really wrong.
• Hard limits vs specified constraints - We should get rid of specified constraints that get applied in the controller, we already started to do that. The hard limits stay to have sane execution.
  EDIT: This has to be solved in a idfferent pr but I made sure it's no blocker anymore here.
• Land/Takeoff special cases - Out of limits, more restrictions, safety critical land detection.
• WeatherVane mess - It's a global object that gets instanciated dynamically and passed around by pointer. It works around the entire architecture and we should move to the local scope where it's actually used.
  EDIT: This has to be solved in a idfferent pr but I made sure it's no blocker anymore here.

Test data / coverage
SITL flies no. Testing for correct takeoff ramp and so on still pending.

Additional context
Discussed starting point from https://docs.google.com/document/d/1N59JHFqL_E9X4b-U0CzxW_eydqXUHZMxAH_FTD30ilk/
Bigger picture tracked in https://github.com/PX4/Firmware/projects/34

[MaEtUgR]

@dagar So far, so good. First focus was to get it working in general. I know SupbscriptionData is fairly inefficient when used in that way. SITL flies now.

• rebase on mc_pos_control: split out header and name consistently #16032
• double-check I didn't overlook a part in the position control module
• couple of marked TODOs in flight mode manager corresponding to the original post points
• MAVROS tests failing EDIT: module was not in the test build and vtol startup 🤦

[MaEtUgR]

I rebased on master now. Let's finish this and push it through before getting huge conflicts.

[MaEtUgR]

• MAVROS tiltrotor shared_ptr.hpp:734: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gazebo::physics::PhysicsEngine; typename boost::detail::sp_member_access<T>::type = gazebo::physics::PhysicsEngine*]: Assertion px != 0 failed. 🤷‍♂️ reran
• MAVROS Rover timeout 🤷‍♂️ works after rebase reran
• Flash
• Rebase on MulticopterPositionControl: Reset task between auto mode switches #16304

[RomanBapst]

@MaEtUgR @dagar I'm working on the first flighttask for fixed wing (most likely ORBIT).

[RomanBapst]

@dagar @MaEtUgR I've been looking at adding the first simple fixed wing flighttask into this architecture. Tasks like LOITER and ORBIT are relatively simple to introduce but would already help cleaning up a lot in navigator. Imagine the entire reposition crap vanish.

Now, in the current flighttask architecture the interface from a flighttask to the controller is the trajectory_setpoint_message which is of type (vehicle_local_position_setpoint_s). This message was clearly designed for use by the multicopter position controller and not the fixed wing position controller.
The question is if we should extend the message or rather create a new one?

The fixed wing L1 controller currently has the following API:

1. Navigate_waypoints -> takes two vectors with lat lon coordinates (line following)
2. Navigate loiter -> takes one vector with lat, lon, has internal switching logic, uses L1 if far away, PD when close to circle
3. Navigate heading
4. Navigate_level->trivial

While the setpoint is given in global WGS84 coordinates, internally the controller calculates local planar vectors, which are 2D vectors in a local frame.
Therefore, we could think about switching to using local coordinates from the beginning, rather than doing the conversion in the controller.

Thoughts?

[MaEtUgR]

@RomanBapst Sounds great, thanks for your input and work on fixed wing support!

The question is if we should extend the message or rather create a new one?

Create a new one if necessary. We should not repurpose existing messages anymore like triplet and position setpoint. I'm happy to discuss the best content.

While the setpoint is given in global WGS84 coordinates, internally the controller calculates local planar vectors, which are 2D vectors in a local frame.

I would use local coordinates for the controller and deal with global frame(s) in the setpoint generation to make the controller lean, understandable and modular with a clear interface.

[RomanBapst]

@MaEtUgR Cool, thanks for your input. I think we are aligned.

[RomanBapst]

@MaEtUgR @dagar I rebased on top of master and tested in SITL both quad and vtol. I did not see any issues which don't occur on master.
Looks like sometimes the vtol backtransition is broken (tries to do a turn during the transition). This also happens on master, so nothing to do with this. Also once I noticed that when the vehicle landed after an RTL there was some weird state switching going on and the console was spammed with all sorts of messages.
Anyway, given the current state of master I think this is actually ok to go in.

[dagar]

Rebased on master (now with enough flash!).

[dagar]

This looks wrong (resetting if trajectory_setpoint didn't update). Given the module is scheduled on vehicle_local_position publications we should probably be resetting it if the module itself hasn't run for a while and you might as well check vehicle_local_position.v_xy_valid as well.

I'd still like to consider filtering and publishing the control data externally so that we always have exactly what was being used.

[MaEtUgR]

Yes, the intention was to not change too many things which gets non-rebasable/-reviewable. Originally this wasn't wrong since the position controller ran when a flight task ran and therefore the derivative had to start again when the position controller ran the next time. With this pr the assumption stays but it's certainly not the best way anymore to probe for the case and also the possible cases may change in the future.

[dagar]

It should be fine for now if the current expectation is that a flight task will publish a trajectory_setpoint for every single consumed vehicle_local_position and both flight_task_manager and mc_pos_control are running at the same limited rate.