Precision landing in SITL issue

Hi all,

I tried to get precision landing to work following the documentation guide here https://docs.px4.io/en/advanced_features/precland.html

I can successfully run

make posix_sitl_default gazebo_iris_irlock

but when I try to run it with

drivers/irlock

added to the posix_sitl_default config file, I get the following from std out


zebo_iris_irlock
ninja: Entering directory `/home/tom/px4/px4_toolchain/src/Firmware/build/posix_sitl_default’
[1/1] Re-running CMake…
– PX4 VERSION: v1.8.0-1508-gd3b54c35
– CONFIG: posix_sitl_default
– Build Type: RelWithDebInfo
– C compiler: cc (Ubuntu 5.4.0-6ubuntu1~16.04.10) 5.4.0 20160609
– C++ compiler: c++ (Ubuntu 5.4.0-6ubuntu1~16.04.10) 5.4.0 20160609
– PX4 ECL: Very lightweight Estimation & Control Library v0.9.0-590-g2354c30
– Configuring done
– Generating done
– Build files have been written to: /home/tom/px4/px4_toolchain/src/Firmware/build/posix_sitl_default
[3/139] Performing configure step for ‘sitl_gazebo’
– install-prefix: /usr/local
– Boost version: 1.58.0
– Found the following Boost libraries:
– system
– thread
– timer
– chrono
– date_time
– atomic
– Boost version: 1.58.0
– Building klt_feature_tracker without catkin
– Building OpticalFlow with OpenCV
– catkin DISABLED
Gazebo version: 7.14
– Using C++17 compiler
– Configuring done
– Generating done
– Build files have been written to: /home/tom/px4/px4_toolchain/src/Firmware/build/posix_sitl_default/build_gazebo
[17/139] Building CXX object src/drive…Files/drivers__irlock.dir/irlock.cpp.o
FAILED: /usr/bin/c++ -DCONFIG_ARCH_BOARD_SITL -DMODULE_NAME=“irlock” -DPX4_MAIN=irlock_app_main -D__DF_LINUX -D__PX4_LINUX -D__PX4_POSIX -D__STDC_FORMAT_MACROS -Dnoreturn_function=“attribute((noreturn))” -I. -Isrc -Isrc/lib -Isrc/modules -I…/…/src -I…/…/src/drivers/boards/sitl -I…/…/src/include -I…/…/src/lib -I…/…/src/lib/DriverFramework/framework/include -I…/…/src/lib/matrix -I…/…/src/modules -I…/…/src/platforms -I…/…/platforms/posix/include -Iexternal/Install/include -g -fno-exceptions -fno-rtti -std=gnu++11 -fno-threadsafe-statics -DCONFIG_WCHAR_BUILTIN -D__CUSTOM_FILE_IO__ -fcheck-new -Wall -Wextra -Werror -Warray-bounds -Wdisabled-optimization -Wdouble-promotion -Wfatal-errors -Wfloat-equal -Wformat-security -Winit-self -Wlogical-op -Wmissing-declarations -Wpointer-arith -Wshadow -Wuninitialized -Wunknown-pragmas -Wunused-variable -Wno-implicit-fallthrough -Wno-missing-field-initializers -Wno-missing-include-dirs -Wno-unused-parameter -Wunused-but-set-variable -Wformat=1 -Wno-overloaded-virtual -Wreorder -Wno-format-truncation -fvisibility=hidden -include visibility.h -fno-strict-aliasing -fomit-frame-pointer -fno-math-errno -funsafe-math-optimizations -ffunction-sections -fdata-sections -fno-strength-reduce -fno-builtin-printf -fdiagnostics-color=always -O2 -g -DNDEBUG -fno-strict-aliasing -fomit-frame-pointer -fno-math-errno -funsafe-math-optimizations -ffunction-sections -fdata-sections -fno-strength-reduce -fno-builtin-printf -fdiagnostics-color=always -std=gnu++11 -MMD -MT src/drivers/irlock/CMakeFiles/drivers__irlock.dir/irlock.cpp.o -MF src/drivers/irlock/CMakeFiles/drivers__irlock.dir/irlock.cpp.o.d -o src/drivers/irlock/CMakeFiles/drivers__irlock.dir/irlock.cpp.o -c …/…/src/drivers/irlock/irlock.cpp
…/…/src/drivers/irlock/irlock.cpp:59:25: fatal error: nuttx/clock.h: No such file or directory
compilation terminated.
[17/139] Performing build step for ‘sitl_gazebo’
[1/57] Building CXX object CMakeFiles/sensor_msgs.dir/IRLock.pb.cc.o
[2/57] Building CXX object CMakeFiles/sensor_msgs.dir/Float.pb.cc.o
[3/57] Building CXX object CMakeFiles/sensor_msgs.dir/Imu.pb.cc.o
[4/57] Building CXX object CMakeFiles/sensor_msgs.dir/Groundtruth.pb.cc.o
[5/57] Building CXX object CMakeFiles/sensor_msgs.dir/Range.pb.cc.o
[6/57] Building CXX object CMakeFiles/sensor_msgs.dir/SITLGps.pb.cc.o
[7/57] Building CXX object CMakeFiles/sensor_msgs.dir/OpticalFlow.pb.cc.o
[8/57] Building CXX object CMakeFiles/mav_msgs.dir/MotorSpeed.pb.cc.o
[9/57] Building CXX object CMakeFiles/mav_msgs.dir/CommandMotorSpeed.pb.cc.o
[10/57] Building CXX object CMakeFiles/std_msgs.dir/Int32.pb.cc.o
[11/57] Building CXX object CMakeFiles/nav_msgs.dir/Odometry.pb.cc.o
[12/57] Building CXX object CMakeFiles/physics_msgs.dir/Wind.pb.cc.o
[13/57] Linking CXX shared library libmav_msgs.so
[14/57] Linking CXX shared library libstd_msgs.so
[15/57] Building CXX object OpticalFlow/klt_feature_tracker/CMakeFiles/klt_feature_tracker.dir/src/trackFeatures.cpp.o
[16/57] Linking CXX shared library libsensor_msgs.so
[17/57] Linking CXX shared library libphysics_msgs.so
[18/57] Linking CXX shared library libnav_msgs.so
[19/57] Building CXX object CMakeFiles/gazebo_motor_model.dir/src/gazebo_motor_model.cpp.o
[20/57] Building CXX object CMakeFiles/LiftDragPlugin.dir/src/liftdrag_plugin/liftdrag_plugin.cpp.o
[21/57] Building CXX object CMakeFiles/gazebo_geotagged_images_plugin.dir/src/gazebo_geotagged_images_plugin.cpp.o
[22/57] Building CXX object CMakeFiles/gazebo_controller_interface.dir/src/gazebo_controller_interface.cpp.o
[23/57] Building CXX object CMakeFiles/gazebo_mavlink_interface.dir/src/gazebo_mavlink_interface.cpp.o
[24/57] Building CXX object CMakeFiles/gazebo_irlock_plugin.dir/src/gazebo_irlock_plugin.cpp.o
[25/57] Building CXX object CMakeFiles/gazebo_mavlink_interface.dir/src/geo_mag_declination.cpp.o
[26/57] Building CXX object CMakeFiles/gazebo_lidar_plugin.dir/src/gazebo_lidar_plugin.cpp.o
[27/57] Building CXX object CMakeFiles/gazebo_wind_plugin.dir/src/gazebo_wind_plugin.cpp.o
[28/57] Building CXX object CMakeFiles/gazebo_imu_plugin.dir/src/gazebo_imu_plugin.cpp.o
[29/57] Building CXX object CMakeFiles/gazebo_gps_plugin.dir/src/gazebo_gps_plugin.cpp.o
[30/57] Linking CXX shared library libgazebo_controller_interface.so
[31/57] Building CXX object CMakeFiles/gazebo_sonar_plugin.dir/src/gazebo_sonar_plugin.cpp.o
[32/57] Linking CXX shared library libgazebo_irlock_plugin.so
[33/57] Linking CXX shared library libgazebo_gps_plugin.so
[34/57] Linking CXX shared library libgazebo_wind_plugin.so
[35/57] Linking CXX shared library libgazebo_lidar_plugin.so
[36/57] Linking CXX shared library libgazebo_imu_plugin.so
[37/57] Linking CXX shared library libLiftDragPlugin.so
[38/57] Linking CXX shared library libgazebo_motor_model.so
[39/57] Building CXX object CMakeFiles/gazebo_uuv_plugin.dir/src/gazebo_uuv_plugin.cpp.o
[40/57] Building CXX object CMakeFiles/gazebo_vision_plugin.dir/src/gazebo_vision_plugin.cpp.o
[41/57] Building CXX object CMakeFiles/gazebo_multirotor_base_plugin.dir/src/gazebo_multirotor_base_plugin.cpp.o
[42/57] Building CXX object CMakeFiles/gazebo_gimbal_controller_plugin.dir/src/gazebo_gimbal_controller_plugin.cpp.o
[43/57] Linking CXX static library OpticalFlow/klt_feature_tracker/libklt_feature_tracker.a
[44/57] Building CXX object OpticalFlow/CMakeFiles/OpticalFlow.dir/src/optical_flow.cpp.o
[45/57] Building CXX object OpticalFlow/CMakeFiles/OpticalFlow.dir/src/px4flow.cpp.o
[46/57] Building CXX object OpticalFlow/CMakeFiles/OpticalFlow.dir/src/flow_px4.cpp.o
[47/57] Building CXX object OpticalFlow/CMakeFiles/OpticalFlow.dir/src/flow_opencv.cpp.o
[48/57] Linking CXX shared library OpticalFlow/libOpticalFlow.so
[49/57] Building CXX object CMakeFiles/gazebo_opticalflow_plugin.dir/src/gazebo_opticalflow_plugin.cpp.o
[50/57] Linking CXX shared library libgazebo_mavlink_interface.so
[51/57] Linking CXX shared library libgazebo_vision_plugin.so
[52/57] Linking CXX shared library libgazebo_sonar_plugin.so
[53/57] Linking CXX shared library libgazebo_multirotor_base_plugin.so
[54/57] Linking CXX shared library libgazebo_gimbal_controller_plugin.so
[55/57] Linking CXX shared library libgazebo_uuv_plugin.so
[56/57] Linking CXX shared library libgazebo_geotagged_images_plugin.so
[57/57] Linking CXX shared library libgazebo_opticalflow_plugin.so
ninja: build stopped: subcommand failed.
Makefile:170: recipe for target ‘posix_sitl_default’ failed
make: *** [posix_sitl_default] Error 1

It seems it can’t find the file nuttx/clock.h

I figured that in SITL, it shouldn’t be looking for any nuttx installed microcontroller?

(In case anyone was wondering, the line modules/landing_target_estimator was already included in my config file, so I was running it with this as well).

Does anyone have any ideas why I can’t build the firmware? I’m running version 1.8.0-dev by the way.

Thanks,

Tom

1 Like

For SITL, you don’t have to modify any config files.

SITL also doesn’t use the nuttx irlock driver, it gets the data from gazebo.

Ah, okay, thanks a lot. I did get it working before without anything added to the config files, but whenever I try to precision land with

command mode auto:precland

it just times out, even though I’m right over the virtual IR lock. The MAV_CMD_NAV_LAND mentioned in the documentation doesn’t seem to exist anymore, but I don’t think it’s to do with that. I’ve tried playing around with the PLD parameters but nothing seems to stop the timeout issue.

Given this similar issue from July https://github.com/PX4/Firmware/issues/9904
I’m wondering if this is a persisting issue.

Has anyone managed to get IRlock with SITL working correctly?

I’d also be interested to hear about anyone who’s used IRlock with a Pixhawk 4. Thanks

Tom

We are using irlocks in hardware (with a Pixracer) and in SITL. We have no problems. It should be noted though, that our firmware is based on PX4 v1.7.4 currently. Perhaps it has been broken since.
You could give that version a try.

How does it time out exactly? Can you share a log?

1 Like

It goes to the set altitude for initiating the precision landing, waits the specified time, then can’t locate the IR lock in the specified time, so times out and initiates normal landing instead. I’ve tried setting the minimum horizontal distance threshold as high as possible (10m), but it doesn’t solve it. I’m not at work now so don’t have the exact logs or a logfile. I can get them tomorrow.

It’s reassuring to know that you’ve got it working with one version of the software though! I’ll see if I can test it with 1.7.4. Thanks a lot Nicolas.

Tom

So here’s the console output I get when building the firmware, taking off, and attempting a precision land. Yesterday, I hadn’t noticed the message “[Wrn] [msgs.cc:1808] Conversion of sensor type[logical_camera] not suppported.” I guess this is an issue with interfacing with Gazebo, but either way I’m not sure how to fix it.

[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.200.13.64
[Wrn] [msgs.cc:1808] Conversion of sensor type[logical_camera] not suppported.
[Dbg] [gazebo_mavlink_interface.cpp:138] <joint_name> not found for channel[0] no joint control will be performed for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:138] <joint_name> not found for channel[1] no joint control will be performed for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:138] <joint_name> not found for channel[2] no joint control will be performed for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:138] <joint_name> not found for channel[3] no joint control will be performed for this channel.
[Wrn] [gazebo_mavlink_interface.cpp:127] joint [zephyr_delta_wing::propeller_joint] not found for channel[4] no joint control for this channel.
[Wrn] [gazebo_mavlink_interface.cpp:127] joint [zephyr_delta_wing::flap_left_joint] not found for channel[5] no joint control for this channel.
[Wrn] [gazebo_mavlink_interface.cpp:127] joint [zephyr_delta_wing::flap_right_joint] not found for channel[6] no joint control for this channel.
[Dbg] [gazebo_mavlink_interface.cpp:138] <joint_name> not found for channel[7] no joint control will be performed for this channel.
[Msg] Using MAVLink protocol v2.0
[Wrn] [RaySensor.cc:359] ranges not constructed yet (zero sized)
SITL COMMAND: /home/tom/px4/px4_toolchain/src/Firmware/build/posix_sitl_default/bin/px4 /home/tom/px4/px4_toolchain/src/Firmware/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -t /home/tom/px4/px4_toolchain/src/Firmware/test_data
INFO [Unknown] Creating symlink /home/tom/px4/px4_toolchain/src/Firmware/ROMFS/px4fmu_common -> /home/tom/px4/px4_toolchain/src/Firmware/build/posix_sitl_default/tmp/rootfs/etc
107 WARNING: setRealtimeSched failed (not run as root?)


| ___ \ \ \ / / / |
| |/ / \ V / / /| |
| __/ / \ / /
| |
| | / /^\ \ ___ |
_| / / |_/

px4 starting.

INFO [Unknown] Calling startup script: /bin/sh etc/init.d-posix/rcS 0
SYS_AUTOSTART: curr: 0 -> new: 1011
BAT_N_CELLS: curr: 0 -> new: 3
CAL_ACC0_ID: curr: 0 -> new: 1376264
CAL_ACC1_ID: curr: 0 -> new: 1310728
CAL_ACC_PRIME: curr: 0 -> new: 1376264
CAL_GYRO0_ID: curr: 0 -> new: 2293768
CAL_GYRO_PRIME: curr: 0 -> new: 2293768
CAL_MAG0_ID: curr: 0 -> new: 196616
CAL_MAG_PRIME: curr: 0 -> new: 196616
COM_DISARM_LAND: curr: -1.0000 -> new: 0.1000
COM_OBL_ACT: curr: 0 -> new: 2
COM_OF_LOSS_T: curr: 0.0000 -> new: 5.0000
COM_RC_IN_MODE: curr: 0 -> new: 1
EKF2_ANGERR_INIT: curr: 0.1000 -> new: 0.0100
EKF2_GBIAS_INIT: curr: 0.1000 -> new: 0.0100
EKF2_MAG_TYPE: curr: 0 -> new: 1
MC_PITCH_P: curr: 6.5000 -> new: 6.0000
MC_PITCHRATE_P: curr: 0.1500 -> new: 0.2000
MC_ROLL_P: curr: 6.5000 -> new: 6.0000
MC_ROLLRATE_P: curr: 0.1500 -> new: 0.2000
MPC_HOLD_MAX_Z: curr: 0.6000 -> new: 2.0000
MPC_Z_VEL_I: curr: 0.0200 -> new: 0.1500
MPC_Z_VEL_P: curr: 0.2000 -> new: 0.6000
NAV_ACC_RAD: curr: 10.0000 -> new: 2.0000
NAV_DLL_ACT: curr: 0 -> new: 2
RTL_DESCEND_ALT: curr: 30.0000 -> new: 5.0000
RTL_LAND_DELAY: curr: -1.0000 -> new: 5.0000
RTL_RETURN_ALT: curr: 60.0000 -> new: 30.0000
SDLOG_DIRS_MAX: curr: 0 -> new: 7
SENS_BOARD_X_OFF: curr: 0.0000 -> new: 0.0000
SENS_DPRES_OFF: curr: 0.0000 -> new: 0.0010

  • RTL_DESCEND_ALT: curr: 5.0000 -> new: 10.0000
  • RTL_LAND_DELAY: curr: 5.0000 -> new: 0.0000
    PWM_MAX: curr: 2000 -> new: 1950
    PWM_MIN: curr: 1000 -> new: 1075
    LTEST_MODE: curr: 0 -> new: 1
    PLD_HACC_RAD: curr: 0.2000 -> new: 0.1000
    INFO [dataman] Unknown restart, data manager file ‘./dataman’ size is 11405132 bytes
    INFO [simulator] Waiting for initial data on UDP port 14560. Please start the flight simulator to proceed…
    Gazebo multi-robot simulator, version 7.14.0
    Copyright © 2012 Open Source Robotics Foundation.
    Released under the Apache 2 License.
    http://gazebosim.org

[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 10.200.13.64
[Wrn] [Event.cc:87] Warning: Deleteing a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
INFO [simulator] Got initial simulation data, running sim…
[Wrn] [msgs.cc:1808] Conversion of sensor type[logical_camera] not suppported.
INFO [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0
INFO [ekf2] Found range finder with instance 0
INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14570 remote port 14550
INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO [logger] logger started (mode=all)
INFO [logger] Start file log
INFO [logger] Opened log file: ./log/2018-11-06/10_29_34.ulg
INFO [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
INFO [Unknown] Startup script returned successfully
pxh> [Wrn] [msgs.cc:1808] Conversion of sensor type[logical_camera] not suppported.
INFO [mavlink] partner IP: 127.0.0.1
INFO [ecl/EKF] EKF aligned, (pressure height, IMU buf: 22, OBS buf: 14)
INFO [ecl/EKF] EKF GPS checks passed (WGS-84 origin set)
INFO [ecl/EKF] EKF commencing GPS fusion
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/iris_irlock/imu, deleting message. This warning is printed only once.
pxh> commander takeoff
pxh> INFO [commander] Takeoff detected
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/iris_irlock/motor_speed/0, deleting message. This warning is printed only once.
pxh> commander mode auto:[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/iris_irlock/gazebo/command/motor_speed, deleting message. This warning is printed only once.
pxh> commander mode auto:precland
pxh> WARN [navigator] Resetting landing position to current position
INFO [navigator] Climbing to search altitude.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/iris_irlock/motor_speed/1, deleting message. This warning is printed only once.
WARN [navigator] Search timed out
WARN [navigator] Falling back to normal land.
[Wrn] [Publisher.cc:141] Queue limit reached for topic /gazebo/default/iris_irlock/motor_speed/2, deleting message. This warning is printed only once.
INFO [commander] Landing detected
INFO [logger] closed logfile, bytes written: 3831392

Yes it looks like something is broken with the Gazebo simulation. The message you mention is not printed when I run it. I see that I have Gazebo 7.0.0 installed. It’s possible that Gazebo’s API has changed since and the logical camera is not supported the way we use it anymore.

1 Like

I’m having a similar issue with Precision Landing in SITL. I attempted various combinations of Firmware and Gazebo versions without luck. @tbrs did you identify the issue?

Thanks!

@ebabyak No I never figured it out. I had meant to try using the same version of firmware that Nicolas is running but got sidetracked. Given it’s not just me experiencing this, I’m guessing there is a genuine problem caused by an update to Gazebo as Nicolas describes.

I was doing the testing to assess the precision landing feature for myself before buying all the necessary sensors. If it’s just a Gazebo issue then hopefully it’s still fine in the real world. I’ll try and find the time to have another go at solving this.

@tbrs2 @Nicolas - After a little digging I discovered landing_target_estimator wasn’t started. I started it from the px4 shell using: landing_target_estimator start and can now do successful precision lands. @tbrs2 I think the logical_camera message you are getting from Gazebo isn’t critical.

1 Like

That worked for me too. Well done ebabyak, and thanks very much!

This was likely caused by

@bkueng I’m not yet familiar with the new SITL init scripts. Where should airframe-/simulation-specific applications be started?

1 Like

I’d add it directly to the vehicle’s startup script:
https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d-posix/1011_iris_irlock#L15.

1 Like

@ebabyak can you confirm that adding landing_target_estimator start as described by @bkueng gives you a functioning system without further modifications? And if so, would you mind opening a PR with the change?

1 Like

@Nicolas This worked for me, created PR at https://github.com/PX4/Firmware/pull/10882

1 Like