Servo#

The servo command performs position control of the driven load. The motion performed by the load may be either rotational or translational. Coordinated multi-axis motion is implementable by sending position setpoints to multiple devices simultaneously.

One application of this control mode is folding or parking VTOL propellers in forward flight mode.

Time-optimal constrained online trajectory generator#

The motion in the servo mode is controlled by a Type-IV online trajectory generator (OTG).

The OTG executes motion by changing the kinematic state \(x(t) = \left( p(t),v(t),a(t),j(t) \right)\) over time \(0 \le t \le T\) from the origin state \(x_0 = \left( p_0,v_0,a_0,j_0 \right)\), where each component may take arbitrary (non-zero) values, towards the target state \(x_t = \left( p_t,v_t,a_t=0,j_t=0 \right)\), where \(p_t,v_t\) are defined arbitrarily by the user, such that \(T\) is minimized and the following holds:

\[\begin{split} \begin{align} v_\min \le & v(t) \le v_\max \\ +a_\min \le & a(t) \le +a_\max & \text{ if } v(t) \ge 0 \\ -a_\max \le & a(t) \le -a_\min & \text{ if } v(t) \lt 0 \\ -j_\max \le & j(t) \le +j_\max \end{align} \end{split}\]

With position \(p\), velocity \(v\), acceleration \(a\), and jerk \(j\); and with the following user-defined constraints[1]:

  • \(v_\min, v_\max\) — forward/reverse velocity limit;

  • \(a_\min, a_\max\) — deceleration/acceleration limit;

  • \(j_\max\) — jerk limit.

\(x_t\) and the constraints may be updated arbitrarily at any moment; the updated trajectory will be commenced immediately.

A trajectory is considered feasible if and only if the following holds:

\[\begin{split} \begin{align} v_\min \le & 0 \le v_\max \\ v_\min \le & v_t \le v_\max \\ 0 \le & v_t \le v_\max & \text{ if } p_0 < p_t \\ v_\min \le & v_t \le 0 & \text{ if } p_0 > p_t \\ a_\min < & 0 < a_\max \\ & 0 < j_\max \end{align} \end{split}\]

When presented with an infeasible trajectory, the OTG may either abort with an error or silently override \(v_t\) as \(\hat{v}_t\) such that the resulting trajectory is feasible and \(\left| v_t - \hat{v}_t \right|\) is minimized.

If \(v_t \neq 0\), then \(p_t\) should be updated by the time \(p(t) \approx p_t\); otherwise, position overshoot with subsequent oscillation around \(p_t\) will result.

../_images/online_trajectory_generator_demo_a.svg

Basic example of a 1D jerk-limited trajectory with asymmetric velocity and acceleration limits (shown in dashed lines).#

Configuration registers#

Changes to the configuration registers documented in this section take effect when the command execution is (re-)started.

The mechanical ratio parameter reflects the relation between the electrical angle of the motor and the position of the load in either linear or mechanical units. The choice of units depends on whether the driven load performs translational or rotational motion; the control system is invariant to that.

Register

Type

Unit, linear

Unit, angular

Description

servo.profile.velocity

real32[2]

\(\frac{\text{meter}}{\text{second}}\)

\(\frac{\text{radian}}{\text{second}}\)

Default \(v_\min, v_\max\)

servo.profile.acceleration

real32[2]

\(\frac{\text{meter}}{\text{second}^2}\)

\(\frac{\text{radian}}{\text{second}^2}\)

Default \(a_\min, a_\max\)

servo.profile.jerk

real32

\(\frac{\text{meter}}{\text{second}^3}\)

\(\frac{\text{radian}}{\text{second}^3}\)

Default \(j_\max\)

Invocation#

The servo command has to be issued periodically to avoid expiration of the deadman timeout. Periodic commands are used to provide new setpoint values and to allow automatic disengagement of the drive if the outer system becomes unable to issue new commands (e.g., due to an interface failure).

Via Cyphal#

The servo command can be invoked by publishing to any of the following topics.

Topic name

Type

setpoint_servo

zubax.telega.ServoCommand

The readiness control subject is an optional safety feature that can be enabled to prevent accidental arming of the drive. If the readiness subscription is configured, no servo setpoint will be acted upon unless the ENGAGED readiness command is published. If the readiness control subject is disabled, published servo commands are accepted immediately.

setpoint_servo#

The type can be any of its structural supertypes of zubax.telega.ServoCommand, down to simply the target position alone represented as uavcan.primitive.scalar.Real32. If supertypes are used, the trajectory constraints are set to the defaults configured via the respective registers.

Definition of zubax.telega.ServoCommand.1.0; extent 63 bytes:

 1# Servo or stepper control message.
 2# It is designed for use with Type-IV trajectory capable servocontrollers.
 3# The units are linear (meter, newton) or rotational (radian, newton*meter) depending on the kind of servo.
 4#
 5# The message defines the target position and velocity along with the motion profile constraints:
 6# the velocity limits (reverse and forward), the acceleration limits (deceleration and acceleration),
 7# and the symmetric jerk limit.
 8# The torque limit is configured in a similar fashion.
 9# Limits that are set to zero are assumed to be undefined, in which case statically preconfigured defaults are used.
10#
11# Observe that this message is semantically compatible with just a scalar position value.
12# By virtue of the implicit zero extension rule, all motion profile values will be replaced with preconfigured defaults.
13
14zubax.physics.dynamics.DoF2nd.1.0 target
15# If the target velocity is non-zero, a new target should replace this one when it is reached to avoid oscillations.
16# The force/torque value sets the torque limit. For open-loop servos or steppers it sets the stator current.
17# The treatment of a non-positive force/torque limit is implementation-defined.
18
19float32[2] profile_velocity_reverse_forward     # {negative, positive}; set both to zero to select default.
20float32[2] profile_deceleration_acceleration    # {negative, positive}; set both to zero to select default.
21float32 profile_jerk                            # positive; set to zero to select default.
22
23@extent 63 * 8

Definition of uavcan.primitive.scalar.Real32.1.0; extent 4 bytes:

1float32 value       # Exactly representable integers: [-16777216, +16777216]
2@sealed

Here is a trivial example using Yakut, assuming that the device is otherwise configured (in particular, the motor model parameters are set correctly) and its node-ID is 125:

1y r 125 uavcan.sub.setpoint_servo.id 27   # Configure the servo setpoint topic arbitrarily.
2y r 125 uavcan.sub.readiness.id 10        # Configure the arming topic arbitrarily.
3y cmd 125 restart                         # Restart the device for the changes to take effect.
4# Now that the device is configured, position the load at 100 radian and hold that position.
5# The default motion constraints will be used, as set through the "servo.*" registers.
6y pub -T 0.1 10:zubax.service.readiness 3 27:uavcan.primitive.scalar.real32 100

Here is another example where the arming control is disabled:

1y r 125 uavcan.sub.readiness.id 65535     # Disable the arming topic.
2y cmd 125 restart                         # Restart the device for the changes to take effect.
3# Use the explicit motion constraints this time.
4y pub -T 0.1 27:zubax.telega.ServoCommand '{target: 500, profile_deceleration_acceleration: [-300, +3.14]}'

Via auxiliary I/O port#

The auxiliary I/O port can be configured to issue servo commands. Refer to the auxiliary I/O chapter for details.

Errors#

The following runtime error codes may be reported by this command:

Error

Reason

1

Requested trajectory is infeasible and no heuristic is known to repair it.

2

Numerical failure, adjust the constraints.

An invalid parameters error may be reported if the motor model parameters are not configured correctly (also see motor identification).

A hardware error is reported if raised by the VSI interface.

System status register#

This command is indicated via the system status register using the following schema.

{ "servo": UNSPECIFIED }

UNSPECIFIED may be replaced with an arbitrary value.