New commander packet

[ataffanel]

Currently the commander packet format assumes a "normal" angle-based control mode and every other modes like altitude hold, position hold, rate mode, are handled using parameter and by fitting the relevant values in the current commander fields.

The plan is to make a new commander packet that encodes both the flight modes and the relevant setpoints values. This would make handling thing like altitude-hold and rate mode easier and it is required to start implementing position control in the Crazyflie.

We need to handle at least:

• Teleop in angle and rate mode
• Altitude-hold in angle and rate mode
• Position hold (keep current position and accepts x/y/z speeds to change position, kind of X,Y,Z teleop)
• Position Set (accepts absolute X,Y,Z)
• Others? ...

[whoenig]

• I am not sure what the difference between "Position hold" and "Position Set" is. For X/Y/Z teleop, position set seems to be sufficient (i.e. using the joystick updates the goal position). So it seems to me that "Position Set" is sufficient.
• Position Set/Hold should also get a target yaw angle (i.e. X/Y/Z/Yaw)
• There should be a more advanced version for the future which takes a trajectory point. The minimal version would be X/Y/Z/X dot/Y dot/Z dot/Yaw. More aggressive controllers might also need the accelerations (i.e. X dot dot, Y dot dot, Z dot dot) and yaw rate. This could be even combined with the simple version (for hover the target velocities and accelerations are simply 0).

[ataffanel]

• With position hold the setpoint is X,Y,Z velocity. With position set the setpoint is an absolute X,Y,Z. I see two different use-case that needs these two modes:
  • For position set the use-case is the copter receiving commands from a computer, for example for waypoint navigation
  • Position hold if more for 'advanced teleop'. For example it has been useful when debuging the internal controller: I flew the copter in the middle of the room and activate position hold, then the copter just stays there and I can move its setpoint. This could also be useful for GPS flight later. Basically in case where a computer is not necessary.
• Agreed
• We should find a way to keep simple control simple and allow for more complex one. If we combine the two mode these extra data could be optional.

[whoenig]

If the first byte of the new command is a mode/type, we could easily add different versions later once required. This is also consistent with the CRTP usage for the logging/parameter subsystem. I think in general it would be nice if the required setpoint packet to send can be as small as possible to increase the possible frequency (flight performance is much smoother at 100 Hz). Hence, I would prefer if it is not a big generic struct with unused values, but rather has a mode and then depending on the exact mode we only send exactly the required data over the radio.

[ataffanel]

Yes agreed for the not-too-generic (and by using a type as fist byte of the packet we can always make a "generic" type later if required ;-)

So What about doing a packet with:

+-------+--------+==================+
| 0x31  |  Type  |  Data            |
+-------+--------+==================+
   |        |        +-> Length and format of data defined by type
   |        +-> Type of packet
   +-> Port 3, channel 1

• The current commander is kept port 3 channel 0 for compatibility
• We can maintained the list of type and format on the Bitcraze Wiki (or in this repos to be added with PR?)
• The Crazyflie just ignores ukknown types setting thrust to 0?

That way we go forward and just add types when we need them.

[theseankelly]

I have some thoughts on this as well, but keep in mind my input is biased more towards bigquad/acro/racing/outdoor type flight, rather than indoor autonomous flight.

I like this idea with the type byte. Makes it nice and future proof.

I don't know much about the "position hold" system -- does it have any dead-reckoning approximations when flying without the loco positioning system? If so, I think it would make sense to drop "altitude hold" as a standalone mode and roll that into position hold -- it's just a single axis version of position hold.

In terms of supported types, I'd like to see at least these in addition to whatever's needed for the position hold system:

• 0x00: teleop, level mode (same as what we have today)
• 0x01: teleop, rate mode (acro)
• 0x02: teleop, horizon mode -- This is a hybrid mode -- some people love it and some people hate it. It's basically a blended level/rate PID where the farther you push the sticks over, the less weight is given to the self-level function, so you can still do rolls and flips. I personally dislike it quite a bit but might be wise to at least leave a placeholder for it for now.

Other open questions:

• Where does tilt/thrust compensation fit in? Perhaps that just applies to all types and is just enabled or disabled like it is today. Seems a little overkill to double the number of types for tiltcomp on or off.
• These modes should/can not be controllable through the parameter framework, right?

Is anyone working on this yet? I've got a hacked up version of the current commander packet to enable switching between level/rate on the fly, but I'd love to get it implemented in a more official way. I'm happy to start plumbing it unless someone else is already on it.

[ataffanel]

• I think things like thrust/tilt compensation is better in a parameter, this is not a setpoint it is more of a controller feature.
• This new format puts the mode and the values in the same packet so that we can switch modes without having to synchronize the parameters and commander packets.

I have started working on the new commander and for once I started with the documentation and it looks like that:

/* The new commander format contains a packet type and data that has to be
 * decoded into a setpoint_t structure. The aim is to make it future-proof
 * by easily allowing the addition of new packets for future use cases.
 *
 * The packet format is:
 * +------+==========================+
 * | TYPE |     DATA                 |
 * +------+==========================+
 *
 * The type is defined bellow together with a decoder function that should take
 * the data buffer in and fill up a setpoint_t structure.
 * The maximum data size is 29 bytes.
 */

/* To add a new packet:
 *   1 - Add a new type in the packetType_e enum.
 *   2 - Implement a decoder function with good documentation about the data
 *       structure and the intent of the packet.
 *   3 - Add the decoder function to the packetDecoders array.
 *   4 - Pull-request your change :-)
 */

How does it looks? I will add an X/Y/Z waypoint type/mode and push it so that we can start adding format for all our preferred control modes :-)

Since this will require a ROS driver modification I hacked X/Y/Z that we need now for the LPS in the current commander (sorry about that...).

@whoenig if we start running an internal controller in the Crazyflie, is there a standard interface for it in ROS (to set position/velocity)?

[whoenig]

This looks good to me. Instead of a "decoder function", I would suggest using a "packed" struct and then just casting the data portion to that struct (similar to how it is done for the logging/parameter subsystem in the firmware). This is very clear as documentation, and can be re-used on the client side as well.

I can change the ROS driver once the firmware is ready. I don't have a problem with breaking changes as long as they make sense:-)

For your question: In ROS it always depends on the robot. Typically, a Twist message (http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html) is used if you control the velocity (i.e. what you call rate mode) and a Pose message (http://docs.ros.org/api/geometry_msgs/html/msg/Pose.html) is used to set the pose (in case of a quadcopter it would only take the position and yaw into account). Of course, more advanced controllers might need more input data to follow a trajectory - in this case you would need to create a custom message in ROS.

[theseankelly]

Casting the data portion to a packed struct doesn't produce a setpoint_t -- I like the array of decoder function idea. That way the code in the commander can be something like decoderFunctions[packet[0]](&setpoint) and won't need a big switch statement for all the types.

But, using a packed struct to define all the types isn't a bad idea, and would be good practice. It just doesn't provide the mapping to setpoint_t.

I'd suggest not naming it the 'new commander' -- what would you call the third generation of the commander..the new new commandr? :D How about 'simple' for the current one and 'advanced' or 'complex' or something like that?

[ataffanel]

I have continued implementing the file and the functions actually decodes with a structure, I agree this is a good documentation for the packet structure. If we where to just provide a structure we would still need some code to copy the data in setpoint_t so having it in separate function seems cleaner and allow for some per-packet custom logic (like unit conversion). The client side consideration is interesting, are you meaning copy-pasting the structure or re-using the same header file?

Though I did hit a problem testing it because the current commander contains a lot of code to multiplex PPM. I will separate the two code (decoding and multiplexing) but I need some tester for it. Now Tobias is back from vacation and you could test as well @theseankelly ;-)

For the name what about commander packet V2, so it solves the future names :-). I don't like complex since this commander packet is supposed to be simpler in the end.

[theseankelly]

Sure, I can take a look. Is there a branch you're working out of?

At the risk of going off topic: Why does the commander have a ping-pong buffer for CommanderCrtpValues? Unless I'm missing something subtle, it doesn't seem to be serving a function since the activeside is updated immediately when a new commander packet is received. Could be extended pretty easily to more of a locking implementation if that's something that's necessary. [Edit: if the answer to this is more complex and involved and unrelated to the commander packet, let me know and I'll start a new issue or forum post on it instead :D]

[ataffanel]

I have been lagging on this ticket for a while, sorry about that, though it becomes required to have at least one packet for position control so lets agree on the format and just add it to the current comander.c code.

@theseankelly This is not off-topic at all, the current ping-pong buffering is something I have been wanting to change. My idea is to replace it with a 1-value FreeRTOS queue and to use the xQueueSendToFront function to update the value. While I wanted to do everything at once maybe revamping the code is good for another ticket as you are saying.

Wrapping it up: we can decide on a packet format in this ticket and maybe clean the actual code in another one. Since this is dealing with interfaces that span multiple projects it is important to decide on something that works well for all of us.

What about the format I described earlier with a type byte and a structure of data corresponding to this type?

[whoenig]

Sounds good to me. Would be nice to have so that we can provide a stock example on how to fly with the onboard position controller.

[theseankelly]

Sounds good to me too. The paranoia inside of me suggests adding a 'version' value right before 'type' to make it more future proof, but the rational part of me says no, that's a waste of packet space.

[whoenig]

Versions could be determined by switching to a new CRTP port/channel. Also, it would be possible to query the firmware version and adjust the logic based on that. Adding it to each packet might make the logic simpler, but is ultimately a big overhead (technically we should have that for each packet then).

[theseankelly]

Yep, agreed. My day job is in a very heavily versioned environment, so any time there's an externally consumed data type without a version, alarms go off. Here, though, it doesn't make sense.

[theseankelly]

Curious whether you've got something ready to commit? :D

[ataffanel]

Not yet, it is on my todo list. I'll try to push a simple first implementation soon.

[krichardsson]

Related to bitcraze/crazyflie-lib-python#32 and bitcraze/crazyflie-clients-python#280

[ataffanel]

We have finally made and pushed the new commander packet infrastructure! It is located there: https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/crtp_commander_generic.c (commander_generic is the least bad name we could come-up with...). We started by implementing a velocity control packet.

I bumped the protocol version to 3 but I also put the new generic commander packet in a new port (CRTP port 7, channel 0). This allows for TX-only clients (like the ios client) to send the new packets without risk. There is now a check for the channel so we have 3 more 'version' possible for this packet ;-).

There has also been a quite extensive refactor of the commander/setpoint code. There is now a single entry point for setpoints that takes a priority value: https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/interface/commander.h#L50. The intent is that any part of the code could push setpoint. The setpoint with the highest priority wins. If the setpoint with high priority is not set for 500ms a setpoint with lower priority will be allowed. This allows to fly with a PPM RC transmitter while being connected to the client (PPM has higher priority). This is also thought to help implementing more autonomous flight in the future (like reading a sequence of setpoint from an SD-Card).

Any comments are welcome! This is very new so we can still change it if we find flaws in it.

[dogty]

Hi,
I'm a student doing my master degree and I'm working on the crazyflie since June. I'm using the port you implemented to get the position and I just tested your new generic commander to send the velocity (I'm planning on sending the acceleration too I might create a new packet). I've updated the ROS topic in order to send the velocity via the port 7 (using a Twister stamped message).
I just wanted to point out that everything works like charm! I'm able to make different non linear controllers without difficulties and everything goes smoothly. I just wanted to thank you for all the effort you put in this drone and I hope to see more updates soon!

[ataffanel]

Thanks! Glad this is useful, I am then closing this ticket and consider it done. Feel free to open a ticket if the commander/setpoint subsystem need more work.