[drivers] mavROS speed control in relative frame of coordinates

[jmplaza]

We are moving to mavROS drivers for drone control. It will replace our ArDrone drivers (based on ICE communications). The API should be equivalent: take off and land commands, and speed control (in relative coordinates) for up/down, front/back and yaw rotation. This is the API most our drones applications use.

@diegomrt has solved the proof of concept using the off-board mode inside MavLink protocol. Thanks @diegomrt!

[pmvera]

I've been testing the proof of concept using the off-board mode inside MavLink protocol developed by @diegomrt. The result is shown in this video. The drone still moves in attitude when the /mavros/setpoint_position/local is disabled and start the /mavros/setpoint_velocity/cmd_vel topic.

I think there's still a topic that must be disabled to avoid this random movement. Any idea @diegomrt?

[diegomrt]

I've been able to reproduce the altitude drift that happens when changing from a /mavros/setpoint_position command to a relative /mavros/setpoint_velocity with vz=0. My test is in this video. It seems that the FCU loop closing for zero vz is not accurate enough to hold altitude in OFFBOARD mode. However, it seems to work well for non-zero values of vz.

I'm not sure if the altitude "random movements" in OFFBOARD mode can be disabled or they are inherent to the SITL simulation of this PX4 mode in Gazebo (coming from noise from the barometric pressure used to hold altitude, or similar). As far as I know, at the moment the OFFBOARD mode is in rework, maybe this will be fixed in a future version.

Meanwhile, what you can do to circumvent it is to use the more complex mavros_msgs/PositionTarget messages via the /mavros/setpoint_raw/local ROS topic. In those messages, using the proper uint16 type_mask (see for example this issue for more details) you can combine relative vx,vy commands with position.z (altitude) holding and yaw settings. I've uploaded an example of a circular trajectory with altitude holding in this video.

[pmvera]

Hi @diegomrt, I've tried to move the drone with /mavros/setpoint_raw/local topic and it's not working perfectly, in fact the attitude still changes and the circular trajectory is worse. You can see the result in this video. I'll continue doing changes in the values of the messages to test if there's a way to stabilized the drone and make the circular trajectory. Thanks for the support!

[diegomrt]

Hi @PabloMorenoVera. I've seen your video and it's a very weird behavior that I cannot reproduce. Are you sure it didn't enter the failsafe mode and tried to climb +10m?

I've tested again the /mavros/setpoint_raw/local setpoints using the Iris drone and I also get good altitude holding, within the precision of having altitude measurements via a noisy barometric sensor (+- 20 cm aprox., as you can see in the rqt_plot) The circular trajectories are also as expected, as you can see in this video.

I'd suggest testing again with the Iris drone and using other gazebo worlds to double check such behavior.

[pmvera]

Hi @diegomrt, I've tried to launch several files but they crashed. The message is the following:
`roslaunch mavros px4.launch
... logging to /home/pablo/.ros/log/93ee3782-f22d-11e8-aec0-3010b39ef6cd/roslaunch-pablo-13887.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://pablo:44809/

SUMMARY

CLEAR PARAMETERS

• /mavros/

PARAMETERS

• /mavros/cmd/use_comp_id_system_control: False
• /mavros/conn/heartbeat_rate: 1.0
• /mavros/conn/system_time_rate: 1.0
• /mavros/conn/timeout: 10.0
• /mavros/conn/timesync_rate: 10.0
• /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
• /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
• /mavros/distance_sensor/hrlv_ez4_pub/id: 0
• /mavros/distance_sensor/hrlv_ez4_pub/orientation: PITCH_270
• /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
• /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
• /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
• /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
• /mavros/distance_sensor/laser_1_sub/id: 3
• /mavros/distance_sensor/laser_1_sub/orientation: PITCH_270
• /mavros/distance_sensor/laser_1_sub/subscriber: True
• /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
• /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
• /mavros/distance_sensor/lidarlite_pub/id: 1
• /mavros/distance_sensor/lidarlite_pub/orientation: PITCH_270
• /mavros/distance_sensor/lidarlite_pub/send_tf: True
• /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
• /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
• /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
• /mavros/distance_sensor/sonar_1_sub/id: 2
• /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
• /mavros/distance_sensor/sonar_1_sub/subscriber: True
• /mavros/fake_gps/eph: 2.0
• /mavros/fake_gps/epv: 2.0
• /mavros/fake_gps/fix_type: 3
• /mavros/fake_gps/geo_origin/alt: 408.0
• /mavros/fake_gps/geo_origin/lat: 47.3667
• /mavros/fake_gps/geo_origin/lon: 8.55
• /mavros/fake_gps/gps_rate: 5.0
• /mavros/fake_gps/mocap_transform: True
• /mavros/fake_gps/satellites_visible: 5
• /mavros/fake_gps/tf/child_frame_id: fix
• /mavros/fake_gps/tf/frame_id: map
• /mavros/fake_gps/tf/listen: False
• /mavros/fake_gps/tf/rate_limit: 10.0
• /mavros/fake_gps/tf/send: False
• /mavros/fake_gps/use_mocap: True
• /mavros/fake_gps/use_vision: False
• /mavros/fcu_protocol: v2.0
• /mavros/fcu_url: /dev/ttyACM0:57600
• /mavros/gcs_url:
• /mavros/global_position/child_frame_id: base_link
• /mavros/global_position/frame_id: map
• /mavros/global_position/gps_uere: 1.0
• /mavros/global_position/rot_covariance: 99999.0
• /mavros/global_position/tf/child_frame_id: base_link
• /mavros/global_position/tf/frame_id: map
• /mavros/global_position/tf/global_frame_id: earth
• /mavros/global_position/tf/send: False
• /mavros/global_position/use_relative_alt: True
• /mavros/image/frame_id: px4flow
• /mavros/imu/angular_velocity_stdev: 0.000349065850399
• /mavros/imu/frame_id: base_link
• /mavros/imu/linear_acceleration_stdev: 0.0003
• /mavros/imu/magnetic_stdev: 0.0
• /mavros/imu/orientation_stdev: 1.0
• /mavros/local_position/frame_id: map
• /mavros/local_position/tf/child_frame_id: base_link
• /mavros/local_position/tf/frame_id: map
• /mavros/local_position/tf/send: False
• /mavros/local_position/tf/send_fcu: False
• /mavros/mission/pull_after_gcs: True
• /mavros/mocap/use_pose: True
• /mavros/mocap/use_tf: False
• /mavros/odometry/frame_tf/body_frame_orientation: frd
• /mavros/odometry/frame_tf/local_frame: vision_ned
• /mavros/plugin_blacklist: ['safety_area', '...
• /mavros/plugin_whitelist: []
• /mavros/px4flow/frame_id: px4flow
• /mavros/px4flow/ranger_fov: 0.118682389136
• /mavros/px4flow/ranger_max_range: 5.0
• /mavros/px4flow/ranger_min_range: 0.3
• /mavros/safety_area/p1/x: 1.0
• /mavros/safety_area/p1/y: 1.0
• /mavros/safety_area/p1/z: 1.0
• /mavros/safety_area/p2/x: -1.0
• /mavros/safety_area/p2/y: -1.0
• /mavros/safety_area/p2/z: -1.0
• /mavros/setpoint_accel/send_force: False
• /mavros/setpoint_attitude/reverse_thrust: False
• /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
• /mavros/setpoint_attitude/tf/frame_id: map
• /mavros/setpoint_attitude/tf/listen: False
• /mavros/setpoint_attitude/tf/rate_limit: 50.0
• /mavros/setpoint_attitude/use_quaternion: False
• /mavros/setpoint_position/mav_frame: LOCAL_NED
• /mavros/setpoint_position/tf/child_frame_id: target_position
• /mavros/setpoint_position/tf/frame_id: map
• /mavros/setpoint_position/tf/listen: False
• /mavros/setpoint_position/tf/rate_limit: 50.0
• /mavros/setpoint_velocity/mav_frame: BODY_NED
• /mavros/startup_px4_usb_quirk: True
• /mavros/sys/disable_diag: False
• /mavros/sys/min_voltage: 10.0
• /mavros/target_component_id: 1
• /mavros/target_system_id: 1
• /mavros/tdr_radio/low_rssi: 40
• /mavros/time/time_ref_source: fcu
• /mavros/time/timesync_avg_alpha: 0.6
• /mavros/time/timesync_mode: MAVLINK
• /mavros/vibration/frame_id: base_link
• /mavros/vision_pose/tf/child_frame_id: vision_estimate
• /mavros/vision_pose/tf/frame_id: map
• /mavros/vision_pose/tf/listen: False
• /mavros/vision_pose/tf/rate_limit: 10.0
• /mavros/vision_speed/listen_twist: True
• /mavros/vision_speed/twist_cov: True
• /rosdistro: kinetic
• /rosversion: 1.12.14

NODES
/mavros (mavros/mavros_node)

auto-starting new master
process[master]: started with pid [13897]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 93ee3782-f22d-11e8-aec0-3010b39ef6cd
process[rosout-1]: started with pid [13910]
started core service [/rosout]
process[mavros-2]: started with pid [13928]
[ INFO] [1543313821.252991390]: FCU URL: /dev/ttyACM0:57600
[ INFO] [1543313821.254468035]: serial0: device: /dev/ttyACM0 @ 57600 bps
[FATAL] [1543313821.254650635]: FCU: DeviceError:serial:open: No such file or directory

REQUIRED process [mavros-2] has died!
process has finished cleanly
log file: /home/pablo/.ros/log/93ee3782-f22d-11e8-aec0-3010b39ef6cd/mavros-2*.log
Initiating shutdown!

[mavros-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
I've changed the FCU_URL from /dev/ttyACM0:57600 to udp://:14540@127.0.0.1:14540, because i haven't got the ttyACM0 folder in my system. It's weird because tha launch file i'm using works but the rest of the giles i try to use are not working. If i made the FCU change, the world appears but i can't move the drone. The message is:rosrun mavros mavsafety arm
Request failed. Check mavros logs`

Also, i post the result of launch my working world (the one of my last video), because i've still got the same issue. I've notice when i launch the world that i get the next message:
`______ __ __ ___
| ___ \ \ \ / / / |
| |/ / \ V / / /| |
| __/ / \ / /| |
| | / /^\ \ ___ |
_| / / |_/

px4 starting.

ERROR [parameters] decoder init failed
ERROR [param] importing from 'rootfs/eeprom/parameters' failed (-61)
Command 'param' failed, returned 1
`
I'm not sure if that is normal o my problem comes for it.

Sorry for the huge post, but i want to give you all the information as posible.

Thanks for all!
Pablo,

[diegomrt]

Hi @PabloMorenoVera. Are you trying to launch mavros to connect with a real FCU present in /dev/ttyACM0 and connected by USB? What kind of FCU is it? What mavlink protocol version does it support (fcu_protocol parameter)? Have you checked the FCU is recognized by the system (ls -l /dev/ttyACM0)?

If you launch mavros with fcu_url:="udp://:14540@127.0.0.1:14557" you will connect to the PX4 SITL (simulation), not to a real FCU.

If PX4 SITL doesn't launch completely with other drones, I'd suggest to make a clean installation. It's probably the fastest way to get it working again.

[diegomrt]

In addition @PabloMorenoVera, I've tested using a Sonar rangefinder to control altitude, instead of the default barometric pressure sensor, and the altitude drift is greatly reduced. Have a look at how steady the Iris drone hovers in this video.

The key is first using a PX4 simulated drone that includes a rangefinder or a PX4flow sensor, and second, changing a PX4 Kalman filter parameter EKF2_HGT_MODE to 2. It can be done via the QGroundControl groundstation app or the PX4 console itself.