Consider reworking the setpoint struct

[krichardsson]

The setpoint struct (https://github.com/bitcraze/crazyflie-firmware/blob/2020.04/src/modules/interface/stabilizer_types.h#L170-L191) is very flexible, but also makes controller implementations hard to understand. Maybe we can find a better structure?

Comments from #567:

@whoenig

I don't like the setpoint struct that much for the reason that not all combinations make sense. It would be nicer to have an enum and union for the desired "flight mode". Then each controller could also decide which flight modes it can actually support properly. In the situation awareness framework, we would only enable the collision avoidance in some of the flight modes. The firmware controllers already go that route and only support setpoint mode combinations that are "common", but it is hard to decipher which combinations are even valid.

@jpreiss

I think the concept of "flight mode" could also potentially subsume the state machine logic in planner.c, the switching between low-level and high-level modes in commander.c, and the low-level setpoint packet types in crtp_commander_generic.c.

[jpreiss]

Thinking about this some more.

Flaws of current design

First, there are some aspects of the current design that could be improved without changing the overall philosophy:

• I can't think of a case when x and y position modes need to be different. We could consolidate them in mode.xy. Same for roll/pitch modes.
• If z control is disabled, then a thrust input is required, but the mode enum does not make this obvious.
• The setpoint contains an acceleration vector, but the enum doesn't indicate how/if the controller should use it.
• It is possible that we want to control both {xy abs, xy vel} with vel as a feedforward term. The setpoint struct can express this, but the enum values cannot. Same for {quat, attitude vel} and {attitude abs, attitude vel}.

Rules of factored design

Next, I tried to figure out the rules to decide whether a combination of mode flags is valid. I am imagining that we add a mode.thrust field to explicitly state if the thrust input is to be used.

Note that it is OK for more then one of {xy, attitude abs, quat, attitude rate} to be controlled, because the higher order derivatives can be interpreted as feedforward terms by the following ordering:

xy abs <- xy vel <- {attitude abs, quat} <- attitude rate

Of course, the user could send nonsensical commands like constant position over time with nonzero velocity, but that is "undefined behavior". On the other hand, breaking any of these rules makes a setpoint nonsensical on its own:

• quat should never be vel - if we want to control angular velocity, use body Euler angles.
• thrust can never be vel.
• If quat is abs, then none of roll/pitch/yaw can also be abs.
• yaw cannot be disabled.
• At least one of {xy, attitude abs, quat, attitude vel} must be set, otherwise the horizontal situation is uncontrolled.
• At least one of {z, thrust} must be set, otherwise altitude is uncontrolled.

These are not hard rules, but combinations that seem strange:

• There is probably no reason to control thrust and z at the same time. If we want a feedforward input for position, we should use the acceleration value.
• It is hard to imagine why we would want to control thrust unless we are also controlling roll/pitch (e.g. cmdVelocityWorld + thrust doesn't make sense).
• If we are controlling roll/pitch only by velocity (quat is disabled), it is like "acro mode" manual flight, so we also should control yaw by velocity, not abs.

Enumeration of valid modes

I implemented these rules in a python script to produce a table (https://repl.it/@jpreiss/crazyfliesetpoints#main.py). It filters down from 729 possible combinations to 53 valid combinations:

             xy        z   thrust roll/pitch  yaw     quat
1           abs      abs  disable        abs  abs  disable
2           abs      abs  disable        abs  vel  disable
3           abs      abs  disable        vel  vel      abs
4           abs      abs  disable        vel  vel  disable
5           abs      abs  disable    disable  abs  disable
6           abs      abs  disable    disable  vel      abs
7           abs      abs  disable    disable  vel  disable
8           abs      vel  disable        abs  abs  disable
9           abs      vel  disable        abs  vel  disable
10          abs      vel  disable        vel  vel      abs
11          abs      vel  disable        vel  vel  disable
12          abs      vel  disable    disable  abs  disable
13          abs      vel  disable    disable  vel      abs
14          abs      vel  disable    disable  vel  disable
15          abs  disable      abs        abs  abs  disable
16          abs  disable      abs        abs  vel  disable
17          abs  disable      abs        vel  vel      abs
18          abs  disable      abs        vel  vel  disable
19          abs  disable      abs    disable  vel      abs
20          vel      abs  disable        abs  abs  disable
21          vel      abs  disable        abs  vel  disable
22          vel      abs  disable        vel  vel      abs
23          vel      abs  disable        vel  vel  disable
24          vel      abs  disable    disable  abs  disable
25          vel      abs  disable    disable  vel      abs
26          vel      abs  disable    disable  vel  disable
27          vel      vel  disable        abs  abs  disable
28          vel      vel  disable        abs  vel  disable
29          vel      vel  disable        vel  vel      abs
30          vel      vel  disable        vel  vel  disable
31          vel      vel  disable    disable  abs  disable
32          vel      vel  disable    disable  vel      abs
33          vel      vel  disable    disable  vel  disable
34          vel  disable      abs        abs  abs  disable
35          vel  disable      abs        abs  vel  disable
36          vel  disable      abs        vel  vel      abs
37          vel  disable      abs        vel  vel  disable
38          vel  disable      abs    disable  vel      abs
39      disable      abs  disable        abs  abs  disable
40      disable      abs  disable        abs  vel  disable
41      disable      abs  disable        vel  vel      abs
42      disable      abs  disable        vel  vel  disable
43      disable      abs  disable    disable  vel      abs
44      disable      vel  disable        abs  abs  disable
45      disable      vel  disable        abs  vel  disable
46      disable      vel  disable        vel  vel      abs
47      disable      vel  disable        vel  vel  disable
48      disable      vel  disable    disable  vel      abs
49      disable  disable      abs        abs  abs  disable
50      disable  disable      abs        abs  vel  disable
51      disable  disable      abs        vel  vel      abs
52      disable  disable      abs        vel  vel  disable
53      disable  disable      abs    disable  vel      abs

Although many of these 53 combinations seem odd, I can't find a reason why any of them are truly nonsensical. I guess the real question is: how many of them have a compelling use case?

We already have 7 setpoint types in crtp_commander_generic.c and these expand to more than 7 flag combinations during parsing.

Sharing this in case it is useful for future discussions. I am not sure how to interpret it.

[krichardsson]

Nice summary @jpreiss !

[jonasdn]

Thanks for all the work in this issue!

The idea to convert the struct to one with 53 enums and 53 unions is interesting but daunting. We are not sure the complexity would be reduced with this approach. But if someone disagrees and wants to show us a PR we would absolutely look closely at that code!

To start with, we would like to use this information from @jpreiss to document the setpoint struct. And possibly to add some kind of validation scheme for setpoints: is_valid(setpoint)

The goal is to make it clear to both users of the setpoint API and for users/implementers of controllers what is valid and not. And it would be very nice if one can look up a controller, in code or docs, and get an idea of which combination (of the 53) this controller supports.

If anyone has an idea of where and how to add this documentation, please comment here!