diff --git a/libraries/AP_Scripting/examples/plane_aerobatics.lua b/libraries/AP_Scripting/examples/plane_aerobatics.lua deleted file mode 100644 index e823263f60..0000000000 --- a/libraries/AP_Scripting/examples/plane_aerobatics.lua +++ /dev/null @@ -1,395 +0,0 @@ --- perform simple aerobatic manoeuvres in AUTO mode - -local running = false - -local roll_stage = 0 - -local ROLL_TCONST = param:get('RLL2SRV_TCONST') * 0.5 -local PITCH_TCONST = param:get('PTCH2SRV_TCONST') * 0.5 - -DO_JUMP = 177 -NAV_WAYPOINT = 16 - -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) - if v < vmin then - v = vmin - end - if v > vmax then - v = vmax - end - return v -end - --- a controller to target a zero roll angle, coping with inverted flight --- output is a body frame roll rate, with convergence over time tconst in seconds -function roll_zero_controller(tconst) - local roll_deg = math.deg(ahrs:get_roll()) - local pitch_deg = math.deg(ahrs:get_pitch()) - local roll_err = 0.0 - if math.abs(pitch_deg) > 85 then - -- close to 90 we retain the last roll rate - roll_err = last_roll_err - elseif roll_deg > 90 then - roll_err = 180 - roll_deg - elseif roll_deg < -90 then - roll_err = (-180) - roll_deg - else - roll_err = -roll_deg - end - last_roll_err = roll_err - return roll_err / tconst -end - - -function wrap_360(angle) - local res = math.fmod(angle, 360.0) - if res < 0 then - res = res + 360.0 - end - return res -end - -function wrap_180(angle) - local res = wrap_360(angle) - if res > 180 then - res = res - 360 - end - 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_location():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) - local roll_deg = math.deg(ahrs:get_roll()) - local pitch_deg = math.deg(ahrs:get_pitch()) - local yaw_deg = math.deg(ahrs:get_yaw()) - - -- get earth frame pitch and yaw rates - local ef_pitch_rate = (target_pitch_deg - pitch_deg) / tconst - local ef_yaw_rate = wrap_180(target_yaw_deg - yaw_deg) / tconst - - local bf_pitch_rate = math.sin(math.rad(roll_deg)) * ef_yaw_rate + math.cos(math.rad(roll_deg)) * ef_pitch_rate - local bf_yaw_rate = math.cos(math.rad(roll_deg)) * ef_yaw_rate - math.sin(math.rad(roll_deg)) * ef_pitch_rate - return bf_pitch_rate, bf_yaw_rate -end - --- a controller for throttle to account for pitch -function throttle_controller(tconst) - local pitch_rad = ahrs:get_pitch() - 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 - -function do_axial_roll(arg1, arg2) - -- constant roll rate axial roll - 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 - local throttle = arg2 - local pitch_deg = math.deg(ahrs:get_pitch()) - local roll_deg = math.deg(ahrs:get_roll()) - if roll_stage == 0 then - if roll_deg > 45 then - roll_stage = 1 - end - elseif roll_stage == 1 then - if roll_deg > -5 and roll_deg < 5 then - running = false - -- we're done - gcs:send_text(0, string.format("Finished roll r=%.1f p=%.1f", roll_deg, pitch_deg)) - vehicle:nav_script_time_done(last_id) - roll_stage = 2 - return - end - end - if roll_stage < 2 then - 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 -end - -local loop_stage = 0 - -function do_loop(arg1, arg2) - -- do one loop with controllable pitch rate and throttle - if not running then - running = true - loop_stage = 0 - gcs:send_text(0, string.format("Starting loop")) - end - local pitch_rate = arg1 - local throttle = throttle_controller() - local pitch_deg = math.deg(ahrs:get_pitch()) - local roll_deg = math.deg(ahrs:get_roll()) - if loop_stage == 0 then - if pitch_deg > 60 then - loop_stage = 1 - end - elseif loop_stage == 1 then - if math.abs(roll_deg) < 90 and pitch_deg > -5 and pitch_deg < 5 then - running = false - -- we're done - gcs:send_text(0, string.format("Finished loop p=%.1f", pitch_deg)) - vehicle:nav_script_time_done(last_id) - loop_stage = 2 - return - end - end - if loop_stage < 2 then - local roll_rate = roll_zero_controller(ROLL_TCONST) - vehicle:set_target_throttle_rate_rpy(throttle, roll_rate, pitch_rate, 0) - end -end - -local rolling_circle_stage = 0 -local rolling_circle_yaw = 0 -local rolling_circle_last_ms = 0 - -function do_rolling_circle(arg1, arg2) - -- constant roll rate circle roll - if not running then - running = true - rolling_circle_stage = 0 - 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 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 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 math.abs(rolling_circle_yaw) > 10.0 then - rolling_circle_stage = 1 - end - elseif rolling_circle_stage == 1 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)) - vehicle:nav_script_time_done(last_id) - rolling_circle_stage = 2 - 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 = 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 - ---- get a location object for a given WP number -function get_wp_location(i) - local m = mission:get_item(i) - local loc = Location() - loc:lat(m:x()) - loc:lng(m:y()) - loc:relative_alt(false) - loc:terrain_alt(false) - loc:origin_alt(false) - loc:alt(math.floor(m:z()*100)) - return loc -end - -function resolve_jump(i) - local m = mission:get_item(i) - while m:command() == DO_JUMP do - i = math.floor(m:param1()) - m = mission:get_item(i) - end - return i -end - -function update() - id, cmd, arg1, arg2 = vehicle:nav_script_time() - if id then - if id ~= last_id then - -- we've started a new command - running = false - last_id = id - initial_yaw_deg = math.deg(ahrs:get_yaw()) - initial_height = ahrs:get_location():alt()*0.01 - - -- work out yaw between previous WP and next WP - local cnum = mission:get_current_nav_index() - local loc_prev = get_wp_location(cnum-1) - local loc_next = get_wp_location(resolve_jump(cnum+1)) - wp_yaw_deg = math.deg(loc_prev:get_bearing(loc_next)) - end - if cmd == 1 then - do_axial_roll(arg1, arg2) - elseif cmd == 2 then - do_loop(arg1, arg2) - elseif cmd == 3 then - do_rolling_circle(arg1, arg2) - end - else - running = false - end - return update, 10 -end - -return update()