Adding the Mavlink handler for the Actuator controls message #7477

AlexisTM · 2017-06-26T15:58:09Z

Allow to use the 4 actuator groups from Mavlink via the ACTUATOR_CONTROLS
Allow an Offboard control of the Gimball, Gripper or servo.

Fixes issue #7440

Do not merge yet, I still have to test it.

Signed-off-by: Alexis Paques alexis.paques@gmail.com

AlexisTM · 2017-06-27T07:28:17Z

NOTE:

The actuator group 6 is not implemented in the rest of the code, so it is still not in the not implemented in the mavlink receiver.

https://dev.px4.io/en/concept/mixing.html#control-group-6-first-payload

LorenzMeier · 2017-07-08T12:27:34Z

Can you test?

AlexisTM · 2017-07-08T12:52:59Z

I would like to, but I have a deadline on rovers on Tuesday. I should be able to test from Wednesday onwards.

AlexisTM · 2017-07-12T08:58:23Z

I just tested it today and it worked as expected. Yet I did not tested it in flight as the weather is very bad right now.

Gripper: EPM OpenGrab v3

http://ardupilot.org/copter/docs/common-electro-permanent-magnet-V3.html

Test yourself:

Build make px4fmu-v2_default
Upload make px4fmu-v2_default
Power the drone from the battery, the gripper (or servo) on AUX1
Connect to the Pixhawk through Mavros
Press on the safety switch
Put the drone in OFFBOARD mode (You have to continuously send setpoints or it will not switch)
Execute the following example

Info:

When sending -1.0, the EPM gripper goes ON sporadically,
Start the EPM : Send 1.0
Stop the EPM : Send -1.0 then send 0.0 after a second

Code: test_gripper.py

#!/usr/bin/env python

import rospy

import sys, tty, termios
from select import select

from mavros_msgs.msg import ActuatorControl

def getch():
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    try:
        tty.setraw(sys.stdin.fileno())
        [i, o, e] = select([sys.stdin.fileno()], [], [], 1)
        if i:
            ch = sys.stdin.read(1)
        else:
            ch = None
    finally:
        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
    return ch

def handle_keyboard():
    global sync_pub
    actuate = rospy.Publisher("/mavros/actuator_control", ActuatorControl, queue_size=3)
    rospy.loginfo("Press i to start the gripper (command 1.0);")
    rospy.loginfo("Press o to stop the gripper (command -1.0);")
    rospy.loginfo("Press k to keep the last command (command 0.0);")
    rospy.loginfo("Press q to quit;")
    
    msg = ActuatorControl()
    msg.header.frame_id = "gripper"
    msg.controls = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    msg.group_mix = 3
    
    while not rospy.is_shutdown():
        what = getch()
        if what == "i":
            msg.controls[5] = 1.0
            rospy.loginfo(msg)
            rospy.loginfo("\nSending 1.0 to the channel 5 of the group mix 3 (AUX1)")
            msg.header.stamp = rospy.Time.now()
            actuate.publish(msg)

        if what == "o":
            msg.controls[5] = -1.0
            rospy.loginfo(msg)
            rospy.loginfo("\nSending -1.0 to the channel 5 of the group mix 3 (AUX1)")
            msg.header.stamp = rospy.Time.now()
            actuate.publish(msg)

        if what == "k":
            rospy.loginfo(msg)
            rospy.loginfo("\nSending 0.0 to the channel 5 of the group mix 3 (AUX1)")
            msg.controls[5] = 0.0
            msg.header.stamp = rospy.Time.now()
            actuate.publish(msg)
        
        if what == "q":
            exit()

if __name__ == '__main__':
    try:
        rospy.init_node('Gripper_test')
        handle_keyboard()
    except rospy.ROSInterruptException:
        pass

TSC21 · 2017-07-27T14:59:42Z

@AlexisTM did you flight test this already?

AlexisTM · 2017-07-27T15:58:38Z

Badly no. As soon as I have time I can allow to it, I will do test flights.

TSC21 · 2017-07-27T16:20:29Z

Badly no. As soon as I have time I can allow to it, I will do test flights.

Ok. Since this was pendent for a month, would be good to get this is ASAP, as I think it is useful.

TSC21 · 2017-08-16T16:07:12Z

@AlexisTM ping

TSC21 · 2017-08-20T16:25:29Z

@AlexisTM is there a way you can "absorb" #4965 on this PR?

andresR8 · 2017-08-24T02:35:48Z

I've tested and it works for me #7854

AlexisTM · 2017-08-24T07:20:05Z

Also seems working for me.

Thanks @andresR8 for testing.

I can put #4965 inside this PR, yet, I'll not be able to test it as we are using the Offboard control and not the Navigator.

If you confirm it, I'll put the changes of src/modules/navigator/mission_feasibility_checker.cpp inside this PR.

https://github.com/PX4/Firmware/pull/4965/files?diff=split#diff-3f698c24ebab3cb3866d13e69f8e5459

TSC21 · 2017-08-24T07:48:58Z

I can put #4965 inside this PR, yet, I'll not be able to test it as we are using the Offboard control and not the Navigator.

It can be tested on SITL, though I think it was already tested on that PR and validated by @LorenzMeier. I suppose you can push all the #4965 PR to this one and recheck if the changes on the mixers make sense.

AlexisTM · 2017-08-24T08:01:04Z

@LorenzMeier had concerns about the mixers. It has to be checked first.

TSC21 · 2017-08-24T08:20:37Z

recheck if the changes on the mixers make sense.

That's what I stated here basically.

andresR8 · 2017-08-26T07:29:41Z

I've realized that when i'm sendig actuator control values to a group 0 and any other group at the same time, the group 0 is overriten to zeros for a short time. I dont know why.

I'm setting two different actuator control messages in ros, that's basically what I'm doing:

void set_actuator_control(){	
   actuator_control_msg.header.stamp = ros::Time::now();
    actuator_control_msg.group_mix = 0;
    actuator_control_msg.controls[0] = 0; 
    actuator_control_msg.controls[1] = 0; 
    actuator_control_msg.controls[2] = getAngularVelocity(); 
    actuator_control_msg.controls[3] = getLinearVelocity();  
    actuator_control_msg.controls[4] = 0.0; 
    actuator_control_msg.controls[5] = 0.0;
    actuator_control_msg.controls[6] = 0.0; 
    actuator_control_msg.controls[7] = 0.0;
}

void set_actuator_control_aux(){	
    actuator_control_aux_msg.header.stamp = ros::Time::now();
    actuator_control_aux_msg.group_mix = 1;
    actuator_control_aux_msg.controls[0] = getServoPan(); 
    actuator_control_aux_msg.controls[1] = getServoTilt();
    
}

int main(int argc, char** argv)
{
  signal(SIGINT, signalHandler);
  ros::init(argc, argv, "rover_teleop");
  ros::NodeHandle n;    
  int rate = 10;
  ros::Rate r(rate);  
  ros::Publisher actuator_controls_pub = 
  n.advertise<mavros_msgs::ActuatorControl("/mavros/actuator_control", 100);

 mavros_msgs::SetMode set_mode;
 set_mode.request.custom_mode = "OFFBOARD"; 

while (n.ok()){
                       if(current_state.mode != "OFFBOARD") {
				ROS_INFO("No Offboard mode");
				if( set_mode_client.call(set_mode) &&  set_mode.response.success){
					ROS_INFO("Offboard enabled");
				}                
			}	

    set_actuator_control();			
    actuator_controls_pub.publish(actuator_control_msg);
    set_actuator_control_aux();
    actuator_controls_pub.publish(actuator_control_aux_msg);

 
       ros::spinOnce();
       r.sleep();
}

TSC21 · 2017-08-28T20:06:41Z

@andresR8 try to setup a delay from one msg to the other in order to avoid concurrency issues.

andresR8 · 2017-08-29T06:29:49Z

@TSC21 It works with the delay, thanks

AlexisTM · 2017-09-04T12:03:13Z

@TSC21 Where would the concurrency be issue located? Mavros or PX4?

TSC21 · 2017-09-04T12:25:31Z

It can be on both. But I suppose that it will start on the ROS side

AlexisTM · 2018-04-05T07:20:43Z

To be rebased

alanypf · 2018-04-05T07:46:14Z

Hi all,

I am working on a project which involves controlling several servos from PIXHAWK AUX outputs.
I managed to send the actuator_control commands to servos in main outputs using FCU_pass.mix through mavros and it worked out fine.
But when I tried to publish the actuator_control topics to AUX output using pass_aux.mix in control group 3 or any other control groups other than group 0, nothing happens. The pwm_output1 from mavlink console remains unchanged.
Did I miss anything? Any advice would be appreciated.
Thank you.

My setup is: PX4 v1.7.3 Pixhawk 2.4.6

AlexisTM · 2018-04-05T07:53:51Z

Hey @alanypf, the following PR is not merged to the master. I just rebased it so there is no merge conflicts.

By Pixhawk 2.4.6, I guess you mean Pixhawk 1, version 2.4.6?

Could you try to apply this PR, upload it to your Pixhawk and try again?

alanypf · 2018-04-07T07:01:06Z

Hi @AlexisTM, yes, I use Pixhawk 1.Thank you for your reply. I will test it as soon as I come back to UNI.I will let you know the result in next Monday.

alanypf · 2018-04-09T08:10:13Z

Hi all and @AlexisTM ,
I tested the codes on pixhawk and also on pixracer. They both worked perfectly.
Thank you for your kind help. It is very helpful.

AlexisTM · 2018-04-09T08:12:00Z

NOTE: This confirms it works fine but it is (if I understood well) not flying.

ArkadiuszNiemiec · 2018-08-03T10:24:59Z

Any update? If I understand correctly there is currently no way to control a gimbal pitch by companion computer?

AlexisTM · 2018-08-03T13:26:59Z

@ArkadiuszNiemiec This mainly require in-flight testing. I have no gimball installed to do so.

ArkadiuszNiemiec · 2018-08-03T14:10:48Z

@AlexisTM Ok, I have built a 1.8.0 version of px4fmu-v2_lpe.px4 with changes from your branch and tested it with my gimbal. It works perfectly in offboard and with Generic Quadrotor x with mount (4002) set as the airframe. We will be flying with this software so tell me what if and what test should we perform.

AlexisTM · 2018-08-03T14:59:22Z

@ArkadiuszNiemiec I am happy to hear that it works well! [This PR is waiting for 1+ year, shame on me]

If you can; try to validate to add a gripper and use two different mixer group at the same time. Except form that, I don't know where the problems could lie.

dagar · 2018-08-03T15:03:28Z

@AlexisTM I apologize for the review delay. This PR looks good to me. Please rebase on current master and I'll merge.

ArkadiuszNiemiec · 2018-08-03T15:14:44Z

@AlexisTM @dagar I have built a px4fmu-v2_lpe.px4 v1.8.0 with this changes and tried it with 3-axis gimbal (only pitch is controllable) in flight. Looks good to me 😄

Just FYI: sending this message keeps an offboard alive.

AlexisTM · 2018-08-03T15:29:23Z

Rebased on current master

AlexisTM · 2018-08-03T15:30:40Z

NOTE: #8907 adds 3 actuator groups (4,5,6) as in the documentation and #8908 solves the same issue

- Allow to use the 4 groups from Mavlink - Allow an Offboard control of the Gimball, Gripper or servoes. Fixes issue #7440 Still need to be tested Signed-off-by: Alexis Paques <alexis.paques@gmail.com>

Signed-off-by: Alexis Paques <alexis.paques@gmail.com>

AlexisTM · 2018-08-09T13:31:32Z

Rebased to current master to retrigger the build.

mzahana · 2018-08-09T16:16:38Z

Does this work only in OFFBOARD mode?

ArkadiuszNiemiec · 2018-08-10T13:04:55Z

@mzahana I believe it works only in OFFBOARD.

dagar · 2018-08-11T15:39:40Z

Restarting CI after #10203.

dagar · 2018-08-12T13:40:34Z

src/modules/mavlink/mavlink_receiver.cpp

-				_actuator_controls_pub = orb_advertise(ORB_ID(actuator_controls_0), &actuator_controls);
+			switch (set_actuator_control_target.group_mlx) {
+			case 0:
+				if (_actuator_controls_pubs[0] == nullptr) {


~~Once last nitpicky thing, can you change these into orb_publish_auto? It includes the nullptr check and initiate advertise in a single call.~~
https://github.com/PX4/Firmware/blob/master/src/modules/uORB/uORB.h#L177

EDIT: Nevermind, you can't do this because the actuator controls will go to a separate instance.

int inst orb_publish_auto(ORB_ID(actuator_controls_0), &_actuator_controls_pubs[0], &actuator_controls, inst, ORB_PRIO_DEFAULT)

AlexisTM · 2018-09-26T08:58:34Z

@mzahana Yes; this is only for the offboard mode: https://github.com/PX4/Firmware/blob/82acf6894ddf8932ed83e3c4fecd8b82edc633bd/src/modules/mavlink/mavlink_receiver.cpp#L1111

RaghunandanVenkatesh · 2021-06-14T12:50:54Z

Hi all,
I am trying the same thing in Gazebo Simulation. Its not working. I have mentioned the steps I followed (here)[https://discuss.px4.io/t/not-able-to-set-actuator-controls-for-fixed-wing-in-sitl/22824).
Please let me know what I am missing.
Thanks

TSC21 mentioned this pull request Aug 23, 2017

Actuator control doens't work with in mavros #7854

Closed

PX4BuildBot added the status/STALE label Jan 17, 2018

PX4BuildBot removed the status/STALE label Apr 5, 2018

dagar self-requested a review August 3, 2018 15:03

dagar added the Admin: Enhancement (improvement) 💡 label Aug 3, 2018

AlexisTM mentioned this pull request Aug 3, 2018

Use control group target from mavlink message. #8908

Closed

AlexisTM added 2 commits August 9, 2018 15:30

Adding the Mavlink handler for the Actuator controls message

5654ab5

- Allow to use the 4 groups from Mavlink - Allow an Offboard control of the Gimball, Gripper or servoes. Fixes issue #7440 Still need to be tested Signed-off-by: Alexis Paques <alexis.paques@gmail.com>

Formatted src/modules/mavlink/mavlink_receiver.cpp

9b7bf2e

Signed-off-by: Alexis Paques <alexis.paques@gmail.com>

AlexisTM mentioned this pull request Aug 9, 2018

CI Failure on Jenkins Firmware Compile: No space left on device #10203

Closed

dagar reviewed Aug 12, 2018

View reviewed changes

dagar approved these changes Aug 12, 2018

View reviewed changes

dagar merged commit 6b8ea8e into PX4:master Aug 12, 2018

jannsta1 mentioned this pull request Oct 5, 2018

Controlling a gimbal via mavros mavlink/mavros#1089

Closed

AlexisTM mentioned this pull request Nov 14, 2018

Actuator control failing (with #7477) #10845

Closed

Adding the Mavlink handler for the Actuator controls message #7477

Adding the Mavlink handler for the Actuator controls message #7477

Conversation

AlexisTM commented Jun 26, 2017 • edited by AuterionWrikeBot Loading

AlexisTM commented Jun 27, 2017

LorenzMeier commented Jul 8, 2017

AlexisTM commented Jul 8, 2017 • edited Loading

AlexisTM commented Jul 12, 2017

TSC21 commented Jul 27, 2017

AlexisTM commented Jul 27, 2017

TSC21 commented Jul 27, 2017

TSC21 commented Aug 16, 2017

TSC21 commented Aug 20, 2017

andresR8 commented Aug 24, 2017

AlexisTM commented Aug 24, 2017

TSC21 commented Aug 24, 2017

AlexisTM commented Aug 24, 2017

TSC21 commented Aug 24, 2017

andresR8 commented Aug 26, 2017 • edited by TSC21 Loading

TSC21 commented Aug 28, 2017

andresR8 commented Aug 29, 2017

AlexisTM commented Sep 4, 2017

TSC21 commented Sep 4, 2017

AlexisTM commented Apr 5, 2018

alanypf commented Apr 5, 2018

AlexisTM commented Apr 5, 2018 • edited Loading

alanypf commented Apr 7, 2018

alanypf commented Apr 9, 2018

AlexisTM commented Apr 9, 2018

ArkadiuszNiemiec commented Aug 3, 2018

AlexisTM commented Aug 3, 2018

ArkadiuszNiemiec commented Aug 3, 2018

AlexisTM commented Aug 3, 2018

dagar commented Aug 3, 2018

ArkadiuszNiemiec commented Aug 3, 2018

AlexisTM commented Aug 3, 2018

AlexisTM commented Aug 3, 2018

AlexisTM commented Aug 9, 2018

mzahana commented Aug 9, 2018

ArkadiuszNiemiec commented Aug 10, 2018

dagar commented Aug 11, 2018

dagar Aug 12, 2018 • edited Loading

Choose a reason for hiding this comment

dagar Aug 12, 2018

Choose a reason for hiding this comment

AlexisTM commented Sep 26, 2018

RaghunandanVenkatesh commented Jun 14, 2021

AlexisTM commented Jun 26, 2017 •

edited by AuterionWrikeBot

Loading

AlexisTM commented Jul 8, 2017 •

edited

Loading

andresR8 commented Aug 26, 2017 •

edited by TSC21

Loading

AlexisTM commented Apr 5, 2018 •

edited

Loading

dagar Aug 12, 2018 •

edited

Loading