Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_Scripting: add bindings for fence #25446

Merged
merged 1 commit into from
Nov 6, 2023

Conversation

IamPete1
Copy link
Member

@IamPete1 IamPete1 commented Nov 2, 2023

This adds bindings allowing fence failsafe actions to be taken by scripting. A simple example:

function update()

  local breaches = fence:get_breaches()
  if breaches ~= 0 then
    local msg = ""
    local breach = false
    if (breaches & 1) ~= 0 then
      msg = msg .. "Maximim altitude"
      breach = true
    end
    if (breaches & 2) ~= 0 then
      if breach then
        msg = msg .. " + "
      end
      msg = msg .. "Circle"
      breach = true
    end
    if (breaches & 4) ~= 0 then
      if breach then
        msg = msg .. " + "
      end
      msg = msg .. "Polygon"
      breach = true
    end
    if (breaches & 8) ~= 0 then
      if breach then
        msg = msg .. " + "
      end
      msg = msg .. "Minimum altitude"
    end

    local breach_time = millis() - fence:get_breach_time()

    gcs:send_text(0, string.format("Breached: %s fence, outside for %0.2f seconds", msg, breach_time:tofloat() * 0.001))

  end


  return update, 1000
end

return update()

Copy link
Contributor

@tridge tridge left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

looks good, but can we get the example in AP_Scripting/examples for others to build on?

@timtuxworth
Copy link
Contributor

Could you not add some other useful fence bindings at the same time? Love to have (at least):

enabled()
present()
get_breach_count()
get_breach_distance()
get_safe_alt_max()
get_safe_alt_min()
get_margin()
get_return_rally()
get_return_altitude()

@tridge
Copy link
Contributor

tridge commented Nov 4, 2023

@timtuxworth most of those are available via the parameter bindings, so adding them would just take up flash for no gain

@IamPete1
Copy link
Member Author

IamPete1 commented Nov 5, 2023

looks good, but can we get the example in AP_Scripting/examples for others to build on?

Tidied up and added the example, Thanks.

@timtuxworth As Tridge says we only add bindings for things that not accessible via params. A few of the things you listed you can pull from params. As a rule of thumb we only add stuff when someone has a need, I'm sure we could come up with situations where users might want a binding for every function in AP, but the more we add costs us flash, memory and performance. This PR adds the bindings I need for my use-case of triggering actions on fence breach.

I did originally also want get_breach_distance however it does not work for polygon fence, so it not that useful and will probably result in confusion.

@timtuxworth
Copy link
Contributor

timtuxworth commented Nov 5, 2023

@timtuxworth most of those are available via the parameter bindings, so adding them would just take up flash for no gain

Must be a definition of the word "most" that I'm not aware of. (3/9?)

@timtuxworth
Copy link
Contributor

@timtuxworth As Tridge says we only add bindings for things that not accessible via params. A few of the things you listed you can pull from params. As a rule of thumb we only add stuff when someone has a need, I'm sure we could come up with situations where users might want a binding for every function in AP, but the more we add costs us flash, memory and performance. This PR adds the bindings I need for my use-case of triggering actions on fence breach.

Yes - "a few" - could you also please add at least present() - bonus points for get_return_rally() and maybe get_return_altitude().

@IamPete1
Copy link
Member Author

IamPete1 commented Nov 5, 2023

Must be a definition of the word "most" that I'm not aware of. (3/9?)

enabled() = FENCE_ENABLE > 0
present() = FENCE_TYPE and checking FENCE_TOTAL > 0 for polygon
get_safe_alt_max() = FENCE_ALT_MAX - FENCE_MARGIN
get_safe_alt_min() = FENCE_ALT_MIN + FENCE_MARGIN
get_margin() = FENCE_MARGIN
get_return_rally() = FENCE_RET_RALLY
get_return_altitude() = FENCE_RET_ALT

I make it 7 / 9, which is most in anyones definition IMHO.

@timtuxworth
Copy link
Contributor

I'm specifically interested because of this PR which is a lua script for fence based pre/arm checks.

#20930

I understand if you don't want to merge it, but right now I can't use it myself because I need some fence bindings - I think mostly present() and AP_Mission method get_landing_sequence_start uint16_t.

@timtuxworth
Copy link
Contributor

timtuxworth commented Nov 5, 2023

I make it 7 / 9, which is most in anyones definition IMHO.

fair enough ... :-) (ok now at 6/9, but only because of shall we say ambiguous comments in AC_Fence.h)

but really? _poly_loader.total_fence_count() > 0 can come from a parameter? It seems a bit convoluted to me; if I try to trace the code, it doesn't seem like it's that simple.

I get the 'flash cost' argument - but this would mean duplicating some reasonably complex logic from cpp into lua. Is that really a good idea?

@timtuxworth
Copy link
Contributor

timtuxworth commented Nov 5, 2023

Must be a definition of the word "most" that I'm not aware of. (3/9?)
get_return_rally() = FENCE_RET_RALLY
get_return_altitude() = FENCE_RET_ALT

Also the cpp comment for get_return_rally says "returns whether returning to fence return point or rally point" - not the same thing as FENCE_RET_RALLY - unless the comment is wrong. Oh - looking at the code I guess the comment is wrong.

I'm assuming the same thing would apply to get_return_altitude() - i.e. its the "actual in flight return altitude" - but the comment looks like it suffers from a copy/paste.

@IamPete1
Copy link
Member Author

IamPete1 commented Nov 5, 2023

but really? _poly_loader.total_fence_count() > 0 can come from a parameter? It seems a bit convoluted to me; if I try to trace the code, it doesn't seem like it's that simple.

Yes, the value should be the same.

In lua the present function would look some thing like this:

local FENCE_TYPE = Parameter("FENCE_TYPE")
local FENCE_TOTAL = Parameter("FENCE_TOTAL")

local function present()
    local enabled_fences = FENCE_TYPE:get()
    local basic_fence = (enabled_fences & 0xB) ~= 0
    local polygon_fence = ((enabled_fences & 4) ~= 0) and FENCE_TOTAL:get() > 0
    return basic_fence or polygon_fence 
end

@IamPete1
Copy link
Member Author

IamPete1 commented Nov 5, 2023

Also the cpp comment for get_return_rally says "returns whether returning to fence return point or rally point" - not the same thing as FENCE_RET_RALLY - unless the comment is wrong.

I'm assuming the same thing would apply to get_return_altitude() - i.e. its the "actual in flight return altitude" - but the comment looks like it suffers from a copy/paste.

In both cases there directly returning the parameter variable.

@timtuxworth
Copy link
Contributor

timtuxworth commented Nov 5, 2023

In both cases there directly returning the parameter variable.

Yes I see that the comment in AC_Fence.h was - shall we say "ambiguous"

@tridge tridge merged commit ae2ab08 into ArduPilot:master Nov 6, 2023
87 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

4 participants