Follow Me mode Second order filter & improvements #19260

junwoo091400 · 2022-02-25T15:08:06Z

Describe problem solved by this pull request
Refer to the document : https://docs.google.com/document/d/1E04Rn6sxYNqKve4-KZYdC2-9LExKmHKuYjWZlwf61rA/edit

Describe your solution

Describe possible alternatives
A clear and concise description of alternative solutions or features you've considered.

Test data / coverage
This change log : https://logs.px4.io/plot_app?log=6561ce69-e2e3-46c3-9e13-e29f2dbc11c7
Alessandro PR log : https://logs.px4.io/plot_app?log=fa76985d-87c6-4c4f-9164-fa2f2c15338b

Additional context

This PR is to be merged to a pre-existing PR : New follow-me flight task #18026
Depends on library added from second order reference model filter with optional rate feedforward #19246 and Angle unwrapping method #19249.
Proposal brainstorming issue : [PROPOSAL] New follow-me mode (discussion) #19251
User Doc Update PR : Update Follow Me documentation reflecting recent refactor in Follow Target PX4-user_guide#1810
Scripts I used for testing the algorithm : https://github.com/junwoo091400/Follow-Target-Algorithm-Scripts
Script used to test Follow-Me in SITL : https://github.com/junwoo091400/PX4-Followme-Sim-Script
Visualizer script that was used for drone's orbit angle set-point overshooting problem : https://github.com/junwoo091400/Follow-Target-uLog-Visualizer

Discussions

Should we just change the 'follow perspective' into a single number parameter (0 ~ 360 degrees), instead of using an enum / selection of : "Front, Back, Right, Left, etc."?
Google Astyle for the member variables : Do we need to add the _ suffix?

TODOS

Reduce follow_target_status logging rate to 200ms interval or 400ms
MAVSDK Param update
- Update follow_me.proto to match PX4 Upstream mavlink/MAVSDK-Proto#287
- Update Follow me code to match upstream changes mavlink/MAVSDK#1770
Later on implement the Multcopter Rate Controller not directly applying acceleration setpoint -> Issue at [Control] Constrain Acceleration Feed Forward when Drone is far from position setpoint #19543
Maybe move onto YAML based Parameter definitions?
Add the Leash mode (as if drone was hovering and the user was connected to the drone, so it would follow around like as if it was leashed)
Add fixed orbit angle mode (staying always on the north side, etc. No orbit angle transitions)
Check if I get a more stable yaw setpoint if I use the heading from the position setpoint to the target estimate, instead of the real position.

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

Jaeyoung-Lim · 2022-03-21T08:25:21Z

@junwoo091400 Probably the flightgear bridge submodule has been updated unintentionally. I don't think this should be part of this PR

tstastny

@junwoo091400 - some great progress on this. lots of nice features and improvements. main points i noticed during the review - try to const when possible, let's avoid adding more internal states when we don't need to, and we should be careful not to disable old functionality while putting in new (unless it is intentional), e.g. seems that the manual stick inputs are the only way to change the follow height angle or distance now. please let me know if you need any clarification on any of the review comments. cheers.

msg/follow_target_status.msg

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

tstastny · 2022-03-21T08:38:11Z

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

+		(ParamFloat<px4::params::NAV_FT_HT>) _param_nav_ft_ht,
 		(ParamFloat<px4::params::NAV_FT_DST>) _param_nav_ft_dst,
 		(ParamInt<px4::params::NAV_FT_FS>) _param_nav_ft_fs,
 		(ParamInt<px4::params::NAV_FT_ALT_M>) _param_nav_ft_alt_m,
-		(ParamInt<px4::params::NAV_FT_GMB_M>) _param_nav_ft_gmb_m,
-		(ParamInt<px4::params::NAV_FT_DELC>) _param_nav_ft_delc
+		(ParamFloat<px4::params::NAV_FT_YAW_T>) _param_ft_yaw_t


since these are all defined locally.. would be great to give the local states more verbose names so that we can actually understand what they are within this flight task. e.g. _param_follow_target_height (only with param prefix if the idea is that we want it to be clear this was from the params.. and we have other sources that could change the actually used follow target height)

Fair point. I was always torn between having them named exactly as the corresponding parameter, and having them named with more details.
Never thought about combining both approaches with prefix + meaning as you mentioned 👍
So you would suggest something like _param_ft_yaw_t_filter_time_constant ?

i would get rid of all acronyms, so that the variable is readable in the code without needing to go check the para.c file -- the param import here already tells where the value comes from, now in the flight task we just need something we can understand in the lines of the code. the only reason i would put the prefix param is to distinguish between "param" set value and the actual follow height that we are commanding (which can be adjusted from sticks as well). so there would in principle be two variables of the same name, unless we distinguish between the two.

There was this discussion 3 years ago exactly about this issue. And it removed all these 'readable' parameter variable names in the code, and reset them to match exactly the parameter name. Could you review this as well for the final decision? thinking @tstastny @potaito

#11686

honestly i totally disagree with that PR. i understand the want to be able to grep for something, but if it is defined all in one place in the header anyway.. this shouldnt be so hard to find the value you want. and the param names are very limited on characters, so is very hard to read. having the _param prefix in front could be a way to identify, but keeping the name totally the same seems like not worth it to me at expense of readability in the code itself.

but anyway - my opinion is not the only one that matters. we could also chat with @MaEtUgR and @bresch about it (maybe offline is easier).

Right I agree that using the direct parameter name in the code decreases code readability 😋

I have to admit I'm siding with #11686 because it's weird to sometimes be able to CTRL+F certain parameters, and other times not, because the handle is not re-using the parameter name 😅

But your argument is valid.

@tstastny Thanks for asking. I totally understand your point all information is there and the verbose internal variable names were used for a very long time before the naming was enforced. But for years I was wasting minutes every day looking up which parameter is used where and what exact parameter contributes to the code I'm reading. I have to say it was such a big improvement when the names were streamlined and finally grepable, I never looked back once. As you can see in the pr this relief was shared by a lot of if not all the core contributors that cared. Correct me if I'm wrong. So I would disadvise reverting this naming strategy.

That said I don't like abbreviations in variable names and I encourage to use descriptive unshortened names. Also, I suggest thinking twice when defining or reviewing parameter names to make them as descriptive as possible given the limitations. Ideally parameter names would not be limited to 16 characters only.

thanks for the extra insight @MaEtUgR and @potaito - i can understand the reasoning. i guess then as we discussed in the dev call we should attack the *heart of the issue .. 16 char limits on param names. I'll follow up with @bkueng offline. @junwoo091400 for now you should then stick with the convention from #11686

potaito

Also, these submodules were updated, but probably not on purpose, right?

src/drivers/gps/devices
Tools/flightgear_bridge
src/modules/mavlink/mavlink

I'll take another look at the PR once TJ's remarks were addressed. Otherwise it will become too much for everyone involved 😂

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

potaito · 2022-03-21T09:11:29Z

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

+	SecondOrderReferenceModel<matrix::Vector3f> _target_pose_filter;
+
+	// Internally tracked Follow Target characteristics, to allow RC control input adjustments
+	float _follow_target_distance{0.0f};


No 🙈
Especially not with height being initialized to 0.0 as well. That's a homing missile right there 😅

I would initialize to the default values of the corresponding parameters. Or initialize to NaN if there is a possibility, that the values have not been initialized when follow-me is running.

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

potaito · 2022-03-21T09:19:29Z

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

+		(ParamFloat<px4::params::NAV_FT_HT>) _param_nav_ft_ht,
 		(ParamFloat<px4::params::NAV_FT_DST>) _param_nav_ft_dst,
 		(ParamInt<px4::params::NAV_FT_FS>) _param_nav_ft_fs,
 		(ParamInt<px4::params::NAV_FT_ALT_M>) _param_nav_ft_alt_m,
-		(ParamInt<px4::params::NAV_FT_GMB_M>) _param_nav_ft_gmb_m,
-		(ParamInt<px4::params::NAV_FT_DELC>) _param_nav_ft_delc
+		(ParamFloat<px4::params::NAV_FT_YAW_T>) _param_ft_yaw_t


Fair point. I was always torn between having them named exactly as the corresponding parameter, and having them named with more details.
Never thought about combining both approaches with prefix + meaning as you mentioned 👍
So you would suggest something like _param_ft_yaw_t_filter_time_constant ?

src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

potaito

git submodule updates still need to be reverted :)

msg/follow_target_status.msg

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

potaito · 2022-03-30T07:16:13Z

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

-	// The drone will move into position as soon as the target starts moving and its heading becomes known.
-	Vector2f current_drone_heading_2d{cosf(_yaw), -sinf(_yaw)};
+	// We don't command the yawspeed, since only the yaw can be calculated off of the target
+	_yawspeed_setpoint = NAN;


Is yaw (not yawspeed) correctly initialized somewhere? I.e. where does the drone point at when follow-me is initiated while the target is not moving?

Current behaviour is to not set yaw at all, until drone & target's distance is bigger than minimum distance for yaw control, then sets as the raw heading from drone to target as the yaw.

But is it initialized correctly? If it's not set at all, what value does _yaw_setpoint have?
We could initialize it to the current yaw angle, just to maintain whatever the current setpoint is.

I don't believe so 🤔. But also I thought that the sudden 360 degrees yaw spin that happens when I activate the follow me mode is from this hard setpoint, which is why I believe I want to not set it at all.

Since we don't know where the target is 🤔

So the FlightTask (the parent class) initializes (resets) it to NAN, so it won't be commanding at all. I guess then it makes sense to command the current yaw since commanding NAN is unpredictible? 🤔

Yes, I would also command the current yaw 👍

No need to do it now, but don't forget that you're getting last_setpoint as a parameter, meaning that you could initialize with it (if finite) to avoid glitches (see

PX4-Autopilot/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

Line 81 in 3ac8fdb

_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;

)

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c

src/modules/logger/logged_topics.cpp

src/modules/mavlink/mavlink_receiver.cpp

src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp

src/lib/mathlib/math/filter/second_order_reference_model.hpp

src/lib/mathlib/math/test/second_order_reference_model_test.cpp

junwoo091400 · 2022-04-26T13:57:21Z

Some related videos I wanted to share:

Upstream Follow-Me behavior
https://www.youtube.com/watch?time_continue=42
Current PR's Follow-Me behavior
From March 26th:
https://www.youtube.com/watch?time_continue=107
From March 11th:
https://www.youtube.com/watch?v=myPFVJJkm04&t=19s

junwoo091400 · 2022-04-26T14:03:42Z

Few Notes on the current development progress:

Right now, this version is ready to go & to be reviewed for merge. It fixed all the major tiny problems that was arising from algorithmical details of Follow Target.
The last challenge was the 'Overshoot' of the 2nd order reference model problem, which was causing the drone to thinking that target was moving backwards, when in fact target is moving still. By including unfiltered velocity into consideration (which, stays at 0 when we have the overshoot phenomenon) this was fixed.

Images related to this overshoot problem (from 4 days ago) :

junwoo091400 · 2022-04-26T15:25:16Z

Squashed so there are 8 PR-specific commits + 12 cherry-picked commits! Ready for review :)

To return Exponential Values, which is helpful for reducing the sensitivity of the stick around the centered value (0), since it's exponential curve. Useful for user adjustment implementations, where accidentally touching the stick wouldn't have so much effect when using the Exponential value, compared to using the raw position value.

junwoo091400 · 2022-05-05T12:43:50Z

I cherry-picked #19591 into the PR and applied the usage of Expo() values for User RC adjustments.

Also, addressed all the comments! Is it ready to merge?

tstastny

@junwoo091400 almost!

A few comments below -- also the style comment was also still not addressed. The method name styles are inconsistent. Should switch them all to lowerCamelCase like the flight task base class and your newly implemented function shown here:
https://github.com/PX4/PX4-Autopilot/blob/20355e8c18515a7b53f52ad6407b742ee042a73e/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp#L257-L267

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

junwoo091400 · 2022-05-05T13:39:45Z

Addressed all the comments! @tstastny

junwoo091400 · 2022-05-08T15:40:13Z

Force pushed to confine FollowPerspective enum to uint8_t type to avoid followperspective being casted to negative value.
https://en.wikipedia.org/wiki/C%2B%2B11#Strongly_typed_enumerations

potaito

LGTM 👏

bresch

It's mostly good to me, just a small mistake on the param update.
There are still way too many comments IMO, but it's not a big problem.

bresch · 2022-05-10T09:18:10Z

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

-	// The drone will move into position as soon as the target starts moving and its heading becomes known.
-	Vector2f current_drone_heading_2d{cosf(_yaw), -sinf(_yaw)};
+	// We don't command the yawspeed, since only the yaw can be calculated off of the target
+	_yawspeed_setpoint = NAN;


No need to do it now, but don't forget that you're getting last_setpoint as a parameter, meaning that you could initialize with it (if finite) to avoid glitches (see

PX4-Autopilot/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

Line 81 in 3ac8fdb

_yaw_sp_prev = PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw;

)

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

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

junwoo091400 · 2022-05-10T13:59:48Z

I will test the yaw setpoint mode you commented about with this branch : https://github.com/junwoo091400/PX4-Autopilot/tree/followme_refactor_yaw_tracking_test

With a Mantis Edu!

junwoo091400 · 2022-05-10T20:18:44Z

Left : With yaw setpoint as (Position Setpoint -> Target) | Right : With yaw setpoint as (Actual Position -> Target)

@bresch I didn't find significant difference in the yaw / yaw rate command difference in between two different yaw setpoint calculation methods.

However, I did find that basing the yaw calculation off of position setpoint can introduce a yaw tracking lag when there's a difference between the setpoint & actual position:

Here, it is visible that the PS (Position Setpoint) is leading the AP (Actual Position), and target is moving in upwards-right direction. The yaw setpoint (PS -> Tgt) is calculated as to point lower-right direction, but the correct yaw setpoint (AP -> Tgt) should point right direction. Therefore, the camera ends up pointing on the trailing side of the target. Which is why in the video I am almost out of frame.

I think keeping the yaw setpoint based on Actual Position makes sense in this regards. What do you think? @bresch

junwoo091400 · 2022-05-10T20:29:51Z

https://youtu.be/C1Jy3ln028M?t=143

This is the video link to where the yaw legging behavior can be observed (timestamp included)!

junwoo091400 · 2022-05-11T07:54:27Z

Oh and yesterday I also tested RC adjustments & Whether changing the Parameter resets the Follow distance, angle and height, it all worked 👍

tstastny requested changes Feb 25, 2022

View reviewed changes

potaito force-pushed the potaito/new-follow-me-task branch from 6467048 to 020f27e Compare February 25, 2022 15:42

junwoo091400 force-pushed the followme_refactor branch 2 times, most recently from a08cf22 to 5e9cf4f Compare February 28, 2022 14:03

junwoo091400 mentioned this pull request Mar 10, 2022

Sticks : Add utility functions to get pitch, roll, throttle and yaw values more intuitively #19309

Merged

junwoo091400 force-pushed the followme_refactor branch from 45b3c86 to a193780 Compare March 10, 2022 16:48

junwoo091400 mentioned this pull request Mar 11, 2022

Update Follow Me documentation reflecting recent refactor in Follow Target PX4/PX4-user_guide#1810

Merged

3 tasks

junwoo091400 force-pushed the followme_refactor branch 5 times, most recently from 60dcb74 to e3b8f51 Compare March 17, 2022 16:35

junwoo091400 force-pushed the followme_refactor branch 3 times, most recently from 9bb7cee to 6c5bacd Compare March 18, 2022 13:01

tstastny requested changes Mar 21, 2022

View reviewed changes

potaito reviewed Mar 21, 2022

View reviewed changes

potaito requested changes Mar 30, 2022

View reviewed changes

bresch reviewed Mar 30, 2022

View reviewed changes

src/lib/mathlib/math/filter/second_order_reference_model.hpp Outdated Show resolved Hide resolved

bresch reviewed Mar 30, 2022

View reviewed changes

src/lib/mathlib/math/test/second_order_reference_model_test.cpp Outdated Show resolved Hide resolved

junwoo091400 force-pushed the followme_refactor branch from 29e5c15 to 68f1e5f Compare April 4, 2022 12:57

junwoo091400 mentioned this pull request Apr 7, 2022

Discussion on changing of Home Position coordinates in-air #19448

Closed

junwoo091400 changed the title ~~[Draft] Follow Me mode Second order filter refactor PR~~ Follow Me mode Second order filter refactor PR Apr 26, 2022

junwoo091400 marked this pull request as ready for review April 26, 2022 13:52

junwoo091400 force-pushed the followme_refactor branch from 87b215c to fddf537 Compare April 26, 2022 15:22

junwoo091400 mentioned this pull request Apr 26, 2022

[Control] Constrain Acceleration Feed Forward when Drone is far from position setpoint #19543

Open

junwoo091400 force-pushed the followme_refactor branch 4 times, most recently from 68a9a00 to 0ef4156 Compare May 5, 2022 12:28

junwoo091400 mentioned this pull request May 5, 2022

Add get*Expo() functions to the Sticks library #19591

Merged

junwoo091400 force-pushed the followme_refactor branch from 0ef4156 to 20355e8 Compare May 5, 2022 12:42

tstastny requested changes May 5, 2022

View reviewed changes

junwoo091400 force-pushed the followme_refactor branch from 20355e8 to f45f85e Compare May 5, 2022 13:39

junwoo091400 force-pushed the followme_refactor branch from f45f85e to 5610524 Compare May 5, 2022 13:46

tstastny approved these changes May 5, 2022

View reviewed changes

junwoo091400 force-pushed the followme_refactor branch from 5610524 to 087c935 Compare May 8, 2022 15:37

potaito approved these changes May 9, 2022

View reviewed changes

junwoo091400 mentioned this pull request May 9, 2022

Update follow_me.proto to match PX4 Upstream mavlink/MAVSDK-Proto#287

Merged

bresch requested changes May 10, 2022

View reviewed changes

junwoo091400 force-pushed the followme_refactor branch from 087c935 to 53a795f Compare May 10, 2022 11:56

junwoo091400 force-pushed the followme_refactor branch from 53a795f to dabd01c Compare May 10, 2022 13:57

bresch approved these changes May 11, 2022

View reviewed changes

junwoo091400 merged commit 3f8f233 into PX4:potaito/new-follow-me-task May 11, 2022

This was referenced May 11, 2022

New follow-me flight task #18026

Merged

Update Follow me code to match upstream changes mavlink/MAVSDK#1770

Merged

junwoo091400 mentioned this pull request Mar 29, 2023

[v1.14 Release] beta: Yaw estimate error in ATL Mantis Edu #21387

Closed

Follow Me mode Second order filter & improvements #19260

Follow Me mode Second order filter & improvements #19260

Conversation

junwoo091400 commented Feb 25, 2022 • edited Loading

Discussions

TODOS

Jaeyoung-Lim commented Mar 21, 2022

tstastny left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tstastny Mar 22, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MaEtUgR Mar 23, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

potaito left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

potaito left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

junwoo091400 commented Apr 26, 2022

junwoo091400 commented Apr 26, 2022 • edited Loading

junwoo091400 commented Apr 26, 2022

junwoo091400 commented May 5, 2022

tstastny left a comment

Choose a reason for hiding this comment

junwoo091400 commented May 5, 2022

junwoo091400 commented May 8, 2022

potaito left a comment

Choose a reason for hiding this comment

bresch left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

junwoo091400 commented May 10, 2022

junwoo091400 commented May 10, 2022 • edited Loading

junwoo091400 commented May 10, 2022

junwoo091400 commented May 11, 2022

junwoo091400 commented Feb 25, 2022 •

edited

Loading

tstastny Mar 22, 2022 •

edited

Loading

MaEtUgR Mar 23, 2022 •

edited

Loading

junwoo091400 commented Apr 26, 2022 •

edited

Loading

junwoo091400 commented May 10, 2022 •

edited

Loading