PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
7.85k stars 13.23k forks source link

[Bug] Takeoff/Land without GPS does not hold position #22250

Open robbie-sps opened 8 months ago

robbie-sps commented 8 months ago

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:

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:

    <include>
      <uri>model://gps</uri>
      <pose>0.05 0 0.04 0 0 0</pose>
      <name>gps0</name>
    </include>
    <joint name='gps0_joint' type='fixed'>
      <child>gps0::link</child>
      <parent>base_link</parent>
    </joint>

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:

. ${R}etc/init.d-posix/airframes/10015_gazebo-classic_iris

Steps for land bug

  1. start simulation
    make px4_sitl gazebo-classic_iris_vision
  2. takeoff drone (either with stick or takeoff command, which will work from original position)
  3. fly to another position (i.e. not over the drone origin)
  4. switch to land mode
    pxh> commander land

    drone lands but flies towards origin

Steps for takeoff bug

  1. As above, but under stick control land the drone in a position away from the origin
  2. switch to takeoff
    pxh> commander takeoff

    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

batuhanuzaldimvc commented 7 months ago

I believe this issue also states the same problem.