Direct Motor Control Access

[Pedro-Roque]

Please use PX4 Discuss or Slack to align on pull requests if necessary.

Test data / coverage
Tested with SITL, video shown below:
https://drive.google.com/open?id=1ojMMRyiiii7A6pIMROro7hTnzVVwQ9em
I can provide logs (is it possible to take them from SITL?)

Describe problem solved by the proposed pull request
This PR enables direct pwm control of the motors by bypassing the mixer results. A control mode flag is set when mavros ActuatorControl->group_mix = 99 (this shall be changed by something more convenient, maybe a change on the message definition to add a constant for that. This control flag is verified at the FMU level (and on PWMSim for the simulation part), and if true, then the actuator_controls from the mixer are overridden. This happens before the safety constraints, so the user inputs still go through them to make sure they are bounded and within the usual limits.

This is a desired feature from the Automatic Control standpoint, as there is the need to have a very low-level control on the vehicle. This solution also does not interfere with any other modules or safety measures/routines.

Minor: fixed a small leak on the FMU, where the _adc_sub was not being unsubscribed at the destructor.

Describe your preferred solution
Proposed solution.

Describe possible alternatives

Additional context

[bresch]

style check failed, please run a make format

[bresch]

#10862 (comment)

[Pedro-Roque]

@bresch let me know if it looks good, and if I should rebase it now :) Thanks in advance!

[dagar]

The main thing I'd like to consider here is adding an alternate interface (uORB) message that carriers the motor commands rather than overloading actuator controls.

If we do this right it could also enable QGC motor testing (https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOTOR_TEST).

We already have a test_motor message (https://github.com/PX4/Firmware/blob/master/msg/test_motor.msg). What if we expanded it to cover all motors and perhaps gave it a more appropriate name? Then we'd basically have modes for the outputs modules (eg px4fmu, px4io, uavcan, etc) to switch between direct motor mode or mixers + actuator_controls input.

Thoughts?

[Pedro-Roque]

The main thing I'd like to consider here is adding an alternate interface (uORB) message that carriers the motor commands rather than overloading actuator controls.

If we do this right it could also enable QGC motor testing (https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOTOR_TEST).

We already have a test_motor message (https://github.com/PX4/Firmware/blob/master/msg/test_motor.msg). What if we expanded it to cover all motors and perhaps gave it a more appropriate name? Then we'd basically have modes for the outputs modules (eg px4fmu, px4io, uavcan, etc) to switch between direct motor mode or mixers + actuator_controls input.

Thoughts?

I like the idea of having direct motor control vs mixers+actuator control+wtv! I can look further on it.

[AlexisTM]

I think there will be a conflict in the Mavlink receiver. If you use the actuator_control to control the AUX (group_mlx = 3), as it is not 99, it will set offboard_control_mode.ignore_mixed_outputs = false; and compete with the other actuator control message.

https://github.com/PX4/Firmware/blob/916961ce86fe2847f8320be0243198ba112a9fb9/src/modules/mavlink/mavlink_receiver.cpp#L1098-L1103

[Pedro-Roque]

I think there will be a conflict in the Mavlink receiver. If you use the actuator_control to control the AUX (group_mlx = 3), as it is not 99, it will set offboard_control_mode.ignore_mixed_outputs = false; and compete with the other actuator control message.

Firmware/src/modules/mavlink/mavlink_receiver.cpp

Lines 1098 to 1103 in 916961c

if (set_actuator_control_target.group_mlx == 99) {
offboard_control_mode.ignore_mixed_outputs = true;

} else {
offboard_control_mode.ignore_mixed_outputs = false;
}

How so? If you use another group mlx, then the information follows its usual path, and the fmu verification for the direct control mode is false, and therefore it does not overwrite anything.

Keep in mind that ignore_mixed_outputs was created on this PR, and so it will not conflict with previous implementations.

[AlexisTM]

You will be willing to send both actuator_control messages at the same time (controlling motors and AUX).

In such case, you'll send a group 99, then 3, then 99. After each message, the ignore_mixed_outputs boolean will switch from true to false.

[Pedro-Roque]

You will be willing to send both actuator_control messages at the same time (controlling motors and AUX).

In such case, you'll send a group 99, then 3, then 99. After each message, the bool will switch from one to the other.

Sorry @AlexisTM , what do you mean as sending group 3 and group 99, alternately? If the offboard mode is set to control motors directly, why would this happen?

I think I understood you! You mean if you still want to use the same topic to control aux AND motors, is that it?

[AlexisTM]

Yes, in the current status, if you want to use AUX and RawPWM, you will end up with switching ignore_mixed_outputs.

Also, it is missing a timeout for ignore_mixed_outputs. If your software fails, it will switch back to failsafe but the ignore_mixed_outputs will stay true.
Finally, what if you want to control raw PWM then switch to OFFBOARD position control mode?

[Pedro-Roque]

Yes, in the current status, if you want to use AUX and RawPWM, you will end up with switching ignore_mixed_outputs.

Also, it is missing a timeout for ignore_mixed_outputs. If your software fails, it will switch back to failsafe but the ignore_mixed_outputs will stay true.
Finally, what if you want to control raw PWM then switch to OFFBOARD position control mode?

Yep, I didn't had the timeout yet - so yes, it would stay true. unless you receive a message with it being false - or, set it to false everywhere else.

However, as discussed yesterday, the way for this PR will be to separate motor control and actuator control into different things, so that such problems do not arise.

I will come back to this soon.

[AlexisTM]

I just listened to the podcast, indeed, I would love to have a separate control to avoid that other modules mess with the PWMs.

[juhannc]

+1, any news on the PR?

[Pedro-Roque]

I am overloaded with other things at the moment, however feel free to fork this!

[juhannc]

HI @Pedro-Roque,

I've encountered a few problems with your implementation. Maybe you could help me figure it out.
First of all, I merged your changes into my devel branch and loaded the FW onto my board (Aerocore 2). Moreover, I cloned your MAVROS PR (#1120) to build the corresponding MAVROS node.
Now to use the Direct Control, I'm running mavros with a secondary custom node based on the Offboard example. But instead of calling local_pos_pub.publish(pose);, I am calling a custom function, which is defined as following:

void send_Actuator_Control_Handler(ros::NodeHandle node, double a0 = 1.0, double a1 = 0.0, double a2 = 0.0, double a3 = 0.5, double a4 = 0.0, double a5 = 0.0, double a6 = 0.0, double a7 = 0.0){
    pub_act_control = node.advertise<mavros_msgs::ActuatorControl>("/mavros/target_actuator_control", loop_rate_);

    mavros_msgs::ActuatorControl act_control_msg;

    act_control_msg.header.stamp = ros::Time::now();
    act_control_msg.header.seq = act_msg_count;
    act_control_msg.header.frame_id = "1";
    act_control_msg.group_mix = 99;
    act_control_msg.controls[0] = a0; // default 1.0
    act_control_msg.controls[1] = a1; // default 0.0
    act_control_msg.controls[2] = a2; // default 0.0
    act_control_msg.controls[3] = a3; // default 0.5
    act_control_msg.controls[4] = a4; // default 0.0
    act_control_msg.controls[5] = a5; // default 0.0
    act_control_msg.controls[6] = a6; // default 0.0
    act_control_msg.controls[7] = a7; // default 0.0

    pub_act_control.publish(act_control_msg);

    ++act_msg_count;
}

The expected behaviour would be, that my system (quadrotor) stays ARMED and in OFFBOARD-Mode with motor 1 spinning at approx. 2000 us, motor 2 and 3 at approx. 1000 us and motor 4 with approx. 1500 us.
But, whenever I start MAVROS and my test-node and as soon as the stream on
/mavros/target_actuator_control starts, the mode switches to AUTO.LAND despite me deactivating all safety features I could find:

• COM_OBL_ACT = Hold mode
• COM_RCIN_MODE = Virtual RC by Joystick
• COM_RC_LOSS_T = 35.0 s
• COM_RC_OVERRIDE = enabled
• NAV_RCL_ACT = Disabled

So my question would be, is this even the supposed way of using your changes? Or is it supposed to work in OFFBOARD-Mode but for me it just won't?

PS: If you feel like, I should open a separate issue for that, I'm more than willing to do so. I just feel like my issue is better fitted in here, mostly as this code is not yet part of the PX4-repo.

[Pedro-Roque]

@JohannLange That is wierd. On my tests on simulator I didn't have that problem. Did you try simulating your build on SITL?

If this is a quadrotor, wouldn't those inputs mess up your system? Why would like to set such motor speeds?

Also, keep in mind this is WIP, so expect the unexpected. I will come back to this from next week onwards...

[juhannc]

@Pedro-Roque I haven't tried SITL yet, but using it via a PX4 app works as expected.
The use case is a special kind of controller, which controls each motor individually

[Pedro-Roque]

@JohannLange I can check it out on a UAV next week, because so far I've been testing it on SITL only, so I don't have a lot more info for you. Anyways, this PR code will be changed according to what was discussed.

[Pedro-Roque]

@JohannLange a friend noticed that the Commander changes are missing here, so maybe that's why it does not work for you. I'll try to look at that later this week!

[Pedro-Roque]

@dagar @TSC21 I've been thinking more and more about this setpoint method, but after discussing with some friends we got to the conclusion that, to properly control the vehicle with motor velocities, we need a high control rate that should be above 100HZ (ideally at least 500). I wonder if we can have such rates through serial on MAVROS...

If not - maybe an Ethernet extension for Pixhawk V6? 💃

[TSC21]

You can get those rates but you would probably have to limit some other message streams.

[Pedro-Roque]

In that case, I will implement the feature anyways and then we discuss the best way to interface it

[juhannc]

high control rate that should be above 100HZ (ideally at least 500)

The standard PWM output frequency is "only" 400 Hz. Thus, I don't think there is any reason to try and achieve 500 Hz or more as a control rate, as long as you don't change the PWM rate.

I wonder if we can have such rates through serial on MAVROS...

Keep in mind, that mavros runs at 57600 baud 19200 baud (minimal telemetry) to 921600 baud (companion link), giving you a total data throughput of 57.6 19.2 .. 921.6 kB/s (8N1 encoding).
Using ROS I got the following numbers:

rostopic hz /mavros/target_actuator_control
subscribed to [/mavros/target_actuator_control]
average rate: 410.265
        min: 0.000s max: 0.003s std dev: 0.00034s window: 402
average rate: 410.135
        min: 0.000s max: 0.003s std dev: 0.00032s window: 813

rostopic bw /mavros/target_actuator_control
subscribed to [/mavros/target_actuator_control]
average: 20.03KB/s
        mean: 0.05KB min: 0.05KB max: 0.05KB window: 100
average: 20.02KB/s
        mean: 0.05KB min: 0.05KB max: 0.05KB window: 100

With: rostopic pub -r 400 /mavros/target_actuator_control mavros_msgs/ActuatorControl "{header: auto, group_mix: 99, controls: [0.0, 0.0, 0.0, 0.0,0.0, 0.0,0.0, 0.0]}"

Honestly, I don't know what that would mean in terms of the bandwidth used by MAVLink, but it shouldn't be much higher if it even would be...

So keep in mind, that for low bandwidth devices running only 57600 baud on 8N1, it could potentially pose a problem, for higher bandwidth devices, there should be plenty bandwidth that only devices running with 57600 baud or 921600 baud would be able to sustain such high bandwidths. For devices with 57600 baud it could actually be almost too much. Maybe mentioning the potential problem in the docs could be a good idea.

Also, if anyone knows how to check the real bandwidth used by MAVLink, I am happy to test that as well.

Edit: according to the parameter description of PWM_RATE in QGroundControl, the maximum rate is 2000 Hz which would, in turn, create the need for a bandwidth of approx. 100 kB/s (100000 baud). Therefore, only the 921600 baud rate is able to handle that high of a bandwidth.

[Pedro-Roque]

@JohannLange The problem is that if you want to do direct motor control offboard, then you also need to stream the IMUs and other relevant data at high frequencies - if you do all that and then still need o send motor control that (from a theoretical stand-point, might need high frequencies), then the bandwidth of the serial port might not be enough. Anyhow, this is something that we can assess more surely afterwards...

[Pedro-Roque]

The main thing I'd like to consider here is adding an alternate interface (uORB) message that carriers the motor commands rather than overloading actuator controls.

If we do this right it could also enable QGC motor testing (https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOTOR_TEST).

We already have a test_motor message (https://github.com/PX4/Firmware/blob/master/msg/test_motor.msg). What if we expanded it to cover all motors and perhaps gave it a more appropriate name? Then we'd basically have modes for the outputs modules (eg px4fmu, px4io, uavcan, etc) to switch between direct motor mode or mixers + actuator_controls input.

Thoughts?

@dagar I will start addressing this for the next days, will come back with news .

[Pedro-Roque]

Closing this and working on it on my fork: github.com/Pedro-Roque/Firmware/tree/motor-ctl . When it is ready, I will do a PR.

[JonathanPlasse]

Hi, is this feature still planned to be implemented ?

[Pedro-Roque]

Not until we get a really fast connection to the FMU, otherwise not worth it :(

[AlexisTM]

That's a limitation that I expected. Also, I prefer the low-level algorithms to be onto the Pixhawk itself for latency [and robustness] reasons. I expect a drone to be self-sufficient to fly and have an onboard computer only to add nice to have features.
Did you try FastRTSP?
Note that the FMUv6 will come with an Ethernet port, speed will likely be high [I wonder if it will be a FullSpeed 100MBits or a 10MBits channel, that would remove the channel limitation and the Pixhawk processor will be the bottleneck]

[Pedro-Roque]

That's a limitation that I expected. Also, I prefer the low-level algorithms to be onto the Pixhawk itself for latency [and robustness] reasons. I expect a drone to be self-sufficient to fly and have an onboard computer only to add nice to have features.
Did you try FastRTSP?
Note that the FMUv6 will come with an Ethernet port, speed will likely be high [I wonder if it will be a FullSpeed 100MBits or a 10MBits channel, that would remove the channel limitation and the Pixhawk processor will be the bottleneck]

Not yet as I am not going to work with something like that any time soon.

The thing of all low level being in Pixhawk is half true: in my lab people develop algorithms for low level control, so most of the times they want to access the motor speeds, but not necessarily want to implement in the stack - they rather have some interface for it.

I am eager to get the hands on the v6 and see if we can do something with it :)

[JonathanPlasse]

Is it possible to help?

[Jawad-RoboLearn]

Hello,
I have build your repo in sitl_gazebo successfully. However, I want to use this repo on hardware. I found that your repo does not have PWMOut folder.

Can we directly run your PWMOut sim code to real time hardware? If not what changes do we need to send the PWM commands to motors on hardware?

Also, I need to customize the firmware according to this repo? Is there any tutorial on customization of firmware?

[Pedro-Roque]

Hi @Jawad-RoboLearn - this was developed a while ago, so I didn't test it in recent versions of PX4. However, running this without a high-frequency control over the system will give you problems. That's important to keep in mind.

[AlexisTM]

I would also advise to use the higher level control if your app is outside of the Pixhawk. (aka Offboard control)

[Jawad-RoboLearn]

Thank you for your response. Yes I am trying to write a python based mixer file to directly integrate with the body rate and thrust commands on MAVROS topic. Thanks once again.

…

On Wed, 24 Nov 2021 at 16:10, Alexis Paques ***@***.***> wrote: I would also advise to use the higher level control if your app is outside of the Pixhawk. (aka Offboard control) — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#10863 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AQ2NPUVQLFHQCYCU5SKNWILUNSMXDANCNFSM4GEZBXBA> . Triage notifications on the go with GitHub Mobile for iOS <https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675> or Android <https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub>.