Follow kinematic shaping #29898

lthall · 2025-04-26T07:37:56Z

This pr provides a kinimatic estimate of the target location and heading.

I have added time out's and resets based on the kinimatic limits and errors.

We need to decide on how we choose between the different data message sources. Ideally I would like to change the name and extend the _ALT_TYPE parameter to be 0: GPI absolute, 1: GPI relative, 2: Follow Target (GPI = global position int).

We also need to decide what the options and default settings should be. We can leave the kinimatic interpolation off by default, we may choose not to process acceleration to reduce noise.

timtuxworth

Some initial comments.

libraries/AP_Follow/AP_Follow.cpp

timtuxworth · 2025-04-27T00:38:58Z

libraries/AP_Follow/AP_Follow.cpp

-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)


This should probably be called get_target_pos_vel_accel_ofs_ned_m() I think. or maybe get_target_pos_vel_accel_ned_ofs_m()

We have two things we can follow, the target and the offset from the target. I felt calling one target and the other ofs was ok. Maybe target and target_ofs.

However putting the ofs after the pos_vel_accel, I think that would be changing the pattern I normally use.

timtuxworth · 2025-04-27T00:45:33Z

libraries/AP_Follow/AP_Follow.cpp

+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();


I think calculating this here means _dist_to_target_m ( which is returned by the get_distance_to_target() method is now returning the actual distance whereas before it was returning the estimated distance, in other words now it doesn't seem like it will update between messages.

This is important to me because I have a binding for get_distance_to_target() and get_bearing_to_target() that I used in my follow Lua script.

Maybe some of these calculations should be moved into update_estimates() and handle_msg() should call update_estimates().

timtuxworth · 2025-04-27T00:48:55Z

libraries/AP_Follow/AP_Follow.cpp

+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());


ditto _bearing_to_target - now it is the bearing to the actual (last received) target position, whereas in the old code, I'm pretty sure it was the bearing to the estimated target position.

libraries/AP_Follow/AP_Follow.cpp

timtuxworth · 2025-04-29T07:58:50Z

libraries/AP_Follow/AP_Follow.cpp

+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();


Should not zero the _target_vel_ned_ms as this will thump any values set in handle_global_position_int_message()

We need to only use one of these messages. This is something we need to add.

When I send FOLLOW_TARGET from a plane, it doesn't have FOLLOW_TARGET_CAPABILITIES.VEL and FOLLOW_TARGET_CAPABILITIES.ACCEL and this breaks the follow code, I've seen it in testing. These lines (710,719) need to go.

Sorry @timtuxworth but I must be missunderstanding what you are saying. We have to set these to zero if they are not provided by the lead aircraft.

This PR currently doesn't let you turn off the consumption of GLOBAL_POSITION_INT when you provide FOLLOW_TARGET so using both will cause problems if one provides more information than the other. This is something I need to take to the Dev Call to get some guidance on how to implement this. Maybe I should do it the way I think it should be done so at least this PR is fully functional.

I see your point, but I still don't think zeroing these values is safe. Surely even stale values would be better than zeros.

BTW - for plane, I can get the same data with GLOBAL_POSITION_INT and ATTITUDE (existing messages) without needing FOLLOW_TARGET. I implemented an ATTITUDE receiver in my Lua script.

Absolutly not. Stale values are worse than zero. Stale valuse will a random cause a random offset error. Zeros will result in a sensible and predictable following error.

There is no point getting the same data from multiple message sources when we can simply select the prefered one. In my implementation of the FOLLOW_TARGET message the data will be different to the GLOBAL_POSITION_INT for example.

timtuxworth · 2025-04-29T07:59:21Z

libraries/AP_Follow/AP_Follow.cpp

+        _target_accel_ned_mss.y = packet.acc[1]; // acceleration east
+        _target_accel_ned_mss.z = packet.acc[2]; // acceleration down
+    } else {
+        _target_accel_ned_mss.zero();


should not zero _target_accel_ned_mss as this will thump any values calculated elsewhere.

We need to only use one of these messages. This is something we need to add.

timtuxworth · 2025-05-02T14:13:10Z

We need to decide on how we choose between the different data message sources. Ideally I would like to change the name and extend the _ALT_TYPE parameter to be 0: GPI absolute, 1: GPI relative, 2: Follow Target (GPI = global position int).

I'd like to see the _ALT_TYPE parameter align with Location::AltFrame, and in particular, ABOVE_TERRAIN = 3. Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.

We also need to decide what the options and default settings should be. We can leave the kinimatic interpolation off by default, we may choose not to process acceleration to reduce noise.

Why would we choose to leave kinematic off?

lthall · 2025-05-02T14:21:57Z

Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.

Currently I have no plans to handle terain altitude. I am not sure how we would specify this.

Why would we choose to leave kinematic off?

The main reason is to maintain the current behaviour.

timtuxworth · 2025-05-03T00:25:23Z

Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.

Currently I have no plans to handle terain altitude. I am not sure how we would specify this.

I do it one of two ways, depending on a parameter

Explicitly specify (via parameter) a fixed altitude value that the following vehicle will use for it's terrain altitude or
Use change_alt_frame to lookup the terrain altitude of the target (with offsets) before using this value as the target altitude for the follow vehicle

lthall · 2025-05-03T12:38:47Z

Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.

Currently I have no plans to handle terain altitude. I am not sure how we would specify this.

I do it one of two ways, depending on a parameter

Explicitly specify (via parameter) a fixed altitude value that the following vehicle will use for it's terrain altitude or

Use change_alt_frame to lookup the terrain altitude of the target (with offsets) before using this value as the target altitude for the follow vehicle

I really don't understand why you would do this. What does this enable?

timtuxworth · 2025-05-04T00:57:24Z

Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.

Currently I have no plans to handle terain altitude. I am not sure how we would specify this.

I do it one of two ways, depending on a parameter

Explicitly specify (via parameter) a fixed altitude value that the following vehicle will use for it's terrain altitude or

Use change_alt_frame to lookup the terrain altitude of the target (with offsets) before using this value as the target altitude for the follow vehicle

I really don't understand why you would do this. What does this enable?

I work with two clients who use fixed wing VTOLs and quadcopter for magnetometry surveys. For them it's very important that each vehicle to follow the terrain at a specific height, often around 40m AGL. For use case #1 the planes are typically flying over mountainous areas. The two planes fly with an Y offset of around 50m -75m and the terrain height can be quite different even only 50m apart. It would give invalid results if the two drones flew at the same AMSL altitude. For example if the two planes are flying along a valley, one must fly much higher to be 40m above the ground if it is flying further up the slope. To be clear as well, because the terrain undulates, the vehicle cannot fly AMSL as it will likely hit something and maintaining a consistent 40m above the ground is important for the correct data to be collected.

For the second use case, the client flies quadcopter drones over remote areas. They fly with magnetometers hanging below the drone and need (again) to maintain 40m AGL. The terrain is not as extreme as the first use case, but flying multiple quadcopters allows them to collect more data faster, but each quad must fly a consistent path above the terrain at it's own location. This client asked me to write a Lua script to overcome the existing Copter follow code to allow it to follow AGL, but it would be much better IMO if the c++ code knew how to follow the terrain, this script is really a bit of a hack.
https://github.com/timtuxworth/ardupilot_scripts/blob/main/copter-follow-terrain.lua

lthall · 2025-05-04T02:02:14Z

Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.

Currently I have no plans to handle terain altitude. I am not sure how we would specify this.

I do it one of two ways, depending on a parameter

Explicitly specify (via parameter) a fixed altitude value that the following vehicle will use for it's terrain altitude or

Use change_alt_frame to lookup the terrain altitude of the target (with offsets) before using this value as the target altitude for the follow vehicle

I really don't understand why you would do this. What does this enable?

I work with two clients who use fixed wing VTOLs and quadcopter for magnetometry surveys. For them it's very important that each vehicle to follow the terrain at a specific height, often around 40m AGL. For use case #1 the planes are typically flying over mountainous areas. The two planes fly with an Y offset of around 50m -75m and the terrain height can be quite different even only 50m apart. It would give invalid results if the two drones flew at the same AMSL altitude. For example if the two planes are flying along a valley, one must fly much higher to be 40m above the ground if it is flying further up the slope. To be clear as well, because the terrain undulates, the vehicle cannot fly AMSL as it will likely hit something and maintaining a consistent 40m above the ground is important for the correct data to be collected.

For the second use case, the client flies quadcopter drones over remote areas. They fly with magnetometers hanging below the drone and need (again) to maintain 40m AGL. The terrain is not as extreme as the first use case, but flying multiple quadcopters allows them to collect more data faster, but each quad must fly a consistent path above the terrain at it's own location. This client asked me to write a Lua script to overcome the existing Copter follow code to allow it to follow AGL, but it would be much better IMO if the c++ code knew how to follow the terrain, this script is really a bit of a hack. https://github.com/timtuxworth/ardupilot_scripts/blob/main/copter-follow-terrain.lua

Thanks for the application example. I can see how that could be important. However, I think we are getting out of the scope of the current follow messages. Neither GPI or FT messages provide terrain altitude. Terrain is also not subject to any kinimatic limitations so it poses additional (but managable) challenges.

As for follow mode terrain following in copter, that has some more obvious and elegent solutions. I have considered that it may be usefull for aircraft to only follow in the horzontal while having manual control of altitude. This could be extended to use terrain following or use a default altitude. All this is a seperate problem to this PR or the update to follow mode I have planned after this.

timtuxworth · 2025-05-04T04:31:44Z

As for follow mode terrain following in copter, that has some more obvious and elegent solutions. I have considered that it may be useful for aircraft to only follow in the horizontal while having manual control of altitude. This could be extended to use terrain following or use a default altitude. All this is a seperate problem to this PR or the update to follow mode I have planned after this.

Yes I agree it's outside the scope of this PR, just wanted to make the _ALT_TYPE parameter aligns with Location::AltFrame, which is how I'm using it.

lthall requested review from peterbarker and timtuxworth April 26, 2025 07:38

timtuxworth suggested changes Apr 29, 2025

View reviewed changes

lthall force-pushed the 20250424_Follow_Kinematic_Shaping branch from 9ebd3d3 to 0ab6554 Compare May 1, 2025 12:48

lthall added 7 commits May 2, 2025 22:43

AP_Follow: Axis and unit naming.

da9717c

Copter: AP_Follow: Axis and unit naming.

9f13186

Rover: AP_Follow: Axis and unit naming.

b887cf9

AP_Math: Control: Add heading input shaping

f7a6fce

AP_Follow: reduce max jitter delay to 500 ms

b468049

AP_Math: fix shape limits

37d2369

AP_Follow: Add kinematic smoothing

2f20759

lthall force-pushed the 20250424_Follow_Kinematic_Shaping branch from 0ab6554 to 2f20759 Compare May 2, 2025 13:14

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Follow kinematic shaping #29898

Follow kinematic shaping #29898

lthall commented Apr 26, 2025

timtuxworth left a comment

timtuxworth Apr 27, 2025

lthall May 1, 2025

timtuxworth Apr 27, 2025

timtuxworth Apr 27, 2025

timtuxworth Apr 29, 2025

lthall May 1, 2025

timtuxworth May 1, 2025

lthall May 2, 2025

timtuxworth May 2, 2025 •

edited

Loading

lthall May 2, 2025

timtuxworth Apr 29, 2025

lthall May 1, 2025

timtuxworth commented May 2, 2025 •

edited

Loading

lthall commented May 2, 2025

timtuxworth commented May 3, 2025 •

edited

Loading

lthall commented May 3, 2025

timtuxworth commented May 4, 2025

lthall commented May 4, 2025

timtuxworth commented May 4, 2025

Follow kinematic shaping #29898

Are you sure you want to change the base?

Follow kinematic shaping #29898

Conversation

lthall commented Apr 26, 2025

timtuxworth 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

Choose a reason for hiding this comment

timtuxworth May 2, 2025 • edited Loading

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

timtuxworth commented May 2, 2025 • edited Loading

lthall commented May 2, 2025

timtuxworth commented May 3, 2025 • edited Loading

lthall commented May 3, 2025

timtuxworth commented May 4, 2025

lthall commented May 4, 2025

timtuxworth commented May 4, 2025

timtuxworth May 2, 2025 •

edited

Loading

timtuxworth commented May 2, 2025 •

edited

Loading

timtuxworth commented May 3, 2025 •

edited

Loading