When using alternative position sensors (e.g. VIO) and without GPS, Takeoff/Land commands make the drone fly off the current position while climbing/descending.
The bug is reproduced below in SITL at tag v1.15.0-alpha1, however this behaviour has been observed on our Cube Orange+ based drones running PX4 forks from both v1.13.2 and v1.14.0-rc1
The issue seems to be caused by the navigator module not checking that the vehicle_global_position uORB has been published, and is valid. When the land mode is commanded, the position_setpoint_triplet is published by the navigator, and the values are ultimately set by the function below in src/modules/navigator/mission_block.cpp:
void
MissionBlock::set_land_item(struct mission_item_s *item)
{
...
/* set the land item */
item->nav_cmd = NAV_CMD_LAND;
// set land item to current position
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->yaw = _navigator->get_local_position()->heading;
The problem is that the vehicle_global_position_s struct that is passed back by get_global_position has never been set, and so it passes back a value of zero. I believe that the FlightModeManager module assumes that when GPS is not being used, that the land mission item will set the desired Lat/Lon to NAN. I have tested this by hardcoding NAN into the Lat/Lon values being set by Navigator, and the behaviour works as expected, but I think there needs to be a check in navigator that the vehicle_global_position_s struct has been recently set to a valid value, and in the case that it is not then the lat/lon should be set to NAN.
To Reproduce
Setup
The behaviour can be observed on any set up that has position sensor but no GPS. Because it was closest to our physical setup, I have been using the gazebo-classic_iris_vision SITL target, but I hacked out the gps from the iris model:
_Tools/simulation/gazebo-classic/sitlgazebo-classic/models/iris/iris.sdf.jinja delete lines 421-429 below:
There is also a typo in the iris vision start up script ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision which needs fixing, see #22249 , should be:
Describe the bug
When using alternative position sensors (e.g. VIO) and without GPS, Takeoff/Land commands make the drone fly off the current position while climbing/descending.
The bug is reproduced below in SITL at tag v1.15.0-alpha1, however this behaviour has been observed on our Cube Orange+ based drones running PX4 forks from both v1.13.2 and v1.14.0-rc1
The issue seems to be caused by the navigator module not checking that the vehicle_global_position uORB has been published, and is valid. When the land mode is commanded, the position_setpoint_triplet is published by the navigator, and the values are ultimately set by the function below in src/modules/navigator/mission_block.cpp:
The problem is that the vehicle_global_position_s struct that is passed back by get_global_position has never been set, and so it passes back a value of zero. I believe that the FlightModeManager module assumes that when GPS is not being used, that the land mission item will set the desired Lat/Lon to NAN. I have tested this by hardcoding NAN into the Lat/Lon values being set by Navigator, and the behaviour works as expected, but I think there needs to be a check in navigator that the vehicle_global_position_s struct has been recently set to a valid value, and in the case that it is not then the lat/lon should be set to NAN.
To Reproduce
Setup
The behaviour can be observed on any set up that has position sensor but no GPS. Because it was closest to our physical setup, I have been using the gazebo-classic_iris_vision SITL target, but I hacked out the gps from the iris model: _Tools/simulation/gazebo-classic/sitlgazebo-classic/models/iris/iris.sdf.jinja delete lines 421-429 below:
There is also a typo in the iris vision start up script ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision which needs fixing, see #22249 , should be:
Steps for land bug
drone lands but flies towards origin
Steps for takeoff bug
drone takes off but flies towards origin
Expected behavior
Land and Takeoff should work as expected without GPS (e.g. take off and land vertically in current position)
Screenshot / Media
https://github.com/PX4/PX4-Autopilot/assets/117604804/8d56025b-00e6-4aa0-a3cb-54ec75ec0681
video matches the uploaded log. Don't know why the gazebo sim is so bouncy when landing.....
there seems to be an issue where the video won't play, but I can download the embedded file and play it locally
Flight Log
https://logs.px4.io/plot_app?log=8e2431d5-c942-4196-9ed9-327741afb76c
Software Version
v1.15.0-alpha1
Flight controller
SITL
Vehicle type
Multicopter
How are the different components wired up (including port information)
No response
Additional context
No response