Commit Graph

253 Commits

Author SHA1 Message Date
Junwoo Hwang 8bae4e5c0e FollowMe : Replace First order target position filter with Second order position and velocity filter
Follow me : tidied second order filter implementation

Added velocity filtered info to uORB follow target status message, and rebase to potaito/new-follow-me-task

FollowMe : Rebasing and missing definition fixes on target position second order filter

Follow Me : Remove Alpha filter delay compensation code, since second order filter is used for pose filtering now

Followme : Remove unused target pose estimation update function

Follow Target : Added Target orientation estimation logic

Follow Target : Replaced offset vector based setpoint to rate controlled orbital angle control

Follow Target : Bug fixes and first working version of rate based control logic, still buggy

Follow Target : Added target orientation, follow angle, orbit angle value into follow_target_status uORB message for debugging

Follow Target : Fix orbit angle step calculation typo bug

Follow Target : Few more fixes

Follow Target : Few fixes and follow angle value logging bug fix

Follow Target : Added lowpass alpha filter for yaw setpoint filtering

Follow Target : Remove unused filter delay compensation param

Follow Target : Add Yaw setpoint filter initialization logic and bufix for when unwrap had an input of NAN angle value

Follow Target : Add Yaw setpoint filtering enabler parameter

Follow Target : Change Target Velocity Deadzone to 1.0 m/s, to accomodate walking speed of 1.5 m/s

Follow Target : Add Orbit Tangential Velocity calculation for velocity setpoint and logging uORB topics

Follow target : Fix indentation in yaw setpoint filtering logic

Follow Target : Fix Follow Target Estimator timeout logic bug that was making the 2nd order pose filter reset to raw value every loop

Follow Target : Remove debug printf statement for target pose filter reset check

Follow Target : Add pose filter natural frequency parameter for filter testing

Follow Target : Make target following side param selectable and add target pose filter natural frequency param description

Follow Target : Add Terrain following altitude mode and make 2D altitude mode keep altitude relative to the home position, instead of raw local position origin

Follow Target : Log follow target estimator & status at full rate for filter characteristics test

Follow Target : Implementing RC control user input for Follow Target control

Follow Target : edit to conform to updated unwrap_pi function

Follow Target : Make follow angle, distance and height RC command input configurable

Follow Target : Make Follow Target not interruptable by moving joystick in Commander

Follow Target : reconfigure yaw, pitch and roll for better user experience in RC adjusting configurations, and add angular rate limit taking target distance into account

Follow Target : Change RC channels used for adjustments and re-order header file for clarity

Follow Target : Fix Parameters custom parent macro, since using DEFINE_PARAMETERS alone misses the parameter updates of the parent class, FlightTask

Follow Target : Fix Orbit Tangential speed actually being angular rate bug, which was causing a phenomenon of drnoe getting 'dragged' towards the target velocity direction

Follow Target : Final tidying and refactoring for master merge - step 1

Add more comments on header file

Follow Target : tidy, remove unnecessary debug uORB elements from follow_target_status message

Follow Target : Turn off Yaw filtering by default

Follow Target : Tidy maximum orbital velocity calculation

Follow Target : add yaw setpoint filter time constant parameter for testing and fix NAV_FT_HT title

Follow Target : Add RC adjustment window logic to prevent drone not catching up the change of follow target parameters

Follow Target : fixes

Follow Target: PR tidy few edits remove, and update comments

Follow Target : apply comments and reviews

Follow Target : Edit according to review comments part 2

Follow Target : Split RC adjustment code and other refactors

- Splitted the RC adjustment into follow angle, height and distance
- Added Parameter change detection to reset the follow properties
- Added comments and removed yaw setpoint filter enabler logic

Follow Target : Modify orbit angle error bufferzone bug that was causing excessive velocity setpoints when setpoint catched up with raw orbit setpoint

Follow Target : Remove buffer zone velocity ramp down logic and add acceleration and rate limited Trajectory generation library for orbit angle and velocity setpoint

Follow Target : Remove internally tracked data from local scope function's parameters to simplify code

Follow Target : Fix to track unwrapped orbit angle, with no wrapping

Follow Target : Apply user adjustment deadzone to reduce sensitivity

Follow Target : Apply suggestions from PR review round 2 by @potaito

Revert submodule update changes, fall back to potaito/new-followme-task

Follow Target : [Debug] Expose max vel and acceleration settings as parameters, instead of using Multicopter Position Controller
's settings

Follow Target : Use matrix::isEqualF() function to compare floats

Follow Target : Add Acceleration feedback enabler parameter and Velocity ramp in initial commit for overshoot phenomenon improvement

Follow Target : Implement Velocity feed forward limit and debug logging values

Follow Target : Apply Velocity Ramp-in for Acceleration as well & Apply it to total velocity setpoint and not just orbit tangential velocity component

Follow Target : Don't set Acceleration setpoint if not commanded

Follow Target : Use Jerk limited orbit angle control. Add orbit angle tracking related uORB values"

Follow Target : Add Orbit Angle Setpoint Rate Tracking filter, to take into consideration for calculating velocity setpoint for trajectory generator for orbit angle

Revert "Follow Target : Add Orbit Angle Setpoint Rate Tracking filter, to take into consideration for calculating velocity setpoint for trajectory generator for orbit angle"

This reverts commit a3f48ac7652adb70baf3a2fed3ea34d77cbd6a70.

Follow Target : Take Unfiltered target velocity into acount for target course calculation to fix overshoot orbit angle 180 deg flip problem

Follow Target : Remove Yaw Filter since it doesn't make a big difference in yaw jitterness

Follow Target : Remove velocity ramp in control & remove debug values from follow_target_status.msg

Follow Target : Tidy Follow Target Status message logging code

Follow Target : Remove jerk and acceleration settings from Follow Target orbit trajectory generation

Follow Target : Change PublicationMulti into Publication, since topics published are single instances

Follow Target : Edit comments to reflect changes in the final revision of Follow Target

Follow Target : Apply incorrectly merged conflicts during rebase & update Sticks function usage for getThrottled()

Follow Target : Apply final review comments before merge into Alessandro's PR

Apply further changes from the PR review, like units

Use RC Sticks' Expo() function for user adjustments to decrease sensitivity around the center (0 value)

Update Function styles to lowerCamelCase

And make functions const & return the params, rather than modifying them
internally via pointer / reference

Specify kFollowPerspective enum as uint8_t, so that it can't be set to negative value when converted from the parameter 'FLW_TGT_FP'

Fix bug in updateParams() to reset internally tracked params if they actually changed.

Remove unnecessary comments

Fix format of the Follow Target code

Fix Follow Perspective Param metadata

follow-me: use new trajectory_setpoint msg

Convert FollowPerspective enum into a Follow Angle float value

1. Increases flexibility in user's side, to set any arbitrary follow
angle [deg]
2. Removes the need to have a dedicated Enum, which can be a hassle to
make it match MAVSDK's side
3. A step in the direction of adding a proper Follow Mode (Perspective)
mode support, where we can support kite mode (drone behaves as if it is
hovering & getting dragged by the target with a leash) or a constant
orbit angle mode (Drone always on the East / North / etc. side, for
cinematic shots)

Continue fixing Follow Target MAVSDK code to match MAVSDK changes

- Support Follow Angle configuration instead of Follow Direction
- Change follow position tolerance logic to use the follow angle
*Still work in progress!

Update Follow Me MAVSDK Test Code to match MAVSDK v2 spec

- Add RC Adjustment Test case
- Change follow direction logic to follow angle based logic completely
- Cleanup on variable names and comment on code

follow-me: disable SITL test

Need to update MAVSDK with the following PR:
https://github.com/mavlink/MAVSDK/pull/1770

SITL is failing now because the follow-me
perspectives are no longer defined the
same way in MAVSDK and in the flight task.

update copyright year

follow-me: mark uORB topics optional

Apply review comments

more copyright years

follow-me sitl test: simpler "state machine"

flight_mode_manager: exclude AutoFollowTarget and Orbit on flash contrained boards

Remove unnecessary follow_target_status message properties

- As it eats up FLASH and consumes uLog bandwidth
2022-06-16 16:14:57 -04:00
Alessandro Simovic de1fa11e96 New follow-me flight task
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 💄
2022-06-16 16:14:57 -04:00
Beat Küng da55256f2f test_multicopter_failure_injection: remove 'Reject before arming' test
Not sure why injection should fail before arming.
2022-06-04 07:40:29 +02:00
Beat Küng 8bf18e31be SITL: enable failure command (SYS_FAILURE_EN=1) 2022-06-04 07:40:29 +02:00
Beat Küng 0a9378e0f6 mavsdk_tests: ensure motor is stopped for motor failure test 2022-05-12 07:58:56 +02:00
Alessandro Simovic b5a3c58a95 sitl: loosen some timeouts
The typhoon_h480 model would not always complete takeoff in 30 seconds
or finish the mission within 60 seconds.
2022-05-12 07:58:56 +02:00
Alessandro Simovic 06f69cd469 Add SITL tests for control allocation 2022-05-12 07:58:56 +02:00
Julian Oes 37fa4bccb6 mavsdk_tests: update MAVSDK dependency
This should fix the CI issue where the test just hangs trying to
connect.
2022-04-20 11:48:53 -04:00
Beat Küng bbc2b703c3 mavsdk_tests: update catch2 to 2.13.8
Fixes this issue on Fedora: https://github.com/catchorg/Catch2/issues/2178
2022-01-24 16:16:29 -05:00
Julian Oes 07d75d85cf mavsdk_tests: trigger flush to stdout every second 2022-01-06 09:24:32 -05:00
Julian Oes 2d8ebc6839 mavsdk_tests: add timestamp to log output
The timestamp is added when the output is queued up, rather than later
when the queue is emptied and some time might have passed.
2022-01-06 09:24:32 -05:00
Julian Oes dd6fb58f82 mavsdk_tests: don't stall mavsdk_tests binary
This is an attempt to fix the test failure where PX4 detects an RC
timeout presumably because the tester process mavsdk_tests is stalled
and does not send RC control messages in time.
2022-01-06 09:24:32 -05:00
Julian Oes 199b5e7d48 Revert "mavsdk_tests: retry gz model spawn command"
This reverts commit c15d3f7cfa.
2021-12-20 13:05:46 -05:00
Julian Oes 9184038359 mavsdk_tests: try to restart gzserver
Sometimes gzserver seems to just not start correctly. With this commit
we try to just restart.
2021-12-20 13:05:46 -05:00
TSC21 9894598b63 mavsdk_tests: update to be able to fetch rtps builds as well 2021-12-20 13:05:46 -05:00
Thomas Debrunner 9312c772f1 mavsdk-tests: Re-try to launch a mission for some time before failing. This mitigates an issue where the flight controller has an uploaded mission, but has not finished the mission feasibility checks yet. 2021-12-20 13:05:46 -05:00
Julian Oes 764af19da4 mavsdk_tests: bump MAVSDK version to v0.48.0
This might help with CI tests where we get RC loss because, presumably,
MAVSDK is not sending them in time.
2021-12-20 13:05:46 -05:00
Julian Oes dcf69144cb mavsdk_tests: fix type for mypy check
This was probably triggered due to a mypy update.
2021-12-17 17:28:54 +01:00
Julian Oes 94884594bb mavsdk_tests: check gzserver and gz model calls
This adds two checks to make sure gzserver has actually started before
continuing, and also whether the gz model call was successful.
2021-12-08 07:26:32 +01:00
Julian Oes c15d3f7cfa mavsdk_tests: retry gz model spawn command
This should help when gzserver does not respond yet and we end up
without a model and hence can't connect later and time out.

This change also required a fix to prevent the tester to hang on
terminating all runners. By using poll instead of only read we can
prevent that and actually properly join the logger thread.
2021-12-02 11:40:26 -05:00
benjinne 06a91ec752
Improve offboard failsafe (#18160)
* state_machine_helper: improve offboard failsafe

* state_machine_helper: add missing parameter to set_link_loss_nav_state

* state_machine_helper: fix no rc and offboard reason

* Fix offboard test by enabling rcl_except

* mavros_test fix offboard_posctl_test with rcl_except

* autopilot_tester make RcLossException bits explicit

Co-authored-by: Julian Oes <julian@oes.ch>

* autopilot_tester change rcl_except to rc_loss_exception

Co-authored-by: Julian Oes <julian@oes.ch>

* autopilot_tester fix rc_loss_exception renaming errors

Co-authored-by: Julian Oes <julian@oes.ch>
2021-09-08 18:19:53 +02:00
Silvan Fuhrer 44f12acafe mavsdk tests: remove hover missions for VTOL
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
2021-07-30 21:27:53 -04:00
Julian Oes af50e25647 mavsdk_tests: update to MAVSDK v0.41.0
This required replacing some of the methods which had been deprecated
and now removed.
2021-07-23 09:05:31 +02:00
TSC21 c4d54d983b mavsdk_tests: bump MAVSDK version to 0.40.0 2021-06-14 09:23:38 +02:00
Julian Oes 4f52c0b6da mavsdk_tests: unused var and index fix 2021-04-29 21:08:33 -04:00
Julian Oes f16913c175 mavsdk_tests: fix timeout at 1x speed 2021-04-29 21:08:33 -04:00
Julian Oes e7fcfbf658 mavsdk_tests: use global position instead of local
We are currently not testing with flow only anyway, so we might as well
remove this for now.
2021-04-29 21:08:33 -04:00
Julian Oes 43fccece61 mavsdk_tests: check installed version
Starting with MAVSDK 0.39.0 we can specify the version required. This
way we can fail at configure time instead of later during compilation or
linking.
2021-04-29 21:08:33 -04:00
Julian Oes 7384bd2675 mavsdk_tests: specify required MAVSDK version
This way it can be better picked up by CI scripts.
2021-04-29 21:08:33 -04:00
Julian Oes bb0fe9ee61 mavsdk_tests: losen time limit for VTOL mission 2021-04-06 11:35:18 -04:00
Julian Oes 2eaf677de3 mavsdk_tests: correctly locate plan 2021-04-06 11:35:18 -04:00
Julian Oes cd66a79747 mavsdk_tests: move waypoints to vehicle position
This way it shouldn't matter where the test is run.
2021-04-06 11:35:18 -04:00
Julian Oes d714c2faec mavsdk_tests: add VTOL mission tests
This adds VTOL mission tests to the CI integration tests.
This depends on MAVSDK v0.38.0.
2021-04-06 11:35:18 -04:00
Julian Oes 1e88939605 mavsdk_tests: report speed factor every second
This helps in debugging slow CI.
2021-04-06 11:35:18 -04:00
Daniel Agar 82200fab9e mavsdk_tests: relax MC "Fly straight" speed threshold 2021-02-21 22:51:51 +01:00
Julian Oes 46e75ebddb mavsdk_tests: remove leftover debug line 2021-02-17 18:13:14 +01:00
Julian Oes 2703a34900 mavsdk_tests: add link back to github action 2021-02-17 18:13:14 +01:00
Julian Oes 64d0ce43b6 mavsdk_tests: upload to logs.px4.io in test_runner
By doing the upload directly inside the mavsdk_test_runner.py we have
more context such as the model, case, etc.
2021-02-16 17:07:21 +01:00
Julian Oes 2315618b85 mavsdk_tests: use autopilot timestamps to sleep
This way we should be able to avoid some of the timeouts happening on
the PX4 side if MAVSDK doesn't send setpoints in time.
2021-02-16 17:07:21 +01:00
Julian Oes 6c26387e85 mavsdk_tests: try to prevent auto-disarm
We should not spend too much time sending RC init messages because if it
takes too long, we might auto-disarm in the meantime.
2021-02-16 17:07:21 +01:00
Julian Oes c1de27acf7 mavsdk_tests: use more conservative speed_factor 2021-02-16 17:07:21 +01:00
Julian Oes 0ebb8c6981 mavsdk_tests: use microseconds to avoid sleep(0) 2021-02-16 17:07:21 +01:00
Julian Oes b6b9ce5f11 mavsdk_tests: robustify test by climbing longer 2021-02-16 17:07:21 +01:00
Julian Oes b79553862e mavsdk_tests: remove timeout for mission upload
Just use what is MAVSDK internal.
2021-02-16 17:07:21 +01:00
Julian Oes aca1e85da4 mavsdk_tests: don't forget to check for rootfs dir 2021-02-11 22:25:07 +01:00
Julian Oes 4184c204c4 autopilot_tester: don't poll mission progress
Instead use subscription here as well in order not to miss an update.
2021-02-11 22:25:07 +01:00
Julian Oes 2971ce8664 mavsdk_tests: don't clear logs 2021-02-11 22:25:07 +01:00
Julian Oes 176d932f23 mavsdk_tests: prevent missing updates
It seems like we are often missing updates from topics like flight mode
or in air state, both topics that are sent out infrequenctly e.g. at 1
Hz. Therefore, instead of polling for that data we should probably
subscribe to the updates and that way get notified of each an every
update.

For instance this should prevent the case where we miss the mode change
from mission to descend and back to mission once landed and disarmed.
2021-02-10 16:37:02 -05:00
Julian Oes 2772022785 mavsdk_tests: disable mag stuck test
The estimator sometimes diverges in vz after landing which makes the
test fail. This needs to be dealt with first before we can enable the
test again.
2021-02-09 11:32:41 +01:00
Julian Oes e592bed7d3 mavsdk_tests: use timestamp from attitude field
The flight_information field was not updated often enough which lead
to timeouts.
2021-02-09 11:32:41 +01:00