[drone_wrapper] Actual drone speeds lower than setpoints

[diegomrt]

The actual drone speeds seen at /mavros/local_position/odom/twist are below the setpoint values given (for the first 15-20 sec).

Happens using both pure and mixed speed controls. Tested for 1 m/s and 2 m/s

[pariaspe]

One possible solution seems to be re-tuning the horizontal velocity error PID. Gains can be changed by overwriting the following parameters:

• MPC_XY_VEL_P
• MPC_XY_VEL_I
• MPC_XY_VEL_D

Actual values are kp = 0.2, ki = 0.02 and kd = 0.016.

By setting ki = 0.1 the response is reduced up to 6-7 secs:

[diegomrt]

Thanks @pariaspe, not perfect but I think that way the drone speed behaviour is much closer to what is expected.

It could be a good idea to include a new method in the dronewrapper class to read and change those three parameters just when needed. What do you think? Doing it via MavROS is quite simple.

[pariaspe]

Hi @diegomrt,

Of course the PID can be tuned better, I did it by eye trying different values (as you can see I'm not an expert tuning PIDs). However, it is not easy to get a really fast response without oscillations.

Including a new method to modify this parameters could be a possible solution as you said. But shouldn't these parameters get modified always? Why not modify them at launching?

[diegomrt]

Our drone_wrapper should work with other drones besides the 3DR IRIS model, including real aircrafts. In this sense it could be risky to modify PID values permanently, for all aircrafts despite their mass, flight characteristics or values for their other low-level PIDs.

I'll do it permanently if we are able to apply it just to IRIS in SITL

[pariaspe]

I've been exploring the different possibilities to change the parameters. This is what I have:

• Python way: set_param.py
• mavparm script:

  rosrun mavros mavparam set <param> <value>

• PX4 param tool:

  pxh> param set <param> <value>

• rosservice:

  rosservice call /mavros/param/set mavros_msgs/Param "[param_id: '<param>', value: [integer: 0 , real: <value>]]"

[pariaspe]

There are several solutions to change the parameters. The following image shows the actual structure and where the parameters could be changed.

Launching sequence

Options

1. At drone_wrapper source. From the exercise launch file the script play_python_code (part of drone_wrapper pkg) is executed. The script launches my_solution.py which calls DroneWrapper class in its execution. Then, there are two possibilities:

   • At my_solution.py file. More control on whether or not the parameters are changed.
   • At DroneWrapper class. It might execute (and change the parameters) every time it is called.

2. At exercise launch file. mavparam tool from MAVROS can be used to change parameters' values by launching a node. I find these three possibilities:

   • Using mavparam set. This tool only change parameters one by one, then it will have to create one node calling mavparam set for each parameter. Config file can not be used. Not recommended.
   • Using mavparam load. Load option allows to set several parameters stored in a file. However, configuration files must have all the parameters needed, not only the ones that you want to change.
   • Using bash scripting. One node will read the configuration file and change all of its parameters by running rosrun mavparam set <parameter> <value>. This gist shows an example of what the script would look like.

3. At MAVROS_PX4_SITL launch file. As in the previous option, mavparam tool can be called in this launch file. The difference between Option 2 and 3 are similar to the two possibilities at Option 1.

4. At PX4/NODE launch file. During MAVROS startup sequence px4_config.yaml is called. It seems that parameters can be changed from here. However, this launch file is part of MAVROS pkg and to modify it we must have to copy a bunch of files, do the modifications wanted and add them to our ROS pkg.

5. At POSIX_SITL launch file. As explained at this PX4 discussion topic, we can create a custom model with these parameters changed.

My proposal

Let's see:

• Option 5 makes no sense if we want to change parameters dynamically. it would be absurd to create a model for each configuration.
• Option 4 looks great but it might be too complicated to just change a bunch of parameters.
• From my point of view, I find one main issue in Option 1. It will be not easy to use a config file. To do it, we should modify play_python_script to pass the file path or use a "static" path (unmodified) where it will be always the config file.

Finally we have two options (Opt. 2 and 3). Personally, I would have the config file at each exercise but I would change the parameters from MAVROS_PX4_SITL file. Moreover, I think the better option to change them would be using the bash script.

[diegomrt]

Nice summary of all available options @pariaspe!

Let's go with option 2, using mavparam set from a single ROS node. We need just to check previously if PX4 (real thing or SITL) is available and ready to process the parameter changes. Once it's ready the node runs a callback funcion, something like this:

def change_parameters():
        try: 
        values = ParamValue(parse values from YML file)  
        set = rospy.ServiceProxy(mavros.get_topic('param', 'set'), ParamSet)
        new_XXX_param = set(param_id="XXX", value=xxx) 
        new_YYY_param = set(param_id="YYY", value=yyy)
            rospy.loginfo("Param XXX changed to xxx"...)
        except rospy.ServiceException, e:
            rospy.logerror("Param change failed: %s"%e)

[pariaspe]

Hi @diegomrt ,

I was searching how to check if the PX4 is ready or not. I've found this:

1. First msg published on /mavros/local_position/pose happens just after EKF aligned notification (see green dot on figure).

2. On topic /mavros/state the parameter mode changes from MANUAL to AUTO.LOITER after commencing GPS fusion notification (see blue dot on figure).

What do you think? Can you think of any other message that may help us?

[diegomrt]

Hi @pariaspe,

I'd go with 1. I think at that point PX4 must be ready to get parameter updates, and is very convenient just suscribing to that topic and wait to get messages to run the callback.

[pariaspe]

Well, I tried both options and I get the same warning: Unknown rosparam: MPC_XY_VEL_I. I guess that PX4 is not ready yet...

EDIT: If I wait a couple of seconds after receiving the message, it works properly. But, as we discussed in the last meeting, we shouldn't wait X seconds.

[diegomrt]

Well, then to be sure if there is a valid flag to mark PX4 as ready, I'd suggest to open a new question directly in the PX4-MAVROS forum at the discuss.px4.io page. It seems a pretty active page.

[pariaspe]

As the problem was solved at the last pr #37, I would like to explain which solution has been chosen how it works.

At the bottom of mavros_px4_sitl.launch file, a script is launched changing the parameters:

#!/bin/bash
rosservice call --wait /mavros/param/pull false

rosparam load $1 /mavros/param/
rosservice call --wait /mavros/param/push

First, a pull is called in order to wait until PX4 param service is ready. Then, the config file is loaded as ROS params and, at last, ROS params are pushed into FCU params.

There is a default config file in drone_wrapper package. However, it can be overwritten with another config file by passing it through each exercise launch file.