Control of Individual Motors/Actuators

Platform details:

  • PX4 v1.14
  • Pixhawk v6x
  • Ubuntu 22.04
  • ROS2 Humble

Using the above software/hardware, our research group at UC Berkeley is attempting to control the motors on our quadcopter in offboard mode using a companion laptop. We want to control the specific PWM values or throttle percentages of the motors, control the length of time the motors run at that value, and we would like to be able to control individual motors.

Using QGC to manually control the motors, we can individually control any motor or all motors with full power accurately.
However, we cannot achieve this functionality otherwise. When running code, The motors do not run at the throttle percentage or PWM value we specify in the code, often running at a much slower speed than the value we specified in the code. They also will still run when we specify 0.0 or NaN or -1.0 input, which should not happen according to PX4 generic actuator control documentation. The motors all run at the exact same speed as one another every time, even when we specify different values for different actuators.

We have been rewriting and testing numerous versions of code based on the offboard_control.cpp code example file provided by the PX4 GitHub, but nothing has succeeded in controlling the motors as expected.
We have attempted to utilize ActuatorMotors messages, and VehicleCommand messages with VEHICLE_CMD_DO_SET_ACTUATOR as well as VEHICLE_CMD_DO_MOTOR_TEST. We have tried countless different parameter values for all of these functions, and have tried various values for target_system, target_component, source_system, and source_component in the publish_vehicle_command function. We have also tested various OffboardControlMode message parameter values, including setting msg.direct_actuator to true and everything else false. We have ensured that the topics “fmu/in/actuator_motors” and “fmu/in/actuator_servos” are listed under subscriptions in the dds_topics.yaml file. We have echoed the “fmu/out/vehicle_control_mode” topic and ensured that the control allocation flag is false. We tested setting AUX 1 - AUX 4 in QGC to “Motor X” as well as “Offboard Actuator Set X”. None of this has succeeded.

How do we control our motors/servos/actuators at specified inputs? And how do we do this for individual motors rather than all at once?

Hi,

I don’t know if you already solved this problem, but in the most recent version of PX4, the ActuatorMotors.msg has its own ROS2 topic. Thus it is simple to specify the thrust level of any motor individually.

I do this with the r1_rover model. If you look at the iris airframe at /ROMFS/px4fmu_common/init.d-posix/airframes/10015_gazebo-classic_iris you can see the following parameters:

param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104

This tell that 4 values needs to be given to ActuatorMotors.control, one for each motor (101, 102, 103, 104). You can check the motor order in the sdf file of the iris model, under the mavlink plugin <control_channels> part (look at <input_index> where the joint with lower value is the PWM_MAIN_FUNC1 101 and so on). As you know, you have to give values in the [-1, 1] range, where 1 is maximum positive thrust and -1 is maximum negative thrust (the negative part can be enable if supported by the model, or you can enable it using the ActutorMotors.reversible_flags [set this value as a series of 1 and 0 where 1 means reversibility ON, whereas 0 means OFF. To have reversibility on all 4 motors, just set reversible_flags = 1111]).

This gives you a complete control on the thrust level of each of the 4 motor.

I am using msg type of ActuatorMotors.msg to publish individual motor velocity command for iris model in ROS2 using PX4. I have a doubt in the msg type, what is NUM_CONTORLS which is by default set to value of 12 and there is an array control of same size in which motor velocities need to be passsed. So please tell what is NUM_CONTROLS and how can I publish individual velocities to my quad-copter. Also tell what should be the value set for timestamp_sample

Hi,

Actually I don’t know precisely what NUM_CONTROLS states, but I think it indicates the number of controls (the number of actuators to control).

The control array is instead where the actuators trust must be indicated. This array should be composed of as many elements as the number of controls you have. There is actually a difference between number of controls and number of actuators, since there can be multiple actuators controlled by a single trust value. This is for example the case for the r1_rover where the right wheels get the same control, and same for the left (as a basic differential robot).

In the iris case there are 4 actuators with 4 separate controls. As I said in the previous post, you can check for the motors order you found in the airframe, by checking the sdf file of the iris:

In a nutshell the control array has to be a sequence of values between -1 and +1 where -1 = maximum negative trust and +1 = maximum positive trust (when the reversible_flag of the motor is set to 1).

Here I provide you a python sample function, to publish a basic ActuatorMotors.msg:

def publish_actuator_motors(self, motor1: float, motor2: float, motor3: float, motor4: float):
    """Publish the actuator motors message"""
    msg = ActuatorMotors()
    msg.reversible_flags = 1111
    msg.control = [motor1, motor2, motor3, motor4]
    self.actuator_motors_publisher.publish(msg)

where actuator_motors_publisher is the ActuatorMotors.msg pubblisher:

# Configure QoS profile for publishing and subscribing
qos_profile = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,
    history=HistoryPolicy.KEEP_LAST,
    depth=1
)

self.actuator_motors_publisher = self.create_publisher(ActuatorMotors, '/fmu/in/actuator_motors', qos_profile)

As you can see, I do not change the NUM_CONTROLS value, since it is not mandatory.

For what concerns timestamps, it is not mandatory to set up them to. The message can still be published. They gave the message a time consistency w.r.t to the publication order. timestamp_sample, from what I understand, is more like an internal timestamp, where you indicate the timestamp of source data used to make the control you are actually publishing. If your not interested in having logging coherence w.r.t. time, you can leave these part of the message blank.

It is instead important to define the reversible_flags correctly. Indeed if you set a certain motor with the 1 flag, the actual trust ranges from -max to +max [-1,1]. Instead if you set it to 0 the trust ranges from 0 to +max [-1,1], meaning that you cannot have the motor spinning in reverse.

In the case of a drone, you should set the reversible_flags = 1111 to actually enable reversibility for all the motors.

1 Like

Hi, I am trying to control the speed of 4 actuators in a similar manner as you are doing, but am relatively a newbie. Could you please provide some more detailed steps on how to exactly manipulate the files you mentioned for the same.
Thanks

You don’t have to manipulate any file. You have just to understand which is the correct order for the msg.control array.

Can you please share more detail of how to do it using ros2 bridge, I am trying the following command but it shows invalid msg type:
ros2 topic pub /fmu/in/actuator_motors px4_msgs/msg/ActuatorControls
‘{
“timestamp”: 0,
“timestamp_sample”: 0,
“reversible_flags”: 15, # Binary 1111 for motors 1–4 as reversible
“control”: [0.3, 0.3, 0.3, 0.3, NaN, NaN, NaN, NaN, NaN, NaN, NaN, NaN]
}’
But instead if I use the message type actuator_output, the message gets published but in the gazebo simulation, I am not able to see any changes.

Well.
In this topic we are talking about an implementation with a programmed ROS2 node.

In your case, you are trying to populate a topic directly from a terminal, which is possible, but not a solution if you want to create an autonomous mission.

The first thing i can say about your example, is that the correct message type for /fmu/in/actuator_motors topic, is px4_msgs/msg/ActuatorMotors.

But the true problem, is that it is not that easy to control the iris from a terminal. Indeed, you need to arm the vehicle, go in offboard mode, and only then you can publish your control on the motors. Moreover, to mantein offboard mode, an heartbeat signal has to be sent to the controller at a frequency of at least 2 Hz.

Even if is theoretically possible to control a vehicle with ros2 from command lines, it is over complicated and impractical. Take a look at ROS 2 Offboard Control Example | PX4 Guide (main), and then try to change the provided sample code, to your needs. All my precedent answers in this topic, are about an implementation of a python ROS2 node to control directly the motors. Hence, you can use them to construct your personal solution.

1 Like