search

tiiuae / fog_sw

Public ROS2 components for TII/SSRC drone project
BSD 3-Clause "New" or "Revised" License
3 stars 4 forks source link

Navigation: drone bounces during takeoff #90

Open jnippula opened 3 years ago

jnippula commented 3 years ago

During takeoff the drone ascends first normally for some time, then quickly descends back to lower altitude and starts raising again with small steps until it reaches the next waypoint altitude. Visually, it looks like drone start taking off, then suddenly starts landing, then again change it's mind in the middle and starts taking off again.

jnippula commented 3 years ago

Quick look at the flight logs shows that the navigation starts planning the path to given waypoint before the takeoff is completed. So, when drone has raised just above the ground, the navigation starts planning vertical flight path to the target altitude and it finish the planning already before the takeoff is completed. Then it starts executing the new path where the first waypoint is already close to the ground where the drone was in the time when planning started. This leads the drone quickly descend towards the ground to reach the first waypoint of the path and starts ascending back to the target altitude through waypoints.

jnippula commented 3 years ago

In following logs we can see that the first 6 waypoints are for vertical movement to reach the target takeoff(?) altitude. Then next waypoints are for flying to the given coordinates. The problems seems to be that the navigation starts planning the path already before the takeoff has been completed.

loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.242175906] [sad03.navigation]: [navigation]: Creating coord transform request
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.242402307] [sad03.navigation]: [navigation]: Calling coord transform
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.243292235] [sad03.navigation]: [navigation]: Coordinate transform returned: -0.59, -4.98
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.243384478] [sad03.navigation]: [navigation]: Waypoint added (LOCAL): -0.59, -4.98, 1.50, -1.57
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.243525705] [sad03.navigation]: [navigation]: Planning started
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.333175879] [sad03.navigation]: [navigation]: Waypoint [-0.59, -4.98, 1.50, -1.57] set as a next goal
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.341652311] [sad03.navigation]: [navigation]: Planner returned 2 waypoints
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.433164456] [sad03.navigation]: [navigation]: Sending 6 waypoints to the control interface:
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.433201362] [sad03.navigation]: [navigation]:        -1.25, 2.89, 0.09, -1.44
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.433235111] [sad03.navigation]: [navigation]:        -1.25, 2.89, 0.49, -1.46
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.433256952] [sad03.navigation]: [navigation]:        -1.25, 2.89, 0.89, -1.48
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.433302300] [sad03.navigation]: [navigation]:        -1.25, 2.89, 1.29, -1.50
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.433318467] [sad03.navigation]: [navigation]:        -1.25, 2.89, 1.69, -1.53
loka 04 14:35:37 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347337.433328707] [sad03.navigation]: [navigation]:        -1.25, 2.89, 1.90, -1.55
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.633076867] [sad03.navigation]: [navigation]: End of current segment reached
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.733019534] [sad03.navigation]: [navigation]: Waypoint [-0.59, -4.98, 1.50, -1.57] set as a next goal
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.791426041] [sad03.navigation]: [navigation]: Planner returned 2 waypoints
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.833050462] [sad03.navigation]: [navigation]: Sending 7 waypoints to the control interface:
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.833078797] [sad03.navigation]: [navigation]:        -1.40, 3.00, 1.80, -1.56
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.833153266] [sad03.navigation]: [navigation]:        -1.64, 2.69, 1.72, -1.57
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.833164186] [sad03.navigation]: [navigation]:        -1.87, 2.37, 1.64, -1.57
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.833169613] [sad03.navigation]: [navigation]:        -2.11, 2.06, 1.56, -1.57
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.833174610] [sad03.navigation]: [navigation]:        -2.34, 1.74, 1.49, -1.57
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.833179490] [sad03.navigation]: [navigation]:        -2.58, 1.43, 1.41, -1.57
loka 04 14:35:43 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347343.833252234] [sad03.navigation]: [navigation]:        -2.60, 1.40, 1.40, -1.57
loka 04 14:35:48 sad03 sh[801]: [component_container_mt-1] [INFO] [1633347348.632811765] [sad03.navigation]: [navigation]: End of current segment reached
stibipet commented 3 years ago

Hello Jari, it's true that navigation may cause problems during takeoff, if the navigation goal is set before the takeoff maneuver is finished. Since PX4 switches out of TAKEOFF mode almost immediately, it is possible to start a new mission much earlier than the desired height is reached. We have implemented a fix, which will reject all goto commands before the takeoff altitude is reached (by reading data from the height estimator). This was tested with our setup and works fine. The fix will be included in the "Odometry update".

However, we have encountered a completely different problem, that also causes "bouncing" during takeoff. We observed the same problem with both baro and distance sensor for height estimate. The issue occurs even when flying without the fog_sw stack, so we created a new issue on PX4: https://github.com/PX4/PX4-Autopilot/issues/18388