mirror of https://github.com/ArduPilot/ardupilot
AP_Scripting: added throttle and height controller to aerobatic example
changed rolling circle to take the radius and number of circles. negative radius for negative yaw rate and negative number of circles for left roll
This commit is contained in:
Andrew Tridgell
parent
140adc126a
commit
fe7e2ed657
|
|
@ -6,24 +6,32 @@ local roll_stage = 0
|
|||
|
||||
local ROLL_TCONST = param:get('RLL2SRV_TCONST') * 0.5
|
||||
local PITCH_TCONST = param:get('PTCH2SRV_TCONST') * 0.5
|
||||
local TRIM_THROTTLE = param:get('TRIM_THROTTLE') * 1.0
|
||||
|
||||
DO_JUMP = 177
|
||||
NAV_WAYPOINT = 16
|
||||
|
||||
local scr_user1_param = Parameter()
|
||||
local scr_user2_param = Parameter()
|
||||
local scr_user3_param = Parameter()
|
||||
local scr_user4_param = Parameter()
|
||||
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')
|
||||
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 SCR_USER1 = bind_param("SCR_USER1") -- height P gain
|
||||
local SCR_USER2 = bind_param("SCR_USER2") -- height I gain
|
||||
local SCR_USER3 = bind_param("SCR_USER3") -- throttle FF from pitch
|
||||
local SCR_USER4 = bind_param("SCR_USER4") -- height knifeedge addition for pitch
|
||||
local SCR_USER5 = bind_param("SCR_USER5") -- speed P gain
|
||||
local SCR_USER6 = bind_param("SCR_USER6") -- speed I gain
|
||||
local TRIM_THROTTLE = bind_param("TRIM_THROTTLE")
|
||||
local TRIM_ARSPD_CM = bind_param("TRIM_ARSPD_CM")
|
||||
|
||||
local last_roll_err = 0.0
|
||||
local last_id = 0
|
||||
local initial_yaw_deg = 0
|
||||
local wp_yaw_deg = 0
|
||||
local initial_height = 0
|
||||
|
||||
-- constrain a value between limits
|
||||
function constrain(v, vmin, vmax)
|
||||
|
|
@ -73,6 +81,106 @@ function wrap_180(angle)
|
|||
return res
|
||||
end
|
||||
|
||||
-- a PI controller implemented as a Lua object
|
||||
local function PI_controller(kP,kI,iMax)
|
||||
-- the new instance. You can put public variables inside this self
|
||||
-- declaration if you want to
|
||||
local self = {}
|
||||
|
||||
-- private fields as locals
|
||||
local _kP = kP or 0.0
|
||||
local _kI = kI or 0.0
|
||||
local _kD = kD or 0.0
|
||||
local _iMax = iMax
|
||||
local _last_t = nil
|
||||
local _I = 0
|
||||
local _P = 0
|
||||
local _total = 0
|
||||
local _counter = 0
|
||||
local _target = 0
|
||||
local _current = 0
|
||||
|
||||
-- update the controller.
|
||||
function self.update(target, current)
|
||||
local now = millis():tofloat() * 0.001
|
||||
if not _last_t then
|
||||
_last_t = now
|
||||
end
|
||||
local dt = now - _last_t
|
||||
_last_t = now
|
||||
local err = target - current
|
||||
_counter = _counter + 1
|
||||
|
||||
local P = _kP * err
|
||||
_I = _I + _kI * err * dt
|
||||
if _iMax then
|
||||
_I = constrain(_I, -_iMax, iMax)
|
||||
end
|
||||
local I = _I
|
||||
local ret = P + I
|
||||
|
||||
_target = target
|
||||
_current = current
|
||||
_P = P
|
||||
_total = ret
|
||||
return ret
|
||||
end
|
||||
|
||||
-- reset integrator to an initial value
|
||||
function self.reset(integrator)
|
||||
_I = integrator
|
||||
end
|
||||
|
||||
function self.set_I(I)
|
||||
_kI = I
|
||||
end
|
||||
|
||||
function self.set_P(P)
|
||||
_kP = P
|
||||
end
|
||||
|
||||
function self.set_Imax(Imax)
|
||||
_iMax = Imax
|
||||
end
|
||||
|
||||
-- log the controller internals
|
||||
function self.log(name, add_total)
|
||||
-- allow for an external addition to total
|
||||
logger.write(name,'Targ,Curr,P,I,Total,Add','ffffff',_target,_current,_P,_I,_total,add_total)
|
||||
end
|
||||
|
||||
-- return the instance
|
||||
return self
|
||||
end
|
||||
|
||||
local function height_controller(kP_param,kI_param,KnifeEdge_param,Imax)
|
||||
local self = {}
|
||||
local kP = kP_param
|
||||
local kI = kI_param
|
||||
local KnifeEdge = KnifeEdge_param
|
||||
local PI = PI_controller(kP:get(), kI:get(), Imax)
|
||||
|
||||
function self.update(target)
|
||||
local target_pitch = PI.update(initial_height, ahrs:get_position():alt()*0.01)
|
||||
local roll_rad = ahrs:get_roll()
|
||||
local ke_add = math.abs(math.sin(roll_rad)) * KnifeEdge:get()
|
||||
target_pitch = target_pitch + ke_add
|
||||
PI.log("HPI", ke_add)
|
||||
return target_pitch
|
||||
end
|
||||
|
||||
function self.reset()
|
||||
PI.reset(math.max(math.deg(ahrs:get_pitch()), 3.0))
|
||||
PI.set_P(kP:get())
|
||||
PI.set_I(kI:get())
|
||||
end
|
||||
|
||||
return self
|
||||
end
|
||||
|
||||
local height_PI = height_controller(SCR_USER1, SCR_USER2, SCR_USER4, 20.0)
|
||||
local speed_PI = PI_controller(SCR_USER5:get(), SCR_USER6:get(), 100.0)
|
||||
|
||||
-- a controller to target a zero pitch angle and zero heading change, used in a roll
|
||||
-- output is a body frame pitch rate, with convergence over time tconst in seconds
|
||||
function pitch_controller(target_pitch_deg, target_yaw_deg, tconst)
|
||||
|
|
@ -92,8 +200,8 @@ end
|
|||
-- a controller for throttle to account for pitch
|
||||
function throttle_controller(tconst)
|
||||
local pitch_rad = ahrs:get_pitch()
|
||||
local thr_ff = scr_user3_param:get()
|
||||
local throttle = TRIM_THROTTLE + math.sin(pitch_rad) * thr_ff
|
||||
local thr_ff = SCR_USER3:get()
|
||||
local throttle = TRIM_THROTTLE:get() + math.sin(pitch_rad) * thr_ff
|
||||
return constrain(throttle, 0.0, 100.0)
|
||||
end
|
||||
|
||||
|
|
@ -102,6 +210,7 @@ function do_axial_roll(arg1, arg2)
|
|||
if not running then
|
||||
running = true
|
||||
roll_stage = 0
|
||||
height_PI.reset()
|
||||
gcs:send_text(0, string.format("Starting roll"))
|
||||
end
|
||||
local roll_rate = arg1
|
||||
|
|
@ -123,7 +232,7 @@ function do_axial_roll(arg1, arg2)
|
|||
end
|
||||
end
|
||||
if roll_stage < 2 then
|
||||
target_pitch = scr_user2_param:get()
|
||||
target_pitch = height_PI.update(initial_height)
|
||||
pitch_rate, yaw_rate = pitch_controller(target_pitch, wp_yaw_deg, PITCH_TCONST)
|
||||
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate)
|
||||
end
|
||||
|
|
@ -171,34 +280,42 @@ function do_rolling_circle(arg1, arg2)
|
|||
if not running then
|
||||
running = true
|
||||
rolling_circle_stage = 0
|
||||
rolling_circle_yaw = initial_yaw_deg
|
||||
rolling_circle_yaw = 0
|
||||
rolling_circle_last_ms = millis()
|
||||
height_PI.reset()
|
||||
|
||||
speed_PI.set_P(SCR_USER5:get())
|
||||
speed_PI.set_I(SCR_USER6:get())
|
||||
speed_PI.reset(math.max(SRV_Channels:get_output_scaled(k_throttle), TRIM_THROTTLE:get()))
|
||||
gcs:send_text(0, string.format("Starting rolling circle"))
|
||||
end
|
||||
local roll_rate = arg1
|
||||
local throttle = arg2
|
||||
local radius = arg1
|
||||
local num_rolls = arg2
|
||||
local pitch_deg = math.deg(ahrs:get_pitch())
|
||||
local roll_deg = math.deg(ahrs:get_roll())
|
||||
local yaw_deg = math.deg(ahrs:get_yaw())
|
||||
local gspeed = ahrs:groundspeed_vector():length()
|
||||
local radius = scr_user4_param:get()
|
||||
if radius < 1 then
|
||||
radius = 50.0
|
||||
end
|
||||
local circumference = math.pi * 2.0 * radius
|
||||
local circumference = math.abs(math.pi * 2.0 * radius)
|
||||
local circle_time = circumference / gspeed
|
||||
local yaw_rate_dps = 360.0 / circle_time
|
||||
local now_ms = millis()
|
||||
local dt = (now_ms - rolling_circle_last_ms):tofloat() * 0.001
|
||||
rolling_circle_last_ms = now_ms
|
||||
|
||||
if radius < 0.0 then
|
||||
yaw_rate_dps = -yaw_rate_dps
|
||||
end
|
||||
|
||||
local roll_rate = (360.0 * num_rolls) / circle_time
|
||||
|
||||
rolling_circle_yaw = rolling_circle_yaw + yaw_rate_dps * dt
|
||||
|
||||
if rolling_circle_stage == 0 then
|
||||
if wrap_180(yaw_deg - initial_yaw_deg) > 45 then
|
||||
if math.abs(rolling_circle_yaw) > 10.0 then
|
||||
rolling_circle_stage = 1
|
||||
end
|
||||
elseif rolling_circle_stage == 1 then
|
||||
if math.abs(wrap_180(yaw_deg - initial_yaw_deg)) < 5 then
|
||||
if math.abs(rolling_circle_yaw) >= 360.0 then
|
||||
running = false
|
||||
-- we're done
|
||||
gcs:send_text(0, string.format("Finished rollcircle r=%.1f p=%.1f", roll_deg, pitch_deg))
|
||||
|
|
@ -207,10 +324,20 @@ function do_rolling_circle(arg1, arg2)
|
|||
return
|
||||
end
|
||||
end
|
||||
|
||||
local target_roll = num_rolls * math.abs(rolling_circle_yaw)
|
||||
local roll_error = wrap_180(target_roll - roll_deg)
|
||||
local roll_error_P = 0.5
|
||||
local roll_rate_corrected = roll_rate + roll_error * roll_error_P
|
||||
|
||||
if rolling_circle_stage < 2 then
|
||||
target_pitch = scr_user2_param:get()
|
||||
pitch_rate, yaw_rate = pitch_controller(target_pitch, rolling_circle_yaw, PITCH_TCONST)
|
||||
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, yaw_rate)
|
||||
target_pitch = height_PI.update(initial_height)
|
||||
vel = ahrs:get_velocity_NED()
|
||||
throttle = speed_PI.update(TRIM_ARSPD_CM:get()*0.01, vel:length())
|
||||
throttle = constrain(throttle, 0, 100.0)
|
||||
speed_PI.log("SPI", 0.0)
|
||||
pitch_rate, yaw_rate = pitch_controller(target_pitch, wrap_360(rolling_circle_yaw+initial_yaw_deg), PITCH_TCONST)
|
||||
vehicle:set_target_throttle_rate_rpy(throttle, roll_rate_corrected, pitch_rate, yaw_rate)
|
||||
end
|
||||
end
|
||||
|
||||
|
|
@ -244,6 +371,7 @@ function update()
|
|||
running = false
|
||||
last_id = id
|
||||
initial_yaw_deg = math.deg(ahrs:get_yaw())
|
||||
initial_height = ahrs:get_position():alt()*0.01
|
||||
|
||||
-- work out yaw between previous WP and next WP
|
||||
local cnum = mission:get_current_nav_index()
|
||||
|
|
|
|||
Loading…
Reference in New Issue