mirror of https://github.com/ArduPilot/ardupilot
AP_Scripting: convert all examples to new parameter constructior
This commit is contained in:
parent
41e36e3128
commit
89311b4227
|
@ -1,10 +1,7 @@
|
|||
-- switch between DCM and EKF3 on a switch
|
||||
|
||||
local scripting_rc1 = rc:find_channel_for_option(300)
|
||||
local EKF_TYPE = Parameter()
|
||||
if not EKF_TYPE:init('AHRS_EKF_TYPE') then
|
||||
gcs:send_text(6, 'init AHRS_EKF_TYPE failed')
|
||||
end
|
||||
local EKF_TYPE = Parameter('AHRS_EKF_TYPE')
|
||||
|
||||
function update()
|
||||
local sw_pos = scripting_rc1:get_aux_switch_pos()
|
||||
|
|
|
@ -21,22 +21,16 @@ assert(param:add_param(PARAM_TABLE_KEY, 6, 'SPD_I', 25), 'could not add param9'
|
|||
DO_JUMP = 177
|
||||
k_throttle = 70
|
||||
|
||||
function bind_param(name)
|
||||
local p = Parameter()
|
||||
assert(p:init(name), string.format('could not find %s parameter', name))
|
||||
return p
|
||||
end
|
||||
|
||||
local HGT_P = bind_param("AEROM_HGT_P") -- height P gain
|
||||
local HGT_I = bind_param("AEROM_HGT_I") -- height I gain
|
||||
local HGT_KE_BIAS = bind_param("AEROM_HGT_KE_ADD") -- height knifeedge addition for pitch
|
||||
local THR_PIT_FF = bind_param("AEROM_THR_PIT_FF") -- throttle FF from pitch
|
||||
local SPD_P = bind_param("AEROM_SPD_P") -- speed P gain
|
||||
local SPD_I = bind_param("AEROM_SPD_I") -- speed I gain
|
||||
local TRIM_THROTTLE = bind_param("TRIM_THROTTLE")
|
||||
local TRIM_ARSPD_CM = bind_param("TRIM_ARSPD_CM")
|
||||
local RLL2SRV_TCONST = bind_param("RLL2SRV_TCONST")
|
||||
local PITCH_TCONST = bind_param("PTCH2SRV_TCONST")
|
||||
local HGT_P = Parameter("AEROM_HGT_P") -- height P gain
|
||||
local HGT_I = Parameter("AEROM_HGT_I") -- height I gain
|
||||
local HGT_KE_BIAS = Parameter("AEROM_HGT_KE_ADD") -- height knifeedge addition for pitch
|
||||
local THR_PIT_FF = Parameter("AEROM_THR_PIT_FF") -- throttle FF from pitch
|
||||
local SPD_P = Parameter("AEROM_SPD_P") -- speed P gain
|
||||
local SPD_I = Parameter("AEROM_SPD_I") -- speed I gain
|
||||
local TRIM_THROTTLE = Parameter("TRIM_THROTTLE")
|
||||
local TRIM_ARSPD_CM = Parameter("TRIM_ARSPD_CM")
|
||||
local RLL2SRV_TCONST = Parameter("RLL2SRV_TCONST")
|
||||
local PITCH_TCONST = Parameter("PTCH2SRV_TCONST")
|
||||
|
||||
local last_roll_err = 0.0
|
||||
local last_id = 0
|
||||
|
|
|
@ -27,14 +27,7 @@ assert(param:add_param(PARAM_TABLE_KEY, 10, 'TRICK_ANG', 90), 'could not add par
|
|||
assert(param:add_param(PARAM_TABLE_KEY, 11, 'TRICK_RAT', 90), 'could not add param11') -- used for trick loop rate, axial roll rates, etc
|
||||
|
||||
|
||||
-- bind a parameter to a variable
|
||||
function bind_param(name)
|
||||
local p = Parameter()
|
||||
assert(p:init(name), string.format('could not find %s parameter', name))
|
||||
return p
|
||||
end
|
||||
|
||||
local trick_id = bind_param("AERO_TRICK_ID")
|
||||
local trick_id = Parameter("AERO_TRICK_ID")
|
||||
local sw_last = 0
|
||||
local sw_current = 0
|
||||
local last_selection = 0
|
||||
|
|
|
@ -35,16 +35,13 @@ local auto_switch = false -- true when auto switching between sources
|
|||
local gps_usable_accuracy = 1.0 -- GPS is usable if speed accuracy is at or below this value
|
||||
local vote_counter_max = 20 -- when a vote counter reaches this number (i.e. 2sec) source may be switched
|
||||
local gps_vs_opticalflow_vote = 0 -- vote counter for GPS vs optical (-20 = GPS, +20 = optical flow)
|
||||
local scr_user1_param = Parameter() -- user1 param (rangefinder altitude threshold)
|
||||
local scr_user2_param = Parameter() -- user2 param (GPS speed accuracy threshold)
|
||||
local scr_user3_param = Parameter() -- user3 param (optical flow quality threshold)
|
||||
local scr_user4_param = Parameter() -- user4 param (optical flow innovation threshold)
|
||||
|
||||
-- initialise parameters
|
||||
assert(scr_user1_param:init('SCR_USER1'), 'could not find SCR_USER1 parameter')
|
||||
assert(scr_user2_param:init('SCR_USER2'), 'could not find SCR_USER2 parameter')
|
||||
assert(scr_user3_param:init('SCR_USER3'), 'could not find SCR_USER3 parameter')
|
||||
assert(scr_user4_param:init('SCR_USER4'), 'could not find SCR_USER4 parameter')
|
||||
local scr_user1_param = Parameter('SCR_USER1') -- user1 param (rangefinder altitude threshold)
|
||||
local scr_user2_param = Parameter('SCR_USER2') -- user2 param (GPS speed accuracy threshold)
|
||||
local scr_user3_param = Parameter('SCR_USER3') -- user3 param (optical flow quality threshold)
|
||||
local scr_user4_param = Parameter('SCR_USER4') -- user4 param (optical flow innovation threshold)
|
||||
|
||||
assert(optical_flow, 'could not access optical flow')
|
||||
|
||||
-- play tune on buzzer to alert user to change in active source set
|
||||
|
|
|
@ -71,22 +71,17 @@ assert(param:add_param(PARAM_TABLE_KEY, 8, 'FLY_TIMEOUT', 30), 'could not add DR
|
|||
assert(param:add_param(PARAM_TABLE_KEY, 9, 'NEXT_MODE', 6), 'could not add DR_NEXT_MODE param') -- mode to switch to after GPS recovers or timeout elapses
|
||||
|
||||
-- bind parameters to variables
|
||||
function bind_param(name)
|
||||
local p = Parameter()
|
||||
assert(p:init(name), string.format('could not find %s parameter', name))
|
||||
return p
|
||||
end
|
||||
local enable = bind_param("DR_ENABLE") -- 1 = enabled, 0 = disabled
|
||||
local enable_dist = bind_param("DR_ENABLE_DIST") -- distance from home (in meters) beyond which the dead reckoning will be enabled
|
||||
local gps_speed_acc_max = bind_param("DR_GPS_SACC_MAX") -- GPS speed accuracy max threshold
|
||||
local gps_sat_count_min = bind_param("DR_GPS_SAT_MIN") -- GPS satellite count min threshold
|
||||
local gps_trigger_sec = bind_param("DR_GPS_TRIGG_SEC") -- GPS checks must fail for this many seconds before dead reckoning will be triggered
|
||||
local fly_angle = bind_param("DR_FLY_ANGLE") -- lean angle (in degrees) during deadreckoning
|
||||
local fly_alt_min = bind_param("DR_FLY_ALT_MIN") -- min alt above home (in meters) during deadreckoning
|
||||
local fly_timeoout = bind_param("DR_FLY_TIMEOUT") -- deadreckoning timeout (in seconds)
|
||||
local next_mode = bind_param("DR_NEXT_MODE") -- mode to switch to after GPS recovers or timeout elapses
|
||||
local wpnav_speedup = bind_param("WPNAV_SPEED_UP") -- maximum climb rate from WPNAV_SPEED_UP
|
||||
local wpnav_accel_z = bind_param("WPNAV_ACCEL_Z") -- maximum vertical acceleration from WPNAV_ACCEL_Z
|
||||
local enable = Parameter("DR_ENABLE") -- 1 = enabled, 0 = disabled
|
||||
local enable_dist = Parameter("DR_ENABLE_DIST") -- distance from home (in meters) beyond which the dead reckoning will be enabled
|
||||
local gps_speed_acc_max = Parameter("DR_GPS_SACC_MAX") -- GPS speed accuracy max threshold
|
||||
local gps_sat_count_min = Parameter("DR_GPS_SAT_MIN") -- GPS satellite count min threshold
|
||||
local gps_trigger_sec = Parameter("DR_GPS_TRIGG_SEC") -- GPS checks must fail for this many seconds before dead reckoning will be triggered
|
||||
local fly_angle = Parameter("DR_FLY_ANGLE") -- lean angle (in degrees) during deadreckoning
|
||||
local fly_alt_min = Parameter("DR_FLY_ALT_MIN") -- min alt above home (in meters) during deadreckoning
|
||||
local fly_timeoout = Parameter("DR_FLY_TIMEOUT") -- deadreckoning timeout (in seconds)
|
||||
local next_mode = Parameter("DR_NEXT_MODE") -- mode to switch to after GPS recovers or timeout elapses
|
||||
local wpnav_speedup = Parameter("WPNAV_SPEED_UP") -- maximum climb rate from WPNAV_SPEED_UP
|
||||
local wpnav_accel_z = Parameter("WPNAV_ACCEL_Z") -- maximum vertical acceleration from WPNAV_ACCEL_Z
|
||||
|
||||
-- modes deadreckoning may be activated from
|
||||
-- comment out lines below to remove protection from these modes
|
||||
|
|
|
@ -44,17 +44,12 @@ assert(param:add_param(PARAM_TABLE_KEY, 5, 'SPEED_DN', 10), 'could not add FDST_
|
|||
assert(param:add_param(PARAM_TABLE_KEY, 6, 'YAW_BEHAVE', 0), 'could not add FDST_YAW_BEHAVE param') -- 0:yaw does not change 1:yaw points toward center
|
||||
|
||||
-- bind parameters to variables
|
||||
function bind_param(name)
|
||||
local p = Parameter()
|
||||
assert(p:init(name), string.format('could not find %s parameter', name))
|
||||
return p
|
||||
end
|
||||
local activate_type = bind_param("FDST_ACTIVATE") -- activate type 0:Guided, 1:Auto's NAV_SCRIPT_TIME
|
||||
local alt_above_home_min = bind_param("FDST_ALT_MIN") -- copter will stop at this altitude above home
|
||||
local circle_radius_max = bind_param("FDST_RADIUS") -- target circle's maximum radius
|
||||
local speed_xy_max = bind_param("FDST_SPEED_XY") -- max target horizontal speed
|
||||
local speed_z_max = bind_param("FDST_SPEED_DN") -- target descent rate
|
||||
local yaw_behave = bind_param("FDST_YAW_BEHAVE") -- 0:yaw is static, 1:yaw points towards center of circle
|
||||
local activate_type = Parameter("FDST_ACTIVATE") -- activate type 0:Guided, 1:Auto's NAV_SCRIPT_TIME
|
||||
local alt_above_home_min = Parameter("FDST_ALT_MIN") -- copter will stop at this altitude above home
|
||||
local circle_radius_max = Parameter("FDST_RADIUS") -- target circle's maximum radius
|
||||
local speed_xy_max = Parameter("FDST_SPEED_XY") -- max target horizontal speed
|
||||
local speed_z_max = Parameter("FDST_SPEED_DN") -- target descent rate
|
||||
local yaw_behave = Parameter("FDST_YAW_BEHAVE") -- 0:yaw is static, 1:yaw points towards center of circle
|
||||
|
||||
-- the main update function
|
||||
function update()
|
||||
|
|
|
@ -13,17 +13,10 @@ local PARAM_TABLE_KEY = 77
|
|||
local PARAM_TABLE_PREFIX = "VTFS_"
|
||||
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 4), 'could not add param table')
|
||||
|
||||
-- bind a parameter to a variable
|
||||
function bind_param(name)
|
||||
local p = Parameter()
|
||||
assert(p:init(name), string.format('could not find %s parameter', name))
|
||||
return p
|
||||
end
|
||||
|
||||
-- add a parameter and bind it to a variable
|
||||
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)
|
||||
return Parameter(PARAM_TABLE_PREFIX .. name)
|
||||
end
|
||||
|
||||
-- consider engine stopped when vibe is low and RPM low for more than 4s
|
||||
|
|
|
@ -1,13 +1,6 @@
|
|||
-- enable use of Lidar on quadplanes only for landing, by changing RNGFN_LANDING
|
||||
|
||||
-- bind a parameter to a variable
|
||||
function bind_param(name)
|
||||
local p = Parameter()
|
||||
assert(p:init(name), string.format('could not find %s parameter', name))
|
||||
return p
|
||||
end
|
||||
|
||||
local RNGFND_LANDING = bind_param("RNGFND_LANDING")
|
||||
local RNGFND_LANDING = Parameter("RNGFND_LANDING")
|
||||
|
||||
MODE_QLAND = 20
|
||||
MODE_QRTL = 21
|
||||
|
|
|
@ -9,19 +9,13 @@ local PARAM_TABLE_KEY = 83
|
|||
local PARAM_TABLE_PREFIX = "FOLL_"
|
||||
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 2), 'could not add param table')
|
||||
|
||||
function bind_param(name)
|
||||
local p = Parameter()
|
||||
assert(p:init(name), string.format('could not find %s parameter', name))
|
||||
return p
|
||||
end
|
||||
|
||||
function bind_add_param(name, index, default_value)
|
||||
assert(param:add_param(PARAM_TABLE_KEY, index, name, default_value), string.format('could not add %s', PARAM_TABLE_PREFIX .. name))
|
||||
return bind_param(PARAM_TABLE_PREFIX .. name)
|
||||
return Parameter(PARAM_TABLE_PREFIX .. name)
|
||||
end
|
||||
|
||||
local FOLL_OFS_X = bind_param("FOLL_OFS_X")
|
||||
local FOLL_OFS_Y = bind_param("FOLL_OFS_Y")
|
||||
local FOLL_OFS_X = Parameter("FOLL_OFS_X")
|
||||
local FOLL_OFS_Y = Parameter("FOLL_OFS_Y")
|
||||
local FOLL_ORB_RADIUS = bind_add_param("ORB_RADIUS", 1, 5)
|
||||
local FOLL_ORB_TIME = bind_add_param("ORB_TIME", 2, 10)
|
||||
|
||||
|
|
|
@ -20,15 +20,8 @@ assert(param:add_param(PARAM_TABLE_KEY, 2, 'TEST2', 5.7), 'could not add param2'
|
|||
|
||||
gcs:send_text(0, string.format("Added two parameters"))
|
||||
|
||||
-- bind a parameter to a variable
|
||||
function bind_param(name)
|
||||
local p = Parameter()
|
||||
assert(p:init(name), string.format('could not find %s parameter', name))
|
||||
return p
|
||||
end
|
||||
|
||||
local param1 = bind_param("MY_TEST")
|
||||
local param2 = bind_param("MY_TEST2")
|
||||
local param1 = Parameter("MY_TEST")
|
||||
local param2 = Parameter("MY_TEST2")
|
||||
|
||||
gcs:send_text(0, string.format("param1=%f", param1:get()))
|
||||
gcs:send_text(0, string.format("param2=%f", param2:get()))
|
||||
|
|
Loading…
Reference in New Issue