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

TradHeli: script for idle control #25706

Merged
merged 1 commit into from
Jan 10, 2024
Merged

Conversation

Ferruccio1984
Copy link
Contributor

allows manual and/or automatic engine rpm control during ground idling.
Handles transition to flight regime and does not affect pre-existing functinalities (idle setting for manual autorotation -H_RSC_AROT_IDLE).
Video below for reference (ch10 as script auxiliary potentiometer/ H_RSC_AROT_IDLE set to 6):
https://www.youtube.com/watch?v=Iau5VVvgd3A

Comment on lines 101 to 105
local now = millis():tofloat() * 0.001
if not _last_t then
_last_t = now
end
local dt = now - _last_t
Copy link
Member

Choose a reason for hiding this comment

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

Better to convert to float as the last step. Currently you will loose precision as millis gets larger.

Suggested change
local now = millis():tofloat() * 0.001
if not _last_t then
_last_t = now
end
local dt = now - _last_t
local now = millis()
if not _last_t then
_last_t = now
end
local dt = (now - _last_t):tofloat() * 0.001

Comment on lines 117 to 118
local I = _I
local ret = P + I
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
local I = _I
local ret = P + I
local ret = P + _I

end
end
end
last_idc_time = millis():tofloat()
Copy link
Member

Choose a reason for hiding this comment

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

Same thing as above, using tofloat too soon.

end
-- smooth out rotor runup
if ramp_up_complete ~= true then
time_now = millis():tofloat()
Copy link
Member

Choose a reason for hiding this comment

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

tofloat again

@Ferruccio1984
Copy link
Contributor Author

Thanks @IamPete1

@Ferruccio1984
Copy link
Contributor Author

@IamPete1 The functionality has been tested in two additional use-cases:
1)idle rpm control(user Petr): https://discuss.ardupilot.org/t/how-to-make-pi-regulator-for-combustion-engine-idle/107611/18
2)manual idle control (user Allister):
image

Copy link
Member

@IamPete1 IamPete1 left a comment

Choose a reason for hiding this comment

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

Must pass luacheck.

This also assumes the servo is not reversed. It should probably check before doing a direct PWM override. Otherwise it might go to full throttle.

There is also lots of extra white space that needs tidying up at the end of lines. There is also a mixture of tabs and spaces for indenting.

@@ -0,0 +1,241 @@
-- idle_control.lua: a closed loop control throttle control while on ground idle (trad-heli)

-- luacheck: only 0
Copy link
Member

Choose a reason for hiding this comment

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

This is cheating, has to pass luacheck.

The errors are:

    libraries/AP_Scripting/applets/idle_control_heli.lua:44:7: unused variable switch
    libraries/AP_Scripting/applets/idle_control_heli.lua:45:7: unused variable rsc_pos
    libraries/AP_Scripting/applets/idle_control_heli.lua:56:7: unused variable last_thr_out
    libraries/AP_Scripting/applets/idle_control_heli.lua:92:10: variable _P is never accessed
    libraries/AP_Scripting/applets/idle_control_heli.lua:95:10: variable _target is never accessed
    libraries/AP_Scripting/applets/idle_control_heli.lua:96:10: variable _current is never accessed
    libraries/AP_Scripting/applets/idle_control_heli.lua:148:9: shadowing upvalue rsc_pos on line 45

Comment on lines 10 to 14
function bind_param(name)
local p = Parameter()
assert(p:init(name), string.format('could not find %s parameter', name))
return p
end
Copy link
Member

Choose a reason for hiding this comment

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

On 4.4 you don't need this, the Parameter object can take a name directly.


function bind_add_param(name, idx, default_value)
assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
return bind_param(PARAM_TABLE_PREFIX .. name)
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
return bind_param(PARAM_TABLE_PREFIX .. name)
return Parameter(PARAM_TABLE_PREFIX .. name)

Comment on lines 32 to 33
local H_RSC_IDLE = Parameter()
H_RSC_IDLE:init('H_RSC_IDLE')
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
local H_RSC_IDLE = Parameter()
H_RSC_IDLE:init('H_RSC_IDLE')
local H_RSC_IDLE = Parameter('H_RSC_IDLE')

Comment on lines 41 to 42
local H_RSC_RUNUP_TIME = Parameter()
H_RSC_RUNUP_TIME:init('H_RSC_RUNUP_TIME')
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
local H_RSC_RUNUP_TIME = Parameter()
H_RSC_RUNUP_TIME:init('H_RSC_RUNUP_TIME')
local H_RSC_RUNUP_TIME = Parameter('H_RSC_RUNUP_TIME')

Comment on lines 35 to 39
local SERVO8_MAX = Parameter()
SERVO8_MAX:init('SERVO8_MAX')

local SERVO8_MIN = Parameter()
SERVO8_MIN:init('SERVO8_MIN')
Copy link
Member

Choose a reason for hiding this comment

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

You fetch the rsc channel later in the script but have the endpoints hard coded to servo 8 here. This should work:

Suggested change
local SERVO8_MAX = Parameter()
SERVO8_MAX:init('SERVO8_MAX')
local SERVO8_MIN = Parameter()
SERVO8_MIN:init('SERVO8_MIN')
local rsc_output = SRV_Channels:find_channel(31)
local SERVO_MAX = Parameter('SERVO' .. (rsc_output+1) .. '_MAX')
local SERVO_MIN = Parameter('SERVO' .. (rsc_output+1) .. '_MIN')

I have not tested.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

yes, I was looking indeed for a better way to retrieve endpoints through the servo function instead of hardcoding stuff (so to consider configs different from default).


gcs:send_text(5, "idle_control_running")

return update()
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
return update()
return update()

@Ferruccio1984
Copy link
Contributor Author

Hi @IamPete1 , implemented the requested changes (hopefully right). I tried the script for automatic code formatting but I wasn't sure it looked alright...so did by hand.
Now luacheck pass (with magic line "removed" :-) ):
Screenshot 2023-12-30 181706
Tested the logic for servo normal/reverse:
Screenshot 2023-12-30 184129
Screenshot 2023-12-30 184838

@IamPete1
Copy link
Member

It passes lua check and had been tested I'm OK with it going in. Does need those two fixup commits squashing into the first.

@rmackay9
Copy link
Contributor

rmackay9 commented Jan 8, 2024

Looking pretty good.

One small thing is I think for vehicle specific scripts we should put the vehicle name at the front (instead of the end).

@Ferruccio1984 Ferruccio1984 changed the title AP_Scripting: script for idle control (gas helicopters) TradHeli: script for idle control Jan 9, 2024
allows manual and/or automatic engine rpm control during ground idling

fix for conversion to float

rename fix
Copy link
Contributor

@bnsgeyer bnsgeyer left a comment

Choose a reason for hiding this comment

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

LGTM. Thanks!

@bnsgeyer bnsgeyer merged commit 8bfd8f2 into ArduPilot:master Jan 10, 2024
91 checks passed
@Ferruccio1984
Copy link
Contributor Author

LGTM. Thanks!

Thanks @bnsgeyer !

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

5 participants