diff --git a/libraries/AP_Scripting/examples/plane_aerobatics.lua b/libraries/AP_Scripting/examples/plane_aerobatics.lua index 86b487d016..6862d7e092 100644 --- a/libraries/AP_Scripting/examples/plane_aerobatics.lua +++ b/libraries/AP_Scripting/examples/plane_aerobatics.lua @@ -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()