fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 00:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

New follow-me flight task

Browse Source 
rename follow_me_status to follow_target_status

enable follow_target_estimator on skynode

implement the responsiveness parameter:
The responsiveness parameter should behave similarly to the previous
follow-me implementation in navigator. The difference here is that
there are now two separate gains for position and velocity fusion.
The previous implemenation in navigator had no velocity fusion.

Allow follow-me to be flown without RC

SITL tests for follow-me flight task

This includes:
- Testing the setting for the follow-me angle
- Testing that streaming position only or position
  and velocity measurements both work
- Testing that RC override works

Most of these tests are done with a simulated model
of a point object that moves on a straight line. So
nothing too spectacular. But it makes the test checks
much easier.

Since the estimator for the target actually checks new
measurements and compares them to old ones, I also added
random gausian noise to the measurements with a fixed seed
for deterministic randomness. So repeated runs produce
exactly the same results over and over.

Half of the angles are still missing in MAVSDK. Need to create
an upstream PR to add center left/right and rear left/right options.
These and the corresponding SITL tests need to be implemented
later.

sitl: Increase position tolerance during follow-me

Astro seems to barely exceed the current tolerance (4.3 !< 4.0)
causing CI to fail. The point of the CI test is not to check
the accuracy of the flight behaviour, but only the fact that the
drone is doing the expected thing. So the exact value of this
tolerance is not really important.

follow-me: gimbal control in follow-me

follow-me: create sub-routines in flight task class

follow-me: use ground-dist for emergency ascent

dist_bottom is only defined when a ground facing distance sensor exist.
It's therefore better to use dist_ground instead, which has the distance
to the home altitude if no distance sensor is available.

As a consequence it will only be possible to use follow-me in a valley
when the drone has a distance sensor.

follow-me: point gimbal to the ground in 2D mode

follow-me: another fuzzy msg handling for the estimator

follow-me: bugfix in acceleration saturation limit

follow-me: parameter for filter delay compensation

mantis: dont use flow for terrain estimation

follow-me: default responsiveness 0.5 -> 0.1

0.5 is way too jerky in real and simulated tests.

flight_task: clarify comments for bottom distance

follow-me: minor comment improvement

follow-me: [debug] log emergency_ascent

follow-me: [debug] log gimbal pitch

follow-me: [debug] status values for follow-me estimator

follow-me: setting for gimbal tracking mode

follow-me: release gimbal control at destruction

mavsdk: cosmetics 💄
This commit is contained in:
Alessandro Simovic
2022-06-16 16:14:57 -04:00
committed by Daniel Agar
parent 285556e463
commit de1fa11e96
32 changed files with 2121 additions and 713 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/mavsdk_tests/test_multicopter_follow_me.cpp
+129
View File
@@ -0,0 +1,129 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <chrono>
#include "autopilot_tester_follow_me.h"
TEST_CASE("Follow target - Position streaming", "[multicopter]")
{
const bool stream_velocity = false;
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
tester.arm();
const float takeoff_altitude = 10.0;
tester.set_takeoff_altitude(takeoff_altitude);
tester.takeoff();
tester.wait_until_hovering();
tester.straight_line_test(takeoff_altitude, stream_velocity);
tester.land();
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Follow target - Position and velocity streaming", "[multicopter]")
{
const bool stream_velocity = true;
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
tester.arm();
const float takeoff_altitude = 10.0;
tester.set_takeoff_altitude(takeoff_altitude);
tester.takeoff();
tester.wait_until_hovering();
tester.straight_line_test(takeoff_altitude, stream_velocity);
tester.land();
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Follow target - Velocity streaming only", "[multicopter]")
{
// Streaming only velocity should not work, the drone should not move.
// There needs to be at least one position message for follow-me
// to be able to integrate velocity.
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
tester.arm();
const float takeoff_altitude = 10.0;
tester.set_takeoff_altitude(takeoff_altitude);
tester.takeoff();
tester.wait_until_hovering();
tester.stream_velocity_only();
tester.land();
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
tester.wait_until_disarmed(until_disarmed_timeout);
tester.check_home_within(1.0f);
}
TEST_CASE("Follow target - Manual takeover", "[multicopter]")
{
AutopilotTesterFollowMe tester;
tester.connect(connection_url);
tester.wait_until_ready();
tester.store_home();
tester.arm();
const float takeoff_altitude = 10.0;
tester.set_takeoff_altitude(takeoff_altitude);
tester.takeoff();
tester.wait_until_hovering();
tester.rc_override_test(takeoff_altitude);
tester.land();
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
tester.wait_until_disarmed(until_disarmed_timeout);
}
TEST_CASE("Follow target - Spamming duplicate messages", "[multicopter]")
{
}