From fe7e2ed657654835fc2ebb338b0c997ed2208e78 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Nov 2021 17:18:21 +1100 Subject: [PATCH] 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 --- .../examples/plane_aerobatics.lua | 178 +++++++++++++++--- 1 file changed, 153 insertions(+), 25 deletions(-) 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()