Fence warn at the margins

[andyp1per]

This PR allows you to get a repeated warning when flying outside FENCE_MARGIN but inside the fence itself. Can be enabled by setting FENCE_OPTIONS=4. Multiple people have requested this because they are required to have a hard fence with failsafe action for regulatory reasons but want to know when they are close so they can avoid the action (typically when flying manually)

[Ryanf55]

This PR allows you to get a repeated warning when flying outside FENCE_MARGIN but inside the fence itself. Can be enabled by setting FENCE_OPTIONS=4. Multiple people have requested this because they are required to have a hard fence with failsafe action for regulatory reasons but want to know when they are close so they can avoid the action (typically when flying manually)

Do you have any references to the specific regulatory requirements for fences?

[rmackay9]

Let's be careful about spamming the user when in Loiter mode and the user pushes the vehicle against the fence

[andyp1per]

Getting the maths right on this was actually quite hard. Think I might have it now.

[Ryanf55]

Getting the maths right on this was actually quite hard. Think I might have it now.

Thoughts on unit tests to prove they are right?

[andyp1per]

Thoughts on unit tests to prove they are right?

I added a unit test, did you not see it?

[andyp1per]

Let's be careful about spamming the user when in Loiter mode and the user pushes the vehicle against the fence

So @rmackay9 what would you suggest here? If they have the option on and avoidance on and are in loiter mode, don't send the message?

[Ryanf55]

Thoughts on unit tests to prove they are right?

I added a unit test, did you not see it?

Totally missed it. Those look great. I'm guessing the choice of sphere approximation over something like ellipsoid with Vincenty's distance is because fences are usually small enough and with not enough accuracy that we can just neglect the difference?

[andyp1per]

Thoughts on unit tests to prove they are right?

I added a unit test, did you not see it?

Totally missed it. Those look great. I'm guessing the choice of sphere approximation over something like ellipsoid with Vincenty's distance is because fences are usually small enough and with not enough accuracy that we can just neglect the difference?

Yeah, I was relying on the reference @tpwrules sent me which shows that for small angles the haversine approximation is totally fine (the unit test demonstrates meter resolution). What wasn't fine was the original implementation which did not work at all for lat/lng 😛
It does make me wonder how accurate the current polyfence calculation is, but I haven't checked it.

[Ryanf55]

Thoughts on unit tests to prove they are right?

I added a unit test, did you not see it?

Totally missed it. Those look great. I'm guessing the choice of sphere approximation over something like ellipsoid with Vincenty's distance is because fences are usually small enough and with not enough accuracy that we can just neglect the difference?

Yeah, I was relying on the reference @tpwrules sent me which shows that for small angles the haversine approximation is totally fine (the unit test demonstrates meter resolution). What wasn't fine was the original implementation which did not work at all for lat/lng 😛 It does make me wonder how accurate the current polyfence calculation is, but I haven't checked it.

Yep, we just might want a wiki note to not try to use fences to define boundaries the size of a country because you can be far enough off that you'll make the wrong people mad.

[andyp1per]

I think you would have to have a margin the size of a country to run into this - which would be an odd thing to do

[tridge]

see if you can use the exiting function

[nexton-winjeel]

It's probably worth putting this new information in the FENCE_STATUS message.

[andyp1per]

Using the Cartesian version of Polygon_closest_distance_point() saves about 1k, the overall cost is still 1k since this was not used before.

[andyp1per]

I added some tests for the existing Polygon_closest_distance_point(), it gives accuracy down to about 30cm - which doesn't seem great, but its <1m so probably acceptable.

[tridge]

unused code?

[peterbarker]

Need to remove the new unused code

Need performance numbers

Move the in-margin stuff to a thread?

[peterbarker]

Suggested change

	bool have_location = AP::ahrs().get_location(loc);
	const bool have_location = AP::ahrs().get_location(loc);

[tridge]

we need the worst case CPU cost, and quite possibly move to a IO callback, maybe at 20Hz or so