Px4-autopilot: Simulated (GAZEBO) vehicle becomes unstable when trying to control a gimbal via the ActuatorControls message

[jannsta1]

The bug
Initially raised this in the sitl repo but was advised to also raise here. Simulated MAV (GAZEBO) becomes unstable when trying to control a gimbal via the ActuatorControls message (in Mavros, which in turn publishes SET_ACTUATOR_CONTROL_TARGET on mavlink). This has also been reported for LED outputs (not sure if this was also simulated or not).

To Reproduce
Steps to reproduce the behavior:

1. roslaunch px4 mavros_posix_sitl.launch vehicle:=typhoon_h480
2. Arm the vehicle and take off
3. Publish /mavros/actuator_control ROS message at 10hz (group_mix=2)
4. The UAV becomes unstable (particularly if non zero setpoints are sent to the controls field of the ActuatorControls topic).

If this process is repeated with a lower publish rate (1 hz works) the vehicle remains stable.

Drone:

• Simulated typhoon h480

[Jaeyoung-Lim]

It seems that all offboard setpoints are ignored when the actuator setpoints are set:

https://github.com/PX4/Firmware/blob/f9324fb76a4af1e2cfe8813e1a7af5d65c2b1ed1/src/modules/mavlink/mavlink_receiver.cpp#L1000-L1011

[julianoes]

Thanks for the report @jannsta1. I think the Typhoon H480 gimbal in SITL needs some care overall. It's not really working properly in general.

[jannsta1]

OK thanks @julianoes - if these updates are unlikely to be worked on for a while I could have a stab at them? (more than happy to leave it to the professionals if its already in the pipeline! :)). If help desired would you be able to outline any other required updates?

The only other thing I've noticed while making use of the model is that the yaw control seems a bit sluggish and/or if you fly outward in a direction from the origin and then turn around and fly back to the starting point there appears to be a large steady state error. Increasing the PID gains seems to help but I also wonder if using previous imu state for the yaw setpoint could result in some drift over time.

Also, from @Jaeyoung-Lim's comment above. it seems like the symptoms described by this issue are not actually contained in the gimbal controller gazebo plugin but are associated with the mavlink receiver. If this is the case then the repair might be a bit beyond me.

[julianoes]

if these updates are unlikely to be worked on for a while I could have a stab at them?

I don't think anyone is currently looking into this :disappointed:, so feel free.

[Jaeyoung-Lim]

@jannsta1 Have your SITL simulation ever crashed when sending actuator setpoints? Sending actuator setpoints over mavros seem to make the SITL crash

[jannsta1]

@Jaeyoung-Lim - it was just the vehicle that was crashing... not gazebo itself. Although I was killing the simulation fairly soon after the loss of control.

Let me know if you want me to try anything in particular (e.g. send a particular message on a control group at a certain rate).

I wonder if you have any suggestions on fixing the central issue? assuming you have identified the root cause (which seems reasonable), a couple of ideas spring to mind:

1) only set the ignore_thrust / body rate... etc flags when the control group is 0, but I guess its not guaranteed that the main motors would be using control group zero.
2) For gimbal control it might be better to use the MAV_CMD_DO_MOUNT_CONTROL message anyway so I could try to create a new mavros plugin for this.

[Jaeyoung-Lim]

@jannsta1

I have tried disabling the ignore flag when the group_mlx=2 However, this still results in erratic behavior of the setpoints. I suspect that the mavlink_receiver somehow has problems of consuming both setpoints, but not sure exactly how this is the case.

For context: I am currently sending setpoint body_rates and thrust commands that make the drone follow a trajectory, while sending actuator_control topics with group_mix=2 to control the gimbal. All mavlinks are sent through mavros

[jannsta1]

@Jaeyoung-Lim - could it be that another module is also publishing to the same actuator_controls topic at the same time? Having a look at the topic factor graph my interpretation is that the mavlink module would subscribe to actuator_controls_XX topics rather than publish to them?

[Jaeyoung-Lim]

@jannsta1 Indeed, there is something resetting the gimbal control values. I made a small fix so that at least I can control the gimbal when I stream actuator setpoints at 100Hz, but this disrupts the setpoint commands for the vehicle(bodyrate and thrust) The problems that I can see are two fold.

1. The gimbal control topics are reset to 0 resulting in oscillatory behavior for low rate setpoints
   The actuator outputs(gimball) in SITL are as the following.
2. Sending actuator_setpoints(group_mix=2) at 10Hz
   log: https://review.px4.io/plot_app?log=6310f80b-8714-4626-810e-6a83a1e680e7
   

• Sending actuator_setpoints(group_mix=2) at 100Hz
  https://review.px4.io/plot_app?log=7482249f-a0c0-44b7-a692-aad373741b8b

You can see that the values are reset to 0 when the actuator_setpoints (group_mix=2) are sent at a lower rate

1. The other setpoints: in my case the body_rate and thrust setpoints get disturbed by the actuator setpoints.
2. Sending actuator_setpoints(group_mix=2) at 10Hz
   

• Sending actuator_setpoints(group_mix=2) at 100Hz
  log:
  

In both cases the body rate and thrust setpoints are sent with the same rate

@julianoes Do you have any suggestions on where to look?

[Jaeyoung-Lim]

Suppressing the outputs from output_rc.cpp by commenting out the following fixes the problem.

https://github.com/PX4/Firmware/blob/5ba56c8c2cb7b48b6987ac06a503fd8841eb9925/src/modules/vmount/output_rc.cpp#L74-L84

• Sending actuator_setpoints(group_mix=2) at 10Hz
  log: https://review.px4.io/plot_app?log=b47fbcf5-babb-402e-9fdd-017e7b09c179

The pink line shows the gimbal setpoint being set continously and not being set to zero as shown in previous examples.
@jannsta1 This is basically a hack, but feel free to try it out: https://github.com/Jaeyoung-Lim/Firmware/tree/pr-actuator-setpoints

I will clean this up and make a proper PR

[jannsta1]

Thanks @Jaeyoung-Lim!

Just a quick point, I was stopping vmount (just by appending vmount stop to the file 6011_typhoon_h480.post) maybe this will prevent having to do what otherwise looks like a complicated fix?

[Jaeyoung-Lim]

@jannsta1 Thanks. You are right. Stopping vmount solved the problem of resetting the values to 0.

[jannsta1]

@Jaeyoung-Lim A bit off topic but have you had other problems with running the typhoon model? As of this week, if I donwload the latest repositories and try to arm/launch the typhoon_h480 model I get messages like Preflight Fail: Compass #0 uncalibrated. All other tested models (iris_xx) seem to work fine and I've tried rolling back to different commits of sitl_gazebo and Firmware but don't seem to be able to get a working version. If needs be I will raise a separate issue for it / look into root cause but since the sitl_gazebo issues section doesn't seem to be monitored very actively it would be great to get a sanity check from someone!

[julianoes]

@jannsta1 have you tried make clean in order to make sure the param file is deleted?

[jannsta1]

Thanks for the idea, I probably wasn't doing make clean as often as I should. Just been through the procedure of cloning the firmware again and now if I start gazebo with roslaunch px4 mavros_posix_sitl.launch vehicle:=typhoon_h480 verbose:=true I can takeoff with pxh> commander takeoff which I don't think I could do before. Also, now when I arm and send setpoints for a first time the vehicle doesn't take off. On a second attempt I get a different error - Critical navigation failure! Check sensor calibration. I guess there is something wrong with my ros node that generates setpoints but strange that this has worked fine up until now and still seems to work OK with other vehicles.

[julianoes]

Hang on, if you're using roslaunch, you need to do this to clean the params:

rm ~/.ros/eeprom/parameters

This is a known issue and really frustrating actually, see: https://github.com/PX4/Firmware/issues/10686

[jannsta1]

Noted! But despite this I find that I can't control the typhoon_h480 model after the following Firmware commit: https://github.com/PX4/Firmware/commit/43e3fc707de823faa19b16091e5ee4a900e3c446.

The previous commit https://github.com/PX4/Firmware/commit/4a4d323a970225ec27e9cd4a409e7a287ad57ff6 and prior work fine.

I wonder if could confirm if you observe the same @julianoes or @Jaeyoung-Lim?

[jannsta1]

logged bug report https://github.com/PX4/Firmware/issues/12221 to document what I think might be a confounding issue.

[Jaeyoung-Lim]

@jannsta1 The MountControl plugin in mavros is now merged! You can control the gimbal continuosly while controlling the vehicle offboard.

[jannsta1]

Nice one! Will be next week before I try to integrate this but this is certainly enough to close this issue.