Week 2 - Heights and models

1. The Height issue

So a big and noticable issue is that the PX4 does not hold its altitude when commanded a velocity in offboard mode (as illustrated in this video by JdeRobot and the video below)

(Click on the image for the video)

There are two methods for providing setpoints to the PX4 in SITL through MAVROS in the OFFBOARD mode. These are:

Note: As mentioned here, only 2 frames are allowed in the OFFBOARD mode: MAV_FRAME_LOCAL_NED and MAV_FRAME_BODY_NED

Both of these include a type_mask that can be set to pass only specific values through a mav_msgs/PositionTarget on the setpoint_raw/local topic. This issue shows an example of the command to control the velocity. Building on this, I tested sending vx, vy,az, and z on setpoint_raw/local with the appropriately set type_mask and was able to get a consistent altitude (+-10% max). This also involved quite a bit of searching and to provide credit where it is due, following are the links that were the most helpful: [1] [2] [3] [4] [5]

Finally, I modified DroneWrapper to use setpoint_raw instead of the setpoint_position or setpoint_velocity which can now be commanded to move with any particular vx, vy and az to hold a given altitude (z). The altitude tracking is now only limited by the tuning of the PID values of the PX4 (as is evident in the video below).

(Click on the image for the video)

Note: An issue related to this has been raised on PX4/Firmware and proposes to have a sort of position hold when the commanded velocity is 0. I shall incorporate this (i.e. git pull) once it is merged into master.

2. The fault in our models

Another task for this week was to figure out what was the issue in the model of the Iris that was causing PX4 and MAVROS to fail on launching iris_1_roadtexturesROS.launch.

Note: Meld is <3

The current JdeRobot-assets has the following changes from the PX4 SITL models:

(Major) fpv_cam does not exist in the JdeRobot-assets and is probably imported from the PX4 directories
(Major) mavlink_tcp not set or used
(Minor) Inertia values for the Iris are different (higher)
(Minor) self_collision tags not set
(Immaterial) Magnetometer and barometer not included in model
(Immaterial) Dronecode sdk ports not set
(Immaterial) vision_estimation set to False
(Immaterial) lockstep is disabled

The terminal dump itself revealed quite a few issues:

px4.sh file does not exist:

 ERROR: cannot launch node of type [mavros/px4.sh]: can't locate node [px4.sh] in package [mavros]

The textures for the house do not exist:

 [Err] [Material.cc:93] Unable to find texture[house_4/materials/Steel_Brushed_Stainless.png] in path[/opt/jderobot/share/jderobot/gazebo/models/house_4/meshes]
 [Err] [Material.cc:93] Unable to find texture[house_4/materials/Wood_Veneer_01.png] in path[/opt/jderobot/share/jderobot/gazebo/models/house_4/meshes]
 [Err] [Material.cc:93] Unable to find texture[house_4/materials/Brick_Antique_01.png] in path[/opt/jderobot/share/jderobot/gazebo/models/house_4/meshes]
 [Err] [Material.cc:93] Unable to find texture[house_4/materials/Roofing_Tile_Spanish.png] in path[/opt/jderobot/share/jderobot/gazebo/models/house_4/meshes]
 [Err] [Material.cc:93] Unable to find texture[house_4/materials/Blinds_Roman_Hobbled_Blue.png] in path[/opt/jderobot/share/jderobot/gazebo/models/house_4/meshes]
 [Err] [Material.cc:93] Unable to find texture[house_4/materials/Translucent_Glass_Gold.png] in path[/opt/jderobot/share/jderobot/gazebo/models/house_4/meshes]

Some unnecessary joints are instantiated?

 [Dbg] [gazebo_mavlink_interface.cpp:135] <joint_name> not found for channel[0] no joint control will be performed for this channel.
 [Dbg] [gazebo_mavlink_interface.cpp:135] <joint_name> not found for channel[1] no joint control will be performed for this channel.
 [Dbg] [gazebo_mavlink_interface.cpp:135] <joint_name> not found for channel[2] no joint control will be performed for this channel.
 [Dbg] [gazebo_mavlink_interface.cpp:135] <joint_name> not found for channel[3] no joint control will be performed for this channel.
 [Wrn] [gazebo_mavlink_interface.cpp:124] joint [zephyr_delta_wing::propeller_joint] not found for channel[4] no joint control for this channel.
 [Wrn] [gazebo_mavlink_interface.cpp:124] joint [zephyr_delta_wing::flap_left_joint] not found for channel[5] no joint control for this channel.
 [Wrn] [gazebo_mavlink_interface.cpp:124] joint [zephyr_delta_wing::flap_right_joint] not found for channel[6] no joint control for this channel.
 [Dbg] [gazebo_mavlink_interface.cpp:135] <joint_name> not found for channel[7] no joint control will be performed for this channel.

The instantiated FCU is unsupported by the autopilot version:

 [ INFO] [1560107184.445152791, 1.000000000]: CON: Got HEARTBEAT, connected. FCU: No valid autopilot
 [ WARN] [1560107186.449309203, 3.001000000]: VER: broadcast request timeout, retries left 4
 [ WARN] [1560107187.451040969, 4.001000000]: VER: broadcast request timeout, retries left 3
 [ WARN] [1560107188.451929025, 5.001000000]: VER: unicast request timeout, retries left 2
 [ WARN] [1560107189.454144900, 6.002000000]: VER: unicast request timeout, retries left 1
 [ WARN] [1560107190.455397268, 7.001000000]: VER: unicast request timeout, retries left 0
 [ WARN] [1560107191.456982063, 8.001000000]: VER: your FCU don\'t support AUTOPILOT_VERSION, switched to default capabilities
 [ INFO] [1560107194.460947995, 11.000000000]: HP: requesting home position
 [ WARN] [1560107195.461889771, 12.000000000]: PR: request list timeout, retries left 2
 [ WARN] [1560107196.463622539, 13.000000000]: PR: request list timeout, retries left 1
 [ WARN] [1560107197.463940161, 14.000000000]: PR: request list timeout, retries left 0
 [ WARN] [1560107200.470114635, 17.001000000]: WP: timeout, retries left 2
 [ WARN] [1560107201.471552573, 18.001000000]: WP: timeout, retries left 1
 [ WARN] [1560107202.474082880, 19.001000000]: WP: timeout, retries left 0
 [ERROR] [1560107203.473500978, 20.001000000]: WP: timed out.
 [ INFO] [1560107204.475591470, 21.000000000]: HP: requesting home position
 [ INFO] [1560107214.490992867, 31.000000000]: HP: requesting home position
 [ INFO] [1560107224.507572507, 41.000000000]: HP: requesting home position
 [ERROR] [1560107234.125480437, 50.600000000]: TM : Time jump detected. Resetting time synchroniser.
 [ INFO] [1560107234.526088468, 51.000000000]: HP: requesting home position
 [ERROR] [1560107241.897759277, 58.360000000]: MODE: Unsupported FCU
 [ WARN] [1560107243.540589254, 60.000000000]: TM: Wrong FCU time.
 [ INFO] [1560107244.542228379, 61.000000000]: HP: requesting home position
 [ERROR] [1560107244.901495877, 61.359000000]: MODE: Unsupported FCU
 [ERROR] [1560107247.907206219, 64.359000000]: MODE: Unsupported FCU
 [ERROR] [1560107250.912552969, 67.360000000]: MODE: Unsupported FCU
 [ERROR] [1560107253.916818570, 70.360000000]: MODE: Unsupported FCU
 [ INFO] [1560107254.558771378, 71.000000000]: HP: requesting home position

Changes in the world files:

grass_plane used instead of ground_plane with mudboxes
House model changed (3 -> 4)
iris_fpv_cam inserted in place of ArDrone2 (Obviously)
Road lowered by 14 cm

Some additional parameters are provided to gazebo in my implementation which don’t exist in iris_1_roadtextures.world:

 <physics name="default_physics" default="0" type="ode">
   <gravity>0 0 -9.8066</gravity>
   <ode>
     <solver>
       <type>quick</type>
       <iters>10</iters>
       <sor>1.3</sor>
       <use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
     </solver>
     <constraints>
       <cfm>0</cfm>
       <erp>0.2</erp>
       <contact_max_correcting_vel>100</contact_max_correcting_vel>
       <contact_surface_layer>0.001</contact_surface_layer>
     </constraints>
   </ode>
   <max_step_size>0.004</max_step_size>
   <real_time_factor>1</real_time_factor>
   <real_time_update_rate>250</real_time_update_rate>
   <magnetic_field>6.0e-6 2.3e-5 -4.2e-5</magnetic_field>
 </physics>

The major point to note here is that the files available reference deprecated software (refer above) and are not really very easy to manipulate (refer Creating new models). The SDF of course can be modified to add cameras directly in it but this is not a viable strategy for any other possible additions in the future. I have tested the new models in the follow_road world and used them as a basis for fixing the model, world and launch file in the JdeRobot-assets. These changes are available as a pull request #20.