ArduPlane: add AC_Fence for QGC4.0 Geofence Support #15288

joshanne · 2020-09-09T09:17:47Z

PR based on #13700 - see further discussion there

Moves Plane from Geofence to AC_Fence for Fence support in QGC.
geofence capability has been dropped in favour of AC_Fence. Some assumptions have been made.
Adds floor breaching to AC_Fence
Adds an autotest for testing Ceiling and Floor breaches. Issue in existing geofence implementation would be carried across (Plane: Landing permanently disable geofence #5544)
Existing autotests for geofence work, mostly untouched.
Discussion to be had around mav status present/enabled bits.

Requires:

Merge AC_Fence: Use int32_t as fence points #15568
Documentation: Wiki: Updates to fences to support fence mission item protocol ardupilot_wiki#3349

Todo:

minimum altitude fence breach - autotests have been added
ensure we can auto-enable the fence when vehicle armed - autotests have been added
should disallow arming if we are outside the fence when we auto-enable - autotests have been added
need tests around MAV_CMD_DO_FENCE_ENABLE
fence_ret_rally works - autotests have been added.
need tests for what happens when fences are disabled while we're breached and undertaking an action - autotests have been added

joshanne · 2020-09-10T03:57:39Z

Some tasks that remain outstanding:
Edit: Question obsolete now that geofence code has been removed.
~~- [ ] How should we handle AC_Fence or Geofence enabling? Currently set up with include guards.~~

peterbarker

Started to review here, but retaining the existing geofence code is confusing the issue.

Why retain the existing geofence implementation? We don't carry dead code, so.... Also, it seems to be half-and-half with retaining geofence functionality.

ArduPlane/ArduPlane.cpp

ArduPlane/GCS_Mavlink.cpp

peterbarker · 2020-09-10T04:48:50Z

Oh, also, alt_min may be <-100 for Subs :-)

peterbarker · 2020-09-10T05:00:27Z

.... also, please squash your commits up into logical changes. There are patches in here which are definitely "fixups" of previous commits. I do realise this is WIP :-)

joshanne · 2020-09-10T05:09:53Z

Why retain the existing geofence implementation? We don't carry dead code, so.... Also, it seems to be half-and-half with retaining geofence functionality.

Fair call - the intention was more for the review process and an ability to swap between the two versions, or for those that want to maintain their existing geofence version (hence the separate ENABLE flags). Obviously, they wouldn't be compatible together. Happy to rip out the dead code, and move to an entirely AC_Fence model.

Oh, also, alt_min may be <-100 for Subs :-)

Good call - still trying to piece together how the documentation/param parsing works.
Note: There's also argument for enabling fence floor for Copter (logically) and Rover (potentially?).

.... also, please squash your commits up into logical changes. There are patches in here which are definitely "fixups" of previous commits. I do realise this is WIP :-)

Apologies, I've tried to nail everything on my first PR, but things were bound to get missed. Certainly can do that.

peterbarker · 2020-09-10T05:44:39Z

The fence min-alt stuff.... floor seems a little tacked-on ATM.

Should we consider making enable(bool value) instead enable(uint8_t mask)?

(I actually have a related PR which would make that enable(FenceMask mask) here: #15073 - still needs a bit of work.

joshanne · 2020-09-10T07:21:03Z

The fence min-alt stuff.... floor seems a little tacked-on ATM.

As tacked-on as it is, keep in mind that it was the goal to change the minimum amount of AC_Fence in order to support Plane and its existing capability. The implementation is mostly using the existing framework to do so. Also, not saying that it's without need of rework, hence reviews! :-) (I've just noticed I've missed at least two other points involving fence floors - I'll add that as well).

Add fence floor calculation to check_destination_within_fence
Add fence floor calculation to get_breach_distance

Should we consider making enable(bool value) instead enable(uint8_t mask)?
(I actually have a related PR which would make that enable(FenceMask mask) here: #15073 - still needs a bit of work.

Certainly steering away from uint8_t's would be good, especially for reporting breach types, fence types and action types. I'll need to look at the related PR in depth a little more to see what your enable function looks like (I couldn't see it on first glance)

joshanne · 2020-09-10T07:42:46Z

Commits logically squashed for adding of AC_Fence to ArduPlane. To address the confusion around leaving geofence in, I'll remove the geofence code in a future commit.

amilcarlucas

This makes one of my PR's ( #7111 ) to add a floor fence to copter mostly obsolete.
One think I have in mine though is that it uses the down-facing lidar, if present, for the floor fence calculations.

amilcarlucas · 2020-09-10T07:46:24Z

libraries/AC_Fence/AC_Fence.cpp

    // @Values: 0:Report Only,1:RTL or Land
    // @User: Standard
    AP_GROUPINFO("ACTION",      2,  AC_Fence,   _action,        AC_FENCE_ACTION_RTL_AND_LAND),

-    // @Param{Copter, Sub}: ALT_MAX
+    // @Param: ALT_MAX


Rover also probably will not want this

Is there a use case where Rover may be climbing a mountain-side and exceed an altitude that needs to RTL?
I do note that it was not in the AP_GROUPINFO_FRAME definition, but, safest to check.

joshanne · 2020-09-10T08:13:48Z

This makes one of my PR's ( #7111 ) to add a floor fence to copter mostly obsolete.
One think I have in mine though is that it uses the down-facing lidar, if present, for the floor fence calculations.

Thanks @amilcarlucas - I'll take a look at your implementation. Perhaps this could be a two part process where the down-facing lidar is added as a follow-up?

amilcarlucas · 2020-09-10T08:21:43Z

Yes that would be great

amilcarlucas · 2020-09-10T08:26:24Z

Some of the CI builds are failing, can you look into that?

joshanne · 2020-09-10T08:39:08Z

Some of the CI builds are failing, can you look into that?

Ahh, rebased but missed one last commit to finish removing geofence code. Using the build_ci.sh script now to check there's no other issue.

peterbarker

PR is incorrect; this will appear in Plane 4.1

FENCE_CHANNEL should be migrated to use the existing RC_Option. This could be done ahead of time as a distinct PR - there's parameter conversion involved, so it's non-trivial

autotests:

ensure we can auto-enable the fence when vehicle armed
should disallow arming if we are outside the fence when we auto-enable
need tests around MAV_CMD_DO_FENCE_ENABLE
minimum altitude fence breach
fence_ret_rall works
need tests for what happens when fences are disabled while we're breached and undertaking an action

This PR is absolutely fantastic. So many red lines!

ArduPlane/ArduPlane.cpp

ArduPlane/Parameters.h

ArduPlane/Plane.h

ArduPlane/commands_logic.cpp

libraries/AC_Fence/AC_Fence.cpp

peterbarker · 2020-09-10T10:38:09Z

libraries/AC_Fence/AC_Fence.cpp

+    // clear alt breach if present
+    if ((_breached_fences & AC_FENCE_TYPE_ALT_MIN) != 0) {
+        clear_breach(AC_FENCE_TYPE_ALT_MIN);
+        _alt_min_backup = 0.0f;


Do we need to zero these?

Implementation for check_fence_alt_min is a reworked check_fence_alt_max, which contains this same assignment. I'd have to confirm the conditions in which the backup fence is used.

I suspect if we dont, the following series may occur:

Flying within fences.

Breach floor. Set backup floor

Breach backup floor. Set new backup floor (repeat n times)

Return inside fence

Breach FENCE_ALT_MIN, (!is_zero(_alt_min_backup) && _curr_alt < _alt_min_backup) is treated as an old breach, because our current alt is less than ALT_MIN but greater than the alt_min_backup.

libraries/AC_Fence/AC_Fence.cpp

libraries/AC_Fence/AC_Fence.h

libraries/AP_Arming/AP_Arming.cpp

peterbarker · 2020-09-10T23:42:42Z

On Thu, 10 Sep 2020, James O wrote: On a dev call it was mentioned there was a script that performs these checks? My google-fu is weak today, apparently.

Yes, we have an auto-formatter. But it won't just check your delta, it will do the entire file, which is kind of bad as you'll end up with lots of unrelated meaningless changes making review a chore. It's ./Tools/CodeStyle/ardupilot-astyle.sh

joshanne · 2020-09-11T02:26:12Z

It's ./Tools/CodeStyle/ardupilot-astyle.sh

Thanks for the pointer to the script. I'll take a look and see if it helps at all. Could be more hassle than it's worth, but good for use in the future.

joshanne · 2020-09-11T04:41:03Z

Oh, also, alt_min may be <-100 for Subs :-)

According to: http://www.ardusub.com/developers/full-parameter-list.html
If the sub can have a min fence < -100, that's not evident in existing documentation.

Edit: Thinking about it... I'm adding in the floor support in this PR - I assume sub never had floor support in the first place!

FENCE_ALT_MIN: Fence Minimum Altitude
Minimum altitude allowed before geofence triggers
Range: -100 100
Increment: 1
Units: m

joshanne · 2020-09-11T07:13:53Z

~~To do removed~~

…ameter

Addresses comments made in review, removes the FENCE_AUTOENABLE param for copter. No param means no tests required.

joshanne · 2021-03-03T12:38:03Z

@tridge

* need to disable tincan by default
* default FENCE_TYPE should be polygon?
* in upgrade handle alt max/min to set type
* restore fence auto-enabled msg
* missing RTL mode?

These issues have been addressed. I can't seem to find where your comment went, but it also addresses the issue of re-triggering a fence breach when jumping out of the FENCE_ACTION mode before fence breach is cleared. (ie. Cruise well beyond the fence wall, enable fence, re-enter cruise mode)

Also, made FENCE_AUTOENABLE a plane specific parameter, but left the ability to use fence floor in. Copter auto-enabling the floor when at RTL altitude should be a follow up change. However, the fence floor can still be used by enabling the fence with an aux switch RCx_OPTION=11, a mission item, or a mavlink cmd.

… disabled while under action

joshanne · 2021-03-04T02:31:21Z

I've now also added an autotest to ensure mode change outside of the fence re-triggers the fence action (as per discovered during testing).

Also added an autotest to capture, and ensure the correctness of the overflow found in get_return_point when no return point is present. This occurs when longitudes are close to the max limits [-180, 180] and if in this state, could result in a fly away. eg. Longitudes close to CMAC, when added together exceed the int32_t size, overflow to negative, and halve to be a longitude of approx -65°, placing the guided location somewhere near South America.

libraries/AC_Fence/AC_PolyFence_loader.cpp

…ched AutoTest: Adds test for fence breach switching to guided mode when no fence return point is present. In upstream, this results in a vehicle fly-away.

joshanne · 2021-03-04T23:28:30Z

@amilcarlucas comments added to int32_t overflow fix

peterbarker · 2021-03-05T00:51:22Z

We should probably make sure that fence behaviour is unchanged when we lose our GPS (and other absolute positioning) sources. This can be tested in SITL using SIM_GPS_DISABLE

This review is rather stale at this point. There are changes to Plane's behavior in the case of GPS loss in the current PR, but they do appear to be for the best.

amilcarlucas · 2021-03-05T08:17:52Z

Many Thanks and Congrats to @joshanne . Great work!

joshanne force-pushed the wip/plane_ac_fence branch from 128b930 to f90ee6f Compare September 9, 2020 09:26

joshanne changed the title ~~Plane4.0 QGC4.0 GeoFence Support~~ Plane4.0 add AC_Fence for QGC4.0 Geofence Support Sep 9, 2020

joshanne force-pushed the wip/plane_ac_fence branch from 40d8deb to 4314765 Compare September 9, 2020 10:53

peterbarker requested changes Sep 10, 2020

View reviewed changes

ArduPlane/ArduPlane.cpp Outdated Show resolved Hide resolved

ArduPlane/GCS_Mavlink.cpp Outdated Show resolved Hide resolved

peterbarker added WikiNeeded needs wiki update WIP labels Sep 10, 2020

joshanne force-pushed the wip/plane_ac_fence branch from 7ce4f59 to c7e2d35 Compare September 10, 2020 07:43

amilcarlucas approved these changes Sep 10, 2020

View reviewed changes

joshanne force-pushed the wip/plane_ac_fence branch from c7e2d35 to c81cc74 Compare September 10, 2020 08:09

joshanne force-pushed the wip/plane_ac_fence branch from c81cc74 to fbd163f Compare September 10, 2020 08:58

peterbarker previously requested changes Sep 10, 2020

View reviewed changes

joshanne changed the title ~~Plane4.0 add AC_Fence for QGC4.0 Geofence Support~~ ArduPlane add AC_Fence for QGC4.0 Geofence Support Sep 10, 2020

joshanne changed the title ~~ArduPlane add AC_Fence for QGC4.0 Geofence Support~~ ArduPlane: add AC_Fence for QGC4.0 Geofence Support Sep 10, 2020

joshanne force-pushed the wip/plane_ac_fence branch from fbd163f to 53da425 Compare September 11, 2020 03:43

joshanne force-pushed the wip/plane_ac_fence branch from 53da425 to d349657 Compare September 11, 2020 05:06

joshanne added 5 commits March 3, 2021 23:32

AC_Fence: Adjustments to default parameters for addition to plane

60205be

AC_Fence: Send text to gcs when fence is auto-enabled

b5c8cdd

ArduCopter: Cleanup - removes autoenabling of fence with AC_Fence par…

ea098e7

…ameter

AutoTest: Copter remove FENCE_AUTOENABLE tests

b078960

Addresses comments made in review, removes the FENCE_AUTOENABLE param for copter. No param means no tests required.

AP_Landing: Autoenable fence if required when landing was aborted

baca85d

amilcarlucas requested a review from tridge March 3, 2021 12:55

ArduPlane: corrections to returning to the previous mode if fence was…

199d953

… disabled while under action

joshanne force-pushed the wip/plane_ac_fence branch from 2bece87 to a31705a Compare March 4, 2021 02:48

amilcarlucas suggested changes Mar 4, 2021

View reviewed changes

libraries/AC_Fence/AC_PolyFence_loader.cpp Outdated Show resolved Hide resolved

joshanne added 2 commits March 5, 2021 10:21

AC_Fence: Fix int32_t overflow with large longitude values

4c3009f

AutoTest: Adds additional autotests to capture mode change while brea…

44d4033

…ched AutoTest: Adds test for fence breach switching to guided mode when no fence return point is present. In upstream, this results in a vehicle fly-away.

joshanne force-pushed the wip/plane_ac_fence branch from a31705a to 44d4033 Compare March 4, 2021 23:23

joshanne requested a review from amilcarlucas March 4, 2021 23:28

tridge approved these changes Mar 5, 2021

View reviewed changes

peterbarker merged commit bcc0da9 into ArduPilot:master Mar 5, 2021

joshanne mentioned this pull request Mar 10, 2021

AC_Fence: Change the default fence specification #13668

Open

IamPete1 removed the DevCallEU label Mar 17, 2021

joshanne mentioned this pull request Mar 24, 2021

Plane: Landing permanently disable geofence #5544

Open

Hwurzburg removed the WikiNeeded needs wiki update label May 31, 2021

tridge added this to Done in Plane 4.1 Jun 13, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ArduPlane: add AC_Fence for QGC4.0 Geofence Support #15288

ArduPlane: add AC_Fence for QGC4.0 Geofence Support #15288

joshanne commented Sep 9, 2020 •

edited

Loading

joshanne commented Sep 10, 2020 •

edited

Loading

peterbarker left a comment

peterbarker commented Sep 10, 2020

peterbarker commented Sep 10, 2020

joshanne commented Sep 10, 2020

peterbarker commented Sep 10, 2020

joshanne commented Sep 10, 2020 •

edited

Loading

joshanne commented Sep 10, 2020

amilcarlucas left a comment

amilcarlucas Sep 10, 2020

joshanne Sep 10, 2020

joshanne commented Sep 10, 2020 •

edited

Loading

amilcarlucas commented Sep 10, 2020

amilcarlucas commented Sep 10, 2020

joshanne commented Sep 10, 2020 •

edited

Loading

peterbarker left a comment

peterbarker Sep 10, 2020

joshanne Sep 10, 2020 •

edited

Loading

peterbarker commented Sep 10, 2020 via email

joshanne commented Sep 11, 2020

joshanne commented Sep 11, 2020 •

edited

Loading

joshanne commented Sep 11, 2020 •

edited

Loading

joshanne commented Mar 3, 2021

joshanne commented Mar 4, 2021 •

edited

Loading

joshanne commented Mar 4, 2021

peterbarker commented Mar 5, 2021

amilcarlucas commented Mar 5, 2021

ArduPlane: add AC_Fence for QGC4.0 Geofence Support #15288

ArduPlane: add AC_Fence for QGC4.0 Geofence Support #15288

Conversation

joshanne commented Sep 9, 2020 • edited Loading

joshanne commented Sep 10, 2020 • edited Loading

peterbarker left a comment

Choose a reason for hiding this comment

peterbarker commented Sep 10, 2020

peterbarker commented Sep 10, 2020

joshanne commented Sep 10, 2020

peterbarker commented Sep 10, 2020

joshanne commented Sep 10, 2020 • edited Loading

joshanne commented Sep 10, 2020

amilcarlucas left a comment

Choose a reason for hiding this comment

amilcarlucas Sep 10, 2020

Choose a reason for hiding this comment

joshanne Sep 10, 2020

Choose a reason for hiding this comment

joshanne commented Sep 10, 2020 • edited Loading

amilcarlucas commented Sep 10, 2020

amilcarlucas commented Sep 10, 2020

joshanne commented Sep 10, 2020 • edited Loading

peterbarker left a comment

Choose a reason for hiding this comment

peterbarker Sep 10, 2020

Choose a reason for hiding this comment

joshanne Sep 10, 2020 • edited Loading

Choose a reason for hiding this comment

peterbarker commented Sep 10, 2020 via email

joshanne commented Sep 11, 2020

joshanne commented Sep 11, 2020 • edited Loading

joshanne commented Sep 11, 2020 • edited Loading

joshanne commented Mar 3, 2021

joshanne commented Mar 4, 2021 • edited Loading

joshanne commented Mar 4, 2021

peterbarker commented Mar 5, 2021

amilcarlucas commented Mar 5, 2021

joshanne commented Sep 9, 2020 •

edited

Loading

joshanne commented Sep 10, 2020 •

edited

Loading

joshanne commented Sep 10, 2020 •

edited

Loading

joshanne commented Sep 10, 2020 •

edited

Loading

joshanne commented Sep 10, 2020 •

edited

Loading

joshanne Sep 10, 2020 •

edited

Loading

joshanne commented Sep 11, 2020 •

edited

Loading

joshanne commented Sep 11, 2020 •

edited

Loading

joshanne commented Mar 4, 2021 •

edited

Loading