edc4 experimental

set taltpid to pidloop(10, 0.7, 1.5, 100, 360).

set steeringmanager:pitchtorquefactor to 0.2.
set steeringmanager:yawtorquefactor to 0.5.
set steeringmanager:rollcontrolanglerange to 180.

steeringmanager:resetpids().
lock x to 90.
set p to 0.
set a to 2.
set talt to 300.
lock steering to heading(x,p).
lock deploy to max(3,airspeed^0.7).
lock tilt to max(75,min(90,alt:radar*90/talt)).
set blades to ship:partsdubbedpattern("blade").
Set servos to ship:partsdubbedpattern("servo").
set rotors to ship:partsdubbedpattern("rotor").
lock tilt to 0.
for rotor in rotors rotor:getmodulebyindex(1):setfield("torque limit(%)",100).
set taltpid:setpoint to talt.
on talt {set taltpid:setpoint to talt. return true. }

lock rpmlimit to taltpid:update(time:Seconds, altitude).
when alt:radar > 100 then { set tilt to 20. lock x to allwaypoints()[2]:geoposition:heading - 90. }
set adjust to true.
when true then {
	For b in blades b:getmodule("ModuleControlSurface"):setfield("deploy angle",deploy).
	for rotor in rotors rotor:getmodulebyindex(1):setfield("rpm limit",rpmlimit).
	For s in servos s:getmodule("ModuleRoboticRotationServo"):setfield("target angle",tilt).
	return adjust.
}

set sgp to 0.
set sggo to true.
set sgf to 8.
set sgpr to 4.
function sgd {
	parameter l.
	parameter d.
	If l:length > sgpr set sgpr to l:length.
	print (l:padleft(sgpr) + ": " +round(d,2)):padright(terminal:width) at (0, sgp).
	set sgp to sgp + 1.
}.

On round(kuniverse:realtime * sgf) {
	set sgp to 0. sgd("vs", ship:verticalspeed).
	sgd("rpmlimit", rpmlimit).
	sgd("output", taltpid:output).
	sgd("error", taltpid:error).
	sgd("setpoint", taltpid:setpoint).
	sgd("P", taltpid:Pterm).
	sgd("I", taltpid:Iterm).
	sgd("D", taltpid:Dterm).
	sgd("kP", taltpid:kp).
	sgd("kI", taltpid:ki).
	sgd("kD", taltpid:kd).
	return sggo.
}