ESC Messages
Following on with my dive into the proposed battery, ESC and servo messages, here is what I got from looking at the ESC dsdl. The ESC messages are a much more complex design than the battery messages, and really show some of the design flaws in the type system for UAVCAV v1.
Before we get into the messages themselves we need to first off discuss āgroupsā in the documentation for ESCs in UDRAL. The doc says:
# ESCs (drives) are segregated into groups. Each ESC in a group has an index that is unique within the group.
# Drives in a group are commanded synchronously by publishing a message containing an array of setpoints.
ok, so having groups could make sense. We donāt need it for ArduPilot (we have a flat namespace of actuator numbering), but I could see some systems using it. The problem is I cannot at all see how the group you are trying to control is communicated. There is no group ID field in any of the messages that I can see, so I wondered if it was a top
level concept Iād missed in the UAVCAN_Specification_v1.0.pdf, so I checked there and while the word āgroupā is used for quite a few different things (eg. a byte is a group of 8 bits), there is no use of group in there that makes sense with this ESC usage of group.
Can someone explain how groups work?
Now on to the messages to command the ESCs to spin. There are two sets of messages,
reg.udral.service.actuator.common.sp.* and
reg.udral.service.common.Readiness.
The wildcard reg.udral.service.actuator.common.sp.* is really odd. It comes from the way zero truncation in UAVCAN v1 is handled. In UAVCAN v0 it is this simple array in 1030.RawCommand:
int14[<=20] cmd
In the proposed URDAL message the recipient needs to subscribe to Vector31.0.1.uavcan which is this:
float16[31] value
but the sender (typically the flight controller) has to choose which of the following messages to send based on the number of actuators:
Scalar.0.1.uavcan
Vector2.0.1.uavcan
Vector3.0.1.uavcan
Vector4.0.1.uavcan
Vector6.0.1.uavcan
Vector8.0.1.uavcan
Vector31.0.1.uavcan
(all of these are down inside the reg.udral.service.actuator.common.sp.* name space).
This then takes advantage of the zero truncation so when the ESC receives Vector31 and the flight controller sends Vector6 that the ESC gets zeros in the later elements.
This way of handling arrays of ESC commands is very awkward, and requires that the flight controller have code for sending a bunch of different messages for the same purpose, presumably using a set of if statements to work out the threshold.
It is also inefficient if the ID number of the ESCs youāre trying to address doesnāt start with zero. For example, you may want your ESCs on a quadplane to be ID numbers 5 to 8 (which is the default for ArduPilot quadplanes). With this structure the flight controller has to send a Vector8, with the first 4 elements as zero. It would be much better to have a bitmask at the start of message saying which ESCs you are wanting to address. That would scale nicely to a larger number of ESCs. As it stands, bandwidth usage will depend a lot on the IDs you choose.
Combined with these sp messages, we have reg.udral.service.common.Readiness, which puts the ESC into a ready
state of SLEEP, STANDBY or ENGAGED. Having a way to change the readiness state could be useful, although I havenāt missed not having it in v0.
Next we look at the ESC feedback messages. There are 4 separate messages for ESC feedback, following along with the philosophy in UAVCAN v1 of breaking things down into small pieces. The 4 feedback
messages are:
feedback reg.udral.service.actuator.common.Feedback
status reg.udral.service.actuator.common.Status
power reg.udral.physics.electricity.PowerTs
dynamics reg.udral.physics.dynamics.rotation.PlanarTs
Letās look at these messages. First the feedback message:
reg.udral.service.actuator.common.Feedback.0.1 length=67
heartbeat: reg.udral.service.common.Heartbeat.0.1 length=2
reg.udral.service.common.Heartbeat.0.1 length=2
readiness: reg.udral.service.common.Readiness.0.1 length=1
reg.udral.service.common.Readiness.0.1 length=1
value: truncated uint2 length=1
health: uavcan.node.Health.1.0 length=1
uavcan.node.Health.1.0 length=1
value: saturated uint2 length=1
demand_factor_pct: saturated int8 length=1
now the status message:
reg.udral.service.actuator.common.Status.0.1 length=67
motor_temperature: uavcan.si.unit.temperature.Scalar.1.0 length=4
uavcan.si.unit.temperature.Scalar.1.0 length=4
kelvin: saturated float32 length=4
controller_temperature: uavcan.si.unit.temperature.Scalar.1.0 length=4
uavcan.si.unit.temperature.Scalar.1.0 length=4
kelvin: saturated float32 length=4
error_count: saturated uint32 length=4
fault_flags: reg.udral.service.actuator.common.FaultFlags.0.1 length=2
reg.udral.service.actuator.common.FaultFlags.0.1 length=2
overload: saturated bool length=1
voltage: saturated bool length=1
motor_temperature: saturated bool length=1
controller_temperature: saturated bool length=1
velocity: saturated bool length=1
mechanical: saturated bool length=1
vibration: saturated bool length=1
configuration: saturated bool length=1
control_mode: saturated bool length=1
: void6 length=1
other: saturated bool length=1
the power message:
reg.udral.physics.electricity.PowerTs.0.1 length=15
timestamp: uavcan.time.SynchronizedTimestamp.1.0 length=7
uavcan.time.SynchronizedTimestamp.1.0 length=7
microsecond: truncated uint56 length=7
value: reg.udral.physics.electricity.Power.0.1 length=8
reg.udral.physics.electricity.Power.0.1 length=8
current: uavcan.si.unit.electric_current.Scalar.1.0 length=4
uavcan.si.unit.electric_current.Scalar.1.0 length=4
ampere: saturated float32 length=4
voltage: uavcan.si.unit.voltage.Scalar.1.0 length=4
uavcan.si.unit.voltage.Scalar.1.0 length=4
volt: saturated float32 length=4
and finally the dynamics message:
reg.udral.physics.dynamics.rotation.PlanarTs.0.1 length=23
timestamp: uavcan.time.SynchronizedTimestamp.1.0 length=7
uavcan.time.SynchronizedTimestamp.1.0 length=7
microsecond: truncated uint56 length=7
value: reg.udral.physics.dynamics.rotation.Planar.0.1 length=16
reg.udral.physics.dynamics.rotation.Planar.0.1 length=16
kinematics: reg.udral.physics.kinematics.rotation.Planar.0.1 length=12
reg.udral.physics.kinematics.rotation.Planar.0.1 length=12
angular_position: uavcan.si.unit.angle.Scalar.1.0 length=4
uavcan.si.unit.angle.Scalar.1.0 length=4
radian: saturated float32 length=4
angular_velocity: uavcan.si.unit.angular_velocity.Scalar.1.0 length=4
uavcan.si.unit.angular_velocity.Scalar.1.0 length=4
radian_per_second: saturated float32 length=4
angular_acceleration: uavcan.si.unit.angular_acceleration.Scalar.1.0 length=4
uavcan.si.unit.angular_acceleration.Scalar.1.0 length=4
radian_per_second_per_second: saturated float32 length=4
torque: uavcan.si.unit.torque.Scalar.1.0 length=4
uavcan.si.unit.torque.Scalar.1.0 length=4
newton_meter: saturated float32 length=4
This ālots of small piecesā approach is really bad for several reasons:
- the flight controller will want to combine the data from these messages at a single point in time in order to make decisions about whether an ESC is in a truly healthy state, or if something may be going wrong. This is needed both for in-flight diagnostics and for accurate logging and analysis of a failure.
- it is much less efficient on the wire than just putting all the info in one message, and rapid ESC messages. When complex vehicles are using CAN ESCs then bxCAN bandwidth is at a premium, so breaking things up like this is a poor use of resources.
- aligning the messages using the timestamp (for dynamics and power) makes it a lot more complex for the flight controller and for log analysis
The breaking down into lots of messages is a really good example of āthe tail wagging the dogā. UAVCAN v1 starts off by defining a huge pile of base SI unit types, and now wants some way to use them all to justify their existence. Pavel has repeatedly said this is good as it makes for more re-use, but it is all topsy turvy. In UAVCAN v0 we have less than 50 messages total that are actually used in ArduPilot (PX4 has considerably fewer). In UAVCAN v1 I count 207 messages, and weāve only got battery, ESC and servo so far. So the āreuseā approach has greatly increased the number of messages.
It would be vastly simpler and more efficient to use flat message definitions and not do this level of abstraction. Adding abstraction layers and nesting is not a good end goal in and of itself.
Ok, back to the message fields. Most ESCs wonāt be able to fill in the angular_position and angular_acceleration fields, and could only give angular_velocity. The units they normally can give is an eRPM, which canāt be converted to radian/second without knowing the number of poles. This message set would mean that you would have to set the number of poles on each ESC, rather than setting it centrally on the flight controller and until you do that you wonāt get RPM logging as the ESC node would have no way to fill it in. Iād much prefer to report an eRPM and also report a number of poles, where zero is used if not known. That would allow ESCs to work out of the box, giving a much better experience for end users while still allowing an RPM to be displayed and (if someone really wanted it) a value in radian_second.
Servos
The servo messages, like ESC messages, are in two parts. The first is the actuator command, for which there are two versions:
reg.udral.physics.dynamics.rotation.Planar
reg.udral.physics.dynamics.translation.Linear
oddly, there are both timestamped and non-timestamped versions of each of these messages. It isnāt clear what the purpose is of sending a timestamp in an actuation command. Is the servo supposed to not act upon the command until the timestamp is past?
The Planar message (which is likely to be the most common type of servo) looks like this:
eg.udral.physics.dynamics.rotation.Planar.0.1 length=16
kinematics: reg.udral.physics.kinematics.rotation.Planar.0.1 length=12
reg.udral.physics.kinematics.rotation.Planar.0.1 length=12
angular_position: uavcan.si.unit.angle.Scalar.1.0 length=4
uavcan.si.unit.angle.Scalar.1.0 length=4
radian: saturated float32 length=4
angular_velocity: uavcan.si.unit.angular_velocity.Scalar.1.0 length=4
uavcan.si.unit.angular_velocity.Scalar.1.0 length=4
radian_per_second: saturated float32 length=4
angular_acceleration: uavcan.si.unit.angular_acceleration.Scalar.1.0 length=4
uavcan.si.unit.angular_acceleration.Scalar.1.0 length=4
radian_per_second_per_second: saturated float32 length=4
torque: uavcan.si.unit.torque.Scalar.1.0 length=4
uavcan.si.unit.torque.Scalar.1.0 length=4
newton_meter: saturated float32 length=4
The Linear message is as follows:
reg.udral.physics.dynamics.translation.Linear.0.1 length=16
kinematics: reg.udral.physics.kinematics.translation.Linear.0.1 length=12
reg.udral.physics.kinematics.translation.Linear.0.1 length=12
position: uavcan.si.unit.length.Scalar.1.0 length=4
uavcan.si.unit.length.Scalar.1.0 length=4
meter: saturated float32 length=4
velocity: uavcan.si.unit.velocity.Scalar.1.0 length=4
uavcan.si.unit.velocity.Scalar.1.0 length=4
meter_per_second: saturated float32 length=4
acceleration: uavcan.si.unit.acceleration.Scalar.1.0 length=4
uavcan.si.unit.acceleration.Scalar.1.0 length=4
meter_per_second_per_second: saturated float32 length=4
force: uavcan.si.unit.force.Scalar.1.0 length=4
uavcan.si.unit.force.Scalar.1.0 length=4
newton: saturated float32 length=4
the way these work is you send NaN for the fields you donāt want. The vast majority of servos will only want angular_position in radians. One unfortunate consequence of the use of SI units is that it wonāt be possible to use this message without the CAN node knowing the angle the servo can go through. I think the only practical thing to do when a CAN node is being used to interface to a non-CAN (eg. PWM) servo is to default to an assumption of 90 degrees range, and allow for a parameter to set the actual range. For native CAN servos they should know the true angular range.
It is unfortunate how bandwidth inefficient these messages are. In UAVCAN v0 each Command was 3 bytes, which is reasonably efficient for something that is typically sent at 50 to 100Hz. In the proposed UDRAL it is 16 bytes per servo. That is a huge increase in bandwidth requirement and is just not practical for something like a complex quadplane with UAVCAN for both ESCs and servos.
The servo message set also offers an alternative approach which is based on the reg.udral.service.actuator.common.sp.* messages, just like ESCs. Unfortunately that suffers from all the problems I mentioned above for ESCs.
Overall I think these ESC and servo messages are a backwards step from what we have now with UAVCAN v0 which is surprising as I think the v0 messages are not great.
Conclusion
Which brings me to the summary from having looked through these 3 sample message sets (battery, ESC and servo). I think it shows that the lessons I tried to make clear in my DS-015 analysis have really not been learnt. There would be a lot of pain in moving from v0 to v1 for users, for developers and for vendors. To balance that pain there would have to be really big gains in what we get out of the protocol. So far all Iām seeing is pain (and loss of functionality) and no gains. So there is absolutely no motivation to adopt UAVCAN v1 with UDRAL.
For ArduPilot to adopt v1 Pavel will need to have a major change of heart, and give up some of the things heās been adamant on. There are many ways to do that that Iāve suggested over the past months. Iām sure there are other approaches, but to just keep insisting that all is well and that the detractors are just exaggerating the problems is not going to get us to a solution.
That is even without the really big issue of a clear migration path from v0 to v1. We would have to develop capabilities messages so we can automatically work out which of the new features we can use in a mixed network (eg. bit rate, message sets). Without that technical underpinning of the migration path then v1 is dead in the water.